# Application of Adaptive Robust CKF in SINS/GPS Initial Alignment with Large Azimuth Misalignment Angle

Authors: Zhang Bing; Wang Xiaodong; Lu Hao; Hao Zhaojun; Gu Changchao
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

When the strapdown inertial navigation system does not perform coarse alignment, the misalignment angle is generally a large angle, and a nonlinear error model and a nonlinear filtering method are required. For large azimuth misalignment, the initial alignment technology with a large azimuth misalignment angle is researched in this paper. The initial alignment technology with a large azimuth misalignment angle is researched in this paper. First, the SINS/GPS nonlinear error model is established. Secondly, in the view of observation gross errors and inaccurate noise statistical characteristics, an adaptive robust CKF algorithm is proposed. Finally, according to the simulation analysis and experiment, the adaptive robust CKF algorithm can augment the stability and improve the filter estimation precision and convergence rate.


---

## Body

## 1. Introduction

As an important means for exploring oceans, underwater vehicles are widely applied in civilian and military. With the continuous improvement of requirements for navigation accuracy, high-precision navigation becomes an important technical guarantee/support for underwater vehicles to reliably and safely complete underwater navigation tasks. Meanwhile, in the face of the complex underwater work environment, high-precision navigation is based on high-precision initial alignment. Also, initial alignment mainly contains the rough alignment process and fine alignment process; once the underwater vehicle encountered bad sea conditions, rough azimuth information is difficult to obtain in the rough alignment process so that the condition of initial alignment is usually with large azimuth misalignment angle. Therefore, in view of the work process and actual application requirements for the underwater vehicle, this paper aims to study the technology of initial alignment with large azimuth misalignment angle.Initial alignment with large azimuth misalignment angle is the key technology of SINS [1, 2]. According to the characteristics of the nonlinear error model, nonlinear EKF and UKF are usually applied on tradition. However, the performance of the EKF depends on linearization to a large extent, in the face of a strongly nonlinear system, the higher-order terms of Taylor expansion of the nonlinear model cannot be ignored so that large truncation errors will be generated, and the filter accuracy will be reduced or even diverged. Also, when dealing with the high-dimensional system, the covariance in the UKF filter process will be nonpositive definite, resulting in the instability of the filter and the possibility of divergence. In view of the problems above, the cubature Kalman filtering (CKF) based on the spherical-radial cubature criterion is widely applied at the present time. This algorithm avoids the linearization of the nonlinear model and overcomes the shortcomings of EKF and other traditional nonlinear filtering algorithms [2].If the satellite signal is unstable, there will be gross bias in the observations, which will affect the estimation effect of the filter. At the same time, we cannot get accurate noise characteristics in most cases [2, 3]; thus, the performance of filter estimation is greatly limited. In view of the above problems, an adaptive robust CKF algorithm based on the traditional CKF is proposed in this paper, which utilizes robust M estimation and an improved Sage–Husa suboptimal posterior estimator to improve the stability and adaptive ability of the filter.


## 2. SINS Large Azimuth Misalignment Angle Error Model

The conversion matrix between the navigation coordinate systemn used which is described by the Euler platform error angle ϕ=ϕxϕyϕzT and the mathematical platform coordinate system n′ is as follows:(1)Cnn′=cϕycϕz−sϕysϕxsϕzcϕysϕz+sϕysϕxcϕz−sϕycϕx−cϕxsϕzcϕxcϕzsϕxsϕycϕz+cϕysϕxsϕzsϕysϕz−cϕysϕxcϕzcϕycϕx.sϕi is sinϕi, and cϕi is cosϕii=x,y,z.Assuming that the angular velocity of then′ system relative to the n system is ωnn′n′, the error angular differential equation of the Euler platform is(2)ϕ˙=f−1ϕωnn′n′,f−1ϕ=1cϕxcϕycϕx0sϕycϕxsϕysϕxcϕx−cϕysϕx−sϕy0cϕy.


### 2.1. Error Equation

