Factor Graph with Local Constraints: A Magnetic Field/Pedestrian Dead Reckoning Integrated Navigation Method Based on a Constrained Factor Graph

Abstract

The method of multi-sensor integrated navigation improves navigation accuracy by fusing various sensor data. However, when a sensor is disturbed or malfunctions, incorrect measurement information will seriously affect the estimation of the trajectory, which will lead to a decrease in accuracy. Existing factor graph models based on weights can neither fully resist the influence of disturbances nor guarantee the local rationality of estimated trajectories. In this paper, a factor graph with local constraints model that fuses the magnetic field and pedestrian dead reckoning data is proposed to navigate complex curved trajectories. First, adding local constraints to the pedestrian dead reckoning measurement converts the navigation solution problem into a hard-constrained nonlinear least squares problem. Then, a mapping model is constructed to reconstruct the variable space and the Adam gradient algorithm is used to realize a fast calculation. The navigation accuracy of this algorithm is better than that of the state-of-the-art method in real-world experiments, with an average accuracy of 0.83 m.

1. Introduction

1.1. Magnetic Navigation

Magnetic fields can be used for navigation and positioning [1]. In environments where satellite signal propagation is blocked, such as indoors, underwater, or urban areas [2], navigation methods that do not rely on the GNSS (Global Navigation Satellite System) for autonomous navigation have become a research hotspot. When navigating based on IMUs (inertial measurement units), a cumulative error of the heading angle will be generated due to the offset of the gyroscope. The emergence of multi-source fusion positioning methods effectively suppresses the cumulative error propagation of IMUs. Among them, magnetic fields have become a very important source due to their relatively stable nature.

Nowadays, many researchers still use the Geomagnetic North Pole direction to calibrate the heading for navigation, which is also the research prototype of magnetic field/INS (Inertial Navigation System)-integrated navigation. Hu G et al. [3] used magnets to correct the course angle of the INS, and achieved an error performance of 1.17 m through combined navigation with the Kalman filter method in an urban area navigation experiment. Relying solely on the direction of the magnetic needle provides highly limited information. Some researchers have extracted more useful information from the magnetic field and developed more complex magnetic features for navigation. Among them, the features represented by magnetic vectors and magnetic gradients have been widely studied, and positioning methods based on fingerprints have also been introduced into research on magnetic navigation. Geomagnetic vectors and an inertial navigation system were combined by Storms W et al. [4] with a Kalman filter, and decimeter-level navigation was realized indoors to predict trajectories with the help of map information constraints. Chen Z et al. [5] integrated geomagnetic vectors with pitch and roll angles to further calculate the gradient information of the magnetic field for trajectory matching, for which the error was at the level of 100 m. The Magneto-Inductive method was used by Wahlström J et al. [6] to assist inertial navigation. Li et al. [7] used magnetic fingerprint matching and heading angle correction for fusion navigation.

Advancements in data fusion technologies have played a significant role. Kauffman K et al. [8] used a particle filter to combine INS and magnetic fingerprints to realize a navigation mode in which the front car collects fingerprint information in real time and the rear car tracks, navigates and locates. Hu F et al. [9] proposed an adaptive error correction extended Kalman (EKF) algorithm to build a multi-sensor navigation and positioning system, which has been applied to a swarm of drones. The combination of the EKF algorithm and a particle filter was used to fuse the magnetic field gradient and inertial navigation information by Gao D et al. [10]. The “plug and play” feature based on a factor graph helps deal with the challenge of multi-sensor asynchrony [11]. A composite positioning inertial/geomagnetic/lidar technology based on graph optimization was proposed by Zhao Y N et al. [12].

1.2. Error Type and Integrity Monitoring

Sensor measurement errors, which are the difference between the measured value and the true value, come from two parts: one part is caused by the sensor’s precision and is always present; the other part of the error, which is abnormal, is caused by possible external interference or sensor failure. The second type of error is often sudden and unpredictable. Unfortunately, few of the above algorithms have considered the impact of this second type of error; their shortcomings are also obvious. Research in the field of integrity monitoring and navigation can improve the detection of the second type of error, especially by using various algorithms applied in the data preprocessing stage. In addition, different sensor combinations may also cause conflicts between sensors due to the first type of error. This is because erroneous measurements often contradict some known conclusions, causing integrity problems.

There have been extensive advancements in developing integrity monitoring algorithms for GNSS and navigation in general [13]. Wang C et al. [14] used a signal quality monitor (SQM) as an important component to address the potential risks caused by satellite-induced signal anomalies. Wang L et al. [15] developed a new denoising algorithm for thunderstorm-induced vibration data to separate the low-frequency disturbance from GNSS displacement time series. The Advanced Receiver Autonomous Integrity Monitoring (ARAIM) framework was used by Yang S et al. [16] in order to enhance the user-level integrity of GNSS positioning. Meng Q et al. [17] took full advantage of the multi-constellation GNSS by using ARAIM. Integrity monitoring research of applied and integrated navigation has also made a lot of progress. Meng Q et al. [18] proposed an integrity monitoring strategy for all source navigation based on the least square form of the EKF and solution separation with sensor exclusion in case of detected faults. Joerger M et al. [19] proposed a method for isolating faulty systems and to reconstitute the Kalman state after excluding the fault to maintain the availability of the integrity solution. Arana G et al. [20] presented a new methodology to quantify robot localization safety by evaluating integrity risk when combining LIDAR and IMU.

1.3. Factor Graph Methods

The traditional factor graph model does not have an effective integrity monitoring mechanism, so researchers have tried to restructure the factor graph model to avoid the effects from external interference or sensor failure. Wei X et al. [21] proposed an improved factor graph algorithm. After introducing a weight function to adjust the credibility of each factor, the weight factor will be reduced to a very low level at the mutation position. Xu C et al. [22] developed a method that can actively monitor and adjust factor weights where errors occur. Limiting the offset of observations by constructing a piecewise limit function has shown some anti-interference performance [23]. However, such limitations from weighted factor graphs are not strict enough. Measurements that are anomalous under more extreme circumstances can still cause sensor conflicts.

