High-degree Cubature Kalman Filtering Approach for GPS Aided In-Flight Alignment of SDINS

Abstract

A High-degree Cubature Kalman Filter (CKF) is proposed to deal with the Strapdown Inertial Navigation System (SDINS) alignment problem. In-flight Alignment (IFA) is an effective method to compensate for attitude errors of the navigation system. While providing precise attitude error compensation, however, the external source aided alignment often creates a nonlinear filtering problem caused by a large misalignment angle. Introduced recently, Cubature Kalman Filter is a suitable technique for various nonlinear problems. In this paper, a higher degree CKF is applied to this accuracy-is-everything SDINS IFA problem. The simulation results show that the proposed technique outperformed a traditional nonlinear filter in terms of precision and alignment time.

1. INTRODUCTION

In modern aerospace applications, a Strapdown Inertial Navigation System (SDINS) provides the position, velocity and attitude of the vehicle, which are critical to a mission’s success. The accuracy of the navigation information depends on the calculated attitude between body and navigation frame during the alignment process. In-Flight Alignment (IFA) is an approach that determines and compensates the attitude with the help of external aiding sources, such as Global Navigation Satellite System (GNSS). Unlike the initial self-alignment, it doesn’t require the vehicle to be in a stationary state. Fig. 1 shows the characteristics of SDINS, GNSS and aided navigation. Due to the integration involved in SDINS, it has tendency of divergence over time, while GNSS output is bounded with instant position errors. IFA incorporates the advantages of each system, making the navigation more accurate and stable.

Fig. 1. Comparison of SDINS, GPS and IFA navigation.

If coarse alignment is performed during flight or under disturbances, the uncertainty of the angular body rate and acceleration may prevent a precise determination of the attitude (Yu et al. 1999). The initial attitude misalignment angle can be large in such cases, causing the IFA problem to be nonlinear and making it inappropriate to apply the small angle approximation rule to the SDINS error model (Salychev 2004). The heading misalignment of the vehicle is likely to be large compared to the other two attitude angles, providing grounds for a large heading misalignment model proposed in Yu et al. (2012). When misalignments are large, the navigation error equations become nonlinear due to the included sinusoidal functions and couplings. Linear filters are not suitable for such cases, since a linear filter applied for linearized nonlinear problems would result in an inaccurate estimation.

Cubature Kalman Filter (CKF) was introduced by Arasaratnam & Haykin (2009), and was proposed as a systematic solution for nonlinear systems with high dimensionality. Although several nonlinear filtering approaches existed prior to CKF, such as EKF and UKF, they had disadvantages due to their characteristics. EKF adopts Taylor series approximation in order to linearize nonlinear problems, leading to inaccuracy if the system has severe nonlinearity. Promising estimation accuracy as there is no linearization, UKF captures the sigma points from the mean and covariance through Unscented Transformation and propagates them through the nonlinear state equations itself. However, it suffers from the curse of dimensionality, as the weight is concentrated into sigma point of its mean if the dimensionality of the state is relatively high. On the other hand, CKF uses a spherical-radial rule, guaranteeing numerical accuracy and convenient extendibility for high dimensional problems. Higher order CKF was studied more recently by other researchers, who expected an enhancement in estimation precision. Jia et al. (2013) proved that the 5th order CKF has increased accuracy with moderate computation cost over other nonlinear filters through a nonlinear target tracking problem. Zhang et al. (2013) applied this filter to initial self-alignment of the navigation system in a stationary base, proving the precision of high-degree CKF over third-ordered CKF.

The high-degree CKF is proposed in this paper to deal with the nonlinear IFA problem of large misalignment model of SDINS. The 5th order CKF was modified for the problem and tested through 6 Degrees-of-freedom (DOF) simulation. The result shows that the proposed algorithm is advantageous compared to other nonlinear filters in terms of accuracy and convergence time. This paper is organized as follows. In Section 2, an IFA problem with a large misalignment model is described. The proposed high-degree CKF algorithm is presented in Section 3. Simulation method and result is explained in Section 4, and the conclusion follows in Section 5.

