A Fast Self-Calibration Method for Dual-Axis Rotational Inertial Navigation Systems Based on Invariant Errors

Abstract

In order to ensure that dual-axis rotational inertial navigation systems (RINSs) maintain a high level of accuracy over the long term, there is a demand for periodic calibration during their service life. Traditional calibration methods for inertial measurement units (IMUs) involve removing the IMU from the equipment, which is a laborious and time-consuming process. Reinstalling the IMU after calibration may introduce new installation errors. This paper focuses on dual-axis rotational inertial navigation systems and presents a system-level self-calibration method based on invariant errors, enabling high-precision automated calibration without the need for equipment disassembly. First, navigation parameter errors in the inertial frame are expressed as invariant errors. This allows the corresponding error models to estimate initial attitude even more rapidly and accurately in cases of extreme misalignment, eliminating the need for coarse alignment. Next, by utilizing the output of a gimbal mechanism, angular velocity constraint equations are established, and the backtracking navigation is introduced to reuse sensor data, thereby reducing the calibration time. Finally, a rotation scheme for the IMU is designed to ensure that all errors are observable. The observability of the system is analyzed based on a piecewise constant system method and singular value decomposition (SVD) observability analysis. The simulation and experimental results demonstrate that this method can effectively estimate IMU errors and installation errors related to the rotation axis within 12 min, and the estimated error is less than 4%. After using this method to compensate for the calibration error, the velocity and position accuracies of a RINS are significantly improved.

1. Introduction

Inertial navigation systems (INSs) are widely used in various automated unmanned vehicles for their advantages, such as good autonomy and strong concealment [1]. The precision of inertial navigation parameters directly affects the performance of an INS. In addition to using high-precision inertial devices, rotation modulation techniques can be introduced to improve the accuracy of an INS. The accuracy of rotational inertial navigation systems (RINSs) can be improved several times when using sensors of the same level. RINSs can self-calibrate by rotating their own gimbal mechanism to excite error parameters, which is more convenient and effective for error calibration [2,3]. Moreover, as the manufacturing cost and volume of inertial devices decrease, RINSs are gradually being applied to various systems and equipment [4,5,6,7,8].

Inertial measurement units (IMUs) typically require high-precision turntable calibration before leaving the factory. However, the service life of an RINS is generally more than ten years. During this period, IMUs can experience changes in the nominal error parameters due to component aging, the deformation of structural components that support the inertial devices, and other factors [9,10]. If the original calibration parameters are used over time, it will lead to a decrease in the accuracy of the inertial navigation system, which can have a negative impact on operational efficiency. Therefore, it is necessary to periodically conduct the high-precision autonomous calibration of RINSs to ensure the system’s performance when in use. However, the existing calibration methods take too long to calibrate and cannot quickly self-calibrate.

Currently, calibration methods are divided into discrete calibration and system-level calibration [11]. The former method involves removing the inertial navigation system, performing calibration using specialized high-precision turntables, and then reinstalling the inertial navigation system. The process is intricate and may introduce new installation errors [12,13]. The latter method uses navigation errors as observation variables to achieve the optimal estimation of inertial navigation system calibration parameters. This method eliminates the need for specific calibration sites and high-cost equipment. RINSs can be conveniently self-calibrated without an external turntable by exciting error parameters through the rotation of their own gimbal mechanisms. A 30-position calibration scheme was proposed for a dual-axis RINS in reference [14], which could calibrate 21 IMU errors by periodically rotating the axis.

Traditional system-level calibration methods for RINSs typically focus on calibrating IMU errors, including zero bias, scale factor errors, and installation errors. Due to the introduction of the gimbal mechanism, the installation errors between the IMU and the rotation axis also need to be accurately calibrated. Reference [15] utilized the difference in attitude between the two RINSs to estimate installation errors, but it could not achieve the self-calibration of the RINS. Reference [16] proposed a system-level self-calibration method for a dual-axis RINS to address two types of installation errors. However, this method requires the knowledge of an accurate initial attitude.

Currently, the primary method for system-level calibration uses the extended Kalman filter (EKF) to estimate errors [17,18]. In the traditional EKF framework, state error definitions, such as position and velocity, only consider magnitude differences and ignore directional differences. When the initial misalignment error is significant, it can lead to inconsistencies in the defined error coordinates [19]. Furthermore, due to the influence of specific force terms in the system matrix, the EKF defined with a linear error state encounters the problem of inconsistent variance estimation. This leads to the filtering estimate of errors that were initially unobservable, causing significant deviations and overly optimistic covariance estimates. To address these issues, the error equation can be projected and transformed to adapt to different application environments. Reference [20] eliminated the need for the high-speed integration of specific forces in the calculation of the Kalman filter transformation matrix by transforming the velocity error, which improved the accuracy and stability of the system. Reference [21] proposed the state transformation extended Kalman filter (ST-EKF), whereby the specific force vector in the system error model is replaced by the nearly constant gravity vector in a geographical coordinate system, ensuring consistent error state definitions and resolving the variance estimation inconsistency problem. In view of the strong non-linearity and high-dimensional problems of the RINS calibration error equation, the filtering method of sampling the probability space is also widely used in system-level calibration [22,23].

In recent years, the Lie group and manifold theories have been applied to some inertial-based applications [24,25]. By representing the states and their errors in double-direct space-isometric groups $S{E}_2\left(3\right)$, new group-based system equations and error equations have been designed [26]. If the system is a group affine, even when the attitude deviation is significant, the attitude can be obtained through a linear error model [27]. Based on the existing EKF framework, Barrau used the Lie group theory to construct navigation parameters, establishing a navigation parameter error equation based on invariant errors, and used an invariant extended Kalman filter (IEKF) to unify the initial alignment process, eliminating the need for coarse alignment [28]. Reference [29] studied the characteristics of $S{E}_k\left(3\right)$ and the specific mathematical expressions of left-invariant and right-invariant errors under various coordinate frames. The right-invariant EKF projects the gravity vector into the inertial frame and has similar properties to the ST-EKF. The left-invariant EKF eliminates the influence of navigation parameters, and the filter performance does not depend on the selected coordinate system. Reference [30] investigated an attitude estimation method using the $S{E}_2\left(3\right)-\mathrm{EKF}$ with consideration of constant drift of the gyro. Therefore, this paper represents navigation parameter errors as invariant errors, transforming a non-linear system into a linear system, thereby improving the accuracy of calibration in cases of large misalignment angles.

To reduce the calibration time and improve calibration accuracy, this paper designs a rotation scheme and utilizes the output of a gimbal mechanism to establish extended measurement equations, rendering all errors observable. Subsequently, the observability of the system is analyzed based on the piecewise constant system (PWCS) observability method and singular value decomposition (SVD) observability analysis. Finally, the paper introduces backtracking navigation [31,32], enabling the sensor data to be reused to enhance the estimation accuracy within a short period.

The rest of this paper is organized as follows. Section 2 introduces the mathematical foundations for expressing navigation states using $S{E}_k\left(3\right)$. Section 3 presents the IMU error model, gimbal mechanism errors, and the navigation error state model based on $S{E}_2\left(3\right)$, demonstrating that this system is a group affine. Section 4 describes the designed rotation scheme, backtracking scheme, angular velocity constraints (AVCs) during rotation, the establishment of the measurement equations, and the observability analysis of the system. Section 5 validates the proposed method through simulations and experiments. Section 6 provides a summary and outlines future prospects.

2. Mathematical Preliminaries and the Framework of the Proposed Method

In order to apply the Lie group theory [33] in RINS system-level self-calibration, this section introduces the mathematical preliminaries of matrix Lie group and Lie algebra to facilitate subsequent error modeling and formula derivation. The matrix Lie group $S{E}_2\left(3\right)$ is used to represent the extended pose as [34].

$$S{E}_2\left(3\right)=\left\{T=\left[\frac{C}{{\mathbf{0}}_{2\times 3}}|\frac{v p}{I_{2\times 2}}\right]\in R^{5\times 5}|\begin{array}{c}C\in SO(3)\\ v,p\in R^{3\times 1}\end{array}\right\}$$

(1)

where $SO\left(3\right)$ is the special orthogonal group and $C,v,p$ are the attitude rotation matrix, speed, and position, respectively. The inverse of $T$ is provided by

$$T^{-1}=\left[\frac{C^{\mathrm{T}}}{{\mathbf{0}}_{2\times 3}}|\frac{-C^{\mathrm{T}}v-C^{\mathrm{T}}p}{I_{2\times 2}}\right]\in S{E}_2(3)$$

(2)

The Lie algebra associated with $S{E}_2\left(3\right)$ is provided by

$$s{e}_2\left(3\right)=\left\{\Xi ={\zeta }^{\wedge }\in R^{5\times 5}|\zeta \in R^{9\times 1}\right\}$$

(3)

where

$${\mathit{\zeta }}^{\wedge }={\left[\begin{array}{c}\mathit{\rho }\\ \mathit{\nu }\\ \mathit{\phi }\end{array}\right]}^{\wedge }=\left[\frac{\mathit{\phi }\times }{{\mathbf{0}}_{2\times 3}}|\frac{\mathit{\nu } \mathit{\rho }}{{\mathbf{0}}_{2\times 2}}\right]\in R^{5\times 5},\mathit{\phi },\mathit{\nu },\mathit{\rho }\in R^{3\times 1}$$

(4)

The Lie algebra and Lie group can be transformed into each other via exponential mapping $\mathrm{Exp}(•)$ and logarithmic mapping $\mathrm{Log}(•)$ as follows:

$$T=\mathrm{Exp}\left(\mathit{\zeta }\right)=\left[\frac{\exp \left(\mathit{\phi }\right)}{{\mathbf{0}}_{2\times 3}}|\frac{J_l\left(\mathit{\phi }\right)\mathit{\nu } J_l\left(\mathit{\phi }\right)\mathit{\rho }}{I_{2\times 2}}\right]$$

(5)

$$\mathit{\zeta }=\mathrm{Log}\left(T\right)=\left[\begin{array}{c}\phi a\\ J_l^{-1}\left(\mathit{\phi }\right)v\\ J_l^{-1}\left(\mathit{\phi }\right)p\end{array}\right]$$

