# Robust Adaptive Cubature Kalman Filter and Its Application to Ultra-Tightly Coupled SINS/GPS Navigation System

^{1}

^{2}

^{3}

^{*}

## Abstract

**:**

## 1. Introduction

- The proposed algorithm could solve the problems of the model uncertainty and the measurement noise statistics inaccuracy. To handle abnormal measurement noise, we adopt robust M-estimation to automatically adjust the measurement noise covariance and gaining adaptivity. Compared with the literature [23,24,25,26], we directly use innovation to calculate the equivalent weight matrix, no-demand partial derivative operation, and iterative operation. To overcome model uncertainty, we derive an adaptive adjustment factor to modify the model as a whole. Compared with STF in the literature [19,20,21,22], the derived adaptive adjustment factor is simpler.
- The proposed algorithm has lower computational complexity, and so it is suitable for dynamic systems with high real-time requirements. Our algorithm is not simply a cumulative combination of existing methods, but a derivation method considering the mathematical complexity and real-time, which ensures the computational efficiency of the algorithm.
- In the application of the algorithm, we designed a stable and reliable ultra-tightly coupled structure based on a double-modulating loop. The effectiveness of the proposed method and the designed structure is verified by a highly dynamical scene experiment.

## 2. CKF Algorithm

#### 2.1. Bayesian Estimation Rule

- (1)
- Time update, calculating the predictive density:$$p({\mathit{x}}_{k}|{\mathit{D}}_{k-1})={\displaystyle \int p({\mathit{x}}_{k},{\mathit{x}}_{k-1}|{\mathit{D}}_{k-1})}d{\mathit{x}}_{k-1}={\displaystyle \int p({\mathit{x}}_{k-1}|{\mathit{D}}_{k-1})}\times p({\mathit{x}}_{k}|{\mathit{x}}_{k-1},{\mathit{u}}_{k-1}),d{\mathit{x}}_{k-1}$$
- (2)
- Measurement updateThe posterior density of the current state can be obtained by Bayesian rule:$$p({\mathit{x}}_{k}|{\mathit{D}}_{k})=\frac{1}{{c}_{k}}p({\mathit{x}}_{k}|{\mathit{D}}_{k-1},{u}_{k-1})p({\mathit{z}}_{k}|{\mathit{x}}_{k},{\mathit{u}}_{k})$$$${c}_{k}={\displaystyle \int p({\mathit{x}}_{k}|{\mathit{D}}_{k-1},{\mathit{u}}_{k})}p({\mathit{z}}_{k}|{\mathit{x}}_{k},{\mathit{u}}_{k})d{\mathit{x}}_{k}$$

#### 2.2. CKF Algorithm Based on a Third-Order Spherical-Radial Cubature Rule

- (1)
- The posteriori probability distribution of a given k − 1 moment is assumed to be:$$p({\mathit{x}}_{k-1}|{\left\{{\mathit{u}}_{i},{\mathit{z}}_{i}\right\}}_{i=1}^{k-1})=\Im ({\mathit{x}}_{k-1};{\widehat{\mathit{x}}}_{k-1|k-1},{\mathit{P}}_{k-1|k-1})$$Let:$${\mathit{P}}_{k-1|k-1}={\mathit{S}}_{k-1|k-1}{\mathit{S}}_{k-1|k-1}^{\mathrm{T}}$$
- (2)
- Calculating state volume points:$${\mathit{X}}_{j,k-1|k-1}^{\hslash}={\mathit{S}}_{k-1|k-1}{\xi}_{j}+{\widehat{\mathit{x}}}_{k-1|k-1}$$
- (3)
- Volume points transformed based on the state equation:$${\mathit{X}}_{j,k-1|k-1}^{*}=f({\mathit{X}}_{j,k-1|k-1}^{\hslash},{\mathit{u}}_{k-1})$$
- (4)
- Weighted mean to compute the state quantity prediction value:$${\widehat{\mathit{x}}}_{k|k-1}=\frac{1}{2n}{\displaystyle \sum _{j=1}^{2n}{\mathit{X}}_{j,k-1|k-1}^{*}}$$
- (5)
- Calculate the covariance matrix of state prediction:$${\mathit{P}}_{k|k-1}=\frac{1}{2n}{\displaystyle \sum _{j=1}^{2n}{\mathit{X}}_{j,k-1|k-1}^{*}{\mathit{X}}_{j,k-1|k-1}^{*\mathrm{T}}}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{x}}}^{\mathrm{T}}{}_{k|k-1}+{\mathit{Q}}_{k-1}$$