The constrained factor graph (CFG) is a factor graph model with hard constraints, and is often used in the field of robotics to model hard-constrained problems. Ta D N et al. [24] extended the modeling capabilities of factor graphs to represent nonlinear dynamics using constraint factors in the quad-copter UAV obstacle avoidance task. These efforts effectively improved the robustness of the system. Sodhi P et al. [25] used the CFG to describe equality constraints like object contact and inequality constraints like collision avoidance. Yang S et al. [26] presented a novel factor-graph-based approach to solve the discrete-time finite-horizon Linear Quadratic Regulator problem subject to auxiliary linear equality constraints within and across time steps. In addition, the robot cluster distributed communication problem also has many constraints. Cunningham A et al. [27] proposed a novel approach to multi-robot SLAM, introduced the CFG as an extended graphical model and formulated Sequential Quadratic Programming (SQP) to solve this new model. Huang Y et al. [28] proposed a two-stage global and local optimization strategy designed for robust compatibility with the smoothing and mapping optimizer.

Constrained factor graphs have powerful modeling capabilities, but an objective function with hard constraints is not easy to optimize. Nashed S B et al. [29] were concerned that a certain combination of linear environmental features such as line segments, planes and sensors will generate rank deficit constraints, and proposed the robust rank deficient SLAM model. For CFGs, the domains of states and landmark variables are not vector spaces but Lie group manifolds [24]. Sodhi P et al. [25] further pointed out that the new optimality conditions introduced by the hard constraints break the matrix structure needed for incremental factorization in these incremental optimization methods. Qadri M et al. [30] incrementally updated the CFG by maintaining subgraphs on nodes using a Bayes tree instead of maintaining a complete matrix.

1.4. Contributions

Aiming at the problem that the existing factor graph model ignores abnormal measurements of local sensors, the error propagation of potentially anomalous sensor measurements is limited by the modeling constraints of stable sensor measurements. A factor graph with local constraints (FGLC) is proposed in this paper, which integrates magnetic and PDR information for navigation based on a CFG. Aiming at the problem that the variable space of the constraint factor graph is no longer a vector space, a mapping relationship is constructed to project the variables into the vector space for calculation. In the field experiment, the FGLC achieved an average error of 0.83 m for indoor navigation of complex trajectories. Analysis of the standard deviation of the predicted trajectory explains the principle of the algorithm. In addition, a simulation platform is independently built to test the navigation performance of the algorithm in an environment with strong magnetic field interference.

The main contributions of this paper are as follows:

1.

A CFG is used to combine magnetic and PDR information for navigation and positioning, so that the integrated navigation problem is transformed into a constrained problem, through which the system gains the ability to resist sensor conflicts.

2.

A new vector space projection method is constructed to convert the navigation state variables into error variables, and all derived formulas are given for solving the hard constraint optimization problem of the CFG.

3.

Comparison with the navigation accuracy of various other sensor fusion algorithms, especially with the state of the art, is shown through a field experiment. The principle of the FGLC is given to explain why it is more accurate and reliable. Time complexity and space complexity analyses were also performed.

4.

A simulation experiment is designed to verify the anti-interference ability of the model.

Figure 1 shows an overview of the schematic diagram of the FGLC algorithm modularly. First, a magnetic fingerprint database is set up offline to provide an interface. Original collected data are transformed into magnetic measurements, step measurements and differential heading measurements, respectively. Then, an estimated trajectory is generated with both step and differential heading constraints. The estimated trajectory is used to calculate magnetic estimation, step estimation and differential heading estimation through the interface of the magnetic database. Finally, the measurements and estimations are combined by the CFG model, which decides how to update the next estimated trajectory.

2. Factor Graph for Indoor Magnetic Field/PDR Integrated Navigation

2.1. Factor Graph Principle

At time i, the true position state of the trajectory is expressed as $X_i={[x_i,y_i]}^T$. $x_i$ is the coordinate in the x direction and $y_i$ is the coordinate in the y direction. The measurement of the state is expressed as $Z_i=h\left(X_i\right)+v_i^{noise}$, where $h(·)$ stands for the measurement function and $v_i^{noise}$ is measurement noise. A factor-graph-based sensor fusion approach uses all state measurements to estimate the states for solving the maximum a posteriori estimation of the joint probability distribution function.

$$X^{MAP}=\underset{X}{argmax}p\left(X\right|Z)$$

(1)

where $p\left(X\right|Z)$ is the posterior probability density function of the states X. Each factor node in the graph can represent an independent term:

$$p\left(X\right|Z)\propto \prod f_i\left(X_i\right)$$

(2)

Factor nodes are represented as the measurement errors of specific sensors [31]. The function of the node of the factor graph can be expressed as an exponential function.

$$f_i\left(X_i\right)=exp(-\frac{1}{2}{\parallel h\left(X_i\right)-Z_i\parallel }_{\Sigma }^2)$$

(3)

where $\parallel ·\parallel$ stands for the Mahalanobis distance and $\Sigma$ stands for the covariance matrix. Therefore, Equation (1) can be transformed into a least squares form.

$$X^{MAP}=\underset{X}{argmin}\sum _i{\parallel h\left(X_i\right)-Z_i\parallel }_{\Sigma }^2$$

(4)

2.2. PDR Factor

The accelerometer and gyroscope output the stable three-axis acceleration and the angular velocity.

$$Z^{IMU}={[a_x,a_y,a_z,{\omega }_x,{\omega }_y,{\omega }_z]}^T$$

(5)

where $Z^{IMU}$ stands for inertial measurement, $a_x,a_y,a_z$ are the acceleration measurement values of the three axes and ${\omega }_x,{\omega }_y,{\omega }_z$ are the angular velocity measurement values of the three axes. According to the step detection algorithm [32], the pedestrian’s step length can be estimated from the three-axis acceleration.

$$steple{n}_i=\beta log\left(a_{pp,i}^{step}\right)+\gamma$$