(6)

where

$$\phi ={\cos }^{-1}\left(\left(\mathrm{tr}\left(C\right)-1\right)/2\right)$$

(7)

$$\exp \left(\mathit{\phi }\right)=\cos \phi I_{3\times 3}+\left(1-\cos \phi \right)a{a}^{\mathrm{T}}+\sin \phi \left(a\times \right)$$

(8)

$$J_l\left(\mathit{\phi }\right)=\left(\sin \phi /\phi \right)I_{3\times 3}+\left(1-\left(\sin \phi /\phi \right)\right)a{a}^{\mathrm{T}}+\left(\left(1-\cos \phi \right)/\phi \right)\left(a\times \right)$$

(9)

$$J_l^{-1}\left(\mathit{\phi }\right)=\left(\phi /2\right)\left(\cot \phi /2\right)I_{3\times 3}+\left(1-\left(\phi /2\right)\left(\cot \phi /2\right)\right)a{a}^{\mathrm{T}}-\left(\phi /2\right)\left(a\times \right)$$

(10)

$\mathit{\phi }=\phi a$ is the rotation vector, $\mathsf{\phi }$ denotes the rotation angle, and $a$ is the unit-length axis of rotation. $\left(•\times \right)$ denotes the skew-symmetric matrix operation. When $\mathit{\phi }$ is assumed to be small, then $\exp \left(\mathit{\phi }\right)\approx I_{3\times 3}+\left(\mathit{\phi }\times \right)$, $J_l\left(\mathit{\phi }\right)\approx I_{3\times 3}+\left(\mathit{\phi }\times \right)/2$, and $J_l^{-1}\left(\mathit{\phi }\right)\approx I_{3\times 3}-\left(\mathit{\phi }\times \right)/2$.

In addition to the traditional IMU error, this article considers the installation error of the gimbal mechanism and stores the gyro, accelerometer, and gimbal mechanism signals outputs in real time. In order to quickly and accurately estimate the error parameters, the navigation parameters are established in the form of invariant errors, the linear Kalman filter is used to accurately estimate the calibration errors, and the stored data is reused through the backtracking algorithm to shorten the filter convergence time. Finally, a fast, autonomous, and accurate dual-axis rotation inertial navigation system-level self-calibration algorithm is achieved. The structure of the Kalman filter based on the invariant error during self-calibration is shown in Figure 1.

3. Gimbal Mechanism Installation Error Model, IMU Error Model, and Navigation Error Model

To better analyze the mechanism of the calibration process, several coordinate systems are defined in

Table 1. Definitions of the reference frames.

Reference Frame	Definition
a-frame	Accelerometer installation frame
g-frame	Gyro installation frame
s-frame	Sensor frame
s0-frame	Sensor frame in the initial state
b-frame	Body frame, which is the orthogonal reference frame of a dual-axis RINS. In this paper, the b-frame coincides with the s0-frame
n-frame	The navigation frame, which is the orthogonal reference frame aligned with the east-north-up geodetic axes
in0-frame	The navigation inertial frame, which is the fixed axes of the n-frame in the inertial space at the beginning of the calibration process
e-frame	Earth-centered earth-fixed (ECEF) frame
e0-frame	ECEF inertial frame in the initial state
r-frame	r-frame, which is the orthogonal reference frame of the gimbal mechanism. Its three axes are defined by $x_r,y_r,z_r$. $x_r$ coincides with the outer axis, $z_r$ coincides with the inner axis, while $y_r$ forms a right-handed orthogonal frame with $x_r$ and $z_r$
r0-frame	r-frame in the initial state

.

3.1. Gimbal Mechanism Installation Error Model

For a dual-axis RINS, the IMU is installed on the dual-axis gimbal mechanism. Due to gimbal mechanism installation errors, the r-frame needs to be established to describe the spatial relationship between the r-frame and the s-frame. There is a transformation between the r-frame and the s-frame in the initial state, which is shown in Figure 2.

Generally, the gimbal mechanism installation error is small, and the transformation matrix between the r-frame and the s-frame is represented as:

$$C_r^s=\left[\begin{array}{ccc}1& -\gamma & \beta \\ \gamma & 1& -\alpha \\ -\beta & \alpha & 1\end{array}\right]=I_{3\times 3}+\left(\mathit{\mu }\times \right)$$

(11)

where $\mathit{\mu }={\left[\alpha ,\beta ,\gamma \right]}^{\mathrm{T}}$ is the installation angle error between the s-frame and the r-frame.

3.2. IMU Error Model

The coordinate system formed with the output of the three-axis accelerometer is the a-frame. The coordinate system formed with the output of the three-axis gyroscope is the g-frame. Due to the installation errors, the a-frame and g-frame are both non-orthogonal coordinate systems. Under normal circumstances, the a-frame and the g-frame are not coincident. Therefore, it is necessary to convert the three-axis acceleration output and the three-axis gyroscope output into the same sensor coordinate system, which is the s-frame. The definition of the s-frame is as follows [16]: $y_s$ is in the $x_a$, $y_a$ plane, $x_s$ coincides with $x_a$, while $z_s$ forms a right-handed orthogonal frame with $x_s$ and $y_s$. The spatial relationships among the g-frame, a-frame, and s-frame are shown in Figure 3.

The IMU error model is defined in the s-frame. The gyros and accelerometers with errors are described as follows:

$$\begin{array}{l}{\mathit{\omega }}_{is}^s=\left(I_{3\times 3}-C_g^s-K_g\right){\mathit{\omega }}_{ig}^g-\mathit{\epsilon }\\ f_{is}^s=\left(I_{3\times 3}-C_a^s-K_a\right)f_{ia}^a-\nabla \end{array}$$

(12)

where $\mathit{\epsilon }={\left[{\mathsf{\epsilon }}_x,{\mathsf{\epsilon }}_y,{\mathsf{\epsilon }}_z\right]}^{\mathrm{T}}$ and $\nabla ={\left[{\nabla }_x,{\nabla }_y,{\nabla }_z\right]}^{\mathrm{T}}$ are the constant gyro drifts and accelerometer biases. ${\mathit{\omega }}_{ig}^g={\left[{\mathsf{\omega }}_{igx}^g,{\mathsf{\omega }}_{igy}^g,{\mathsf{\omega }}_{igz}^g\right]}^{\mathrm{T}}$ and $f_{ia}^a={\left[f_{iax}^a,f_{iay}^a,f_{iaz}^a\right]}^{\mathrm{T}}$ are the input of the gyro and accelerometer. $K_g$ and $K_a$ are the scale factor errors of the gyros and accelerometers. $k_{gi}\left(i=x,y,z\right)$ is the gyro-scale factor errors corresponding to the Xg-axis, Yg-axis, and Zg-axis. $k_{ai}\left(i=x,y,z\right)$ is the accelerometer-scale factor errors corresponding to the Xg-axis, Yg-axis, and Zg-axis. They are expressed as follows:

$$K_g=\left[\begin{array}{ccc}{k}_{gx}& 0& 0\\ 0& k_{gy}& 0\\ 0& 0& k_{gz}\end{array}\right],K_a=\left[\begin{array}{ccc}{k}_{ax}& 0& 0\\ 0& k_{ay}& 0\\ 0& 0& k_{az}\end{array}\right]$$

(13)

$C_g^s$, $C_a^s$ are the installation errors of the gyros and accelerometers. Through Figure 3, $C_g^s$ and $C_a^s$ can be expressed as follows:

$$C_g^s=\left[\begin{array}{ccc}0& S_{gxz}& S_{gxy}\\ S_{gyz}& 0& S_{gyx}\\ S_{gzy}& S_{gzx}& 0\end{array}\right]=\left(S_g\oplus \right) C_a^s=\left[\begin{array}{ccc}0& 0& 0\\ S_{ayz}& 0& 0\\ S_{azy}& S_{azx}& 0\end{array}\right]=\left(S_a\odot \right)$$

(14)

where $S_g={\left[S_{gxz},S_{gxy},S_{gyz},S_{gyx},S_{gzy},S_{gzx}\right]}^{\mathrm{T}}$ is the installation angle error between the g-frame and s-frame. $S_a={\left[S_{ayz},S_{azy},S_{azx}\right]}^{\mathrm{T}}$ is the installation angle error between the a-frame and s-frame. $\left(•\oplus \right)$ is the operation that constructs $S_g$ into $C_g^s$. $\left(•\odot \right)$ is the operation that constructs $S_a$ into $C_a^s$.

Through the above analysis, it can be confirmed that the gyro and accelerometer errors in the r-frame are expressed as:

$$\begin{array}{l}\mathit{\delta }{\mathit{\omega }}_{ir}^r=\left[\left(S_g\oplus \right)+K_g+\left(\mathit{\mu }\times \right)\right]{\mathit{\omega }}_{ir}^r+\mathit{\epsilon }+w_g\\ \mathit{\delta }{f}_{ir}^r=\left[\left(S_a\odot \right)+K_a+\left(\mathit{\mu }\times \right)\right]f_{ir}^r+\nabla +w_a\end{array}$$

(15)

where $\mathit{\epsilon },\nabla ,K_g,K_a,S_g,S_a,\mathit{\mu }$ are random constant variables, that is, their derivatives are all zero. $w_g$ is the measurement noise of the gyros. $w_a$ is the measurement noise of the accelerometer.

3.3. Navigation Error State Model in the Inertial Frame

According to the SINS mechanization in the in0-frame in [24], the transformed dual-axis RINS mechanization in the in0-frame is provided by:

$$\left[\begin{array}{c}{\dot{C}}_r^{in0}\\ {\dot{\overline{v}}}_{en}^{in0}\\ {\dot{r}}_{en}^{in0}\end{array}\right]=\left[\begin{array}{c}{C}_r^{in0}\left({\mathit{\omega }}_{ir}^r\times \right)\\ C_r^{in0}{f}_{ir}^r+C_n^{in0}{G}^n\\ {\overline{v}}_{en}^{in0}\end{array}\right]$$

(16)