**Measurement updating:**

- (1)
- Matrix factorization:$${\mathit{P}}_{k|k-1}={\mathit{S}}_{k|k-1}{\mathit{S}}_{k|k-1}^{\mathrm{T}}$$
- (2)
- Calculate volume points:$${\mathit{X}}_{j,k|k-1}^{\hslash}={\mathit{S}}_{k|k-1}{\xi}_{j}+{\widehat{\mathit{x}}}_{k|k-1}$$
- (3)
- Transform volume points based on the measurement equation:$${\mathit{Z}}_{j,k-1|k-1}^{*}=h({\mathit{X}}_{j,k|k-1})$$
- (4)
- Forecast the measurement value:$${\widehat{\mathit{z}}}_{k|k-1}=\frac{1}{2n}{\displaystyle \sum _{j=1}^{2n}{\mathit{Z}}_{j,k|k-1}^{*}}$$
- (5)
- Calculate the prediction residual covariance matrix:$${\mathit{P}}_{zz,k|k-1}={\displaystyle \sum _{j=1}^{2n}{w}_{j}{\mathit{Z}}_{j,k|k-1}^{*}{\mathit{Z}}_{j,k|k-1}^{*\mathrm{T}}}-{\widehat{\mathit{z}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}+{\mathit{R}}_{k}$$
- (6)
- Calculate the cross covariance matrix:$${\mathit{P}}_{xz,k|k-1}={\displaystyle \sum _{j=1}^{2n}{w}_{j}{\mathit{X}}_{j,k|k-1}^{\hslash}{\mathit{Z}}_{j,k|k-1}^{*\mathrm{T}}}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}$$
- (7)
- Calculate the Kalman filter gain:$${\mathit{W}}_{k}={\mathit{P}}_{xz,k|k-1}{\mathit{P}}_{zz,k|k-1}^{-1}$$
- (8)
- Update the state:$${\widehat{\mathit{x}}}_{k|k}={\widehat{\mathit{x}}}_{k|k-1}+{\mathit{W}}_{k}({\mathit{z}}_{k}-{\widehat{\mathit{z}}}_{k|k-1})$$
- (9)
- Update the state covariance matrix:$${\mathit{P}}_{k|k}={\mathit{P}}_{k|k-1}-{\mathit{W}}_{k}{\mathit{P}}_{zz,k|k-1}{\mathit{W}}_{k}^{\mathrm{T}}$$

## 3. Novel Robust Adaptive CKF

**Theorem**

**1.**

**Proof**

**of**

**Theorem**

**1.**

**Step****1.**- Set the initial condition.$${\widehat{\mathit{x}}}_{0}=E({\mathit{x}}_{0})\begin{array}{cc}& \end{array}{\mathit{P}}_{0}=E{\left[({\mathit{x}}_{0}-{\widehat{\mathit{x}}}_{0})({\mathit{x}}_{0}-{\widehat{\mathit{x}}}_{0})\right]}^{\mathrm{T}}$$
**Step****2.**- Forecast the update.For a given A and B, according to Equations (12)–(26), the state prediction value C and its prediction covariance matrix D are obtained.
**Step****3.**- Calculate the state volume points in the measurement updating.The matrix decomposition of the state prediction covariance matrix is completed by using Equation (17), and replacing Equation (18) with the following equation, adaptive adjustment of the model will be complete:$${\mathit{X}}_{j,k|k-1}^{\hslash}=\sqrt{{\vartheta}_{k}^{-1}}{\mathit{S}}_{k|k-1}{\xi}_{j}+{\widehat{\mathit{x}}}_{k|k-1}$$
**Step****4.**- Calculate the predicted values and the residual covariance matrix.Computational observational prediction ${\widehat{\mathit{z}}}_{k|k-1}$ and predictive residual covariance matrix ${\mathit{P}}_{zz,k|k-1}$ are calculated by Equations (19)~(21).
**Step****5.**- Robust correction.The robust correction of the algorithm is completed according to the Equations (26)–(29).
**Step****6.**- Adaptive factor regulation model.Use the approximate expression Equation (45) to calculate the optimal adaptive factor, if the value is 1, then continue the following steps; if the value is less than 1, the value is substituted into Equation (48), and then by the Equations (19), (20) and (26) calculate the adaptive factor to adjust the revised prediction residual covariance matrix.
**Step****7.**- Update the measurement.According to Equations (22)–(25), the update calculation of the state variable and its corresponding state covariance matrix is completed.