(6)

where $steple{n}_i$ represents the measurement step length at time i, $\beta$ is the coefficient and $\gamma$ is the bias. $a_{pp}^{step}$ is the acceleration range during a single step.

Since pedestrians are always walking on a horizontal plane, only the z-axis angular velocity is used to estimate the heading angle when calculating the attitude.

$${\theta }_i={\theta }_1+\sum _j^i{\omega }_{z,j}\times \Delta t$$

(7)

where ${\theta }_i$ is the heading angle measurement at time i and $\Delta t$ is the time interval between two adjacent measurements. It is believed that the measured values of angular velocity are subject to error, which would eventually lead to cumulative errors in heading angles.

$${\omega }_{z,j}={\omega }_{z,j}^{real}+v_j$$

(8)

$${\theta }_i={\theta }_1+\sum _j^i{\omega }_{z,j}^{real}\times \Delta t+\sum _j^i{v}_j\times \Delta t$$

(9)

where ${\omega }_{z,j}^{real}$ is the real z-axis angular velocity at time j and $v_j$ is the measurement error. It is clear that the variance of the error component in the heading angle increases over time, which is the main cause of cumulative errors. It is believed that calibration of the IMU before an experiment is necessary to avoid large zero bias in $v_j$.

Performing differential operations on adjacent measured values of the heading angle and replacing them with the differential heading angle can help avoid introducing errors with larger variances.

$$\Delta {\theta }_i={\theta }_{i+1}-{\theta }_i={\omega }_{z,i+1}^{real}\times \Delta t+v_{i+1}\times \Delta t$$

(10)

where $\Delta {\theta }_i$ is the differential heading angle measurement, whose error is only related to $v_{i+1}$. Therefore, compared to the heading angle, each differential heading angle is more independent and additionally easy to estimate.

Under the assumption that the single-step completion time is consistent, the error of the single-step differential heading angle conforms to the same probability distribution. The factors of the differential heading angle can be expressed as:

$$f_i^{head}=exp(-\frac{1}{2}{\parallel h^{Dangle}(X_i,X_{i+1},X_{i+2})-\Delta {\theta }_i\parallel }_{{\Sigma }_{Dangle}}^2)$$

(11)

$$h^{Dangle}(X_i,X_{i+1},X_{i+2})=h^{angledif}(h^{angle}(X_{i+1},X_{i+2}),h^{angle}(X_i,X_{i+1}))$$

(12)