According to Chang [4], it can be concluded that the SINS attitude error equation is(3)ϕ˙=f−1ϕI−Cnn′ω^inn+Cnn′δωinn−Cbn′δωibb.The velocity error equation is(4)δv˙n=I−Cn′nTCbn′f^b+Cn′nTCbn′δfb−2δωien+δωenn×v^n−δvn−2ω^ien+ω^enn×δvn+δgn.


### 2.2. Filtering Model

It is assumed that the measurement error of gyroscopeδωibb is mainly random constant drift error εb and zero-mean Gaussian white noise wg. The measurement error of the accelerometer is mainly random normal zero deviation ∇b and zero-mean Gaussian white noise wa. If the horizontal misalignment angle is small and the height error is not considered, the SINS large azimuth error model is(5)ϕ˙=I−Cnn′ω^inn+Cnn′δωinn−Cbn′εb+Cbn′wgδv˙n=I−Cnn′TCbn′f^b+Cnn′TCbn′∇b−2δωien+δωenn×vn−2ωien+ωenn×δv+Cnn′TCbn′waε˙x=ε˙y=ε˙z=0∇˙x=∇˙y=∇˙z=0.The system state quantity isx=ϕxϕyϕzδvxδvyεxεyεz∇x∇yT. The white noise of the gyroscope and accelerometer is w=wgxwgywgzwaxwayT.Take velocity and position informationz=δvxδvyT as the measurement.The system state equation and measurement equation are(6)x˙t=fxt,t+Gwwtzt=htxt+vt.Gw is the process noise input matrix; ht is the measuring matrix, and ht=02×3I2×202×5; wt is the process noise; vt is the measurement noise and follows Gaussian distribution vt∼N0,R.


## 2.1. Error Equation

According to Chang [4], it can be concluded that the SINS attitude error equation is(3)ϕ˙=f−1ϕI−Cnn′ω^inn+Cnn′δωinn−Cbn′δωibb.The velocity error equation is(4)δv˙n=I−Cn′nTCbn′f^b+Cn′nTCbn′δfb−2δωien+δωenn×v^n−δvn−2ω^ien+ω^enn×δvn+δgn.


## 2.2. Filtering Model

It is assumed that the measurement error of gyroscopeδωibb is mainly random constant drift error εb and zero-mean Gaussian white noise wg. The measurement error of the accelerometer is mainly random normal zero deviation ∇b and zero-mean Gaussian white noise wa. If the horizontal misalignment angle is small and the height error is not considered, the SINS large azimuth error model is(5)ϕ˙=I−Cnn′ω^inn+Cnn′δωinn−Cbn′εb+Cbn′wgδv˙n=I−Cnn′TCbn′f^b+Cnn′TCbn′∇b−2δωien+δωenn×vn−2ωien+ωenn×δv+Cnn′TCbn′waε˙x=ε˙y=ε˙z=0∇˙x=∇˙y=∇˙z=0.The system state quantity isx=ϕxϕyϕzδvxδvyεxεyεz∇x∇yT. The white noise of the gyroscope and accelerometer is w=wgxwgywgzwaxwayT.Take velocity and position informationz=δvxδvyT as the measurement.The system state equation and measurement equation are(6)x˙t=fxt,t+Gwwtzt=htxt+vt.Gw is the process noise input matrix; ht is the measuring matrix, and ht=02×3I2×202×5; wt is the process noise; vt is the measurement noise and follows Gaussian distribution vt∼N0,R.


## 3. Robust Kalman Filtering Algorithm

In SINS/GPS initial alignment, the gross errors will affect the estimated results [5, 6], and individual large gross errors will make a greater deviation. However, traditional Kalman filtering cannot effectively deal with gross errors; in order to solve the above problems, a robust Kalman filtering algorithm is proposed in this paper. The algorithm combines the robust estimation theory and adopted a more practical robust M estimation. On the basis of M estimation, the principle of equivalent weight is applied to transform it into the form of least squares [6]. How to choose weighting factors is the key of robust Kalman filtering. In this paper, weight function is used to replace the observed noise covariance matrix to reduce or eliminate the influence of the gross errors.The original observation weight isPi; then, its equivalence weight P¯=P¯i=Piwi, and the weighting factors are as follows:(7)wi=1vi≤k0σvi,vi>kiσvik0σ0vi×ki−vi/σ0ki−k02k0σvi<vi≤kiσvi.vi is the residual error of observation quantity; σvi is the mean square error of vi, σvi=σ0/qvi, qvi is the weight reciprocal, variance factor σ0=medqvi,vi/0.6745, and k0 and ki are 1.5∼2.5 and 3.0∼5.0.