2. SDINS IFA ERROR MODEL

A general SDINS error model with a large misalignment angle for the IFA problem is described in this section. Unlike the SDINS large heading attitude model of Yu et al. (2012), level axis attitude errors were also considered to be large in this paper assuming poor initial coarse alignment. Throughout this paper, an East-North-Up coordinate is applied. Position \((\delta p )\), velocity \(( \delta v^n )\) and attitude \((\Phi)\) of each axis error models were used for the filter state variables. That is,

\(X=[\delta\phi~~~\delta\lambda~~~\delta h~~~\delta v_E~~~\delta v_N~~~\delta v_U~~~\Phi_E~~~\Phi_N~~~\Phi_U]^T.\)                                                                         (1)

The bias error estimation is not considered in this paper as the IFA filter estimated bias errors of the accelerometers and gyros as large due to the large initial attitude errors. The resulting estimation became unstable since those estimated bias errors were far from the actual bias. Therefore, the bias error states were removed from the filter states in order to unchain the loop of instability. The filter without the sensor bias estimation successfully estimated the large attitude error of the simulation.

2.1 Large Misalignment Error Model

The position error model consists of latitude \((\phi)\), longitude \((\lambda)\) and altitude \((h)\). The error model is as follows:

\(\delta \dot{\phi} ={1 \over R_e⁡+ h} \delta v_N\)                                                                                                                        

\(\delta \lambda ̇={v_E \over (R_e⁡+ h) \cos⁡\phi} \tan⁡\phi \delta \phi+ {\delta v_E \over (R_e⁡+ h) \cos⁡\phi }\)                                                                                      

\(\delta \dot{h}=\delta v_U\)                                                                                                                            (2)

where the spherical earth model with radius \(R_e\) is assumed.

Velocity and attitude errors are given for the order of east, north and up, respectively.

\(\delta \dot{v}^n=(C_n^p-I) f^n-(2\omega_{ie}^n+\rho^n )×\delta v^n-(2\delta \omega_{ie}^n+\delta \rho^n )×v^n-\delta g^n\)                                                                        

\(\dot{\phi}=(C_n^p-I) \omega_{in}^n+\delta \omega_{in}^n\)                                                                                                                                 (3)

Here, matrix \(I\) implies the 3^rd dimensional identity matrix, \(f^n=C_b^n a^b\) is the specific force in navigation frame, measured acceleration on body frame \(a^b =[a_x~ a_y~ a_z]^T\), \(C_b^n\) is the calculated Directional Cosine Matrix (DCM) between body frame and navigation frame, \(\omega_{ie}^n=[0~~~\Omega \cos\phi~~~\Omega \sin\phi]^T\) is earth rate for each axis and its error \(\delta\omega_{ie}^n=[0~~~-\Omega \sin\phi\delta\phi~~~\Omega \cos\phi\delta\phi]^T\) where the angular rate of the Earth \(\Omega\) given in constant, transport rate \(\rho^n=[{-v_N \over R_e+h}~~{v_E \over R_e+h}~~{v_E \tan⁡\phi \over R_e+h}]^T\) and its error \(\delta\rho^n=\left[-{\delta v_N \over R_e+h}~~ {\delta v_E \over R_e+h}~~{\delta v_E \over R_e+h} \tan⁡ \phi+{v_E \over R_e+h} {\rm sec}^2 ⁡\phi \delta\phi)\right]\) and \(\delta g^n\) is gravity error vector. Here, \(C_n^p\) is a DCM from the navigation frame to the platform frame, an image of the navigation frame deviated by errors. When those errors are large,

\(C_n^p=\left[\begin{array}{ccc}c_{11}&c_{12}&c_{13} \\ c_{21}&c_{22}&c_{23} \\ c_{31}&c_{32}&c_{33} \end{array} \right]\)                                                                                                     (4)

