Invariant Kalman Filter Design for Securing Robust Performance of Magnetic–Inertial Integrated Navigation System under Measurement Uncertainty

Abstract

This study proposes an enhanced integration algorithm that combines the magnetic field-based positioning system (MPS—Magnetic Pose Estimation System) with an inertial system with the advantage of an invariant filter structure. Specifically, to mitigate the nonlinearity of the propagation model and perturbing effect from the estimated uncertainty, the formulation of the invariant Kalman filter was derived in detail. Then, experiments were conducted to validate the algorithm with an unmanned vehicle equipped with an IMU and MPS receiver. As a result, the navigation performance of the IEKF-based inertial and magnetic field integration system was presented and compared with the conventional Kalman filter results. Furthermore, the convergence and navigation performance were evaluated in the presence of state variable initialization errors. The findings indicate that the inertial and magnetic field coupled with the IEKF outperforms the typical KF approach, particularly when dealing with initial estimate uncertainties.

1. Introduction

To operate an unmanned vehicle, it is essential to estimate the vehicle’s position, velocity, and attitude at every moment with high accuracy. The inertial navigation system (INS) is widely used to estimate a vehicle’s state. The INS uses measurements from the Inertial Measurement Unit (IMU) on the vehicle to calculate position, velocity, and attitude. The IMU consists of a gyroscope that measures angular velocity and an accelerometer that measures linear acceleration. However, the INS accumulates errors over time, and drifts occur. As a solution to this problem, measurements from other systems are expected to be used to correct the error. Typically, the INS uses position and velocity from the Global Navigation Satellite System (GNSS). The GNSS provides a real-time position, with meter-level accuracy in open-sky environments [1]. It is also relatively inexpensive and accessible, allowing it to be integrated with many other systems. However, it cannot be used in indoor, underground, or underwater environments where GNSS signals are unavailable, and multipath error increases in urban environments with many obstacles, such as buildings [1,2,3].

In recent years, vision and LiDAR sensors have been widely used in environments where the GNSS is not available [4,5]. These two sensors can generate 3D maps and modeling to acquire rich environmental information. However, both sensors can distort measurements in extreme motion and contaminate measurements in light-affected environments. Vision sensors, such as cave environments, are also challenging to use in dark environments without light [6]. However, LiDAR sensors can be challenging in environments where light is reflected [7] and foggy conditions where objects are difficult to detect [8].

Localization using magnetic fields has several advantages over the above systems. Common objects in the indoor environment are usually magnetically permeable, so the magnetic field is less affected by these objects. Magnetic field localization can also be used underwater and on the ground. There have been many studies that use magnetic fields for localization [9,10,11,12].

This paper uses a Magnetic Pose Estimation System (MPS) based on an AC magnetic field. As a pure magnetic field intrinsically demonstrates excellent permeability characteristics except for ferromagnetic materials within the propagation path, it has the advantage that the magnetic attenuation effect is negligible for many environmental media, e.g., air, wood, plastics, organic body, etc. Therefore, studies using artificially generated magnetic field signals as a localization means indoors and outdoors, underwater, or inside the human body have been published recently [13,14]. The MPS uses four coils to generate an artificial AC magnetic field. Then, the residuals of the theoretical magnetic field based on the circular loop model and the magnetic field measurement through the magnetometer are used to estimate the position and attitude using the numerical optimization process. Despite its advantages, environmental interference, such as the eddy current effect caused by concrete–iron buildings, requires more robust estimation performance.

Meanwhile, when integrating multiple sensors, state variables are typically estimated using an extended Kalman filter (EKF) structure. This is because, for general nonlinear systems, the EKF approach can provide many inherent advantages for estimating state variables in sensor fusion problems due to its computational efficiency, algorithmic simplicity, and real-time performance [15]. As a result, the EKF adapted in this paper also has the most popular structure, incorporating position, velocity, and attitude represented by a quaternion and IMU bias as state variables [16,17]. This filter typically treats the state variables as vectors, which is different from the IEKF approach that uses a matrix Lie group of state variables. While the EKF has the advantages described above, it has the disadvantage that the filter estimation performance is highly dependent on the initial state estimation. Thus, if the initial state variable estimation is uncertain, the estimation performance will continue to be degraded. Recently, several studies have adopted the invariant filter framework to estimate state variables in SLAM and navigation problems [18,19,20]. For instance, it has been used in navigation using LiDAR inertial integration and radar in underwater environments. The IEKF, as a representative example, has been developed based on the theory of invariant observer design [21], which states that the error is invariant under the operator of a matrix Lie group. This property is called the symmetry of the system. The advantage of the IEKF is its strong convergence compared to the EKF when initialization is poor. If the system satisfies the group affine property, the error satisfies the log-linearity property by the symmetric property, as mentioned in [22]. In other words, system linearization does not generally depend on the performance of state variable estimation.

With this background, this study aims to realize MPS/INS integration based on the invariant filter framework, which aims to overcome performance degradation due to the inevitable magnetic measurement perturbations. For empirical validation, we installed four coils in an indoor environment and then used a UGV to generate multiple trajectories. Through experiments with multiple obstacles in the indoor fields, the performance of the invariant filter was analyzed, considering measurement anomalies in the navigation environment. Specifically, this paper compares the performance of the proposed IEKF and EKF when initialization is not good. The contributions of this paper are as follows: First, we proposed an invariant filter framework to integrate INS and MPS systems. To achieve this, a new mathematical model suitable for the process and measurement models was derived. Furthermore, an optimized filter structure, including the state variable augmentation, was proposed to secure improved estimation characteristics. The proposed invariant filter’s performance was verified through indoor environment ground vehicle experiments, where there were significant initial estimation errors or interference-type noise caused by the building environment. Particularly, the analysis of experimental results demonstrates that the proposed technique exhibits superior convergence and robustness compared to the conventional integration method.

The remaining contents of this paper are as follows: Section 2 describes the Lie group theory and magnetic positioning system. Section 3 describes the state and measurement model of the invariant EKF. Next, Section 4 describes the experiments and results, and Section 5 includes the conclusion.

2. Background Methods

2.1. Lie Group Theory

This paper describes the IEKF based on matrix Lie groups. So, this chapter describes the theory and operators of the matrix Lie group used in this paper. A Lie group ${\rm M}$ is a smooth manifold that satisfies the group property. Also, the tangent space at the identity of Lie group ${\rm M}$ is called a Lie algebra $\mathfrak{m}$.

First, let us consider the linear mapping in a cartesian vector space ${\mathbb{R}}^{dim\left(\mathfrak{m}\right)}$ and the exponential mapping $\mathrm{e}\mathrm{x}\mathrm{p}\left(\right)$ of Lie algebra. The Hat operator ${\left(·\right)}^{\wedge }$ represents the transformation of a vector into a skew-symmetric matrix, and the closed-form solution [22] of the exponential map $exp\left( \right)$ is given below. Here, the capitalized $Exp\left( \right)$ is the mapping from the cartesian vector space ${\mathbb{R}}^{dim\left(\mathfrak{m}\right)}$ to the Lie group ${\rm M}$. The following equations are all examples in the Lie group ${\rm M}$ at $\mathit{\chi }$ and the tangent space $\mathfrak{m}$ at ${\mathit{\xi }}^{\wedge }$:

$${\left(·\right)}^{\wedge }:{\mathbb{R}}^{dim\left(\mathfrak{m}\right)}⟼\mathfrak{m}; \mathit{\xi }⟼{\mathit{\xi }}^{\wedge }$$

$$exp:\mathfrak{m}⟼{\rm M}; {\mathit{\xi }}^{\wedge }⟼\mathit{\chi }=exp\left({\mathit{\xi }}^{\wedge }\right)$$

$$Exp: {\mathbb{R}}^{dim\left(\mathfrak{m}\right)}⟼{\rm M}; \mathit{\xi }⟼\mathit{\chi }=Exp\left(\mathit{\xi }\right)$$

(1)

where

$$\mathit{\xi }={\left[\begin{array}{ccc}{\xi }_1& {\xi }_2& {\xi }_3\end{array}\right]}^T$$