## 4. Adaptive Kalman Filtering Algorithm

Research shows that inaccurate noise characteristics often lead to unstable and even filter divergence [7, 8]. Therefore, adaptive filtering technology has been widely studied. Sage–Husa suboptimal unbiased maximum posterior (MAP) noise estimator has been widely used due to its advantages of simple calculation and clear principle [9]. However, Sage–Husa noise estimator cannot simultaneously estimate and measure system noise; otherwise, it will lead to filter divergence [9, 10]. In the actual environment, the measurement noise can be obtained by the physical characteristics of the sensor, but due to the influence of measurement instrument precision, external interference, and other factors, it is difficult to obtain the system noise accurately [11–13]. In view of the above problems, combining with the covariance matching criterion and introducing the adaptive attenuation factor, the divergence phenomenon of the adaptive filtering was suppressed, and the stability and adaptability were improved.


### 4.1. System Noise Estimation Method

In this paper, improved Sage–Husa is used to estimate the inaccurate system noiseqk and Qk:(8)q^k=1−dk−1q^k−1+dk−1x^k/k−Φkx^k−1/k−1,Q^k=1−dk−1Q^k−1+dk−1KkwkwkTKkT+Pk/k−ΦkPk−1/k−1ΦkT.dk−1=1−b1−bk, 0.95<b<0.99 is the forgetting factor, and the higher the value of b is, the faster the noise statistics change.


### 4.2. Suppression of Filter Divergence

Sage–Husa estimator usually diverges, so the covariance matching criterion is used in this paper.z˜kTz˜k≤StrEz˜kz˜kT is used to determine whether the estimator diverges [14, 15]; if it diverges, it will be corrected as follows: Pk/k−1, in which S≥1 is the adjustable coefficient, z˜k is the residual sequence, and z˜k=zk−hx^k/k−1.(9)Pk/k−1=λk∑i=02nwiXi,k|k−1∗−x^k/k−1Xi,k|k−1∗−x^k/k−1T+Q^k−1,λk=λ0λ0≥11λ0≥1,λ0=trC0,k−RkTtr∑i=12nwiZi,k/k−1−z^k/k−1Zi,k/k−1−z^k/k−1T,C0,k=z˜kz˜kTk=1ρC0,k+z˜kz˜kT1+ρk>1,where 0<ρ≤1 is the attenuation coefficient, usually about 0.95; the larger the value of ρ is, the more prominent the influence of the current residual vector is.


## 4.1. System Noise Estimation Method

In this paper, improved Sage–Husa is used to estimate the inaccurate system noiseqk and Qk:(8)q^k=1−dk−1q^k−1+dk−1x^k/k−Φkx^k−1/k−1,Q^k=1−dk−1Q^k−1+dk−1KkwkwkTKkT+Pk/k−ΦkPk−1/k−1ΦkT.dk−1=1−b1−bk, 0.95<b<0.99 is the forgetting factor, and the higher the value of b is, the faster the noise statistics change.


## 4.2. Suppression of Filter Divergence

Sage–Husa estimator usually diverges, so the covariance matching criterion is used in this paper.z˜kTz˜k≤StrEz˜kz˜kT is used to determine whether the estimator diverges [14, 15]; if it diverges, it will be corrected as follows: Pk/k−1, in which S≥1 is the adjustable coefficient, z˜k is the residual sequence, and z˜k=zk−hx^k/k−1.(9)Pk/k−1=λk∑i=02nwiXi,k|k−1∗−x^k/k−1Xi,k|k−1∗−x^k/k−1T+Q^k−1,λk=λ0λ0≥11λ0≥1,λ0=trC0,k−RkTtr∑i=12nwiZi,k/k−1−z^k/k−1Zi,k/k−1−z^k/k−1T,C0,k=z˜kz˜kTk=1ρC0,k+z˜kz˜kT1+ρk>1,where 0<ρ≤1 is the attenuation coefficient, usually about 0.95; the larger the value of ρ is, the more prominent the influence of the current residual vector is.