**Remark**

**1.**

## 4. SINS/GPS Ultra-Tightly Coupling Structure Based on a Double Loop

#### 4.1. Design of SINS/GPS Ultra-Tightly Coupling Structure Based on a Double Loop

#### 4.2. Pre-Filter Tracking Modeling

- (1)
- Pre-filter state equation establishmentState variables take:$$\mathit{X}={[\begin{array}{ccccc}A& \delta \tau & \delta \varphi & \delta f& \delta a\end{array}]}^{\mathrm{T}}$$$$\begin{array}{ll}\dot{\mathit{X}}& ={[\begin{array}{ccccc}\dot{A}& \delta \dot{\tau}& \delta {\dot{\varphi}}_{0}& \delta {\dot{f}}_{0}& \delta {\dot{a}}_{0}\end{array}]}^{\mathrm{T}}=\mathit{F}\mathit{X}+\mathit{G}\mathit{w}\\ & =\left(\begin{array}{ccccc}0& 0& 0& 0& 0\\ 0& 0& 0& \beta & 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0\end{array}\right)\left(\begin{array}{c}A\\ \delta \tau \\ \delta {\varphi}_{0}\\ \delta {f}_{0}\\ \delta {a}_{0}\end{array}\right)+\left(\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& \beta & 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{w}_{A}\\ {w}_{\tau}\\ {w}_{{\varphi}_{0}}\\ {w}_{{f}_{0}}\\ {w}_{{a}_{0}}\end{array}\right)\end{array}$$
- (2)
- Pre-filter measurement equation establishmentThe system’s observation is obtained by in-phase signal I and quadrature signal Q outputting by the correlator. I and Q include the early (E) signal, the present (P) signal and the later (L) signal. The expression of the measurement equation is as follows:$$\left[\begin{array}{l}{I}_{P}\\ {I}_{E}\\ {I}_{L}\\ {Q}_{P}\\ {Q}_{E}\\ {Q}_{L}\end{array}\right]=\left[\begin{array}{c}ATD({t}_{k}-\tau )R(\delta \tau )\mathrm{sin}c(\delta fT/2)\mathrm{cos}(\delta fT/2+\delta \varphi )\\ ATD({t}_{k}-\tau )R(\delta \tau -0.5\Delta )\mathrm{sin}c(\delta fT/2)\mathrm{cos}(\delta fT/2+\delta \varphi )\\ ATD({t}_{k}-\tau )R(\delta \tau +0.5\Delta )\mathrm{sin}c(\delta fT/2)\mathrm{cos}(\delta fT/2+\delta \varphi )\\ ATD({t}_{k}-\tau )R(\delta \tau )\mathrm{sin}c(\delta fT/2)\mathrm{sin}(\delta fT/2+\delta \varphi )\\ ATD({t}_{k}-\tau )R(\delta \tau -0.5\Delta )\mathrm{sin}c(\delta fT/2)\mathrm{sin}(\delta fT/2+\delta \varphi )\\ ATD({t}_{k}-\tau )R(\delta \tau +0.5\Delta )\mathrm{sin}c(\delta fT/2)\mathrm{sin}(\delta fT/2+\delta \varphi )\end{array}\right]+\left[\begin{array}{c}{n}_{{I}_{P}}\\ {n}_{{I}_{E}}\\ {n}_{{I}_{L}}\\ {n}_{{Q}_{P}}\\ {n}_{{Q}_{E}}\\ {n}_{{Q}_{L}}\end{array}\right]$$