where $C_r^{in0}$ is the direction of the cosine matrices from the r-frame to the in0-frame. ${\overline{v}}_{en}^{in0}$ is the auxiliary velocity vector. $r_{en}^{in0}$ is the position in the in0-frame. ${\mathit{\omega }}_{ir}^r$ is the angular velocity expressed in the r-frame measured with the gyros. $f_{ir}^r$ is the specific force in the r-frame measured with the accelerometers. $G^n$ is the global gravitational vector, and the auxiliary velocity vector is provided by:

$${\overline{v}}_{en}^{in0}=v_{en}^{in0}+\left({\mathit{\omega }}_{in0e}^{in0}\times \right)r_{en}^{in0}$$

(17)

where $v_{en}^{in0}$ is the velocity in the in0-frame. ${\mathit{\omega }}_{in0e}^{in0}$ is the earth’s rotation in the in0-frame.

The relationship between $r_{en}^{in0}$ and geographical $p_{en}^n={\left[\mathsf{\lambda },L,h\right]}^{\mathrm{T}}$ is provided by:

$$r_{en}^{in0}=C_e^{in0}{r}_{en}^e=C_e^{in0}\Gamma \left(p_{en}^n\right)$$

(18)

where $\mathsf{\lambda }$, $L$, and $h$ are the longitude, latitude, and height, respectively, obtained from the GPS.

$$r_{en}^e=\Gamma \left(p_{en}^n\right)=\left[\begin{array}{c}\left(R_N+h\right)\cos L\cos \lambda \\ \left(R_N+h\right)\cos L\sin \lambda \\ \left[R_N\left(1-e^2\right)+h\right]\sin L\end{array}\right]$$

(19)

The attitude transformation matrix $C_e^{in0}$ is provided by:

$$C_e^{in0}=C_{e0}^{in0}{C}_e^{e0}$$

(20)

where $C_{e0}^{in0}$ is determined by the initial location and $p_{en}^n\left(0\right)$, $C_e^{e0}$ is determined by the earth rotation and calibration time.

$$C_{e0}^e=\left[\begin{array}{ccc}\cos \left({\omega }_{ie}t\right)& \sin \left({\omega }_{ie}t\right)& 0\\ -\sin \left({\omega }_{ie}t\right)& \cos \left({\omega }_{ie}t\right)& 0\\ 0& 0& 1\end{array}\right]C_{e0}^{in0}=\left[\begin{array}{ccc}-\sin {\lambda }_0& \cos {\lambda }_0& 0\\ -\sin L_0\cos {\lambda }_0& -\sin L_0\sin {\lambda }_0& \cos L_0\\ \cos L_0\cos {\lambda }_0& \cos L_0\sin {\lambda }_0& \sin L_0\end{array}\right]$$

(21)

The relationship between ${\overline{v}}_{en}^{in0}$ and ground velocity $v_{en}^n$

$${\overline{v}}_{en}^{in0}=C_e^{in0}\left({\mathit{\omega }}_{in0e}^e\times \right)r_{en}^e+C_n^{in0}{v}_{en}^n$$

(22)

It can be used to initialize ${\overline{v}}_{en}^{in0}$ for the velocity calculation in (22) and can also be used as the measurement for the dual-axis RINS calibration. $v_{en}^n$ can be obtained using a speed sensor, such as an odometer.

Formulating $C_r^{in0}$, ${\overline{v}}_{en}^{in0}$, and $r_{en}^{in0}$ as elements of the matrix Lie group $S{E}_2\left(3\right)$:

$$\mathit{\chi }=\left[\frac{C_r^{in0}}{{\mathbf{0}}_{2\times 3}}|\frac{{\overline{v}}_{en}^{in0}{r}_{en}^{in0}}{I_{2\times 2}}\right]$$

(23)

According to Equation (23), the differential equation of the $\mathit{\chi }$ can be calculated as:

$$\begin{array}{l}\dot{\mathit{\chi }}={\Gamma }_u\left(\mathit{\chi }\right)=\mathit{\chi }{W}_1+W_2\mathit{\chi }\\ =\left[\begin{array}{ccc}0& G^{in0}& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right]\mathit{\chi }+\mathit{\chi }\left[\begin{array}{ccc}\left({\mathit{\omega }}_{ir}^r\times \right)& f_{ir}^r& 0\\ 0& 0& -1\\ 0& 0& 0\end{array}\right]\end{array}$$

(24)

It is obvious to verify that any solution ${\mathit{\chi }}_1,{\mathit{\chi }}_2$ of Equation (23) satisfies Equation (24). Hence, the dynamical equation ${\Gamma }_u\left(\mathit{\chi }\right)$ is group affine. The group-affine system owns the log-linear property of the corresponding error propagation [23]. Such striking property is just the fundamental of the linear KF based an initial alignment with an arbitrary large misalignment [24].

$${\Gamma }_u\left({\mathit{\chi }}_1\right){\mathit{\chi }}_2+{\mathit{\chi }}_1{\Gamma }_u\left({\mathit{\chi }}_2\right)-{\mathit{\chi }}_1{\Gamma }_u\left(I_5\right){\mathit{\chi }}_2={\Gamma }_u\left({\mathit{\chi }}_1{\mathit{\chi }}_2\right)$$

(25)

If $\mathit{\chi }$ represents the ground truth value and $\tilde{\mathit{\chi }}$ represents the estimated value of $\mathit{\chi }$, the invariant error $\mathit{\delta }\mathit{\chi }$ is defined in

Table 2. Definitions of invariant error.

Invariant Error Model Based on $S{E}_2\left(3\right)$	Name	Corresponds to Error Model Based on $SO\left(3\right)$
$\begin{array}{l}\mathit{\delta }{\mathit{\chi }}_{Rse}=\mathit{\chi }{\tilde{\mathit{\chi }}}^{-1}=\\ \left[\frac{C_r^{in0}{\tilde{C}}_r^{in0\mathrm{T}}}{{\mathbf{0}}_{2\times 3}}|\frac{{\overline{v}}_{en}^{in0}-C_r^{in0}{\tilde{C}}_r^{in0\mathrm{T}}{\tilde{\overline{v}}}_{en}^{in0}{r}_{en}^{in0}-C_r^{in0}{\tilde{C}}_r^{in0\mathrm{T}}{\tilde{r}}_{en}^{in0}}{I_{2\times 2}}\right]\end{array}$ (26)	Right Invariant	$\begin{array}{l}{I}_{3\times 3}+\left({\mathit{\phi }}^{in0}\times \right)=C_r^{in0}{\tilde{C}}_r^{in0\mathrm{T}}\\ \mathit{\delta }{\overline{v}}_{en}^{in0}={\tilde{\overline{v}}}_{en}^{in0}-{\overline{v}}_{en}^{in0}\\ \mathit{\delta }{r}_{en}^{in0}={\tilde{r}}_{en}^{in0}-r_{en}^{in0}\end{array}$ (27)
$\begin{array}{l}\mathit{\delta }{\mathit{\chi }}_{Lse}={\tilde{\mathit{\chi }}}^{-1}\mathit{\chi }=\\ \left[\frac{{\tilde{C}}_r^{in0\mathrm{T}}{C}_r^{in0}}{{\mathbf{0}}_{2\times 3}}|\frac{{\tilde{C}}_r^{in0\mathrm{T}}\left({\overline{v}}_{en}^{in0}-{\tilde{\overline{v}}}_{en}^{in0}\right){\tilde{C}}_r^{in0\mathrm{T}}\left(r_{en}^{in0}-{\tilde{r}}_{en}^{in0}\right)}{I_{2\times 2}}\right]\end{array}$ (28)	Left Invariant	$\begin{array}{l}{I}_{3\times 3}+\left({\mathit{\phi }}^r\times \right)={\tilde{C}}_r^{in0\mathrm{T}}{C}_r^{in0}\\ \mathit{\delta }{\overline{v}}_{en}^{in0}={\tilde{\overline{v}}}_{en}^{in0}-{\overline{v}}_{en}^{in0}\\ \mathit{\delta }{r}_{en}^{in0}={\tilde{r}}_{en}^{in0}-r_{en}^{in0}\end{array}$ (29)

. In error modeling based on $SO\left(3\right)$, only the attitude is established in the Lie group space, and the velocity error and position error are established in the Euclidean space, which is $\mathit{\delta }\mathit{\chi }\in SO\left(3\right)+R^6$. Different from the error modeling based on $SO\left(3\right)$, the invariant error considers the direction errors of velocity and position and mathematically solves the problem of inconsistent coordinate systems defined by velocity error and position error, which is $\mathit{\delta }\mathit{\chi }\in S{E}_2\left(3\right)$.

3.3.1. Error State Model Based on $SO\left(3\right)$

Derive both sides of Equation (27) simultaneously and substitute it into Equation (16). It can be deduced that the error model in the inertial frame with ${\mathit{\phi }}^{in0}$, $\mathit{\delta }{\overline{v}}_{en}^{in0}$, and $\mathit{\delta }{r}_{en}^{in0}$ as navigation parameter errors is:

$$\begin{array}{l}{\dot{\mathit{\phi }}}^{in0}=-{\tilde{C}}_r^{in0}\mathit{\delta }{\mathit{\omega }}_{ir}^r\\ \mathit{\delta }{\dot{\overline{v}}}_{en}^{in0}=\left({\tilde{C}}_r^{in0}{\tilde{f}}_{ir}^r\right)\times {\mathit{\phi }}^{in0}+{\tilde{C}}_r^{in0}\mathit{\delta }{f}_{ir}^r\\ \mathit{\delta }{\dot{r}}_{en}^{in0}=\mathit{\delta }{\overline{v}}_{en}^{in0}\end{array}$$

(30)

Derive both sides of Equation (29) simultaneously and substitute it into Equation (16). It can be deduced that the error model in the r-frame with ${\mathit{\phi }}^r$, $\mathit{\delta }{\overline{v}}_{en}^{in0}$, and $\mathit{\delta }{r}_{en}^{in0}$ as navigation parameter errors is:

$$\begin{array}{l}{\dot{\mathit{\phi }}}^r=-\left({\tilde{\mathit{\omega }}}_{ir}^r\times \right){\mathit{\phi }}^r-\mathit{\delta }{\mathit{\omega }}_{ir}^r\\ \mathit{\delta }{\dot{\overline{v}}}_{en}^{in0}={\tilde{C}}_r^{in0}\left({\tilde{f}}_{ir}^r\times \right){\mathit{\phi }}^r+{\tilde{C}}_r^{in0}\mathit{\delta }{f}_{ir}^r\\ \mathit{\delta }{\dot{r}}_{en}^{in0}=\mathit{\delta }{\overline{v}}_{en}^{in0}\end{array}$$

(31)

It can be seen from Equations (30) and (31) that the error state model contains ${\tilde{C}}_r^{in0}$, which is dependent on the trajectory and is affected by the application environment of the calibration model.

For the above two error models, their measurement models are consistent and can be expressed as:

$${\mathit{z}}_{so}=\left[\begin{array}{c}{z}_{sov}\\ z_{sor}\end{array}\right]=\left[\begin{array}{c}\mathit{\delta }{\overline{v}}_{en}^{in0}\\ \mathit{\delta }{r}_{en}^{in0}\end{array}\right]=\left[\begin{array}{c}{\tilde{\overline{v}}}_{en}^{in0}-{\overline{v}}_{en}^{in0}\\ {\tilde{r}}_{en}^{in0}-r_{en}^{in0}\end{array}\right]+V_{so}$$

(32)

where ${\tilde{\overline{v}}}_{en}^{in0}$ and ${\tilde{r}}_{en}^{in0}$ are the velocity and position solved by the dual-axis RINS. ${\tilde{\overline{v}}}_{en}^{in0}$ and ${\tilde{r}}_{en}^{in0}$ are the velocity and position solved using Equations (18) and (22).

For the error modeling in Equations (31) and (32), the feedback correction for the navigation parameters is provided by Equation (33).

$$\{\begin{array}{c}{\widehat{C}}_r^{in0}=\exp \left({\widehat{\mathit{\phi }}}^{in0}\right){\tilde{C}}_r^{in0},{\widehat{C}}_r^{in0}={\tilde{C}}_r^{in0}\exp \left({\widehat{\mathit{\phi }}}^r\right)\\ {\widehat{\overline{v}}}_{en}^{in0}={\tilde{\overline{v}}}_{en}^{in0}-\mathit{\delta }{\overline{v}}_{en}^{in0}\\ {\widehat{r}}_{en}^{in0}={\tilde{r}}_{en}^{in0}-\mathit{\delta }{r}_{en}^{in0}\end{array}$$

(33)

3.3.2. Right-Invariant Error State Model Based on $S{E}_2\left(3\right)$

Denote the Lie algebra $d{\overline{x}}_R={\left[{\mathit{\phi }}^{in0\mathrm{T}},d{\overline{v}}^{in0\mathrm{T}},d{r}^{in0\mathrm{T}}\right]}^{\mathrm{T}}$ corresponding to the invariant error $\mathit{\delta }{\mathit{\chi }}_{Rse}$. According to Equation (5), we have:

$$C_r^{in0}{\tilde{C}}_{in0}^r=\exp \left({\mathit{\phi }}^{in0}\right)\approx I_{3\times 3}+\left({\mathit{\phi }}^{in0}\times \right)$$

(34)

$$d{\overline{v}}^{in0}=J_l^{-1}\left({\mathit{\phi }}^{in0}\right)\left({\overline{v}}_{en}^{in0}-\exp \left({\mathit{\phi }}^{in0}\times \right){\tilde{\overline{v}}}_{en}^{in0}\right)=-\mathit{\delta }{\overline{v}}_{en}^{in0}+{\tilde{\overline{v}}}_{en}^{in0}\times {\mathit{\phi }}^{in0}$$

(35)

$$d{r}^{in0}=J_l^{-1}\left({\mathit{\phi }}^{in0}\right)\left(r_{en}^{in0}-\exp \left({\mathit{\phi }}^{in0}\times \right){\tilde{r}}_{en}^{in0}\right)=-\mathit{\delta }{r}_{en}^{in0}+{\tilde{r}}_{en}^{in0}\times {\mathit{\phi }}^{in0}$$

(36)

The differential equation of ${\mathit{\phi }}^{in0}$, $d{\overline{v}}^{in0}$, and $d{\overline{r}}^{in0}$ are provided by:

$$\left({\dot{\mathit{\phi }}}^{in0}\times \right)={\dot{C}}_r^{in0}{\tilde{C}}_{in0}^r+C_r^{in0}{\dot{\tilde{C}}}_{in0}^r=\left(-C_r^{in0}\mathit{\delta }{\mathit{\omega }}_{ir}^r\right)\times$$

(37)

$$d{\dot{\overline{v}}}^{in0}=-\mathit{\delta }{\dot{\overline{v}}}_{en}^{in0}+{\dot{\tilde{\overline{v}}}}_{en}^{in0}\times {\mathit{\phi }}^{in0}+{\tilde{\overline{v}}}_{en}^{in0}\times {\dot{\mathit{\phi }}}^{in0}=G^{in0}\times {\mathit{\phi }}^{in0}-C_r^{in0}\mathit{\delta }{f}_{ir}^r-\left({\tilde{\overline{v}}}_{en}^{in0}\times \right)C_r^{in0}\mathit{\delta }{\mathit{\omega }}_{ir}^r$$

(38)

$$d{\dot{r}}^{in0}=-\mathit{\delta }{\dot{r}}_{en}^{in0}+{\dot{\tilde{r}}}_{en}^{in0}\times {\mathit{\phi }}^{in0}+{\tilde{r}}_{en}^{in0}\times {\dot{\mathit{\phi }}}^{in0}=d{\overline{v}}^{in0}-\left({\tilde{r}}_{en}^{in0}\times \right)C_r^{in0}\mathit{\delta }{\mathit{\omega }}_{ir}^r$$

(39)

Add the calibration errors to the state vector. The corresponding state-space model is provided by:

$$\left[\begin{array}{c}{\dot{\mathit{\phi }}}^{in0}\\ d{\dot{\overline{v}}}^{in0}\\ d{\dot{r}}^{in0}\\ {\dot{x}}_c\end{array}\right]=\left[\begin{array}{c}\underset{\_}{\begin{array}{cc}\begin{array}{ccc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ \left(G^{in0}\times \right)& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& I_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}|& F_{Rc}\end{array}}\\ {\mathbf{0}}_{24\times 33}\end{array}\right]\left[\begin{array}{c}{\mathit{\phi }}^{in0}\\ d{\overline{v}}^{in0}\\ d{r}^{in0}\\ x_c\end{array}\right]+\left[\begin{array}{c}\underset{\_}{\begin{array}{cc}-C_r^{in0}& {\mathbf{0}}_{3\times 3}\\ -\left({\tilde{\overline{v}}}_{en}^{in0}\times \right)C_r^{in0}& -C_r^{in0}\\ -\left({\tilde{r}}_{en}^{in0}\times \right)C_r^{in0}& {\mathbf{0}}_{3\times 3}\end{array}}\\ {\mathbf{0}}_{24\times 6}\end{array}\right]\left[\begin{array}{c}{w}_g\\ w_a\end{array}\right]$$

(40)

where $x_c={\left[{\mathit{\epsilon }}^{\mathrm{T}},{\nabla }^{\mathrm{T}},{K_g}^{\mathrm{T}},{K_a}^{\mathrm{T}},{S_g}^{\mathrm{T}},{S_a}^{\mathrm{T}},{\mathit{\mu }}^{\mathrm{T}}\right]}^{\mathrm{T}}$ is the state vector of the parameters to be calibrated. $F_{Rc}$ is provided by:

$$F_{Rc}=\left[\begin{array}{ccccccc}-C_r^{in0}& {\mathbf{0}}_{3\times 3}& -C_r^{in0}D\left({\mathit{\omega }}_{ir}^r\right)& {\mathbf{0}}_{3\times 3}& -C_r^{in0}{\Gamma }_g\left({\mathit{\omega }}_{ir}^r\right)& {\mathbf{0}}_{3\times 3}& C_r^{in0}\left({\mathit{\omega }}_{ir}^r\times \right)\\ -\left({\tilde{\overline{v}}}_{en}^{in0}\times \right)C_r^{in0}& -C_r^{in0}& -\left({\tilde{\overline{v}}}_{en}^{in0}\times \right)C_r^{in0}D\left({\mathit{\omega }}_{ir}^r\right)& -C_r^{in0}D\left(f_{ir}^r\right)& -\left({\tilde{\overline{v}}}_{en}^{in0}\times \right)C_r^{in0}{\Gamma }_g\left({\mathit{\omega }}_{ir}^r\right)& -C_r^{in0}{\Gamma }_a\left({\mathit{\omega }}_{ir}^r\right)& \left({\tilde{\overline{v}}}_{en}^{in0}\times \right)C_r^{in0}\left({\mathit{\omega }}_{ir}^r\times \right)+C_r^{in0}\left(f_{ir}^r\times \right)\\ -\left({\tilde{r}}_{en}^{in0}\times \right)C_r^{in0}& {\mathbf{0}}_{3\times 3}& -\left({\tilde{r}}_{en}^{in0}\times \right)C_r^{in0}D\left({\mathit{\omega }}_{ir}^r\right)& {\mathbf{0}}_{3\times 3}& -\left({\tilde{r}}_{en}^{in0}\times \right)C_r^{in0}{\Gamma }_g\left({\mathit{\omega }}_{ir}^r\right)& {\mathbf{0}}_{3\times 3}& \left({\tilde{r}}_{en}^{in0}\times \right)C_r^{in0}\left({\mathit{\omega }}_{ir}^r\times \right)\end{array}\right]$$

(41)

where $D(•)$, ${\Gamma }_g(•)$ and ${\Gamma }_a(•)$ are provided by:

$$D\left(a\right)=\left[\begin{array}{ccc}{a}_x& 0& 0\\ 0& a_y& 0\\ 0& 0& a_z\end{array}\right],{\Gamma }_g\left(a\right)=\left[\begin{array}{cccccc}{a}_y& a_z& 0& 0& 0& 0\\ 0& 0& a_x& a_z& 0& 0\\ 0& 0& 0& 0& a_x& a_y\end{array}\right],{\Gamma }_a\left(a\right)=\left[\begin{array}{ccc}0& 0& 0\\ a_x& 0& 0\\ 0& a_x& a_y\end{array}\right],a=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$$

(42)

The right-invariant error measurement equation is provided by:

$$z_{Rse}=\left[\begin{array}{c}{z}_{Rsev}\\ z_{Rser}\end{array}\right]=\left[\begin{array}{c}\mathit{\delta }{\overline{v}}_{en}^{in0}\\ \mathit{\delta }{r}_{en}^{in0}\end{array}\right]=\left[\begin{array}{c}-d{\overline{v}}^{in0}+{\tilde{\overline{v}}}_{en}^{in0}\times {\mathit{\phi }}^{in0}\\ -d{r}^{in0}+{\tilde{r}}_{en}^{in0}\times {\mathit{\phi }}^{in0}\end{array}\right]+V_{Rse}$$

(43)

It can be seen from Equation (40) that the navigation parameter system equations in the right-invariant model are only related to the known quantity $G^{in0}$ and is trajectory-independent. Although the system is group affine and can use the linear-KF to solve non-linear problems, the calibration parameters are strongly coupled with the navigation parameters, which affects the calibration accuracy.

For the right-invariant error modeling in Equation (40), the feedback correction for the navigation parameters is provided by:

$$\begin{array}{l}{\widehat{C}}_r^{in0}=\exp \left({\widehat{\mathit{\phi }}}^{in0}\right){\tilde{C}}_r^{in0}\\ {\widehat{\overline{v}}}_{en}^{in0}=\exp \left({\widehat{\mathit{\phi }}}^{in0}\right){\tilde{\overline{v}}}_{en}^{in0}+J_l\left({\widehat{\mathit{\phi }}}^{in0}\right)d{\widehat{v}}^{in0}\\ {\widehat{r}}_{en}^{in0}=\exp \left({\widehat{\mathit{\phi }}}^{in0}\right){\tilde{r}}_{en}^{in0}+J_l\left({\widehat{\mathit{\phi }}}^{in0}\right)d{\widehat{r}}^{in0}\end{array}$$

(44)

3.3.3. Left-Invariant Error State Model Based on $S{E}_2\left(3\right)$

Denote the Lie algebra $d{\overline{x}}_L={\left[{\phi }^{r\mathrm{T}},d{\overline{v}}^{r\mathrm{T}},d{r}^{r\mathrm{T}}\right]}^{\mathrm{T}}$ corresponding to the invariant error $\mathit{\delta }{\mathit{\chi }}_{Lse}$. According to Equation (5), we have:

$${\tilde{C}}_{in0}^r{C}_r^{in0}=\exp \left({\mathit{\phi }}^r\right)=I_{3\times 3}+\left({\mathit{\phi }}^r\times \right)$$

(45)

$$d{\overline{v}}^r=J_l^{-1}\left({\mathit{\phi }}^r\right)\left(-{\tilde{C}}_r^{in0\mathrm{T}}\mathit{\delta }{\overline{v}}_{en}^{in0}\right)=-{\tilde{C}}_r^{in0\mathrm{T}}\mathit{\delta }{\overline{v}}_{en}^{in0}$$

(46)

$$d{\overline{r}}^r=J_l^{-1}\left({\mathit{\phi }}^{in0}\right)\left({\tilde{C}}_r^{in0\mathrm{T}}\mathit{\delta }{r}_{en}^{in0}\right)=-{\tilde{C}}_r^{in0\mathrm{T}}\mathit{\delta }{r}_{en}^{in0}$$

(47)

The differential equations of ${\mathit{\phi }}^r$, $d{\overline{v}}^r$, and $d{\overline{r}}^r$ are provided by:

$$\left({\dot{\mathit{\phi }}}^r\times \right)={\dot{\tilde{C}}}_{in0}^r{C}_r^{in0}+{\tilde{C}}_{in0}^r{\dot{C}}_r^{in0}=-\left[\left({\mathit{\omega }}_{ir}^r\times \right){\mathit{\phi }}^r\right]\times -\left(\mathit{\delta }{\mathit{\omega }}_{ir}^r\times \right)$$

(48)

$$d{\dot{\overline{v}}}^r=-{\dot{\tilde{C}}}_r^{in0\mathrm{T}}\mathit{\delta }{\overline{v}}_{en}^{in0}-{\tilde{C}}_r^{in0\mathrm{T}}\mathit{\delta }{\dot{\overline{v}}}_{en}^{in0}=-\left({\tilde{\mathit{\omega }}}_{in0r}^r\times \right)d{\overline{v}}^r-\left(f_{ir}^r\times \right){\mathit{\phi }}^r-\mathit{\delta }{f}_{ir}^r$$

(49)

$$d{\dot{\overline{r}}}^r=-{\dot{\tilde{C}}}_r^{in0\mathrm{T}}\mathit{\delta }{r}_{en}^{in0}-{\tilde{C}}_r^{in0\mathrm{T}}\mathit{\delta }{\dot{r}}_{en}^{in0}=-\left({\tilde{\mathit{\omega }}}_{in0r}^r\times \right)d{\overline{r}}^r+d{\overline{v}}^r$$

(50)

Add the calibration errors to the state vector. The corresponding state-space model is provided by:

$$\left[\begin{array}{c}{\dot{\mathit{\phi }}}^r\\ d{\dot{\overline{v}}}^r\\ d{\dot{r}}^r\\ {\dot{x}}_c\end{array}\right]=\left[\begin{array}{c}\underset{\_}{\begin{array}{cc}\begin{array}{ccc}-\left({\tilde{\mathit{\omega }}}_{ir}^r\times \right)& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ -\left({\tilde{f}}_{ir}^r\times \right)& -\left({\tilde{\mathit{\omega }}}_{ir}^r\times \right)& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& I_{3\times 3}& -\left({\tilde{\mathit{\omega }}}_{ir}^r\times \right)\end{array}|& \begin{array}{c}{F}_{Lc}\\ {\mathbf{0}}_{3\times 24}\end{array}\end{array}}\\ {\mathbf{0}}_{24\times 33}\end{array}\right]\left[\begin{array}{c}{\mathit{\phi }}^r\\ d{\overline{v}}^r\\ d{r}^r\\ x_c\end{array}\right]+\left[\begin{array}{c}\underset{\_}{\begin{array}{cc}-I_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& -I_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}}\\ {\mathbf{0}}_{24\times 6}\end{array}\right]\left[\begin{array}{c}{w}_g\\ w_a\end{array}\right]$$