## 5. Adaptive Robust CKF Algorithm

The adaptive robust CKF combines the advantages of the robust Kalman filtering algorithm and the adaptive Kalman filtering algorithm, which can not only overcome the influence of the observed gross errors but also restrain the problem of filter divergence caused by the unknown or time-varying noise of the system.Assume that the state equation and measurement equation of the nonlinear system are(10)xk=fxk−1+gxk−1wk−1zk=hxk+jxkvk.wk−1 and vk are the Gaussian white noise with zero mean, and the variance matrix is Q and R.Adaptive robust CKF algorithm is as follows:(1)
Calculate the volume point and weight:(11)ξi=n1iwi=12ni=1,2,…,2n.(2)
Time update:(12)Pk−1/k−1=Sk−1/k−1Sk−1/k−1T,Xi,k−1/k−1=Sk−1/k−1ξi+x^k−1/k−1,Xi,k−1/k−1∗=fXi,k−1/k−1,x^k/k−1=12n∑i=12nXi,k/k−1∗,Pk/k−1=12n∑i=12nXi,k/k−1∗Xi,k/k−1∗T−x^k/k−1x^k/k−1T+Q^k−1.(3)
Measurement update:(13)Pk/k−1=Sk/k−1Sk/k−1T,Xi,k/k−1=Sk/k−1ξi+x^k/k−1,Zi,k/k−1=hXi,k/k−1,z^k/k−1=12n∑i=12nZi,k/k−1,R¯k=P¯−1.P¯ is the equal weight, and it can be obtained by the M estimation equivalence weight principle.Suppose the original observation weight isP¯, P¯=P¯i=Piw˜i. And the power factor is(14)w˜i=1vi≤k0σvi,vi>kiσvik0σ0vi×ki−vi/σ0ki−k02k0σvi<vi≤kiσvi,z^k/k−1=12n∑i=12nZi,k/k−1.Divergent judgment is as follows.Divergence is judged according to equation (15), and if divergence is detected, it is corrected according to equations (16)–(19). Otherwise, go directly to equation (20).(15)z˜kTz˜k≤StrEz˜kz˜kT,(16)Pk/k−1=λk∑i=02nwiXi,k/k−1∗−x^k/k−1Xi,k/k−1∗−x^k/k−1T+Q^k−1,(17)λk=λ0λ0≥11λ0≥1,(18)λ0=trC0,k−RkTtr∑i=02nwiZi,k/k−1−z^k/k−1Zi,k/k−1−z^k/k−1T,(19)C0,k=z˜kz˜kTk=1ρC0,k+z˜kz˜kT1+ρk>1,(20)Pzz,k/k−1=12n∑i=12nZi,k/k−1Zi,k/k−1T−z^k/k−1z^k/k−1T+R¯k,Pxz,k/k−1=12n∑i=12nXi,k/k−1Zi,k/k−1T−x^k/k−1z^k/k−1T,Kk=Pxz,k/k−1Pzz,k/k−1−1,x^k/k=x^x/k−1+Kkzk−z^k/k−1,Pk/k=Pk/k−1−KkPzz,k/k−1KkT,q^k=1−dk−1q^k−1+dk−1x^k/k−x^k/k−1,Q^k=1−dk−1Q^k−1+dk−1KkwkwkTKkT−KkPzz,k/k−1KkT.In order to solve imprecision of the system noise and the observation gross error, the adaptive robust CKF algorithm has a targeted solution: Sage–Husa adaptive filter is used to estimate system noiseQk online, and the filter divergence is suppressed. The equivalent weight coefficient w˜i is used to adjust the influence degree of the observation noise error to obtain x^k/k and Pk/k.


## 6. Simulation