$${\mathit{\xi }}^{\wedge }={\left[\mathit{\xi }\right]}_{\times }^{ }=\left[\begin{array}{ccc}0& -{\xi }_3& {\xi }_2\\ {\xi }_3& 0& -{\xi }_1\\ {-\xi }_2& {\xi }_1& 0\end{array}\right]$$

$$exp\left({\mathit{\xi }}^{\wedge }\right)={\mathit{{\rm I}}}_{3\times 3}+\frac{\sin \left(‖\mathit{\xi }‖\right)}{‖\mathit{\xi }‖}{\mathit{\xi }}^{\wedge }+\left(\frac{1-\cos \left(‖\mathit{\xi }‖\right)}{{‖\mathit{\xi }‖}^2}\right){\left({\mathit{\xi }}^{\wedge }\right)}^2$$

The log mapping $log\left(\right)$ of Lie group ${\rm M}$ is defined as follows: Here, the capitalized $Log\left(\right)$ is the mapping from the Lie group ${\rm M}$ to the cartesian vector space ${\mathbb{R}}^{dim\left(\mathfrak{m}\right)}$. The closed-form solution of the logarithmic mapping can be found in Ref. [22]. As above, the equations below are all examples in the Lie group ${\rm M}$ at $\mathit{\chi }$ and in the tangent space $\mathfrak{m}$ at ${\mathit{\xi }}^{\wedge }$:

$${\left(·\right)}^{\vee }:\mathfrak{m}⟼{\mathbb{R}}^{dim\left(\mathfrak{m}\right)}; {\xi }^{\wedge }⟼{\left({\xi }^{\wedge }\right)}^{\vee }$$

$$log: {\rm M}⟼\mathfrak{m}; \mathit{\chi }⟼{\mathit{\xi }}^{\wedge }=log\left(\mathit{\chi }\right)$$

$$Log: {\rm M}⟼{\mathbb{R}}^{dim\left(\mathfrak{m}\right)}; \mathit{\chi }⟼\mathit{\xi }=Log\left(\mathit{\chi }\right)$$

(2)

where

$$log\left(\mathit{\chi }\right)=\frac{‖\mathit{\phi }‖}{2\sin ‖\mathit{\phi }‖}\left(\mathit{\chi }-{\mathit{\chi }}^T\right)$$

$$‖\mathit{\phi }‖={\cos }^{-1}\left(\frac{tr\left(\mathit{\chi }\right)-1}{2}\right)$$

The adjoint operator ${Ad}_{\mathit{\chi }}$ of the matrix Lie group ${\rm M}$ at $\mathit{\chi }$ is as follows:

$${Ad}_{\mathit{\chi }}:\mathfrak{m}⟼\mathfrak{m}; {\mathit{\xi }}^{\wedge }⟼{Ad}_{\mathit{\chi }}\left({\mathit{\xi }}^{\wedge }\right)=\mathit{\chi }{\mathit{\xi }}^{\wedge }{\mathit{\chi }}^{-1}$$

(3)

The right Jacobian ${}_{ }{}^R{\mathcal{J}}_{\mathit{\chi }}^{f\left(\mathit{\chi }\right)}$ and left Jacobian ${}_{ }{}^L{\mathcal{J}}_{\mathit{\chi }}^{f\left(\mathit{\chi }\right)}$ of Lie group ${\rm M}$ at $\mathit{\chi }$ are defined as follows: The definitions of the box-plus and box-minus operators can be found in [23].

$${}_{ }{}^R{\mathcal{J}}_{\mathit{\chi }}^{f\left(\mathit{\chi }\right)}\triangleq {}_{ }{}^R\frac{Df\left(\mathit{\chi }\right)}{D\mathit{\chi }}\triangleq \underset{\mathit{\xi }\to 0}{\lim }\frac{f\left(\mathit{\chi }⊞\mathit{\xi }\right)\boxminus f\left(\mathit{\chi }\right)}{\mathit{\xi }}$$

(4)

$${}_{ }{}^L{\mathcal{J}}_{\mathit{\chi }}^{f\left(\mathit{\chi }\right)}\triangleq {}_{ }{}^L\frac{Df\left(\mathit{\chi }\right)}{D\mathit{\chi }}\triangleq \underset{\mathit{\xi }\to 0}{\lim }\frac{f\left(\mathit{\xi }⊞\mathit{\chi }\right)\boxminus f\left(\mathit{\chi }\right)}{\mathit{\xi }}$$

(5)

In this paper, we use the right Jacobian.

The following are the definitions of the right- and left-invariant errors ${\mathit{\eta }}^R$, ${\mathit{\eta }}^L$ of the estimated Lie group $\widehat{\mathit{\chi }},$ and the true $\mathit{\chi }$ used in the IEKF:

$${\mathit{\eta }}^R=\widehat{\mathit{\chi }}{\mathit{\chi }}^{-1}$$

(6)

$${\mathit{\eta }}^L={\mathit{\chi }}^{-1}\widehat{\mathit{\chi }}$$

(7)

Since the MPS pose used as the IEKF measurement is a left measurement, we use the left-invariant error. Also, if log mapping is employed, it can be represented as follows:

$$\mathit{\xi }=Log\left(\mathit{\eta }\right)$$

(8)

2.2. IEKF Formulation

This chapter describes the IEKF formulation. As mentioned in the previous chapter, if a system satisfies the group affine property and the action of a matrix Lie group, then the system satisfies the log-linear property. First, we define the system dynamics of Lie group ${\mathit{\chi }}_t$ at time $t$ as follows:

$$\frac{d}{dt}{\mathit{\chi }}_t={\mathit{f}}_{{\mathit{u}}_t}\left({\mathit{\chi }}_t\right)$$

(9)

where

$${\mathit{u}}_t=\left[\begin{array}{c}{\mathsf{\omega }}_b\\ {\mathbf{a}}_b\end{array}\right]$$

Here, ${\mathit{u}}_t$ is the input variable, and ${\mathsf{\omega }}_b$ and ${\mathbf{a}}_b$ are the angular velocity and acceleration measurement vectors at the body frame (b frame), respectively. The subscript indicates that it is a vector in the b frame.

A system is said to be group affine [20] if its dynamics ${\mathit{f}}_{{\mathit{u}}_t}\left({\mathit{\chi }}_t\right)$ satisfy the following equation:

$${\mathit{f}}_{{\mathit{u}}_t}\left({\mathit{\chi }}_1{\mathit{\chi }}_2\right)={\mathit{f}}_{{\mathit{u}}_t}\left({\mathit{\chi }}_1\right){\mathit{\chi }}_2+{\mathit{\chi }}_1{\mathit{f}}_{{\mathit{u}}_t}\left({\mathit{\chi }}_2\right)-{\mathit{\chi }}_1{\mathit{f}}_{{\mathit{u}}_t}\left({\mathit{{\rm I}}}_d\right){\mathit{\chi }}_2$$

(10)

For all times, $t>0$, and for the ${\mathit{\chi }}_1,{\mathit{\chi }}_2\in$ Lie group ${\rm M},$ ${\mathit{{\rm I}}}_d$ is a $d\times d$ identity group matrix. In other words, if Equation (10) is satisfied, the right- and left-invariant errors are trajectory independent, and the following Equation (11) is satisfied. In this paper, we describe the left-invariant error ${\mathit{\eta }}_t$.

$$\frac{d}{dt}{\mathit{\eta }}_t={\mathit{g}}_{{\mathit{u}}_t}\left({\mathit{\eta }}_t\right)$$

(11)

where

$${\mathit{g}}_{{\mathit{u}}_t}\left({\mathit{\eta }}_t\right)={\mathit{f}}_{{\mathit{u}}_t}\left({\mathit{\eta }}_t\right)-{\mathit{f}}_{{\mathit{u}}_t}\left({\mathit{{\rm I}}}_d\right){\mathit{\eta }}_t$$

The matrix ${\mathbf{F}}_t$ satisfies the following Equation (12). Then, ${\mathit{g}}_{{\mathit{u}}_t}\left({\mathit{\eta }}_t\right)$ can be expressed as follows:

$${\mathit{g}}_{{\mathit{u}}_t}\left({\mathit{\eta }}_t\right)={\mathbf{F}}_t{\mathit{\xi }}_t+\mathcal{O}\left({‖{\mathit{\xi }}_t‖}^2\right)$$

(12)

Thus, from Equation (12), for $t>0$, ${\mathit{\xi }}_t$ is defined by the linear differential equation:

$$\frac{d}{dt}{\mathit{\xi }}_t={\mathit{F}}_t{\mathit{\xi }}_t$$

(13)

For an initial error, ${\mathit{\xi }}_0\in {\mathbb{R}}^{dim\left(\mathit{\xi }\right)}$, if ${\mathit{\eta }}_0=Exp\left({\mathit{\xi }}_0\right)$; then, for all, $t\ge 0$:

$${\mathit{\eta }}_t=Exp\left({\mathit{\xi }}_t\right)$$

(14)

This is the log-linear property of the error. Therefore, the nonlinear error can be recovered from the time-varying linear differential Equation (14), where a better convergence is expected than the EKF.

2.3. Magnetic Pose Estimation System (MPS)

In this paper, we use horizontal position and quaternion attitude estimates from the Magnetic Pose Estimation System (MPS) as measurements, which are loosely coupled to the INS model using the EKF and IEKF, as described later. The MPS is an algorithm that uses four coils to generate an artificial magnetic field and estimates the position and attitude through a least square algorithm. Specifically, the residual between the theoretical magnetic field based on the circular loop magnetic field model and the magnetic field measurements from the 3-axis magnetometer mounted on the receiver is used as a numerical metric [13].

While the MPS estimates the three-dimensional position and attitude in an n frame, in this paper, we reduce it to UGV measurements and use only the two-dimensional position and attitude perturbation, as the MPS states. Equation (15) is the state of the MPS, where ${\mathbf{p}}_n$ is the position in the n frame (navigation frame), and $\delta \mathit{\theta }$ is the attitude perturbation.

$${\mathit{x}}_{MPS}={\left[\begin{array}{cc}{\mathbf{p}}_n& \delta \mathit{\theta }\end{array}\right]}^T={\left[\begin{array}{ccc}{\mathrm{p}}_{x,n}& {\mathrm{p}}_{y,n}& {\delta \mathit{\theta }}^T\end{array}\right]}^T$$

(15)

$$\delta \mathit{\theta }={\left[\begin{array}{ccc}{\delta \theta }_x& {\delta \theta }_y& {\delta \theta }_z\end{array}\right]}^T$$

(16)

In this paper, a total of four circular coils were used to generate the AC magnetic field. The carrier frequency of each coil transmitter was 500, 600, 700, and 800 Hz, and the theoretical AC magnetic field ${\mathbf{B}}_c$ in the local coil coordinate frame, obtained using the circular loop magnetic field model at each coil, is defined as follows:

$${\mathbf{B}}_c={\left[\begin{array}{ccc}{\mathrm{B}}_{x, c}& {\mathrm{B}}_{y,c}& {\mathrm{B}}_{z,c}\end{array}\right]}^T$$

(17)

The detailed theoretical formulation and expansion of Equation (17) is omitted. In [13], the formula is newly developed using Legendre polynomials and is effectively designed to compute elliptic integrals. Here, ${\mathbf{B}}_c$ is a function of the position ${\mathit{p}}_c$ in the coil frame and can, therefore, be expressed as ${\mathbf{B}}_c\left({\mathit{p}}_c\right)$.The magnetometer mounted on the receiver measures the magnetic field in the b frame; therefore, transforming ${\mathbf{B}}_c$ to the body frame to express the observation model in the body frame, the magnetic field vector in the b frame is finally represented by:

$${\mathbf{B}}_b={\left[\begin{array}{ccc}{\mathrm{B}}_{x, b}& {\mathrm{B}}_{y,b}& {\mathrm{B}}_{z,b}\end{array}\right]}^{\mathrm{T}}={{\mathit{C}}_n^b\mathit{C}}_c^n{\mathbf{B}}_c\left({\mathit{p}}_c\right)={\mathit{C}}_n^b{\mathbf{B}}_n\left({\mathit{p}}_n\right)$$

(18)

where ${\mathit{C}}_c^n$ is the direction cosine matrix from the coil frame to n frame, which is determined by the pose of the installed coil.

Note that in Equation (18), ${\mathit{C}}_n^b$ is related to the attitude perturbation $\delta \mathit{\theta }$, and ${\mathbf{B}}_n\left({\mathit{p}}_n\right)$ is related to ${\mathit{p}}_n$. This allows us to derive the observation matrix below and, consequently, estimate the pose.