## 5. Experimental Analysis of a Highly Dynamical Scene

#### 5.1. Experimental Scheme

#### 5.2. Setup of the Experimental Conditions

^{2}in the north direction acceleration and AU = 0 m/s

^{2}in the day direction acceleration, respectively. Eastward is variable acceleration motion, in which 0–20 s, 21–40 s, and 41–60 s are all constant acceleration motion, and eastward accelerations are AE = 100 m/s

^{2}, AE = 200 m/s

^{2}, and AE = −200 m/s

^{2}, respectively. 20–21 s and 40–41 s are the constant jerk motions, the numerical value of which are 100 m/s

^{3}and −400 m/s

^{3}, respectively. As a result, the maximum absolute speed is about 6000 m/s, the maximum absolute acceleration is about 20 g, and the maximum jerk is about 40 g/s, which is fully in line with the highly dynamical characteristics.

_{0}) of all the satellite signals is set to 20 dB-Hz. There are currently 13 visible satellites in the sky. In addition, in order to verify the proposed method of adaptive adjusting performance, the measurement noise parameters are expanded by 20 times among 5–10 s, so that gross error of observations are simulated equivalently; on the other hand, the high maneuvering scenario set in this section can evaluate the adaptive performance of the algorithm when the dynamic model is imprecise, so it is not necessary to set the dynamic model error.

#### 5.3. Experimental Results and Analysis

## 6. Conclusions

## Author Contributions

## Funding

## Conflicts of Interest

## References