where,

\(c_{11}=\cos⁡ \phi_N \cos⁡ \phi_U -\sin⁡ \phi_E \sin⁡ \phi_N \sin⁡ \phi_U \\ c_{12}=\cos⁡ \phi_N \sin⁡ \phi_U +\sin⁡ \phi_E \sin⁡ \phi_N \cos⁡ \phi_U \\ c_{13}=-\cos⁡ \phi_E \sin⁡ \phi_N \\ c_{21}=-\cos⁡ \phi_E \sin⁡ \phi_U \\ c_{22}=\cos⁡ \phi_E \cos⁡ \phi_U \\ c_{23}=\sin⁡ \phi_E \\ c_{31}=\sin⁡ \phi_N \cos⁡ \phi_U +\sin⁡ \phi_E \cos⁡ \phi_N \sin⁡ \phi_U \\ c_{32}=\sin⁡ \phi_N \sin⁡ \phi_U -\sin⁡ \phi_E \cos⁡ \phi_N \cos⁡ \phi_U \\ c_{33}=\cos⁡ \phi_E \cos⁡ \phi_N\)

In Yu et al. (2012), the large heading misalignment model was proposed as follows:

\([C_n^p ]_{LH}=\left[ \begin{array}{ccc} \cos⁡\phi_U &-\sin⁡\phi_U &\phi_N \\ \sin⁡\phi_U &\cos⁡\phi_U &\phi_E \\\phi_E \sin⁡\phi_U -\phi_N \cos⁡\phi_U &\phi_E \cos⁡\phi_U +\phi_N \sin⁡\phi_U &1 \end{array} \right]\)                                                                       (5)

In Eq. (5), level attitude errors are assumed to be small \((\Phi_E, \phi_N <<1° )\). On the other hand, large misalignments on level axis are considered by using Eq. (4) in cases where there is poor initial alignment. The sinusoidal functions and the coupling included in this matrix made the whole error model nonlinear.

2.2 GPS Aided IFA Model

The GPS aided SDINS IFA filter scheme is shown in Fig. 2. During the operation, SDINS calculates the navigation equation given in Eqs. (2) and (3) and outputs position, velocity and error deviated attitude for every navigation cycle. At the same time, the GPS receiver calculates the position and velocity with lower frequency. The output of SDINS minus that of GPS is the measurement for the IFA filter, which estimates the navigation errors to correct the SDINS output.

Fig. 2. GPS aided SDINS in-flight alignment filter.

3. NONLINEAR FILTERING

In this paper, a filtering problem with nonlinear dynamic and observation model in discrete time is considered (Särkkä 2013):

\(x_k=f(x_{k-1} )+q_{k-1}\)                                                                                                    

\(z_k=h(x_k )+r_k\)                                                                                                     (6)

where \(x_k\) is the state variable vector in discrete time \(k\) in dimension n, \(f\) and \(h\) are known state dynamics and observation system model, \(z_k\) is the measurement at time \(k\), and qk-1 and \(r_k\) are independent Gaussian white noise with covariance \(Q_{k-1}\) and \(R_k\) , respectively.

In every filtering cycle, the filter iterates states repeating cubature points capture, propagation and measurement update.

3.1 Prediction

Form the matrix of cubature points:

\(X_{k-1}^{(i)}=m_{k-1}+\sqrt{(n+2) P_{k-1}}~~ \xi^{(i)},~~i=1,2,⋯2n^2+1\)                                                                           (7)

here, \(m_{k-1}\) is the state mean of the previous state and \((i)\) denotes the \(i^{\rm th}\) element of the matrix. The cubature points \(\xi^{(i)}\) for each class are defined as follows:

\(\begin{array}{ll} 1st & 0_{n \times 1}~~~~~~~~~~i=1 \\ 2nd & \left\{ \begin{array}{lc} e_i & i=2,3,⋯,n+1 \\-e_i & i=n+2,⋯,2n+1 \end{array} \right. \\ 3rd & \left\{ \begin{array}{lc} s_i^+ & i=2n+2,⋯,1/2 n^2+3/2 n+1 \\ -s_i^+ & i=1/2 n^2+3/2 n+2,⋯,n^2+n+1 \\ s_i^- & i=n^2+n+2,⋯,3/2 n^2+1/2 n+1 \\ -s_i^- & i=3/2 n^2+1/2 n+2,⋯,2n^{2+1} \end{array} \right. \end{array}\)                                                                           (8)

\(e_i\) denotes a unit vector, and

\(s_i^+={e_j+e_l \over \sqrt{2}}, j<l, j, l=1,2,⋯n\)

\( s_i^-={e_j-e_l \over \sqrt{2}}, j<l, j, l=1,2,⋯n\)

Finally, the weight of each cubature point classes,

\(w_i \left\{ \begin{array}{l} 1st:{2 \over n+2},i=1 \\ 2nd:{4-n \over 2(n+2)^2}, i=2,⋯,2n+1 \\ 3rd:{1\over (n+2)^2},i=2n+2,⋯,2n^2+1 \end{array} \right.\)

Propagate the cubature points through the process dynamics:

\(\hat{X}_k^{(i)}=f\left(X_{k-1}^{(i)} \right), i=1,2,⋯2n^2+1\)                                                                                 (9)

Compute the predicted mean \(m_k^-\) and the covariance \(P_k^-\)

\(m_k^-=\sum\limits_{i=1}^{2n^2+1} w_i \hat{X}_k^{(i)}\)                                                                                                                            

\(P_k^-=\sum\limits_{i=1}^{2n^2+1} w_i \left(\hat{X}_k^{(i)}-m_k^- \right) \left(\hat{X}_k^{(i)}-m_k^- \right)^T+Q_{k-1}\)                                                                     (10)

3.2 Update

Form the cubature points:

\(X_k^{-(i)}=m_k^- + \sqrt{(n+2) P_k^-} \xi^{(i)},i=1,2,⋯2n^2+1\)                                                                     (11)

The cubature points \(\xi^{(i)}\) are defined in Eq. (8).

Propagate the points through the linear observation mode:

\(Y ̂_k^{(i)}=H \cdot X_k^{-(i)},i=1,2,⋯2n^2+1\)                                                                                    

\(H=[I_{6 \times 6}~~~0_{6 \times (n-6)} ]\)                                                                                                    (12)

Compute the predicted mean \(\mu_k\), the covariance of the measurement \(S_k\), and the cross covariance of the state and the measurement \(C_k\):

\(\mu_k=\sum\limits_{i=1}^{2n^2+1} w_i \hat{Y}_k^{(i)}\)                                                                                                                            

\(S_k=\sum\limits_{i=1}^{2n^2+1} w_i \left(\hat{Y}_k^{(i)}-\mu_k \right) \left(\hat{Y}_k^{(i)}-\mu_k \right)^T+R_k\)                                                                                

\(C_k=\sum\limits_{i=1}^{2n^2+1} w_i \left(X_k^{-(i)}-m_k^- \right) \left(\hat{Y}_k^{(i)}-\mu_k\right )^T \)                                                                                (13)

Compute the Kalman gain \(K_k\), the filtered state mean \(m_k\) and the covariance \(P_k\), conditional on the measurement \(y_k\):

\(K_k=C_k S_k^{-1}\)                                                                                                                      

\(m_k=m_k^-+K_k (y_k-\mu_k )\)                                                                                                    

\(P_k=P_k^- -K_k S_k K_k^T\)                                                                                                    (14)

4. SIMULATION & RESULT

The 6DOF simulation was set as follows. The flight vehicle suffered from misalignment angle due to insufficient initial alignment, which was particularly large on the heading angle. In the early stage of the simulation, GPS was not available for 120 seconds and the misalignment affected the SDINS velocity and position calculation. The conditions for the simulation are described in Table 1. The GPS reference signal was recovered at t = 121 [sec], and the IFA filtering began. Figs. 3 and 4 show the position and velocity errors of the simulated vehicle. After the recovery of the aid signal, those errors vanished immediately. However, calibration of the misalignment angle took more time, and the performance and time consumed were different depending on the filtering algorithms.

Table 1. Simulation condition.

	IMU		GPS
	Accelerometer	Gyro
Bias error Scale factor	150 [ug] 100 [ppm]	0.02 [deg/h] 100 [ppm]	Position error Velocity error	8 [m] 0.2 [m/s]
IFA filter conditions
Process noise covariance matrix	Q = diag{0, 0, (0.15 m)2, (0.003 m/s)2, (0.003 m/s)2, (0.003 m/s)2,        (0.00005 deg)2, (0.00005 deg)2, (0.00005 deg)2}
Measurement noise covariance matrix	R = diag{(1/Re)2, (1/Re)2, (2 m)2, (0.01 m/s)2, (0.01 m/s)2, (0.01 m/s)2}
Initial error covariance matrix	P0 = diag{(10000 m)2, (10000 m)2, (30 m)2, (100 m/s)2, (100 m/s)2, (3 m/s)2,         (5.73 deg)2, (5.73 deg)2, (17.19 deg)2}

Fig. 3. Position error of the flight vehicle.

Fig. 4. Velocity error of the flight vehicle.

Here, two separate simulations were presented in order to show the effectiveness of the high order CKF compared to 3rd order CKF, the special case of UKF. Table 2 shows the initial misalignment conditions for the simulations on each axis. Case 1 in Table 2 is the remaining attitude error when initial alignment was unsuccessful, and case 2 has a particularly large misalignment on the heading angle. The attitude errors were investigated through 100-run of Monte-Carlo simulations to check the remaining error after the IFA filter estimation.

Table 2. Initial misalignment angle.

Attitude error [mrad]	Horizontal axis	Vertical axis
Case 1 Case 2	100 100	300 1000

 

In Figs. 5 and 6, both UKF and high-degree CKF estimated the attitude of the vehicle effectively, and the remaining errors were clearly decreased on every axis. Figs. 7 and 8, however, with a significantly large initial heading attitude error, made the convergence characteristic of the filter worse. The converging error bounced back at t = 160 [sec] due to a sudden change in the vehicle rolling motion. It kept its error until t = 300 [sec], when the error converged due to the increase of the observability from the large back-to-turn motion. The simulated vehicle motion is shown in Fig. 9. The result of the 6DOF simulation is summarized in Fig. 10 and Table 3. The filtering performances of the algorithms were similar for case 1 with low heading error. With relatively large heading misalignment, however, the heading estimation of the 5th CKF was 13.9% more accurate than UKF by the end of the simulation at t = 600 [sec].

Fig. 5. Level attitude error of case 1.

Fig. 6. Heading attitude error of case 1.

Fig. 7. Level attitude error of case 2.

Fig. 8. Heading attitude error of case 2.

Fig. 9. Simulated vehicle motion.

Fig. 10. Remaining heading error of case 1 and 2.

Table 3. Remaining heading error.

Heading error [deg]	UKF	5th CKF
Case 1 Case 2	0.0081 0.0222	0.0074 0.0195

 

5. CONCLUSIONS

High-degree CKF is proposed in this paper to deal with the large misalignment IFA filtering problem. Poor self-alignment and unavailability of external sensors may result in large horizontal and vertical attitude error, resulting in low grade navigation information for the mission. The navigation error model including those large attitude errors was considered in this paper. The model was specifically built for the large misalignment simulation, and the adaptation to the actual SDINS needs to be discussed in future research. Two nonlinear filters were compared for different initial misalignments to show the effectiveness of the 5^th CKF. Compared to UKF, the high ordered CKF showed more accurate estimation and faster convergence of the attitude angle.