(51)

where $F_{Lc}$ is provided by:

$$F_{Lc}=\left[\begin{array}{ccccccc}-I_{3\times 3}& {\mathbf{0}}_{3\times 3}& -D\left({\mathit{\omega }}_{ir}^r\right)& {\mathbf{0}}_{3\times 3}& -{\Gamma }_g\left({\mathit{\omega }}_{ir}^r\right)& {\mathbf{0}}_{3\times 3}& \left({\mathit{\omega }}_{ir}^r\times \right)\\ {\mathbf{0}}_{3\times 3}& -I_{3\times 3}& {\mathbf{0}}_{3\times 3}& -D\left(f_{ir}^r\right)& {\mathbf{0}}_{3\times 6}& -{\Gamma }_a\left({\mathit{\omega }}_{ir}^r\right)& \left(f_{ir}^r\times \right)\end{array}\right]$$

(52)

The left-invariant error measurement equation is provided by:

$$z_{Lse}=\left[\begin{array}{c}{z}_{Lsev}\\ z_{Lser}\end{array}\right]=\left[\begin{array}{c}\mathit{\delta }{\overline{v}}_{en}^{in0}\\ \mathit{\delta }{r}_{en}^{in0}\end{array}\right]=\left[\begin{array}{c}-{\tilde{C}}_r^{in0}d{v}^r\\ -{\tilde{C}}_r^{in0}d{r}^r\end{array}\right]+V_{Lse}$$

(53)

Compared with the right-invariant model, ${\tilde{C}}_r^{in0}$ is only in the measurement equation of the left-invariant model, and the system equation is only related to ${\mathit{\omega }}_{ir}^r$ and $f_{ir}^r$, which is less dependent on the trajectory and its error propagation would be autonomous.

For the left-invariant error modeling in Equation (51), the feedback correction for the navigation parameters is provided by:

$$\begin{array}{l}{\widehat{C}}_r^{in0}={\tilde{C}}_r^{in0}\exp \left({\widehat{\mathit{\phi }}}^r\right)\\ {\widehat{\overline{v}}}_{en}^{in0}={\tilde{\overline{v}}}_{en}^{in0}+{\tilde{C}}_r^{in0}{J}_l\left({\widehat{\mathit{\phi }}}^r\right)d{\widehat{v}}^r\\ {\widehat{r}}_{en}^{in0}={\tilde{r}}_{en}^{in0}+{\tilde{C}}_r^{in0}{J}_l\left({\widehat{\mathit{\phi }}}^r\right)d{\widehat{r}}^r\end{array}$$

(54)

In summary, the left-invariant error model is selected as the filtering algorithm in this paper. The state variables of the proposed method are chosen as follows:

$$X={\left[{\mathit{\phi }}^{r\mathrm{T}},d{\overline{v}}^{r\mathrm{T}},{\mathit{\epsilon }}^{\mathrm{T}},{\nabla }^{\mathrm{T}},{K_g}^{\mathrm{T}},{K_a}^{\mathrm{T}},{S_g}^{\mathrm{T}},{S_a}^{\mathrm{T}},{\mathit{\mu }}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(55)

where ${\mathit{\phi }}^r=\left[{\phi }_x^r,{\phi }_y^r,{\phi }_z^r\right]$ is left-invariant attitude error in the Euler angles. $d{\overline{v}}^r=\left[d{v}_x^r,d{v}_y^r,d{v}_z^r\right]$ is left-invariant velocity error. $\mathit{\epsilon }=\left[{\epsilon }_x,{\epsilon }_y,{\epsilon }_z\right]$ is the gyro drift error. $\nabla =\left[{\nabla }_x,{\nabla }_y,{\nabla }_z\right]$ is the accelerometer bias error. $K_g=\left[K_{gx},K_{gy},K_{gz}\right]$ is the gyro-scale factor error. $K_a=\left[K_{ax},K_{ay},K_{az}\right]$ is the accelerometer-scale factor error. $S_a=\left[S_{ayx},S_{azx},S_{azy}\right]$ is the accelerometer installation angle error. $S_g=\left[S_{gxy},S_{gxz},S_{gyx},S_{gyz},S_{gzx},S_{gzy}\right]$ is the gyro installation angle error. $\mathit{\mu }=\left[{\mu }_x,{\mu }_y,{\mu }_z\right]$ is installation angle error between the s-frame and r-frame.

The state equation and the measurement equation can be designed as follows:

$$\begin{array}{l}\dot{X}=FX+GW\\ Z_{Lse}=\mathit{\delta }{\overline{v}}_{en}^{in0}=H_{Lse}X+V_{Lse}\end{array}$$

(56)

where $W$ and $V_{Lse}$ are the process noise and measurement noise, respectively. $G$ is the process noise matrix. The matrix $F$ and the matrix $H_{Lse}$ are expressed as:

$$F=\left[\begin{array}{c}\underset{\_}{\begin{array}{cc}\begin{array}{cc}-\left({\tilde{\mathit{\omega }}}_{ir}^r\times \right)& {\mathbf{0}}_{3\times 3}\\ -\left({\tilde{f}}_{ir}^r\times \right)& -\left({\tilde{\mathit{\omega }}}_{ir}^r\times \right)\end{array}|& F_{Lc}\end{array}}\\ {\mathbf{0}}_{24\times 30}\end{array}\right],H_{Lse}=\left[\begin{array}{ccc}{\mathbf{0}}_{3\times 3}& -{\tilde{C}}_r^{in0}& {\mathbf{0}}_{3\times 24}\end{array}\right]$$

(57)

The algorithm flow for discretizing the continuous Kalman filter is shown in Algorithm 1.

Algorithm 1 Discrete Kalman filter algorithm flow

(1) State and variance matrix initialization

${\widehat{X}}_0=X\left(t_0\right),{\widehat{P}}_0=P\left(t_0\right)$

(2) State propagation process

$\begin{array}{l}{\widehat{X}}_{k/k-1}={\Phi }_{k/k-1}{\widehat{X}}_{k-1}\\ {\widehat{P}}_{k/k-1}={\Phi }_{k/k-1}{\widehat{P}}_{k-1}{\Phi }_{k/k-1}^{\mathrm{T}}+{\Gamma }_{k-1}Q{\Gamma }_{k-1}^{\mathrm{T}}\\ {\Phi }_{k/k-1}\approx I+F\left(t_{k-1}\right)\Delta T+F\left(t_{k-1}\right)F\left(t_{k-1}\right)\Delta T^2/2\\ {\Gamma }_{k-1}\approx \left(I+F\left(t_{k-1}\right)\Delta T/2+F\left(t_{k-1}\right)F\left(t_{k-1}\right)\Delta T^2/6\right)G\end{array}$

(3) Update filter state

$\begin{array}{l}{K}_k=P_{k/k-1}{H}_k^T{\left(H_k{P}_{k/k-1}{H}_k^{\mathrm{T}}+R_k\right)}^{-1}\\ P_k=\left(I-K_k{H}_k\right)P_{k/k-1}\\ {\widehat{X}}_k={\widehat{X}}_{k/k-1}+K_k\left(z-H_k{\widehat{X}}_{k/k-1}\right)\end{array}$

(4) Update navigation state

where $\Delta T$ is the discrete time. $Q$ and $R$ are device random noise.

4. The Fast Self-Calibration Method and Observability Analysis of the Dual-Axis RINS

4.1. Rotation Scheme Design and Observability Analysis

In order to accurately estimate all the state variables in the self-calibration procedure, the rotation scheme should be properly designed to ensure that all the state variables are observable. Reference [35] proposed design principles for a rotation scheme, which included pointing upward and downward alternately for each accelerometer and a bidirectional rotation on each gyro. According to the above principles and the limitations of the gimbal mechanism, the rotation scheme proposed in this article is shown in Figure 4. In a rotation period, first, the outer axis rotates 270 degrees and −270 degrees (corresponding to A and B in Figure 4). Then, the inner axis rotates to −90 degrees (corresponding to C in Figure 4), and the outer axis rotates bidirectionally (corresponding to D and E in Figure 4). Finally, the inner axis rotates bidirectionally (corresponding to F and G in Figure 4) and returns to position A. During the rotation, the angular velocity is $\mathit{\omega }$, and it stops for $T_s$ seconds after each rotation of 90 degrees. According to Equation (16), except for $\mathit{\epsilon }$ and $\nabla$, the remaining calibration errors require ${\mathit{\omega }}_{ir}^r$ and $f_{ir}^r$ to excite. In order to excite the error of the gyros, the gyros should be continuously sensitive to the angular velocity of the gimbal mechanism. In the same way, to excite the error of the accelerometers, the accelerometer should be in the vertical direction of the horizontal plane for a long time. Therefore, in order to fully excite the IMU error, $\mathit{\omega }$ is set to 18°/s and the stop time $T_s$ is set to 30 s. The complete rotation period is 600 s.

This article uses the PWCS method to verify the feasibility of the designed rotation scheme. In each short time period, the state matrix and measurement matrix are assumed to be constant matrices. At this time, the linear time-varying system transforms into a linear time-invariant system, and the observable matrix of each time period can be obtained. In the i-th time period, the observability matrix of the dynamic system can be expressed as:

$$Q_i={\left[{H_i}^{\mathrm{T}},{\left(H_i{F}_i\right)}^{\mathrm{T}},{\left(H_i{F}_i^2\right)}^{\mathrm{T}},\cdots ,{\left(H_i{F}_i^{n-1}\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(58)

where $H_i$ and $F_i$ are the measurement matrix and the state matrix in the i-th time interval and $n$ is the dimension of the state. The stripped observability matrix (SOM) is:

$$Q_{SOM}\left(r\right)={\left[Q_1^{\mathrm{T}},Q_2^{\mathrm{T}},Q_3^{\mathrm{T}},\cdots ,Q_r^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(59)

When the rank of $Q_{SOM}\left(r\right)$ is n, the system is fully observable, while if the rank of $Q_{SOM}\left(r\right)$ is less than n, the system is not fully observable. Performing a singular value decomposition on $Q_{SOM}\left(r\right)$, Equation (60) can be obtained:

$$Q_{SOM}\left(r\right)=U\Sigma V^{\mathrm{T}}={\displaystyle \sum _{i=1}^n{u}_i{\mathit{\sigma }}_i{v}_i^{\mathrm{T}}}$$

(60)

where $U=\left[u_1,u_2\cdots u_{rmn}\right]$ and $V=\left[v_1,v_2\cdots v_n\right]$ are $rmn\times rmn$-dimensional and $n\times n$-dimensional unitary matrices, respectively. $\Sigma ={\left[{\Sigma }_0,{\mathbf{0}}_{n\times (rmn-n)}\right]}^{\mathrm{T}}$, ${\Sigma }_0=\mathrm{diag}\left({\mathit{\sigma }}_1,{\mathit{\sigma }}_2,\cdots {\mathit{\sigma }}_n\right)$, and ${\mathit{\sigma }}_i\left(i=1,\cdots ,m\right)$ are the singular values of $Q_{SOM}\left(r\right)$.

Define the singular value ${\mathit{\sigma }}_k$ corresponding to any element $X_k$ of the state vector $X$ to be the singular value ${\mathit{\sigma }}_i$ that maximizes ${\left\{\left(u_i^{\mathrm{T}}Y/{\mathit{\sigma }}_i\right)v_i\right\}}_k\left(i=1,2,\cdots ,m\right)$. The observability degree of $X_k$ is defined as:

$${\mathit{\eta }}_k={\mathit{\sigma }}_k/{\mathit{\sigma }}_0$$

(61)

where $Y={\left[Y_1^{\mathrm{T}},Y_2^{\mathrm{T}},\cdots Y_r^{\mathrm{T}}\right]}^{\mathrm{T}}$, $Y_i={\left[Z_i^{\mathrm{T}},{\dot{Z}}_i^{\mathrm{T}},\cdots Z^{\left(n-1\right)\mathrm{T}}\right]}^{\mathrm{T}}$ is the measurement in each time period, and its derivatives of each order. ${\mathit{\sigma }}_0$ is the singular value corresponding to the state component that can be directly obtained from external information.

Corresponding to the left-invariant error model of Equation (57), the rank of the system is 27. $Q_{SOM}\left(r\right)$ is less than the state dimension, indicating that the system is not completely observable. Calculate the observability degrees of each variable at the end of calibration. The observable degrees of ${\mathsf{\epsilon }}_x$, ${\mathsf{\epsilon }}_y$, ${\mathsf{\epsilon }}_z$, ${\nabla }_x$, ${\nabla }_y$, ${\nabla }_z$, $k_{ax}$, $k_{ay}$, $k_{az}$, $k_{gx}$, $k_{gy}$, $k_{gz}$, $S_{gxy}$, $S_{gxz}$, $S_{gyx}$, $S_{gyz}$, $S_{gzx}$, $S_{gzy}$, $S_{ayx}$, $S_{azx}$, $S_{azy}$, $\alpha$, $\beta$, and $\gamma$ are 2.23 × 10⁻⁴, 2.25 × 10⁻⁴, 1.48 × 10⁻⁴, 0.062, 0.063, 0.062, 8.56 × 10⁻⁴, 7.51 × 10⁻⁵, 2.3 × 10⁻⁸, 0.29, 0.33, 0.61, 5.52 × 10⁻⁴, 7.71 × 10⁻⁶, 6.31 × 10⁻⁴, 7.71 × 10⁻⁴, 5.78 × 10⁻⁴, 5.07 × 10⁻⁴, 0.21, 0.26, 0.29, 2.29 × 10⁻¹⁴, 2.29 × 10⁻¹⁴, and 3.64 × 10⁻¹⁶ respectively. The observability degrees of $\alpha$, $\beta$, and $\gamma$ are the worst and is unobservable, which is the reason why the rank of $Q_{SOM}\left(r\right)$ and $n$ differ by 3. The system error of the gyros and the installation error angle between the r-frame and the s-frame are poorly observable degrees. In response to this situation, the angular velocity constraint equation is added to the measurement equation to render all the parameters observable.

4.2. Angular Velocity Constraint Equation

During the calibration process, the gimbal mechanism drives the IMU rotation. The angular rate of the gimbal mechanism can be utilized as a reference to calibrate the systematic errors of the gyros and the installation error between the s-frame and the r-frame. The outputs of the gyros in the s-frame can be expressed as follows:

$${\mathit{\omega }}_{ig}^g=C_s^g{C}_r^s\left(C_{in0}^r{\mathit{\omega }}_{ie}^{in0}+{\mathit{\omega }}_{br}^r+\mathit{\epsilon }\right)+w_g$$

(62)

where ${\mathit{\omega }}_{ie}^{in0}={\left[0,{\mathsf{\omega }}_{ie}\cos L_0,{\mathsf{\omega }}_{ie}\sin L_0\right]}^{\mathrm{T}}$ is the angular velocity of the earth expressed in the in0-frame. $L_0$ is the latitude in the initial location. ${\mathit{\omega }}_{br}^r$ is the angular velocity of the gimbal mechanism.

By substituting Equations (11) and (12) into Equation (62), the second-order small quantity can be ignored and the IMU measurement constraint equation can be expressed as:

$$\begin{array}{l}{\mathit{\omega }}_{ig}^g=\left(I_{3\times 3}+S_g\oplus +K_g+\mathit{\mu }\times \right){\mathit{\omega }}_1+\mathit{\epsilon }+w_g\\ {\mathit{\omega }}_1=C_{in0}^r{\mathit{\omega }}_{ie}^{in0}+{\mathit{\omega }}_{br}^r\end{array}$$

(63)

Equation (63) can be converted into the measurement equation of the calibration parameters:

$$\begin{array}{l}{Z}_{AVC}=H_{AVC}X+V_{AVC}={\mathit{\omega }}_{ig}^g-{\mathit{\omega }}_1\\ H_{AVC}=\left[{\mathbf{0}}_{3\times 6},I_{3\times 3},{\mathbf{0}}_{3\times 3},D\left({\mathit{\omega }}_1\right),{\mathbf{0}}_{3\times 3},{\Gamma }_g\left({\mathit{\omega }}_1\right),{\mathbf{0}}_{3\times 3},\left({\mathit{\omega }}_1\right)\times \right]\end{array}$$

(64)

Combine Equations (32), (57) and (64) to obtain the new extended measurement equation:

$$Z_{LseAVC}=H_{LseAVC}X+V_{LseAVC}={\left[Z_{Lse}^{\mathrm{T}},Z_{AVC}^{\mathrm{T}}\right]}^{\mathrm{T}},H={\left[H_{Lse}^{\mathrm{T}},H_{AVC}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(65)

$$Z_{soAVC}=H_{soAVC}X+V_{soAVC}={\left[z_{zov}^{\mathrm{T}},Z_{AVC}^{\mathrm{T}}\right]}^{\mathrm{T}},H_{soAVC}={\left[H_{sov}^{\mathrm{T}},H_{AVC}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(66)

where $H_{sov}=[{\mathbf{0}}_{3\times 3},I_{3\times 3},{\mathbf{0}}_{3\times 24}]$.

For the left-invariant error model with $F$ and $H$ as the system matrix and the measurement matrix, its rank of $Q_{SOM}\left(r\right)$ is 30. The observability degrees of ${\mathsf{\epsilon }}_x$, ${\mathsf{\epsilon }}_y$, ${\mathsf{\epsilon }}_z$, ${\nabla }_x$, ${\nabla }_y$, ${\nabla }_z$, $k_{ax}$, $k_{ay}$, $k_{az}$, $k_{gx}$, $k_{gy}$, $k_{gz}$, $S_{gxy}$, $S_{gxz}$, $S_{gyx}$, $S_{gyz}$, $S_{gzx}$, $S_{gzy}$, $S_{ayx}$, $S_{azx}$, $S_{azy}$, $\alpha$, $\beta$, and $\gamma$ are 0.98, 0.98, 0.98, 2.71 × 10⁻⁴, 2.72 × 10⁻⁴, 2.71 × 10⁻⁴, 0.2, 0.2, 0.2, 2.41 × 10⁻³, 2.21 × 10⁻³, 2.36× 10⁻³, 0.23, 0.21, 0.21, 0.21, 0.22, 0.21, 1.3 × 10⁻³, 1.1 × 10⁻³, 8.9 × 10⁻⁴, 0.38, 0.35, and 0.23, respectively. The angular velocity constraint equation is added to the measurement equations, which improves the system’s observability, and all calibration parameters can be observed.

From the perspective of the analytical analysis of the variable observability, the accelerometer calibration error has a linear relationship with the first derivative of the velocity error. The gyro error is linearly related to the derivative of the second-order velocity error. It can be seen from the angular velocity constraint equation that there is a linear relationship between the gyros systematic error parameter and $Z_g$. Adding it to the measurement equation can more directly observe the gyro system error and the installation error between the s-frame and r-frame, effectively shortening the calibration time.

4.3. Backtracking Scheme Design in the Inertial Frame

Backtracking navigation reduces the calibration time by utilizing stored data multiple times. The backtracking scheme is shown in Figure 5. During the calibration process, the SINS update algorithm in the inertial frame is applied for forward navigation. During the first forward navigation process, the data of the IMU and gimbal mechanism are stored. Subsequently, using the stored data multiple times, the calibration parameter error can be accurately estimated in a short time.

5. Simulation Test and Experimental Analysis

This section is devoted to numerically evaluating the calibration performance based on different error state models. The calibration method based on the error state model (30) and measurement model (32) is denoted as RSO-KF. The calibration method based on the error state model (31) and measurement model (32) is denoted as LSO-KF. The calibration method based on the error state model (30) and measurement model (65) is denoted as RSOAVC-KF. The calibration method based on the error state model (31) and measurement model (65) is denoted as LSOAVC-KF. The calibration method based on the error state model (40) and measurement model (66) is denoted as RSEAVC-KF. The calibration method based on the error state model (51) and measurement model (66) is denoted as LSEAVC-KF (the proposed method). The above methods all apply the backtracking method to unify the algorithm process and verify the impact of different error modeling methods on the calibration algorithm. During the simulation and experiment, the dual-axis RINS remained stationary. The location was obtained from a GPS, and the speed was zero.

5.1. Simulation Test

To verify the effectiveness of the proposed algorithm, the simulations are demonstrated in this section. The initial location was 31.94° N, 118.79° E, 15 m. The initial attitude error was 0.1°(roll), 0.1°(pitch), 0.5°(heading). The random gyro drifts were 0.005 deg/h, and the random accelerometer bias was 50 μg. The error of the IMU-scale factor was 100 ppm. The error of the IMU installation angle was 20″. The error of installation angle between the s-frame and r-frame was 200″. The error of the angle of the gimbal mechanism was 0.5″. The error of the angular rate of the gimbal mechanism was 0.7″/s. The rotation scheme is consistent with that described in Section 4.1. The number of backtracking navigations was four.

The simulation was divided into two parts. First, the validity of the angular velocity constraints was verified. Then, the advantages of accuracy were verified and compared with the error modeling method based on $SO\left(3\right)$.

5.1.1. Verification of the Observability Analysis

The observability was assessed with a covariance analysis. The convergence process of the root mean square (RMS) of the calibration error covariance is shown in Figure 6. It can be seen from Figure 6 that the RMSs of the $k_{gz}$, $S_{gyz}$, $S_{ayx}$, $\alpha$, and $\gamma$ error covariance are large. The observability is consistent with the analysis in Section 4. After introducing AVC constraints into the measurement equation, all the calibration error parameters could be estimated well. Moreover, the RMS of the error covariance became smaller faster.

5.1.2. Comparison of the Calibration Accuracy

In order to eliminate the influence of unobservable installation errors in traditional calibration algorithms, AVC constraints are added as a comparison algorithm to verify the superiority of the calibration algorithm based on invariant error modeling. The four algorithms are evaluated for comparison.

Figure 7, Figure 8, Figure 9 and Figure 10 describe the estimation results of the calibration errors; the red dotted lines represent real values, and the solid lines represent the estimation result of the proposed method (LSEAVC-KF). It took 600 s in the first stage, and then the time spent on backtracking navigation was calculated using the Matlab platform to be 106.1 s. We can see all errors present good convergence with the proposed method within 12 min. Except for the gyro drift error, the rest of the calibration errors converged to near the true value in the first stage. The estimation error of the gyro drift was less than 4%, and the rest of the calibration errors were less than 2%.

In order to further verify the superiority of the proposed method, the estimation accuracy of the remaining three methods was compared, and 100 Monte Carlo simulations were performed for the calibration. The statistical results of the mean error (ME) and root mean square error (RMSE) of each error parameter are shown in

Table 3. Simulation results from the four methods.

Parameter	RSEAVC-KF		RSOAVC-KF		LSEAVC-KF		LSOAVC-KF
	ME	RMSE	ME	RMSE	ME	RMSE	ME	RMSE
${\mathit{\epsilon }}_x$ (°/h)	0.0003	0.0042	0.0005	0.0058	0.0002	0.0035	0.0004	0.0052
${\mathit{\epsilon }}_y$ (°/h)	0.0006	0.0041	0.0003	0.0042	0.0005	0.0039	0.0002	0.0042
${\mathit{\epsilon }}_z$ (°/h)	−0.0003	0.0034	−0.0004	0.0039	−0.0001	0.0020	−0.0005	0.0039
${\nabla }_x$ (μg)	−0.2131	0.8136	−0.2794	0.8935	−0.1434	0.7252	−0.2327	0.868
${\nabla }_y$ (μg)	0.1595	0.74	0.3689	0.8136	0.0796	0.7375	0.2049	0.744
${\nabla }_z$ (μg)	0.053	0.3953	0.1245	0.4452	0.0436	0.3771	0.0983	0.44
$k_{ax}$ (ppm)	0.1447	0.1668	0.1808	0.1938	0.1429	0.1644	0.1644	0.169
$k_{ay}$ (ppm)	−0.1493	0.1658	−0.152	0.169	−0.1464	0.1646	−0.1517	0.1674
$k_{az}$ (ppm)	−0.1449	0.1584	−0.1561	0.1739	−0.1396	0.1535	−0.1553	0.1585
$k_{gx}$ (ppm)	0.0321	0.7175	−0.2059	0.7305	−0.0222	0.6592	−0.1809	0.7255
$k_{gy}$ (ppm)	0.4286	0.704	0.5241	1.911	0.3087	0.6776	0.5095	0.9855
$k_{gz}$ (ppm)	0.0267	0.3216	0.0654	0.4094	0.0055	0.298	0.061	0.389
$S_{gxy}$ (″)	0.0376	0.3052	0.1986	0.3549	0.015	0.2579	0.1828	0.3166
$S_{gxz}$ (″)	0.0594	0.2630	0.0811	0.2888	0.0523	0.2264	0.0697	0.2843
$S_{gyx}$ (″)	0.2352	0.3658	0.4452	0.5174	0.2215	0.3441	0.3935	0.51
$S_{gyz}$ (″)	0.0532	0.1663	0.0762	0.1759	0.0526	0.1375	0.07	0.1702
$S_{gzx}$ (″)	0.3509	0.4442	0.4905	0.5414	0.3338	0.4142	0.4750	0.4712
$S_{gzy}$ (″)	0.4519	0.4801	0.4741	0.4991	0.3329	0.3547	0.4523	0.4833
$S_{ayx}$ (″)	0.1472	0.3518	0.4257	0.5364	0.1296	0.3454	0.3866	0.4828
$S_{azx}$ (″)	0.3752	0.5953	0.542	0.6552	0.365	0.4648	0.5214	0.6058
$S_{azy}$ (″)	0.3949	0.4742	0.5194	0.5836	0.3363	0.4468	0.5133	0.5067
$\mathsf{\alpha }$ (″)	0.1522	0.2978	0.2748	0.3139	0.1301	0.1754	0.2526	0.3022
$\mathsf{\beta }$ (″)	−0.115	0.3132	−0.2536	0.3381	−0.112	0.2757	−0.2136	0.3158
$\mathsf{\gamma }$ (″)	0.0036	0.295	0.1852	0.3563	0.0025	0.2461	0.1637	0.3353

. Among the four methods, LSEAVC-KF had the best effect. The error of the invariant error modeling method was significantly smaller than the error of the error modeling method based on $SO\left(3\right)$, which proves that the invariant error can express the error characteristics more accurately.

In order to prove the calibration performance of LSEAVC-KF in the case of large misalignment angles, the calibration errors under different heading angle errors are shown in

Table 4. Simulation results of different heading angles.

Parameter	0°	30°	60°	90°	120°	150°	180°
${\mathit{\epsilon }}_x$ (°/h)	0.0001	0.0002	0.0003	0.0004	0.0002	0.0003	0.0004
${\mathit{\epsilon }}_y$ (°/h)	0.0002	0.0002	0.0003	0.0004	0.00005	0.0004	0.0005
${\mathit{\epsilon }}_z$ (°/h)	−0.0002	0.0003	−0.0003	0.0003	−0.0003	0.0002	−0.0003
${\nabla }_x$ (μg)	−0.8282	0.8047	−0.7621	0.678	−0.6723	0.9058	−1.1413
${\nabla }_y$ (μg)	−0.4469	0.4493	−0.4953	0.4189	−0.4932	0.5359	−0.7016
${\nabla }_z$ (μg)	0.2836	0.003	0.4408	0.5425	0.0634	0.3509	0.8046
$k_{ax}$ (ppm)	0.2310	0.2100	0.1528	0.3561	0.1059	0.4795	0.2095
$k_{ay}$ (ppm)	−0.1640	−0.1014	−0.1108	−0.1751	−0.2135	0.1915	−0.1813
$k_{az}$ (ppm)	−0.1199	−0.1785	−0.1568	−0.1210	−0.1812	−0.1964	−0.2170
$k_{gx}$ (ppm)	0.0834	0.5543	−0.7975	1.1018	0.9184	−0.1006	−0.9222
$k_{gy}$ (ppm)	0.5652	0.4680	0.2568	0.4750	0.5810	−0.7252	1.1168
$k_{gz}$ (ppm)	0.0618	0.3218	0.0911	0.6660	0.2012	0.3106	−0.6793
$S_{gxy}$ (″)	−0.1669	0.1989	−0.1252	0.2270	−0.1389	0.1585	0.5027
$S_{gxz}$ (″)	0.2890	0.3614	0.1423	0.5180	0.0331	0.3029	0.5712
$S_{gyx}$ (″)	−0.3021	−0.3992	−0.2673	−0.5448	−0.3001	−0.5239	−0.7781
$S_{gyz}$ (″)	0.1665	0.1887	0.3553	0.7222	0.3628	0.6499	0.9026
$S_{gzx}$ (″)	0.1506	0.2519	0.2645	0.4042	0.1984	0.2353	0.5648
$S_{gzy}$ (″)	−0.2413	−0.1235	−0.3786	−0.4283	−0.4807	−0.4395	−0.5220
$S_{ayx}$ (″)	0.3448	0.8615	0.4011	0.7038	0.3126	0.5732	0.9560
$S_{azx}$ (″)	0.1160	0.3993	0.3379	0.3820	0.5392	0.3255	0.9529
$S_{azy}$ (″)	0.4830	0.3893	0.4517	0.4436	0.3257	0.3720	0.7392
$\alpha$ (″)	0.0563	0.0611	0.2052	0.2174	0.2938	0.2404	0.3120
$\beta$ (″)	−0.1074	0.5557	−0.1808	0.3342	−0.3589	0.5153	−0.7725
$\gamma$ (″)	0.0307	0.0573	0.0952	0.0495	0.3831	0.6351	0.6807

. With an increasing heading angle error, it can be observed that the estimation error experiences a slight growth, but it remains within an acceptable range. The proposed algorithm demonstrates greater applicability and can accurately calibrate errors even in harsh environmental conditions. In comparison to traditional calibration methods, it is less reliant on complex instrumentation and offers a faster convergence rate.

5.2. Experiment

In order to verify the feasibility of the proposed method in the actual environment, this paper uses a dual-axis SINS to conduct the experiments, as shown in Figure 11. The dual-axis SINS consists of three fiber optic gyroscopes and three quartz accelerometers. The specific performance is shown in

Table 5. Specifications of the dual-axis rotational inertial navigation system.

Characteristics	Description
Sampling frequency	200 Hz
Gyro-constant drift	$\le$0.005 deg/h
Gyro-stochastic drift	$\le$0.001 deg/sqrt(h)
Gyro-scale factor repeatability	$\le$50 ppm $\left(1\mathsf{\sigma }\right)$
Gyro-constant drift repeatability	$\le$0.001 deg/h $\left(1\mathsf{\sigma }\right)$
Gyro installation error	$\le$0.001 rad
Accelerometer constant bias	$\le$10 μg
Accelerometer-scale factor repeatability	$\le$50 ppm $\left(1\mathsf{\sigma }\right)$
Accelerometer constant bias repeatability	$\le$10 μg $\left(1\mathsf{\sigma }\right)$
Accelerometer installation error	$\le$0.001 rad
Rotation angle accuracy	0.5″
Rotation velocity accuracy	0.7″/s

.

Since the true error of the IMU cannot be known, the accuracy of the error parameter estimation was verified through navigation performance. The higher the accuracy of the IMU parameter, the better the navigation performance. Hence, a navigation experiment with different calibration results was adopted. LSOAVC-KF was chosen as the traditional algorithm for comparison. The velocity errors and position errors under different calibration results are shown in Figure 12 and Figure 13.

The statistical results of the navigation errors are listed in

Table 6. Comparison of the velocity and position errors.

Parameter		LSEAVC-KF	LSOAVC-KF
Ve error (m/s)	RMSE Maximum	0.0595 0.0870	0.1002 0.1515
Vn error (m/s)	RMSE Maximum	0.0403 0.0661	0.0706 0.1092
Pe error (m)	RMSE Maximum	110.0472 175.9277	130.3365 216.0348
Pn error (m)	RMSE Maximum	40.4820 100.6326	82.8674 185.4080

. In Table 6, Ve and Vn denote the east and north velocities, respectively, in the navigation frame. Pe and Pn denote the east and north positions, respectively, in the navigation frame. It can be seen in Table 6 and in Figure 12 and Figure 13 that the navigation performance of the inertial navigation system was improved with LSEAVC-KF. The maximum error and root mean square error of LSEAVC-KF are smaller than those of LSOAVC-KF.

Therefore, it can be seen from the simulation and experimental results that the proposed algorithm can quickly and accurately estimate the IMU error and installation error between the r-frame and s-frame within 12 min, effectively improving the navigation performance of a dual-axis rotational inertial navigation system.

6. Conclusions

Inertial navigation systems require rapid and accurate error calibration to ensure their performance. A system-level self-calibration method for a dual-axis rotational inertial navigation system based on invariant errors is proposed. Navigation errors are represented as an element of $S{E}_2\left(3\right)$, allowing the system to converge quickly even during large misalignment angles. Compared to error modeling methods based on $SO\left(3\right)$, the proposed method is not affected by the calibration environment and does not require a coarse alignment. The method utilizes the information from the gimbal mechanism to establish angular velocity constraint equations, rendering all errors simultaneously estimated. The rotation scheme for the dual-axis rotational inertial navigation system is designed and discussed, and the observability of the system is analyzed using the PWCS method and an SVD-based observability analysis method. A backtracking navigation algorithm is used to shorten the calibration time. Finally, the superiority of the proposed algorithm is verified through simulations and laboratory experiments, demonstrating that the algorithm can rapidly and accurately estimate IMU systematic errors and installation errors between the r-frame and s-frame. Therefore, the proposed calibration algorithm has significant practical value and can significantly improve navigation accuracy.