- Haus, B.; Mercorelli, P. An extended Kalman filter for time delay inspired by a fractional order model. Lect. Notes Electr. Eng.
**2018**, 496, 151–163. [Google Scholar] - Schimmack, M.; Haus, B.; Mercorelli, P. An extended Kalman filter as an observer in a control structure for health monitoring of a matal-polymer hybrid soft actuator. IEEE/ASME Trans. Mechatron.
**2018**, 23, 1477–1487. [Google Scholar] [CrossRef] - Allotta, B.; Caiti, A.; Costanzi, R.; Fanelli, F.; Meli, E.; Ridolfi, A. Development and online validation of an UKF-based navigation algorithm for AUVs. IFAC-PaperOnLine
**2016**, 49, 69–74. [Google Scholar] [CrossRef] - Meng, D.; Miao, L.J.; Shao, H.J.; Shen, J. A Seventh-degree Cubature Kalman Filter. Asian J. Control
**2018**, 20, 250–262. [Google Scholar] [CrossRef] - Hu, G.G.; Gao, S.S.; Zhong, Y.M. A derivative UKF for tightly coupled INS/GPS integrated navigation. ISA Trans.
**2015**, 56, 135–144. [Google Scholar] [CrossRef] [PubMed] - Julier, S.J.; Uhlman, J.K. Unscented filtering and nonlinear estimation. Proc. IEEE
**2004**, 92, 401–422. [Google Scholar] [CrossRef] - Lee, D.S.; Chia, N.K.K. A particle algorithm for sequential Bayesian parameter estimation and model selection. IEEE Trans. Signal Process.
**2002**, 50, 326–336. [Google Scholar] [CrossRef] - Boers, Y.; Driessen, J.N. Interacting multiple model particle filter. IEEE Proc. Radar Sonar Navig.
**2003**, 150, 344–349. [Google Scholar] [CrossRef] - Martino, L.; Read, J.; Elvira, V.; Louzada, F. Cooperative Parallel Particle Filters for on-Line Model Selection and Applications to Urban Mobility. Digit. Signal Process.
**2017**, 60, 172–185. [Google Scholar] [CrossRef] - Li, K.L.; Chang, L.B. Robust gaussian particle filter based on modified likelihood function. IET Sci. Meas. Technol.
**2018**, 12, 132–137. [Google Scholar] [CrossRef] - Ienkaran, A.; Simon, H. Cubature Kalman Filters. IEEE Trans. Autom. Control
**2009**, 54, 1254–1269. [Google Scholar] [Green Version] - Pesonen, H.; Piche, R. Cubature-based Kalman filters for positioning. In Proceedings of the 7th Workshop on Positioning Navigation and Communication, Dresden, Germany, 11–12 March 2010; pp. 45–49. [Google Scholar]
- Sun, F.; Tang, L.J. INS/GPS integrated navigation filter algorithm based on cubature Kalman filter. Control Decis.
**2012**, 27, 1032–1036. [Google Scholar] - Zhao, Y.W. Perfomance of Cubature Kalman filter in a GPS/IMU tightly-coupled navigation system. Signal Process.
**2016**, 119, 67–79. [Google Scholar] [CrossRef] - Miao, Z.Y.; Lv, Y.L.; Xu, D.J. Analysis of a variational Bayesian adaptive cubature Kalman filter tracking loop for high dynamic conditions. GPS Solut.
**2017**, 21, 111–122. [Google Scholar] [CrossRef] - Ienkaran, A.; Simon, H.; Hurd, T.R. Cubature Kalman Filtering for Continuous-Discrete Systems: Theory and Simulations. IEEE Trans. Signal Process.
**2010**, 58, 4977–4993. [Google Scholar] - Mu, J.; Cai, Y.L.; Zhang, J.M. Square Root Cubature Particle Filter. Adv. Mater. Res.
**2011**, 219–220, 727–731. [Google Scholar] [CrossRef] - Mu, J.; Cai, Y.L. Iterated cubature Kalman filter and its application. Syst. Eng. Electron.
**2011**, 33, 1454–1458. [Google Scholar] - Sun, Y.; Lu, D.Q.; Chen, Q.J. An improved cubature Kalman filters based on strong tracking. J. Huazhong Univ. Sci. Technol.
**2013**, 41, 451–454. [Google Scholar] - Xu, S.S.; Lin, X.G.; Li, X.F. Strong tracking adaptive square-root cubature Kalman filter algorithm. Acta Electron. Sin.
**2014**, 42, 2394–2400. [Google Scholar] - Zhao, L.Q.; Wang, J.L.; Yu, T.; Jian, H.; Liu, T.J. Design of adaptive robust square-root cubature Kalman filter with noise statistic estimator. Appl. Math. Comput.
**2015**, 256, 352–367. [Google Scholar] [CrossRef] - Cui, N.G.; Zhang, L.; Wang, X.G.; Yang, F.; Lu, B.G. Application of adaptive high-degree cubature Kalman filter in target tracking. Acta Aeronaut. Astronaut. Sin.
**2015**, 36, 3885–3895. [Google Scholar] - Huang, Y.; Wu, L.H.; Sun, F. Robust Cubature Kalman filter based on Huber M estimator. Control Decis.
**2014**, 29, 572–576. [Google Scholar] - Wu, H.; Chen, S.X.; Yang, B.F.; Chen, K. Robust cubature Kalman filter target tracking algorithm based on genernalized M-estiamtion. Acta Phys. Sin.
**2015**, 64, 0218401. [Google Scholar] [CrossRef] - Li, K.L.; Hu, B.Q.; Chang, L.B.; Li, Y. Robust square-root cubature Kalman filter based on Huber’s M-estimation methodology. Proc. Inst. Mech. Eng. Part G J. Aerosp. Eng. June
**2015**, 229, 1236–1245. [Google Scholar] [CrossRef] - Zhang, C.L.; Zhi, R.R.; Li, T.C.; Corchado, J.M. Adaptive M-estimation for Robust Cubature Kalman Filtering. In Proceedings of the Sensor Signal Processing for Defence (SSPD), Edinburgh, UK, 22–23 September 2016; pp. 1–5. [Google Scholar]
- Cui, B.B.; Chen, X.; Xu, Y.; Huang, H.Q.; Liu, X. Performance analysis of improved iterated cubature Kalman filter and its application to GNSS/INS. ISA Trans.
**2017**, 66, 460–468. [Google Scholar] [CrossRef] [PubMed] - Hlinka, O.; Slučiak, O.; Hlawatsch, F.; Djurić, P.M.; Rupp, M. Likelihood Consensus and Its Application to Distributed Particle Filtering. IEEE Trans. Signal Process.
**2012**, 60, 4334–4349. [Google Scholar] [CrossRef] [Green Version] - Zhao, H.D.; Li, Z.P. Ultra-tight GPS/IMU integration based long-range rocket projectile navigation. Def. Sci. J.
**2016**, 66, 64–70. [Google Scholar] [CrossRef] - Wang, Q.T. The Theory and Application Research of Adaptive-Robust UKF for Satellite Integrated Navigation System. Ph.D. Thesis, Huazhong University of Science and Technology, Wuhan, China, 2010. [Google Scholar]
- Gui, Q.; Zhang, J. Robust biased estimation and its application in geodetic adjustments. J. Geod.
**1998**, 72, 430–435. [Google Scholar] [CrossRef] - Wu, F.M.; Yang, Y.X. An extended adaptive Kalman filtering in tight coupled GPS/INS integration. Surv. Rev.
**2010**, 42, 146–154. [Google Scholar] - Song, Q.; Han, J.D. An adaptive UKF algorithm for the state and parameter estimations of a mobile robot. Acta Autom. Sin.
**2008**, 34, 72–79. [Google Scholar] [CrossRef] - Hwang, D.H.; Lim, D.W.; Cho, S.L.; Lee, S.J. Unified Approach to Ultra-Tightly-Coupled GPS/INS Integrated Navigation System. IEEE Aerosp. Electron. Syst. Mag.
**2011**, 3, 30–38. [Google Scholar] [CrossRef] - Niu, X.J.; Ban, Y.L.; Zhang, T.S.; Liu, J.N. Research progress and prospects of GNSS/INS deep integration. Acta Aeronaut. Astronaut. Sin.
**2016**, 37, 2895–2908. [Google Scholar] - Zeng, Q.H.; Meng, Q.; Liu, J.Y.; Feng, S.J.; Wang, H.H. Acquisition and loop control of ultra-tight INS/BeiDou integration system. Optik
**2016**, 127, 8082–8089. [Google Scholar] [CrossRef] [Green Version]