Combined with the adaptive robust CKF and SINS error model proposed in this paper, numerical simulation is carried out.The initial estimate of system stateX0=0; initial misalignment angle ϕx=ϕy=1∘, ϕz=10∘; the initial velocity error is 0.1m/s; the constant drift of the gyroscope is 0.02∘/h; random noise is 0.01∘/h; the accelerometer having zero deviation is 1×10−4g; random noise is 5×10−5g; the GPS speed measurement error is 0.1m/s; then, the initial variance matrix P0, system noise matrix Q, and measurement noise array R are(21)P0=diag1°2,1°2,10°2,0.1m/s2,0.1m/s2,0.02°/h2,0.02°/h2,0.02°/h2,100μg2,100μg2,Q=diag0.01°/h2,0.01°/h2,0.01°/h2,50μg2,50μg2,0,0,0,0,0,R=diag10m2,10m2,0.1m/s2,0.1m/s2.(1)
In a certain period of time, the velocity observation increases the coarse difference of3m/s, and the filter sampling frequency is also increased. Two nonlinear filtering methods, CKF and robust CKF, were compared, respectively. The simulation time is 300 s. The simulation results are shown in Figures 1–3.As can be seen from Figures1 and 2 and Table 1, the precision of the robust CKF estimation is higher for horizontal misalignment angle, the error curve is relatively smooth, and the convergence speed is fast. It can be seen from Figure 3 and Table 1 that the precision and convergence speed of the robust CKF estimation are better than those of CKF, and there are no more burrs, so the filtering is more stable.(2)
On the basis of simulation condition (1), the actual system noise is assumed to beQ′=10Q. The two filtering methods of robust CKF and adaptive robust CKF were compared, respectively. The simulation time is 300 s. The simulation results are shown in Figures 4–6.As can be seen from Figures4–6 and Table 2, the precision of the two filtering algorithms is almost the same as that of the eastbound misalignment angle, and the adaptive robust CKF can maintain a high performance for the estimation of northbound misalignment angle and azimuth misalignment angle; besides, the convergence speed is more faster.Figure 1
The estimated error curve of the east misalignment angle.Figure 2
The estimated error curve of the north misalignment angle.Figure 3
Estimation error curve of the azimuth misalignment angle.Figure 4
The estimated error curve of the east misalignment angle.Figure 5
The estimated error curve of the north misalignment angle.Figure 6
Estimation error curve of the azimuth misalignment angle.Table 1
The alignment result based on the CKF and robust CKF filtering algorithm.
Filtering algorithmEast misalignment angleNorth misalignment angleAzimuth misalignmentCKF21.9″20.4″6.08′Robust CKF19.2″18.9″5.13′Table 2
The alignment result based on the robust CKF and adaptive robust CKF filtering algorithm.
Filtering algorithmEast misalignment angleNorth misalignment angleAzimuth misalignmentRobust CKF32.3″31.7″6.12′Adaptive robust CKF27.6″29.2″5.67′


## 7. Experiment Test

In order to verify the adaptive robust CKF filtering algorithm feasibility of actual initial alignment, based on the original measured data and offline analysis, test of the azimuth misalignment angle estimation error curve is shown in Figure7. The experiment results are shown in Table 3.Figure 7
Estimation error curve of the azimuth misalignment angle.Table 3
The alignment result based on the CKF and adaptive robust CKF filtering algorithm.
Filtering algorithmAzimuth misalignment estimation errorCKF5.21′Adaptive robust CKF3.76′From Figure7 and Table 1, the azimuth misalignment estimation error of CKF is 5.21′, and the azimuth misalignment estimation error of adaptive robust CKF is 3.76′.The test result shows that the alignment precision of the adaptive robust CKF filtering algorithm is higher than that of the traditional CKF filtering algorithm, which solves the error caused by inaccurate measurement information or system noise and improves the initial alignment precision.


## 8. Conclusion

In this paper, an adaptive robust CKF filtering algorithm is studied, which can simultaneously observe the errors caused by gross error and inaccurate system noise. Besides, the adaptive robust CKF can effectively realize the initial alignment of the large azimuth misalignment angle, improve the estimation precision and convergence speed of the misalignment angle, and enhance the stability and self-adaptive ability of the filtering algorithm under the condition of the observed information anomaly or the unknown system noise characteristic.



---

*Source: 7398706-2021-09-25.xml*