$$h^{angle}(X_{i-1},X_i)=\left\{\begin{array}{cc}arctan\left(\frac{y_i-y_{i-1}}{x_i-x_{i-1}}\right)+\pi \hfill & \mathrm{if}{y}_i-y_{i-1}\ge 0and{x}_i-x_{i-1}<0,\hfill \\ arctan\left(\frac{y_i-y_{i-1}}{x_i-x_{i-1}}\right)-\pi \hfill & \mathrm{if}{y}_i-y_{i-1}<0and{x}_i-x_{i-1}<0,\hfill \\ arctan\left(\frac{y_i-y_{i-1}}{x_i-x_{i-1}}\right)\hfill & \mathrm{other}.\hfill \end{array}\right.$$

(13)

$$h^{angledif}({\alpha }_1,{\alpha }_2)=\left\{\begin{array}{cc}d\hfill & \mathrm{if}d<\pi ,\hfill \\ 2\pi -d\hfill & \mathrm{if}d\ge \pi .\hfill \end{array}\right.$$

(14)

$$d=|({\alpha }_{1}mod2\pi )-({\alpha }_{2}mod2\pi )|$$

(15)

where $h^{Dangle}(·)$ stands for the differential heading angle measurement function. ${\Sigma }_{Dangle}$ is the covariance matrix of the differential heading angle. $h^{angle}(·)$ is the heading angle measurement function. $h^{angledif}(·)$ is the function to calculate theh angle difference. Similarly, the factors of the step length can be expressed as:

$$f_i^{step}=exp(-\frac{1}{2}{\parallel h^{step}(X_i,X_{i+1})-steple{n}_i\parallel }_{{\Sigma }_{step}}^2)$$

(16)

$$h^{step}(X_i,X_{i+1})=\sqrt{{(x_i-x_{i+1})}^2+{(y_i-y_{i+1})}^2}$$

(17)

where $h^{step}(·)$ stands for the step-length measurement function. $steple{n}_i$ stands for the step length measurement. ${\Sigma }_{step}$ is the covariance matrix of the step length.

2.3. Magnetic Factor

The earth’s magnetic field forms an anomalous field inside steel structure buildings. Thus, the indoor magnetic field is no longer the original geomagnetic field. It should be measured additionally instead of applying classical geomagnetic models to obtain maps.

The magnetic field is a vector field, and the strengths and directions of vectors in the anomalous magnetic field can be measured by a three-axis magnetometer. When using the magnetic field intensity as the key feature of the fingerprint database for positioning, it is difficult for the magnetic field to provide enough information for independent positioning because of its low information dimension and low degree of distinction. This is because the same magnetic intensity value may appear in multiple locations, so it is difficult to determine its true location based on the magnetic field information alone.

$${magn}_i=\sqrt{{{magn}_{d1,i}}^2+{{magn}_{d2,i}}^2+{{magn}_{d3,i}}^2}$$

(18)

where $mag{n}_i$ indicates the measured value of the magnetic intensity at time i. ${magn}_{d1,i}$, ${magn}_{d2,i}$, ${magn}_{d3,i}$ indicate the measured values from a magnetic sensor in three axes.

In the factor graph algorithm, the magnetic field information is fully utilized. Even if a single magnetic field source is not enough to directly meet the positioning requirements, it can help eliminate the cumulative error of the inertial navigation system. In this paper, a fingerprint library is constructed through offline magnetic field acquisition, which has a mapping relationship equation from the state to the magnetic field strength.

$${magn}_i^{est}=h^{mag}\left(\widehat{X_i}\right)$$

(19)

where ${magn}_i^{est}$ indicates the estimated value of the magnetic field at time i. The position of the non-acquisition point can be interpolated with the help of the surrounding known points through a bi-cubic function. The main steps are shown in Figure 2.

1.

An input point is used by the floor function to determine the matching area. The matching area is a square with four vertices. They are $p_1=(\lfloor \widehat{x_i}\rfloor ,\lfloor \widehat{y_i}\rfloor )$, $p_2=(\lfloor \widehat{x_i}\rfloor ,\lfloor \widehat{y_i}\rfloor +1)$, $p_3=(\lfloor \widehat{x_i}\rfloor +1,\lfloor \widehat{y_i}\rfloor )$, $p_4=(\lfloor \widehat{x_i}\rfloor +1,\lfloor \widehat{y_i}\rfloor +1)$.

2.

Each vertex is a fingerprint point. $g(·)$ shows the magnetic field intensity at these points. $g_x(·)=\frac{\partial }{\partial x}g$ shows the partial derivative of the magnetic field intensity in the x-direction. $g_y(·)=\frac{\partial }{\partial y}g$ shows the partial derivative in the y-direction. Furthermore, $g_{xy}(·)=\frac{{\partial }^2}{\partial x\partial y}g$ shows the cross partial derivative.

3.

Calculate intermediate matrix A.

$$A=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\\ -3& 3& -2& -1\\ 2& -2& 1& 1\end{array}\right]\left[\begin{array}{cccc}g\left(p_1\right)& g\left(p_2\right)& g_y\left(p_1\right)& g_y\left(p_2\right)\\ g\left(p_3\right)& g\left(p_4\right)& g_y\left(p_3\right)& g_y\left(p_4\right)\\ g_x\left(p_1\right)& g_x\left(p_2\right)& g_{xy}\left(p_1\right)& g_{xy}\left(p_2\right)\\ g_x\left(p_3\right)& g_x\left(p_4\right)& g_{xy}\left(p_3\right)& g_{xy}\left(p_4\right)\end{array}\right]\left[\begin{array}{cccc}1& 0& -3& 2\\ 0& 0& 3& -2\\ 0& 1& -2& 1\\ 0& 0& -1& 1\end{array}\right]$$

(20)

4.

Calculate the interpolated output of the magnetic field intensity, $h^{mag}\left(\widehat{X_i}\right)$.

$$h^{mag}\left(\widehat{X_i}\right)={\left[\begin{array}{c}1\\ \widehat{x_i}-\lfloor \widehat{x_i}\rfloor \\ {(\widehat{x_i}-\lfloor \widehat{x_i}\rfloor )}^2\\ {(\widehat{x_i}-\lfloor \widehat{x_i}\rfloor )}^3\end{array}\right]}^{T}A\left[\begin{array}{c}1\\ \widehat{y_i}-\lfloor \widehat{y_i}\rfloor \\ {(\widehat{y_i}-\lfloor \widehat{y_i}\rfloor )}^2\\ {(\widehat{y_i}-\lfloor \widehat{y_i}\rfloor )}^3\end{array}\right]$$

(21)

The bi-cubic interpolation method guarantees the continuity of the first-order partial derivative of the interpolation surface, $h^{mag}\left(\widehat{X_i}\right)$, is differentiable with respect to $\widehat{X_i}$.

$$\frac{\partial }{\partial x_i}{h}^{mag}\left(\widehat{X_i}\right)={\left[\begin{array}{c}0\\ 1\\ 2(\widehat{x_i}-\lfloor \widehat{x_i}\rfloor )\\ 3{(\widehat{x_i}-\lfloor \widehat{x_i}\rfloor )}^2\end{array}\right]}^{T}A\left[\begin{array}{c}1\\ \widehat{y_i}-\lfloor \widehat{y_i}\rfloor \\ {(\widehat{y_i}-\lfloor \widehat{y_i}\rfloor )}^2\\ {(\widehat{y_i}-\lfloor \widehat{y_i}\rfloor )}^3\end{array}\right]$$

(22)

$$\frac{\partial }{\partial y_i}{h}^{mag}\left(\widehat{X_i}\right)={\left[\begin{array}{c}1\\ \widehat{x_i}-\lfloor \widehat{x_i}\rfloor \\ {(\widehat{x_i}-\lfloor \widehat{x_i}\rfloor )}^2\\ {(\widehat{x_i}-\lfloor \widehat{x_i}\rfloor )}^3\end{array}\right]}^{T}A\left[\begin{array}{c}0\\ 1\\ 2(\widehat{y_i}-\lfloor \widehat{y_i}\rfloor )\\ 3{(\widehat{y_i}-\lfloor \widehat{y_i}\rfloor )}^2\end{array}\right]$$

(23)

The factors of the magnetic field can be expressed as:

$$f_i^{mag}=exp(-\frac{1}{2}{\parallel h^{mag}\left(X_i\right)-mag{n}_i\parallel }_{{\Sigma }_{mag}}^2)$$

(24)

where ${\Sigma }_{mag}$ represents the covariance matrix of the magnetic measurement error.

3. Factor Graph with Local Constraints Model

Existing factor graph models only focus on the goal of maximizing the object function but pay little attention to the rationality of local information. When a certain sensor of the navigation system is severely disturbed or a huge error occurs at a certain moment, the existing factor graph models cannot identify such errors. Assume that there is a significant deviation in the system measurement at moment k, then the value of the corresponding factor $f\left(X_k\right)$ will approach 0. This will result in the equation $\prod f_i\left(X_i\right)$ approaching 0. In this case, the estimation of the states by the factor graph model must deviate from the real states.

3.1. Improved Factor Graph

The IFG (improved factor graph) [21] is currently the best unconstrained factor graph model for sensor fusion. A weight function is developed and introduced into the factor graph algorithm to realize the adaptive change in the measurement of each sensor.

$$\widehat{X}=argmin\sum _i{\parallel S\left(r_i\right)·(h\left(X_i\right)-Z_i)\parallel }_{\Sigma }^2$$

(25)

$$r_i=|h\left(X_i\right)-Z_i|$$

(26)

where $S\left(r_i\right)$ denotes the dynamic weight function and $r_i$ represents the residual of each measured and predicted value.

$$S\left(r\right)=\left\{\begin{array}{cc}\hfill 1,& 0\le r<r_{base}\hfill \\ \hfill \frac{1}{1+exp\left(\alpha (r-r_{base})\right)},& r\ge r_{base}\hfill \end{array}\right.$$

(27)

where $r_{base}$ is threshold. When the residual is larger than the threshold, the weighed function will be closer to zero, which indicates that the corresponding sensor measurement information is isolated.

3.2. Harder Constraints for the Model

Factors with significant errors depend on the information of other normal factors for correction, while the existing weight-based factor graph algorithms only serve as soft constraints to limit errors. In this paper, hard constraints are introduced for normal factor nodes to resist the adverse interference of factors with significant measurement errors in state estimation. Therefore, the new factor graph model is expressed in the form of a constraint factor graph.

$$\begin{array}{c}{X}^{MAP}=\underset{X}{argmax}\prod f_i\left(X_i\right)\end{array}$$

(28)

$$\begin{array}{c}s.t.\forall i\parallel h\left(X_i\right)-Z_i{\parallel }_{\sigma }^2\le {\delta }_i\end{array}$$

(29)

where ${\delta }_i$ is the upper limit of the residual of the measured and predicted value at time i.

When the predicted value is very close to the real value, $\parallel h\left(X_i\right)-Z_i{\parallel }_{\sigma }$ will be very close to the meaning of measurement deviation. The maximum measurement deviation can be calculated statistically based on the measurement error of the sensors.

In other words, since the measured values are invariant in one experiment, only the estimated values can change the residuals. The estimated values that make the residuals larger than the max measurement deviation are definitely not close to the real values. Therefore, these estimated values should be ignored during optimization. Thus, Equation (29) provides a reasonable way to limit the estimated values. However, existing models like EKF, FG and IFG do not have such an ability; unreasonable estimated values are still considered during optimization, which is the key difference to the FGLC. The FGLC navigation and positioning method model in this paper can be expressed as follows.

$$\begin{array}{c}Q\left(X\right)=\sum _i{\parallel h^{mag}\left(X_i\right)-mag{n}_i\parallel }_{{\Sigma }_{mag}}^2+\sum _i{\parallel h^{step}(X_i,X_{i+1})-steple{n}_i\parallel }_{{\Sigma }_{step}}^2\\ +\sum _i{\parallel h^{Dangle}(X_i,X_{i+1},X_{i+2})-\Delta {\theta }_i\parallel }_{{\Sigma }_{Dangle}}^2\end{array}$$

(30)

$$X^{MAP}=\underset{X}{argmin}Q$$

(31)

$$\begin{array}{c}s.t.\forall i|h^{step}(X_i,X_{i+1})-steple{n}_i|\le {\delta }_i^{step}\end{array}$$

(32)

$$\begin{array}{c}|h^{Dangle}(X_i,X_{i+1},X_{i+2})-\Delta {\theta }_i|\le {\delta }_i^{Dangle}\end{array}$$

(33)

where ${\delta }_i^{step}$ is the upper limit of differential heading angle deviation and ${\delta }_i^{Dangle}$ is the the upper limit of step length deviation. In this paper, we set ${\delta }_i^{step}$ to be three times the standard deviation (STD) of the step length measurement and ${\delta }_i^{Dangle}$ to be three times the STD of the differential angle measurement, following the guidelines proposed by Raida.

The IMU relies entirely on its own accelerometer and gyroscope, making it less susceptible to external interference than magnetic sensors [33]. Therefore, there would be no particularly significant error in the measurement of the IMU. Firstly, the raw data of the IMU can be preprocessed, through which some outliers can be processed in advance. Secondly, it is believed that the probability of the IMU having sensor problems is far less than the probability of abnormalities in other sensors that require external signal inputs to work. Thirdly, the model should trust the IMU more than other sensors, at least when sensor conflict occurs. Fourthly, preprocessing IMU data is more convincing due to the higher correlation among IMU data.

Adding local constraints to the IMU-related factor nodes can play a role in constraining other factor nodes, thereby avoiding unreasonable deviations in the predicted trajectory. The PDR factor is a factor that only depends on inertial devices. Introducing local constraints to the PDR factor would make the system trust PDR measurements more when multi-sensor measurements conflict with each other. The predicted states beyond the constraints of Equations (32) and (33) can all be considered to be due to magnetic field anomalies. When these anomalies occur, the constraints ensure that the navigation system trusts the IMU measurements more.

The factor model of the FGLC navigation and positioning method in this paper is shown in Figure 3. The filled circles represent factors corresponding to different measurements. The triangles represent local constraints for corresponding factors, while existing factor graph models do not have such constraint nodes.

3.3. Sensor Conflict

Equations (32) and (33) define what a sensor conflict is in this paper, which occurs when measurements from the IMU and the magnetic sensor, respectively, exceed the constraints. At the same time, Equations (32) and (33) also clearly show that the system’s strategy is to trust the IMU more when a conflict occurs. At this time, the measurement of the magnetic sensor is considered abnormal. The magnetic sensor may be being disturbed or may have an error itself. Of course, even if the magnetic sensor is working properly, it may still cause a conflict.

Conflicts may also occur when the sensors are working properly. As shown in Figure 4, obtaining step size measurements from two sensors may not satisfy the constraints due to the large difference and cause sensor conflicts. This shows that the FGLC can detect more sensor conflicts than other algorithms. Thus, we believe that even under normal circumstances, better accuracy can be obtained by using the FGLC algorithm.

4. Model Solution

The FGLC transforms the navigation solution problem into a hard-constrained nonlinear least squares problem, and the algorithm used in the existing factor graph is no longer applicable.

4.1. Variable Space Mapping

Override the constraints in (32) and (33):

$$\begin{array}{c}{h}^{step}(X_i,X_{i+1})-steple{n}_i=\frac{2}{\pi }arctan\left(w_i\right){\delta }_i^{step}\end{array}$$

(34)

$$\begin{array}{c}{h}^{Dangle}(X_i,X_{i+1},X_{i+2})-\Delta {\theta }_i=\frac{2}{\pi }arctan\left(v_i\right){\delta }_i^{Dangle}\end{array}$$

(35)

where $w_i,v_i\in R$; thus, $\frac{2}{\pi }arctan\left(w_i\right)\in (-1,1)$ and $\frac{2}{\pi }arctan\left(v_i\right)\in (-1,1)$. It is easy to see Equations (34) and (35) are equal to Equations (32) and (33).

Make $h^{step}(X_i,X_{i+1})$ the step length estimation at time i as $\widehat{ste{p}_i}$.

While $h^{Dangle}(X_i,X_{i+1},X_{i+2})$, the differential heading estimation is $\widehat{Dangl{e}_i}$. There would be the following derivations.

$$\begin{array}{c}\widehat{ste{p}_i}=steple{n}_i+\frac{2}{\pi }arctan\left(w_i\right){\delta }_i^{step}\end{array}$$

(36)

$$\begin{array}{c}\widehat{Dangl{e}_i}=\Delta {\theta }_i+\frac{2}{\pi }arctan\left(v_i\right){\delta }_i^{Dangle}\end{array}$$

(37)

$$\begin{array}{c}\widehat{{\theta }_i}={\theta }_1+\sum _j^i\widehat{Dangl{e}_j}\end{array}$$

(38)

$$\begin{array}{c}\widehat{X_{i+1}}=\widehat{X_i}+\widehat{ste{p}_i}{[cos\left(\widehat{{\theta }_i}\right),sin\left(\widehat{{\theta }_i}\right)]}^T\end{array}$$

(39)

where $\frac{2}{\pi }arctan\left(w_i\right){\delta }_i^{step}$ is the deviation of the step length measurement from the estimation and $\frac{2}{\pi }arctan\left(v_i\right){\delta }_i^{Dangle}$ is the difference between the measured and estimated differential heading angle. It is easy to prove that the above two deviations always satisfy the constraints. $\widehat{{\theta }_i}$ is the estimation of heading angles and ${\theta }_1$ is the starting heading angle. $\widehat{X_i}$ is the estimation of the navigation states. It is not difficult to uniquely confirm a predicted trajectory through all w and v variables. The variable space of the problem is successfully mapped from a non-vector space to a vector space and it is surjective.

4.2. Gradient Algorithm Solution

The objective function $Q\left(X\right)$ in Equation (30) is differentiable with respect to $w_i$ and $v_i$. Since there is no closed solution to the nonlinear least squares problem, variables must be updated iteratively. There are two main derivating steps as follows.

$$\begin{array}{c}\frac{\partial }{\partial w_k}Q=\sum _{j=k}^n\left[\frac{2}{{\left({\sigma }^{mag}\right)}^2}(h^{mag}\left(\widehat{X_j}\right)-ma{g}_j)·\frac{\partial }{\partial w_k}{h}^{mag}\left(\widehat{X_j}\right)\right]\\ +{\left(\frac{{\delta }_k^{step}}{{\sigma }^{step}}\right)}^2\frac{4}{\pi }arctan\left(w_k\right)\frac{2}{\pi ({w_k}^2+1)}\end{array}$$

(40)

$$\begin{array}{c}\frac{\partial }{\partial w_k}{h}^{mag}\left(\widehat{X_j}\right)=\frac{\partial }{\partial x_j}{h}^{mag}\left(\widehat{X_j}\right)\frac{\partial x_j}{\partial w_k}+\frac{\partial }{\partial y_j}{h}^{mag}\left(\widehat{X_j}\right)\frac{\partial y_j}{\partial w_k}\end{array}$$

(41)

$$\begin{array}{c}\frac{\partial x_j}{\partial w_k}=cos\left(\widehat{{\theta }_k}\right){\delta }_k^{step}\frac{2}{\pi ({w_k}^2+1)}\end{array}$$

(42)

$$\begin{array}{c}\frac{\partial y_j}{\partial w_k}=sin\left(\widehat{{\theta }_k}\right){\delta }_k^{step}\frac{2}{\pi ({w_k}^2+1)}\end{array}$$

(43)

where $\frac{\partial }{\partial w_k}Q$ is the partial derivative in the $w_k$ direction.

$$\begin{array}{c}\frac{\partial }{\partial v_k}Q=\sum _{j=k}^n\left[\frac{2}{{\left({\sigma }^{mag}\right)}^2}(h^{mag}\left(\widehat{X_j}\right)-ma{g}_j)·\frac{\partial }{\partial v_k}{h}^{mag}\left(\widehat{X_j}\right)\right]\\ +{\left(\frac{{\delta }_k^{Dangle}}{{\sigma }^{Dangle}}\right)}^2\frac{4}{\pi }arctan\left(v_k\right)\frac{2}{\pi ({v_k}^2+1)}\end{array}$$

(44)

$$\begin{array}{c}\frac{\partial }{\partial v_k}{h}^{mag}\left(\widehat{X_j}\right)=\frac{\partial }{\partial x_j}{h}^{mag}\left(\widehat{X_j}\right)\frac{\partial x_j}{\partial v_k}+\frac{\partial }{\partial y_j}{h}^{mag}\left(\widehat{X_j}\right)\frac{\partial y_j}{\partial v_k}\end{array}$$

(45)

$$\begin{array}{c}\frac{\partial x_j}{\partial v_k}=\frac{2}{\pi ({v_k}^2+1)}\sum _{i=k}^j\left[-\widehat{ste{p}_i}sin\left(\widehat{{\theta }_i}\right){\delta }_i^{Dangle}\right]\end{array}$$

(46)

$$\begin{array}{c}\frac{\partial y_j}{\partial v_k}=\frac{2}{\pi ({v_k}^2+1)}\sum _{i=k}^j\left[-\widehat{ste{p}_i}cos\left(\widehat{{\theta }_i}\right){\delta }_i^{Dangle}\right]\end{array}$$

(47)

where $\frac{\partial }{\partial v_k}Q$ is the partial derivative in the $v_k$ direction. The necessary condition for the minimum value of $Q\left(X\right)$ is $\frac{\partial }{\partial w_k}Q=\frac{\partial }{\partial v_k}Q=0$. Then, the parameters are updated iteratively using the Adam gradient method [34]. Finally, a predicted trajectory is uniquely restored as the output of the algorithm.

When considering the zero deviation of the heading angle, $\frac{\partial }{\partial v_k}Q$ can be expressed in the following form.

$$\begin{array}{c}\frac{\partial }{\partial v_k}Q=\sum _{j=k}^n\left[\frac{2}{{\left({\sigma }^{mag}\right)}^2}(h^{mag}\left(\widehat{X_j}\right)-ma{g}_j)·\frac{\partial }{\partial v_k}{h}^{mag}\left(\widehat{X_j}\right)\right]\\ +{\left(\frac{{\delta }_k^{step}}{{\sigma }^{step}}\right)}^2\frac{8}{{\pi }^2}\frac{arctan\left(v_k\right)+bia}{({v_k}^2+1)}\end{array}$$

(48)

where $bia$ is the bias of the differential heading angle. When it is smaller than $arctan\left(v_k\right)$, the bias has little effect.

4.3. Computational and Memory Complexity Analysis

Suppose the length of the trajectory is denoted as n. When solving the model, it requires a minimum of O(2n) additional memory space to store the temporary estimated value of the trajectory, as well as O(2n) memory space to store the temporary variables mapped to the W and V spaces. Furthermore, the model also needs to store the entire uninterpolated magnetic field map, and the memory size depends on the size of the map.

To update iteratively, a sliding window with a window size of $winsize$ is used. The model needs to perform at least $n\times winsize$ cycles during the solving process. In each cycle, the model needs to iterate the variables to the local maximum and perform two variable space mappings. The time required for the model to iterate to the maximum value in each cycle depends on the convergence speed of the gradient optimization algorithm, with a time complexity of O(T), where T represents the average number of iterations. The time complexity of completing two variable space mappings is O(2n), and implementing caching becomes challenging in high dimensions.

Compared to existing factor graph algorithms, the FGLC has a higher time complexity when iterative methods are used for solving. The main cost gap lies in the variable space mapping.

5. Field Experiment and Demonstration

In order to verify the performance of the algorithm proposed in this paper, field experiments were conducted. In order to fully test the reliability and stability of the algorithm in this paper, the experimental test path is an irregular curve, rather than a regular one.

5.1. Offline Fingerprint Collection

The triaxial magnetic field intensities at the grid point positions were collected in the experimental area in advance. The fingerprint library interface was constructed using the bicubic interpolation algorithm. In the navigation solution stage, the estimated value and gradient of the magnetic field strength at the estimated position in the experimental area can be obtained immediately through this interface. The collected magnetic field fingerprint information is shown in Figure 5 after interpolation. The maximum length of the experimental area is 20 m and the maximum width is 10 m. The colored area is the intensity heat map obtained after interpolation of the magnetic field data collected offline. The color bar indicates the range of magnetic strength in the area.

5.2. Navigation Test

During the data collection phase, when the tester walked in the experimental area, another tester recorded the experiment with a handheld camera. In the data analysis stage, each frame of the video showing the tester’s steps was compared with the actual map and marked accordingly. The coordinates of the testers’ positions in the grid were recorded as a reference path. As shown in Figure 6, the footprints are considered as the actual positions. The pedestrian will swing from left to right when walking, so the reference path will be a little jerky.

A variety of combination algorithms were used for research. The PDR method was used as a baseline. The performances of the extended Kalman filter (EKF) [9]; the existing factor graph algorithm (FG) [11]; the improved factor graph (IFG) [21], which is the state-of-the-art method; and the factor graph with local constraints algorithm (FGLC) proposed in this paper were compared, respectively.

5.3. Solution Analysis

Figure 7 shows the comparison between the trajectories predicted by the above mentioned algorithms and the reference path. The trajectory predicted by the FGLC algorithm proposed in this paper is closest to the reference path. The existing factor graph models can also predict the trajectory relatively accurately, but there are relatively large errors in some areas.

Figure 8 shows the comparison of the absolute error performance between the trajectories predicted by the algorithms,

Table 1. Statistical analysis of trajectory errors predicted by different algorithms.

Algorithm	Mean Error (m)	RMSE (m)	Max Error (m)	Mean y Bias (m)	Mean x Bias (m)	Time Consumed (s)
PDR	2.92	3.33	5.40	2.39	−1.35	<0.02
EKF	1.15	1.45	4.36	0.39	0.28	0.02
FG	1.43	1.62	3.56	0.42	−0.42	1.60
IFG	1.10	1.17	2.07	0.36	−0.31	>2
FGLC	0.83	0.91	1.50	0.30	−0.26	5

shows the types of errors and Figure 8 shows the cumulative probability distribution of the trajectories predicted by the algorithms. The errors of all the pace points of the FGLC are below 1.6 m, and the average error is 0.83 m. The average error of the EKF is 1.15 m; that of IFG is 1.10 m. The existing factor graph algorithm is worse, with an average error of 1.43 m. The maximum error of the EKF is 4.36 m. Compared with the IFG, the average error of the FGLC algorithm is reduced by 24.5%. The FGLC algorithm is the most time-consuming method and the EKF algorithm is the least time-consuming method. Since the fastest implementation of the IFG is unknown, only an estimated range is given.

Figure 9 shows the cumulative probability distribution plot of the absolute error at each step of the trajectories predicted by the algorithms. A total of 90% of the errors of the FGLC are distributed within an interval of less than 1.5 m. In the same interval, the IFG accounts for 85%, the EKF accounts for 70% and the FG accounts for less than 60%. The trajectory error predicted solely by the PDR algorithm is the largest, with a median of 3 m.

Figure 10 shows predicted the trajectories’ bias both in the x and y directions. The predicted trajectory of each algorithm exhibits a positive shift in the y direction and a negative shift in the x direction. This offset direction aligns with that of the PDR, suggesting that the trajectory error is influenced by the PDR offset. However, the offset is relatively small and it is primarily concentrated around zero.

Figure 11 and Figure 12 show the standard deviation analysis of the step length and differential heading angle in the predicted paths (EKF/FG/IFG/FGLC), which can be considered an integrity monitoring method. In Figure 11, the errors of the FGLC predicted trajectory step length are less than 3 times the standard deviation. However, the step length errors of EKF/FG are greater than three standard deviations at several positions. The result shows that there are strides larger than 2 m in the FG prediction trajectory, which is obviously against common sense. The step-length errors of the IFG stay below 3 times the standard deviation but are generally larger than that of the FGLC.

The FGLC inherently fully considers the local constraints on the step length, so its estimation of each step of the step lengths is within a reasonable range. A similar conclusion is shown in Figure 12. The ratios of the FGLC predicted trajectory differential heading angle error to the standard deviation are always maintained at a low level, while the ratios of the differential heading angle error of the FG-predicted trajectory fluctuated violently. It is clear that these fluctuations are statistically abnormal. The design of the FGLC using differential heading angle constraints makes the predicted trajectory smoother and more reasonable. In summary, the FGLC provides a guarantee and theoretical support for high-accuracy navigation.

6. Simulation Experiment

Due to the limited area of the indoor test environment, it was not convenient to carry out large-scale tests. Therefore, a test platform was built independently to simulate the performance of the algorithms under a wide range of long-term test conditions. The algorithms’ anti-interference performances, denoting their capacity to mitigate environmental interference, were assessed through the introduction of intense magnetic interference at random locations while monitoring the resultant decline in accuracy. As stated in the introduction, measurement errors are inevitably influenced by environmental interference.

6.1. Test Parameters

The simulated indoor magnetic field has a rectangular area of 300 m × 300 m. The number of test steps was 500, and the test trajectory was randomly generated. The starting point is the position with coordinates is (150,150). The standard deviation of the single-step drift error of the gyroscope heading angle is 1 degree, the standard deviation of the single-step measurement error of the step length is 0.1 m, the standard deviation of the magnetic measurement error is 5 μT and the standard deviation of the magnetic field interference noise is 30 μT.

When generating the simulated magnetic field map, the maximum and minimum values of the regional magnetic field were set as 50 μT and 20 μT, respectively. Additionally, the number of magnetic field extreme positions within the same area was adjusted to match the real magnetic field, with an approximate density of four extreme values per 200 square meters. Considering the inherent uncertainty of indoor magnetic fields, the positions of these extreme points were randomly distributed. The simulated magnetic map in the experiment is shown in Figure 13.

6.2. Simulation Experiment and Analysis

As shown in Figure 14 and Figure 15, only relying on the PDR to predict the trajectory will produce serious cumulative errors, and the predicted trajectory deviates from the real trajectory. However, the FGLC uses the information of the magnetic field to correct the cumulative error of the PDR, so that the absolute errors of the predicted trajectory are always kept in a stable and low error range. Meanwhile, the absolute error will not increase with time.

It is worth mentioning that the EKF, FG and IFG algorithms cannot always predict accurate trajectories in this simulation experiment because their predicted trajectories often deviate from the reference a lot, as shown in Figure 16. Thus, the above-mentioned algorithms will not be considered in the following discussions.

Figure 17 shows that the trajectories predicted in the four experiments are affected by interference in the environment. The green areas represent locations in the trajectory that are strongly disturbed. It can be seen that the influence of interference on the predicted trajectories is very limited, and the predicted trajectories always fit the real trajectory regardless of where the interference occurs. Figure 18 shows the errors of the trajectory corresponding to the four experiments in Figure 17. The red transparent area is the trajectory position directly affected by strong interference. All the errors are generally maintained below 2 m. The errors increase at locations with strong interference, but the maximum error does not exceed 4 m and none of the errors propagate to further locations.

The reason why none of the errors propagate to further locations is that the FGLC relies on the local constraint mechanism, which allows the FGLC to lock the estimated interval of the predicted positions. Outliers beyond the feasible range due to strong magnetic interference were excluded. The simulation experiment shows that the FGLC can predict the trajectory with low error even in the face of very strong magnetic field interference.

7. Conclusions

In this paper, a factor graph with local constraints is proposed, which is a magnetic field/PDR integrated navigation method based on a constraint factor graph. The factor graph nodes are reconstructed by constraints, so that the navigation problem is transformed into a hard-constrained optimization problem. Aiming at the problem that there is no closed solution for hard-constrained nonlinear least squares, a new variable space mapping relationship is developed according to the particularity of the problem so that the problem can be solved using gradient algorithms. Due to this, the solution speed of the algorithm is guaranteed. Through a complexity analysis, it is determined that the model requires more computing time to complete the variable space mapping than the existing factor graph algorithm.

The experimental findings demonstrate that the FGLC algorithm exhibits a superior positioning accuracy, as evidenced by an RMSE of 0.91m. The principle of the FGLC algorithm is explained by analyzing the trajectories predicted by different algorithms. The FGLC only relies on magnetic and PDR information in a wide indoor plane area when performing navigation experiments on complex curved trajectories. Simulation experiments with a complex and long-term trajectory with strong interference were conducted; these verified that the FGLC algorithm has reliable anti-interference ability.

There are several further research opportunities: Firstly, the FGLC can be continually used as a sensor-combining model to solve navigation problems with more kinds of sensors. Secondly, it is claimed in this paper that the FGLC trusts the IMU sensor more when conflicting with the magnetic sensor. Therefore, in future work, the priority between different sensors can be adjusted accordingly. Thirdly, sensors (a magnetic sensor in this paper) that cannot be used for positioning alone can also be used to assist positioning and navigation.