Satellite Signal Tracking Error Terms Corresponded to Four Methods | 0–20 s | 20–40 s | 40–60 s | |
---|---|---|---|---|

Doppler shift errors (Hz) | DLB-UKF | 0.9153 | 1.0574 | |

DLB-CKF | 0.5345 | 0.7375 | 1.5053 | |

DLB-AHCKF | 0.2483 | 0.6547 | 1.3267 | |

DLB-STCKF | 0.4755 | 0.3003 | 0.5004 | |

DLB-RACKF | 0.1125 | 0.1220 | 0.2455 | |

Code phase errors (chips) | DLB-UKF | 0.0128 | 0.0277 | 0.2413 |

DLB-CKF | 0.0123 | 0.0245 | 0.0474 | |

DLB-RACKF | 0.0091 | 0.0060 | 0.0077 |

Algorithms | DLB-UKF | DLB-CKF | DLB-AHCKF | DLB-STCKF | DLB-RACKF |
---|---|---|---|---|---|

Runtime (s) | 1.832600 | 0.7332 | 1.2109 | 1.2973 | 0.9368 |

© 2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Zhao, X.; Li, J.; Yan, X.; Ji, S.
Robust Adaptive Cubature Kalman Filter and Its Application to Ultra-Tightly Coupled SINS/GPS Navigation System. *Sensors* **2018**, *18*, 2352.
https://doi.org/10.3390/s18072352

**AMA Style**

Zhao X, Li J, Yan X, Ji S.
Robust Adaptive Cubature Kalman Filter and Its Application to Ultra-Tightly Coupled SINS/GPS Navigation System. *Sensors*. 2018; 18(7):2352.
https://doi.org/10.3390/s18072352

**Chicago/Turabian Style**

Zhao, Xin, Jianli Li, Xunliang Yan, and Shaowen Ji.
2018. "Robust Adaptive Cubature Kalman Filter and Its Application to Ultra-Tightly Coupled SINS/GPS Navigation System" *Sensors* 18, no. 7: 2352.
https://doi.org/10.3390/s18072352