Next, to express the magnetic field of each coil, we use the following notation: Express the magnetic field vector of the i-th coil as ${{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^{\mathit{i}}$.

The observation model $h\left(\mathit{x}\right)$ for the MPS is as follows:

$${h\left(\mathit{x}\right)}_{MPS}=\left[\begin{array}{c}{\mathit{C}}_n^b·{{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^1\\ {\mathit{C}}_n^b·{{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^2\\ {\mathit{C}}_n^b·{{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^3\\ {\mathit{C}}_n^b·{{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^4\end{array}\right]$$

(19)

where DCM ${\mathit{C}}_n^b$ can be expressed as two DCMs using the chain rule as follows: $\hat{b}$ denotes the intermediate b frame by $\hat{\mathit{q}}$. Refer to [13] for a detailed derivation.

$$\begin{array}{c}{\mathit{C}}_n^b={\mathit{C}}_{\widehat{b}}^b·{\mathit{C}}_n^{\hat{b}}\end{array}$$

$${\mathit{C}}_{\widehat{b}}^b\cong \left[\begin{array}{ccc}1& {\delta \theta }_z& -{\delta \theta }_y\\ -{\delta \theta }_z& 1& {\delta \theta }_x\\ {\delta \theta }_y& -{\delta \theta }_x& 1\end{array}\right]$$

The observation matrix $\mathit{H}$ is the Jacobian of the observation model with respect to the MPS state.

$$\begin{array}{cc}{\mathit{H}}_{MPS}=\frac{\partial {h\left(\mathit{x}\right)}_{MPS}}{\partial {\mathit{x}}_{MPS}}& ={\left[\begin{array}{cc}\frac{\partial {h\left(\mathit{x}\right)}_{MPS}}{\partial {\mathbf{p}}_n}& \frac{\partial {h\left(\mathit{x}\right)}_{MPS}}{\partial \delta \mathit{\theta }}\end{array}\right]}_{4i\times 6}\\ & \cong \left[\begin{array}{cc}{\mathit{C}}_n^b·\frac{\partial {{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^1}{\partial {\mathit{p}}_n}& {\left[{\mathit{C}}_n^{\widehat{b}}·{{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^1\right]}_{\times }^{ }\\ {\mathit{C}}_n^b·\frac{\partial {{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^2}{\partial {\mathit{p}}_n}& {\left[{\mathit{C}}_n^{\widehat{b}}·{{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^2\right]}_{\times }^{ }\\ {\mathit{C}}_n^b·\frac{\partial {{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^3}{\partial {\mathit{p}}_n}& {\left[{\mathit{C}}_n^{\widehat{b}}·{{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^3\right]}_{\times }^{ }\\ {\mathit{C}}_n^b·\frac{\partial {{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^4}{\partial {\mathit{p}}_n}& {\left[{\mathit{C}}_n^{\widehat{b}}·{{\mathbf{B}}_n\left({\mathit{p}}_n\right)}^4\right]}_{\times }^{ }\end{array}\right]\\ & ={\left[\begin{array}{cccccc}{H}_{\mathrm{1,1}}& H_{\mathrm{1,2}}& H_{\mathrm{1,3}}& H_{\mathrm{1,4}}& H_{\mathrm{1,5}}& H_{\mathrm{1,6}}\\ H_{\mathrm{2,1}}& H_{\mathrm{2,2}}& H_{\mathrm{2,3}}& H_{\mathrm{2,4}}& H_{\mathrm{2,5}}& H_{\mathrm{2,6}}\\ H_{\mathrm{3,1}}& H_{\mathrm{3,2}}& H_{\mathrm{3,3}}& H_{\mathrm{3,4}}& H_{\mathrm{3,5}}& H_{\mathrm{3,6}}\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\end{array}\right]}_{12\times 6}\end{array}$$

(20)

where

$$\begin{array}{c}\frac{\partial {\mathbf{B}}_n\left({\mathit{p}}_n\right)}{\partial {\mathit{p}}_n}=\frac{\partial {\mathbf{B}}_n\left({\mathit{p}}_n\right)}{\partial {\mathit{p}}_c}·\frac{\partial {\mathit{p}}_c}{\partial {\mathit{p}}_n}\\ =\frac{\partial \left({\mathit{C}}_c^n·{\mathbf{B}}_c\left({\mathit{p}}_c\right)\right)}{\partial {\mathit{p}}_c}·\frac{\partial \left({\mathit{C}}_n^c·{\mathit{p}}_n\right)}{\partial {\mathit{p}}_n}={\mathit{C}}_c^n·\left[\begin{array}{ccc}\frac{\partial B_{x,c}}{\partial x_c}& \frac{\partial B_{x,c}}{\partial y_c}& 0\\ \frac{\partial B_{y,c}}{\partial x_c}& \frac{\partial B_{y,c}}{\partial y_c}& 0\\ 0& 0& \frac{\partial B_{z,c}}{\partial z_c}\end{array}\right]·{\mathit{C}}_n^c\end{array}$$

(21)

The Jacobian matrix in (21) can be further derived via the partial derivative of the spatial representation model of the magnetic fields.

Next, let the real magnetic field measurement from i-th coil be $M_b^i$. Here, note that $M_b^i$ is computed by the demodulation process, as the raw magnetometer measurement contains concurrent AC fields from all TX coils. The detailed demodulation process is out of scope and skipped. Then, the formula for estimating position and attitude perturbation error is established using the numerical optimization method. Specifically, the formula for estimating position and attitude perturbation error using the least square is depicted as:

$$\left[\begin{array}{c}{\tilde{\mathit{p}}}_n\\ \delta \tilde{\mathit{\theta }}\end{array}\right]={({{\mathit{H}}_{MPS}}^T\ast {\mathit{H}}_{MPS})}^{-1}\ast {{\mathit{H}}_{MPS}}^T\ast (\left[\begin{array}{c}{M}_b^1\\ M_b^2\\ M_b^3\\ M_b^4\end{array}\right]-{h\left(\mathit{x}\right)}_{MPS})$$

(22)

where ${\tilde{\mathit{p}}}_n$ and $\delta \tilde{\mathit{\theta }}$ are the errors of ${\mathit{p}}_n$ and $\delta \mathit{\theta }$, respectively. Since the position error and attitude perturbation error are estimated using the least square described above, the formula for updating the position and attitude is as follows:

$${{\widehat{\mathit{p}}}_n}_{k+1}={{\widehat{\mathit{p}}}_n}_k+{\tilde{\mathit{p}}}_n$$

(23)

$${\widehat{\mathit{q}}}_{k+1}={\widehat{\mathit{q}}}_k⊛{\left[\begin{array}{cc}\mathit{cos}\frac{‖\delta \tilde{\mathit{\theta }}‖}{2}& \mathit{sin}\frac{‖\delta \tilde{\mathit{\theta }}‖}{2}·\frac{{\delta \tilde{\mathit{\theta }}}^T}{‖\delta \tilde{\mathit{\theta }}‖}\end{array}\right]}^T$$

(24)

where $⊛$ is the quaternion multiplication.

Since this paper deals with 2D horizontal experiments, the reduced observation matrix, least square formula, and state update equation are as follows:

$${\mathit{H}}_{MPS,2D}={\left[\begin{array}{cccccccc}{H}_{\mathrm{1,1}}& H_{\mathrm{2,1}}& H_{\mathrm{4,1}}& H_{\mathrm{5,1}}& H_{\mathrm{7,1}}& H_{\mathrm{8,1}}& H_{\mathrm{10,1}}& H_{\mathrm{11,1}}\\ H_{\mathrm{1,2}}& H_{\mathrm{2,2}}& H_{\mathrm{4,2}}& H_{\mathrm{5,2}}& H_{\mathrm{7,2}}& H_{\mathrm{8,2}}& H_{\mathrm{10,2}}& H_{\mathrm{11,2}}\\ H_{\mathrm{1,6}}& H_{\mathrm{2,6}}& H_{\mathrm{4,6}}& H_{\mathrm{5,6}}& H_{\mathrm{7,6}}& H_{\mathrm{8,6}}& H_{\mathrm{10,6}}& H_{\mathrm{11,6}}\end{array}\right]}^T$$

(25)

$$\left[\begin{array}{c}{\tilde{\mathit{p}}}_{n,2D}\\ {\delta \tilde{\theta }}_z\end{array}\right]={\left({{\mathit{H}}_{MPS,2D}}^T·{\mathit{H}}_{MPS,2D}\right)}^{-1}·{{\mathit{H}}_{MPS,2D}}^T·{\left(\left[\begin{array}{c}{M}_b^1\\ M_b^2\\ M_b^3\\ M_b^4\end{array}\right]-{h\left(\mathit{x}\right)}_{MPS}\right)}_{2D}$$

(26)

$${\widehat{\mathit{p}}}_{n,k+1}={\widehat{\mathit{p}}}_{n,k}+{\left[\begin{array}{c}{\tilde{\mathit{p}}}_{n,2D}\\ 0\end{array}\right]}_{3\times 1}$$

(27)

$${\widehat{\mathit{q}}}_{k+1}={\widehat{\mathit{q}}}_k⊛\left[\begin{array}{cc}\cos \frac{{\delta \tilde{\theta }}_z}{2}& \sin \frac{{\delta \tilde{\theta }}_z}{2}·\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\end{array}\right]$$

(28)

3. Invariant Extended Kalman Filter Design

3.1. IEKF State Representation

Before defining the full state of the IEKF, we first define a partial state, $\mathit{\chi }$ (a group of double direct isometries ${SE}_2\left(3\right)$), that does not include the IMU bias.

$$\mathit{\chi }=\left[\begin{array}{ccc}{\mathbf{C}}_{\mathrm{b}}^{\mathrm{n}}& {\mathbf{v}}_n& {\mathbf{p}}_n\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]\in {SE}_2\left(3\right)$$

(29)

where ${\mathbf{C}}_{\mathrm{b}}^{\mathrm{n}}$ is the direction cosine matrix from the b frame to the n frame, and ${\mathbf{v}}_n$, ${\mathbf{p}}_n$ are the velocity and position vectors in the n frame, respectively. The inverse of the matrix Lie group ${SE}_2\left(3\right)$ state is expressed in Equation (30).

$${\mathit{\chi }}^{-1}=\left[\begin{array}{ccc}{\mathbf{C}}_{\mathrm{n}}^{\mathrm{b}}& -{\mathbf{C}}_{\mathrm{n}}^{\mathrm{b}}{\mathbf{v}}_n& -{\mathbf{C}}_{\mathrm{n}}^{\mathrm{b}}{\mathbf{p}}_n\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]$$

(30)

The inputs $\mathbf{u}$ are expressed as Equation (31) and are the angular velocity ${\mathsf{\omega }}_b$ and linear acceleration ${\mathbf{a}}_b$ measured by the gyroscope and accelerometer, respectively, in the b frame. Letting $\mathbf{u}$ be the true Inertial Measurement Unit (IMU) measurement in Equation (31), we can express it as follows, considering bias and noise:

$$\mathbf{u}={\left[\begin{array}{cc}{\mathsf{\omega }}_b& {\mathbf{a}}_b\end{array}\right]}^T$$

$${\mathsf{\omega }}_b={\tilde{\mathsf{\omega }}}_b-{\mathbf{b}}_b^g-{\mathbf{n}}^g$$

$${\mathbf{a}}_b={\tilde{\mathbf{a}}}_b-{\mathbf{b}}_b^a-{\mathbf{n}}^a$$

$$\mathbf{w}={\left[\begin{array}{cccc}{\mathbf{n}}^g& {\mathbf{n}}^a& {\mathbf{n}}^{{\mathbf{b}}_b^g}& {\mathbf{n}}^{{\mathbf{b}}_b^a}\end{array}\right]}^{\mathrm{T}}$$

(31)

where ${\tilde{\mathsf{\omega }}}_b$ and ${\tilde{\mathbf{a}}}_b$ are the gyroscope and accelerometer outputs from the experiment, respectively. The subscript n and b imply the n frame and b frame, respectively. ${\mathbf{n}}^g$ and ${\mathbf{n}}^a$ are the Gaussian white noise of the gyroscope and accelerometer measurements, respectively. Gyroscope bias ${\mathbf{b}}_b^g$ and accelerometer bias ${\mathbf{b}}_b^a$ are modeled as the random walk process with Gaussian noises, respectively.

When using IMU sensors in real-world experiments, the bias estimation of gyroscopes and accelerometers is necessary. Therefore, when using the EKF, it is usually estimated by adding the IMU bias to the state. However, since the IEKF uses a matrix Lie group, the IMU bias does not fit into a matrix Lie group form. Therefore, adding IMU bias to the IEKF state means the error estimation is not state independent. However, according to [23], even with the addition of IMU bias to the state, it still converges better than a typical EKF. So, the IEKF state in this paper is Equation (32). Equation (32) is the result of combining Equation (29) with the IMU biases.

$$\mathit{x}={\left[\begin{array}{ccc}\mathit{\chi }& {\mathbf{b}}_b^g& {\mathbf{b}}_b^a\end{array}\right]}^T\in {{SE}_2\left(3\right)\times \mathbb{R}}^6$$

(32)

Since Equation (32) is a compound manifold $\mathcal{G}=\mathit{x}\in {{SE}_2\left(3\right)\times \mathbb{R}}^6$ with an IMU bias added to the ${SE}_2\left(3\right)$ state, we define box-plus $⊞$ and box-minus $\boxminus$ operators in [24].

$$\left[\begin{array}{c}\mathit{\chi }\\ {\mathit{b}}_1\end{array}\right]⊞\left[\begin{array}{c}\mathit{\xi }\\ {\mathit{b}}_2\end{array}\right]\triangleq \left[\begin{array}{c}\mathit{\chi }·Exp\left(\mathit{\eta }\right)\\ {\mathit{b}}_1+{\mathit{b}}_2\end{array}\right]$$

$$\left[\begin{array}{c}{\mathit{\chi }}_1\\ {\mathit{b}}_1\end{array}\right]\boxminus \left[\begin{array}{c}{\mathit{\chi }}_2\\ {\mathit{b}}_2\end{array}\right]\triangleq \left[\begin{array}{c}Log\left({\mathit{\chi }}_2^{-1}·{\mathit{\chi }}_1\right)\\ {\mathit{b}}_1-{\mathit{b}}_2\end{array}\right]$$

(33)

where $\mathit{\chi }\in {\rm M}$, $\mathit{\xi }\in$ ${\mathbb{R}}^{dim\left(\mathfrak{m}\right)}$, and $\mathit{b}\in {\mathbb{R}}^n$.

The Lie algebra of the error $\mathit{\xi }$ of Lie group ${SE}_2\left(3\right)$ $\mathit{\chi }$ is expressed as follows:

$${\mathit{\xi }}^{^}=log\left({\widehat{\mathit{\chi }}}_{ }^{-1}·{\mathit{\chi }}_{ }\right)=\left[\begin{array}{ccc}{\left[\delta \begin{array}{c}{\widehat{\mathit{\theta }}}_{ }\end{array}\right]}_{\times }^{ }& \delta {\widehat{\mathbf{v}}}_n& \delta {\widehat{\mathbf{p}}}_n\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]$$

(34)

Therefore, the error state of the IEKF is expressed as

$$\delta \begin{array}{c}\widehat{\mathit{x}}\end{array}=\mathit{x}\boxminus \widehat{\mathit{x}}={\left[\begin{array}{ccc}Log\left({\widehat{\mathit{\chi }}}_{ }^{-1}·{\mathit{\chi }}_{ }\right)& {\mathbf{b}}_b^g-{\widehat{\mathbf{b}}}_b^g & {\mathbf{b}}_b^a-{\widehat{\mathbf{b}}}_b^a\end{array}\right]}^T$$

$$={\left[\begin{array}{ccccc}\delta {\widehat{\mathit{\theta }}}_{ }& \delta {\widehat{\mathbf{v}}}_n& \delta {\widehat{\mathbf{p}}}_n& \delta {\widehat{\mathbf{b}}}_b^g& \delta {\widehat{\mathbf{b}}}_b^a\end{array}\right]}^T\in {\mathbb{R}}^{15}$$

(35)

The state continuous model at time $t$ is represented as follows: Since this paper addresses navigation in indoor spaces, we use the following simple model.

$$\frac{d}{dt}{{\mathbf{C}}_{\mathrm{b}}^{\mathrm{n}}}_t={{\mathbf{C}}_{\mathrm{b}}^{\mathrm{n}}}_t{\left[{{\tilde{\mathsf{\omega }}}_b}_t-{{\mathbf{b}}_b^g}_t-{\mathbf{n}}^g\right]}_{\times }^{ }$$

$$\frac{d}{dt}{{\mathbf{v}}_n}_t={{\mathbf{C}}_{\mathrm{b}}^{\mathrm{n}}}_t\left({{\tilde{\mathbf{a}}}_b}_t-{{\mathbf{b}}_b^a}_t-{\mathbf{n}}^a\right)+{\mathbf{g}}_n$$

$$\frac{d}{dt}{{\mathbf{p}}_n}_t={{\mathbf{v}}_n}_t$$

$$\frac{d}{dt}{{\mathbf{b}}_b^g}_t={\mathbf{n}}^{bg}$$

$$\frac{d}{dt}{{\mathbf{b}}_b^a}_t={\mathbf{n}}^{ba}$$

(36)

where ${\mathbf{g}}_n$ is the gravitational acceleration in the n frame. The error state covariance following continuous time is expressed by the Riccati equation in Equation (37).

$$\frac{d}{dt}{\mathit{P}}_t={\mathit{F}}_{\mathrm{t}}{\mathit{P}}_t+{\mathit{P}}_t{\mathit{F}}_t^T+{\mathit{Q}}_t$$

(37)

where ${\mathit{F}}_{\mathrm{t}}$ is the state transition matrix.

According to Ref. [24], the error state continuous model is as follows: These equations are first-order approximations after the derivative of the error state in Equation (36).

$$\frac{d}{dt}{\left[\delta {\widehat{\mathit{\theta }}}_t\right]}_{\times }^{ }=\frac{d}{dt}\left({{{\mathbf{C}}_{\mathrm{b}}^{\mathrm{n}}}_t}^{\mathit{T}}{{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_t\right)\approx {\left[-{\left[{{\tilde{\mathsf{\omega }}}_b}_t-\delta {{\widehat{\mathbf{b}}}_b^g}_t\right]}_{\times }^{ }\begin{array}{c}\delta {\widehat{\mathit{\theta }}}_t-\delta {{\widehat{\mathbf{b}}}_b^g}_t-{\mathbf{n}}^g\end{array}\right]}_{\times }^{ }$$

$$\begin{array}{c}\frac{d}{dt}\delta {{\widehat{\mathbf{v}}}_n}_t=\frac{d}{dt}{{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_t^T\left({{\widehat{\mathbf{v}}}_n}_t-{{\mathbf{v}}_n}_t\right)\\ \approx -{\left[\begin{array}{c}{{\tilde{\mathbf{a}}}_b}_t-\end{array}{{\widehat{\mathbf{b}}}_b^a}_t\right]}_{\times }^{ }\delta {\widehat{\mathit{\theta }}}_t-{\left[\begin{array}{c}{{\tilde{\mathsf{\omega }}}_b}_t\end{array}-{{\widehat{\mathbf{b}}}_b^g}_t\right]}_{\times }^{ }\delta {{\widehat{\mathbf{v}}}_n}_t-\delta {{\widehat{\mathbf{b}}}_b^a}_t-{\mathbf{n}}^a\end{array}$$

$$\frac{d}{dt}\delta {{\widehat{\mathbf{p}}}_n}_t=\frac{d}{dt}{{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_t^T\left({{\widehat{\mathbf{p}}}_n}_t-{{\mathbf{p}}_n}_t\right)\approx \delta {{\widehat{\mathbf{v}}}_n}_t-{\left[{{\tilde{\mathsf{\omega }}}_b}_t-{\widehat{\mathbf{b}}}_t^g\right]}_{\times }^{ }\delta {\widehat{\mathbf{p}}}_t$$

(38)

From the above equation, the state transition matrix of continuous time is obtained as follows: Equation (39) is the Jacobian of the continuous model Equation (38) with respect to the error state in Equation (35).

$${\mathit{F}}_{\mathrm{t}}=\left[\begin{array}{ccccc}-{\left[{{\tilde{\mathsf{\omega }}}_b}_t-{{\widehat{\mathbf{b}}}_b^g}_t\right]}_{\times }^{ }& 0& 0& {-\mathit{{\rm I}}}_{3\times 3}& 0\\ -{\left[\begin{array}{c}{{\tilde{\mathbf{a}}}_b}_t-\end{array}{{\widehat{\mathbf{b}}}_b^a}_t\right]}_{\times }^{ }& -{\left[{{\tilde{\mathsf{\omega }}}_b}_t-{{\widehat{\mathbf{b}}}_b^g}_t\right]}_{\times }^{ }& 0& 0& {-\mathit{{\rm I}}}_{3\times 3}\\ 0& {\mathit{{\rm I}}}_{3\times 3}& -{\left[{{\tilde{\mathsf{\omega }}}_b}_t-{{\widehat{\mathbf{b}}}_b^g}_t\right]}_{\times }^{ }& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]$$

(39)

Therefore, the following is the process of expanding the discrete model equations for the error state covariance and state of the left IEKF. The superscript on ${\mathit{P}}_{k+1}^{-}$ means that the covariance ${\mathit{P}}_{k+1}^{-}$ is an a priori covariance.

$${\mathit{P}}_{k+1}^{-}={\mathit{\Phi }}_{\mathrm{k}}{\mathit{P}}_k{\mathit{\Phi }}_k^T+{\mathit{Q}}_d$$

(40)

$${\mathit{Q}}_d=\underset{t_k}{\overset{t_{k+1}}{\int }}{{\mathit{\Phi }}_{\mathrm{k}}\mathit{Q}}_t{\mathit{\Phi }}_k^T\mathrm{d}\mathrm{t}\approx {{\mathit{\Phi }}_k\mathit{Q}}_k{\mathit{\Phi }}_k^T∆t$$

(41)

$${\mathit{\Phi }}_k=exp\left({\mathit{F}}_{\mathrm{t}}∆t\right)$$

(42)

The analytical solution of the transition matrix ${\mathit{\Phi }}_k$ can be found in [24]. The following is the discrete time analytical integral equation of the left IEKF state in the continuous time (Equation (36)). A more detailed derivation can be found in [24].

$${{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_{k+1}^{-}={{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_k{\mathsf{\Gamma }}_0\left({{\tilde{\mathsf{\omega }}}_b}_t∆t\right)$$

$${\widehat{\mathbf{v}}}_{k+1}^{-}={\widehat{\mathbf{v}}}_k+{{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_k{\mathsf{\Gamma }}_1\left({{\tilde{\mathsf{\omega }}}_b}_t∆t\right){{\tilde{\mathbf{a}}}_b}_t∆t+{\mathbf{g}}_n∆t$$

$${\widehat{\mathbf{p}}}_{k+1}^{-}={\widehat{\mathbf{p}}}_k+{\widehat{\mathbf{v}}}_k∆t+{\widehat{\mathbf{C}}}_{\mathrm{b}k}^{\mathrm{n}}{\mathsf{\Gamma }}_2\left({\tilde{\mathsf{\omega }}}_{bt}∆t\right){{\tilde{\mathbf{a}}}_b}_t∆t^2+\frac{1}{2}{\mathbf{g}}_n∆t^2$$

$${\widehat{\mathbf{b}}}_{\mathrm{b}k+1}^{a-}={{\widehat{\mathbf{b}}}_b^a}_k$$

$${\widehat{\mathbf{b}}}_{\mathrm{b}k+1}^{g-}={{\widehat{\mathbf{b}}}_b^g}_k$$

(43)

where

$${\mathsf{\Gamma }}_0\left(\mathit{\phi }\right)={\mathit{{\rm I}}}_{3\times 3}+\frac{\mathit{sin}\left(‖\mathit{\phi }‖\right)}{‖\mathit{\phi }‖}{\left[\mathit{\phi }\right]}_{\times }^{ }+\left(\frac{1-\mathit{cos}\left(‖\mathit{\phi }‖\right)}{{‖\mathit{\phi }‖}^2}\right){\left({\left[\mathit{\phi }\right]}_{\times }^{ }\right)}^2$$

$${\mathsf{\Gamma }}_1\left(\mathit{\phi }\right)={\mathit{{\rm I}}}_{3\times 3}+\frac{1-\cos \left(‖\mathit{\phi }‖\right)}{{‖\mathit{\phi }‖}^2}{\left[\mathit{\phi }\right]}_{\times }^{ }+\left(\frac{‖\mathit{\phi }‖-\cos \left(‖\mathit{\phi }‖\right)}{{‖\mathit{\phi }‖}^3}\right){\left({\left[\mathit{\phi }\right]}_{\times }^{ }\right)}^2$$

$${\mathsf{\Gamma }}_2\left(\mathit{\phi }\right)=\frac{1}{2}{\mathit{{\rm I}}}_{3\times 3}+\frac{‖\mathit{\phi }‖-\sin \left(‖\mathit{\phi }‖\right)}{{‖\mathit{\phi }‖}^3}{\left[\mathit{\phi }\right]}_{\times }^{ }+\left(\frac{{‖\mathit{\phi }‖}^2+2\cos \left(‖\mathit{\phi }‖\right)-2}{2{‖\mathit{\phi }‖}^4}\right){\left({\left[\mathit{\phi }\right]}_{\times }^{ }\right)}^2$$

3.2. IEKF Measurement Update

In this paper, we use the MPS’s n-frame position and quaternion attitude as the measurement ${\mathbf{z}}_k$ of the IEKF. Therefore, the measurement ${\mathbf{z}}_k$ is an $SE\left(3\right)$ Lie group. However, since the IEKF state is a compound state ${{SE}_2\left(3\right)\times \mathbb{R}}^6$, we convert it to an $SE\left(3\right)$ and fit it into an $SE\left(3\right)$ Lie group, which is the same form as the measurement. Therefore, the measure ${\mathbf{z}}_k$ is expressed as follows:

$${\mathbf{z}}_k=\mathcal{L}\left({\mathit{x}}_{\mathit{k}}\right){\mathbf{b}}_k⊞{\mathit{W}}_k$$

(44)

$${\mathit{b}}_k={\mathit{I}}_{4\times 4}$$

(45)

where $\mathcal{L}\left(·\right)$ is the ${{SE}_2\left(3\right)\times \mathbb{R}}^6⟼ SE\left(3\right)$ conversion operator, and ${\mathit{b}}_k$ is the measurement model’s design matrix. ${\mathit{W}}_k$ is the measurement noise, which is assumed to be zero mean Gaussian white noise.

Using the above equation, the measurement model is expressed as follows: $f_{quaternion}^{DCM}$ is the $quaternion$ to the DCM conversion function.

$$\left[\begin{array}{cc}{f}_{quaternion}^{DCM}\left({\mathit{q}}_{\mathrm{M}\mathrm{P}\mathrm{S}}\right)& {\mathbf{p}}_{n,\mathrm{M}\mathrm{P}\mathrm{S}}\\ 0& 1\end{array}\right]=\mathcal{L}\left({\mathit{x}}_{\mathit{k}}\right){\mathbf{b}}_k⊞{\mathit{W}}_k$$

(46)

$$\left[\begin{array}{cc}{\mathbf{C}}_{\mathrm{b},\mathrm{M}\mathrm{P}\mathrm{S}}^{\mathrm{n}}& {\left[\begin{array}{ccc}{\mathrm{p}}_{x,n,\mathrm{M}\mathrm{P}\mathrm{S}}& {\mathrm{p}}_{y,n,\mathrm{M}\mathrm{P}\mathrm{S}}& 0\end{array}\right]}^{\mathrm{T}}\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{\mathbf{C}}_{\mathrm{b}}^{\mathrm{n}}& {\mathbf{p}}_n\\ 0& 1\end{array}\right]{\mathbf{b}}_k⊞{\mathit{W}}_k$$

(47)

The residual ${\mathbf{r}}_k$ is given by

$${\mathbf{r}}_k=\mathcal{L}\left({\mathit{x}}_k^{-1}\right){\mathbf{z}}_k\boxminus {\mathbf{b}}_k=Log\left(\mathcal{L}\left({\mathit{x}}_k^{-1}\right){\mathbf{z}}_k\right)$$

(48)

Using the residual ${\mathbf{r}}_k$ obtained above, the state update equation is as follows:

$${\widehat{\mathit{x}}}_{k+1}={\widehat{\mathit{x}}}_k⊞\left(\delta \begin{array}{c}\widehat{\mathit{x}}\end{array}\right)={\widehat{\mathit{x}}}_{k}Exp\left(\delta \begin{array}{c}\widehat{\mathit{x}}\end{array}\right)$$

$$\delta \begin{array}{c}\widehat{\mathit{x}}\end{array}={\mathbf{K}}_k{\mathbf{r}}_k$$

(49)

$${{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_{k+1}={{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_k{\mathsf{\Gamma }}_0\left(\delta {\widehat{\mathit{\theta }}}_{ }\right)$$

$${\widehat{\mathbf{v}}}_{k+1}={\widehat{\mathbf{v}}}_k+{{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_k{\mathsf{\Gamma }}_0\left(\delta {\widehat{\mathit{\theta }}}_{ }\right)\delta {\widehat{\mathit{v}}}_b$$

$${\widehat{\mathbf{p}}}_{k+1}={\widehat{\mathbf{p}}}_k+{{\widehat{\mathbf{C}}}_{\mathrm{b}}^{\mathrm{n}}}_k{\mathsf{\Gamma }}_0\left(\delta {\widehat{\mathit{\theta }}}_{ }\right)\delta {\widehat{\mathit{p}}}_b$$

$${\widehat{\mathbf{b}}}_{b,\mathrm{k}+1}^a={\widehat{\mathbf{b}}}_{b,\mathrm{k}}^a+\delta {\widehat{\mathbf{b}}}_b^a$$

$${\widehat{\mathbf{b}}}_{b,\mathrm{k}+1}^g={\widehat{\mathbf{b}}}_{b,\mathrm{k}}^g+\delta {\widehat{\mathbf{b}}}_b^g$$

(50)

where ${\mathbf{K}}_k$ is the Kalman gain, expressed as follows:

$${\mathit{K}}_k={\mathit{P}}_{k+1}^{-}{\mathit{H}}_k^{\mathit{T}}{\mathit{S}}_k^{-1}$$

(51)

where

$${\mathbf{S}}_k={\mathit{H}}_k{\mathit{P}}_{k+1}^{-}{\mathit{H}}_k^{\mathit{T}}+{\overline{\mathit{N}}}_k$$

$${\overline{\mathit{N}}}_k={\mathit{E}}_k{\mathit{N}}_k{\mathit{E}}_k^{\mathit{T}}$$

For detailed derivations of ${\mathit{H}}_k$ and ${\mathit{E}}_k$, see Appendix A and Appendix B. ${\mathit{N}}_k$ is the covariance matrix.

$${\mathit{H}}_k={\mathcal{J}}_{{\mathit{\xi }}_k}^{{\mathbf{r}}_k}={\mathcal{J}}_R^{-1}\left({\mathbf{r}}_k\right){\mathcal{J}}_{{\mathit{\eta }}_k}^{{\mathit{\eta }}_k\ast {\mathit{b}}_k}$$

(52)

$${\mathit{E}}_k={\mathcal{J}}_{{\mathit{w}}_k}^{{\mathbf{r}}_k}={\mathcal{J}}_R^{-1}\left({\mathbf{r}}_k\right){\mathcal{J}}_{{\mathbf{z}}_k}^{{\widehat{\mathit{\chi }}}^{-1}\ast {\mathbf{z}}_k}$$

(53)

Finally, the error state covariance update expression can be expressed as follows:

$${\mathit{P}}_{k+1}=\left(\mathit{{\rm I}}-{\mathit{K}}_k{\mathit{H}}_k\right){\mathit{P}}_{k+1}^{-}{\left(\mathit{{\rm I}}-{\mathit{K}}_k{\mathit{H}}_k\right)}^{\mathit{T}}+{\mathit{K}}_k{\overline{\mathit{N}}}_k{\mathit{K}}_k^{\mathit{T}}$$

(54)

4. Experimental Setup

In this experiment, we used a UGV in an indoor space to drive in square and free trajectories. The indoor environment in which the experiment was conducted is 3.6 m by 3.6 m, and Figure 1a shows the starting point and coil placement. In Figure 1a, you can see a picture of a UGV. The UGV is equipped with an MPS receiver and an NUC computer. The MPS receiver is equipped with an ADIS 16448 IMU (Analog Devices Inc., Wilmington, MA, USA) and Honeywell HMC 1001 1-axis and 1002 2-axis magnetometer sensors, and the NUC computer saves the OptiTrack data. Ground truth was obtained using the motion capture device Optitrack (NaturalPoint, Inc, Corvalis, OR, USA).

The following shows the specifications of the employed sensors:

• ADIS 16448 IMU’s gyroscopes have an in-run bias stability of ${14.5}^{°}$/hr and angular random walk of ≤${0.66}^{°}$/$\sqrt{hr}$. The IMU’s accelerometers have a bias stability of 0.25 mg−1σ and a velocity random walk of 0.11 m/s/$\sqrt{hr}$. The dynamic ranges of the gyroscopes and accelerometers are $\pm {1200}^{°}$/$s$ and $\pm 18 g$, respectively. In this experiment, the sensor measurement rate is 500 Hz;
• The Honeywell HMC 1101 (1-axis) and HMC 1002 (2-axis) magnetometers have a field range of $\pm$2 gauss. Both sensors also have a resolution of 117 μGauss, sensitivity of 1.0 mV/V/gauss, and noise density of 48 nV/sqrt $\sqrt{\mathrm{H}\mathrm{z}}$;
• In the experiment, we obtained reference pose data using the OptiTrack system with $prime \times 22 cameras$, a motion capture device. In a 9 m × 9 m area, the OptiTrack system has a 3D position error of $\pm$0.15 mm and an attitude error of less than 0.5 degrees.

To generate an artificial AC magnetic field, we used four coils with a diameter of 1 m and 7.5 m between each coil. Each coil has a frequency of 500, 600, 700, and 800 Hz. We analyzed the effect on AC magnetic field-based navigation by placing obstacles commonly found in indoor environments. We placed several obstacles made of wood, plastic, and acrylic mirrors within the UGV’s moving trajectory to verify whether they affect the AC magnetic field signals, as shown in Figure 1b. It is notable that an experimental scenario, such as Figure 1b, provides a harsh navigation environment for both vision and Lidar SLAM as it includes mirrors, glasses, and unstable lighting sources.

The ${\mathit{P}}_0$, ${\mathit{Q}}_k$, and ${\mathit{N}}_k$ covariances for the IEKF and EKF were set as follows:

$${\mathit{P}}_0=\left[\begin{array}{ccccc}{\sigma }_{{\delta {\mathit{\theta }}_{ }}_{3\times 3}}^2& 0& 0& 0& 0\\ 0& {\sigma }_{{\delta {\mathbf{v}}_n}_{3\times 3}}^2& 0& 0& 0\\ 0& 0& {\sigma }_{{\delta {\mathbf{p}}_n}_{3\times 3}}^2& 0& 0\\ 0& 0& 0& {\sigma }_{{\delta {\mathbf{b}}_b^g}_{3\times 3}}^2& 0\\ 0& 0& 0& 0& {\sigma }_{{\delta {\mathbf{b}}_b^a}_{3\times 3}}^2\end{array}\right]$$

(55)

$${\mathit{Q}}_k=\left[\begin{array}{cccc}{\sigma }_{{{\mathbf{n}}^g}_{3\times 3}}^2& 0& 0& 0\\ 0& {\sigma }_{{{\mathbf{n}}^a}_{3\times 3}}^2& 0& 0\\ 0& 0& {\sigma }_{{{\mathbf{n}}^{{\mathbf{b}}_b^g}}_{3\times 3}}^2& 0\\ 0& 0& 0& {\sigma }_{{{\mathbf{n}}^{{\mathbf{b}}_b^a}}_{3\times 3}}^2\end{array}\right]$$

(56)

$${\mathit{N}}_k=\left[\begin{array}{cc}{\sigma }_{{{\mathbf{r}}_{\mathit{\theta }}}_{3\times 3}}^2& 0\\ 0& {\sigma }_{{{\mathbf{r}}_{\mathit{p}}}_{3\times 3}}^2 \end{array}\right]$$

(57)

$$\begin{array}{c}\mathrm{where} {\sigma }_{\delta {\mathit{\theta }}_{ }}^2={10}^{-6}{\mathrm{rad}}^2, {\sigma }_{{\delta \mathbf{v}}_n}^2={10}^{-4}{\mathrm{m}}^2/{\mathrm{s}}^2, {\sigma }_{{\delta \mathbf{p}}_n}^2={10}^{-6}{\mathrm{m}}^2, {\sigma }_{\delta {{\mathbf{b}}_b^g}_{ }}^2={10}^{-7}{\mathrm{rad}}^2/{\mathrm{s}}^2, \\ {\sigma }_{\delta {{\mathbf{b}}_b^a}_{ }}^2={10}^{-5}{\mathrm{m}}^2/{\mathrm{s}}^4, {\sigma }_{{\mathbf{n}}^g}^2={10}^{-3}\mathrm{rad}/\mathrm{s}/\sqrt{\mathrm{Hz}}, {\sigma }_{{\mathbf{n}}^a}^2={10}^{-3}\mathrm{m}/{\mathrm{s}}^2/\sqrt{\mathrm{Hz}},\\ {\sigma }_{{\mathbf{n}}^{{\mathbf{b}}_b^g}}^2={10}^{-15}\mathrm{rad}/{\mathrm{s}}^2/\sqrt{\mathrm{Hz}}, {\sigma }_{{\mathbf{n}}^{{\mathbf{b}}_b^a}}^2={10}^{-13}\mathrm{m}/{\mathrm{s}}^3/\sqrt{\mathrm{Hz}}, {\sigma }_{{\mathbf{r}}_{\mathit{\theta }}}^2={10}^{-1}{\mathrm{rad}}^2, {\sigma }_{{\mathbf{r}}_{\mathit{p}}}^2={10}^{-1}{\mathrm{m}}^2\end{array}$$

4.1. Square Path Case

Next, we present three subsections to demonstrate the performance of the proposed filter ion compared with the conventional method. The first is the result of driving a square path. When the UGV drove a square trajectory of 3.6 m by 3.6 m, the estimated horizontal position result is shown in Figure 2a, and the position error and RMSE result are shown in Figure 2b and

Table 1. Pose RMSE results of square path.

	X (m)	Y (m)	Z (m)	Roll (Degree)	Pitch (Degree)	Yaw (Degree)
INS/MPS IEKF	0.0533	0.0670	0.0198	0.1154	0.2635	1.5660

, respectively. Also, the Euler attitude RMSE results can be seen in Table 1. This presents the result when initialization is in a relatively good condition.

4.2. Free Path Case

The second case is the free path driving results. The UGV drove freely in a 3.6 m square area. The horizontal position results are shown in Figure 3a. In addition, the position error and the RMSE result are shown in Figure 3b and

Table 2. Pose RMSE results of free path.

	X (m)	Y (m)	Z (m)	Roll (Degree)	Pitch (Degree)	Yaw (Degree)
INS/MPS IEKF	0.0524	0.0494	0.0260	0.1909	0.2066	1.4566

, respectively. Also, the Euler attitude RMSE results can be observed in Table 2. Assuming accurate initial states, the results demonstrate good RMS error performance. Consequently, it can be verified that the free path results are not significantly different from the square path navigation results when there are no obstacles, even though typical obstacles were placed around the coil and in the driving trajectory. Thus, it means that various obstacles rarely affected the MPS performance via the AC magnetic fields. This reveals that the proposed integration shows superior performance compared to conventional mapping and localization methods, as the SLAM performance essentially relies on the quality of optical measurements.

4.3. Convergence and Performance Evaluation

In this chapter, we evaluate the navigation and convergence performance of the proposed IEKF when the filter initialization is poor. This mainly originates from the magnetic measurement perturbations and the resulting estimate uncertainty during real experiments. For this purpose, we ran 200 Monte Carlo simulations within a range of the initial positions x, y, and yaw angles, as shown in

Table 3. Initial conditions range.

	Position x (m)	Position y (m)	Yaw (Degree)
Initial Condition	$-1.8 ~ 1.8$	$-1.8 ~ 1.8$	$-30° ~ 30°$

. We compare its performance with a quaternion-based error-state Kalman filter for performance evaluation. The initial error covariances, P, Q, and R, and the initial IMU bias are the same for the IEKF and EKF.

The results of the Monte Carlo simulation of the position x, y, and yaw angle are plotted in Figure 4. This is the result of randomly setting initial conditions within the initial condition range in Table 3. In this paper, by adding IMU bias to the IEKF state, it can be verified that the convergence performance of the IEKF is still better than that of Q-EKF, although it is an imperfect matrix Lie group.

To quantitatively evaluate the performance of the IEKF when the initialization is poor, we analyzed the navigation results in four cases, as shown in

Table 4. Initial condition cases.

Case	Position x (m)	Position y (m)	Yaw (Degree)
1	$0.45$	$0.45$	$7.5°$
2	$0.9$	$0.9$	$15°$
3	1.35	1.35	$22.5°$
4	$1.8$	$1.8$	$30°$

. Therefore, we applied both the EKF and IEKF in these four cases. The results of the EKF and IEKF position x, y, and yaw attitude over time, divided by case, are shown in Figure 5. The IEKF has better convergence performance, and its estimation performance is also better, as shown in

Table 5. Pose RMSE comparison: IEKF vs. EKF.

Filter/Case	X (m)	Y (m)	Yaw (Degree)
EKF/1	0.0879	0.0997	3.5351
EKF/2	0.1353	0.1499	6.7250
EKF/3	0.1900	0.2069	10.1776
EKF/4	0.2475	0.2663	13.6961
IEKF/1	0.0645	0.0751	2.6355
IEKF/2	0.0947	0.1054	3.0628
IEKF/3	0.1299	0.1411	3.7056
IEKF/4	0.1699	0.1784	4.4678

.

5. Conclusions

This study proposes an integrated inertial navigation system and AC magnetic field positioning navigation algorithm using the IEKF. The navigation performance was evaluated by driving a UGV on several paths in an indoor environment where the GNSS is unavailable. A comparison with the EKF was made to evaluate the performance of the IEKF under poor initialization conditions, and the IEKF showed better navigation performance and convergence. We also analyzed whether common indoor obstacles affect the performance of magnetic field-based positioning. The results show that common obstacles in indoor environments do not affect the AC magnetic field signal.