# Robust Estimation in Continuous–Discrete Cubature Kalman Filters for Bearings-Only Tracking

^{1}

^{2}

^{3}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Continuous–Discrete System Model and Square-Root Continuous–Discrete Cubature Kalman Filter Algorithm for Target Tracking

#### 2.1. Continuous–Discrete System Model

#### 2.2. Square-Root Continuous–Discrete Cubature Kalman Filter Algorithm

Algorithm 1: SRCD-CKF. |

Time updateStep 1 Expectation and covariance matrix initialization: $\widehat{x}\left({t}_{k}\right)={\widehat{x}}_{k-1\left|k-1\right.}$, $P\left({t}_{k}\right)={P}_{k-1\left|k-1\right.}$.Step 2.1 Covariance decomposition: $P({t}_{k})=S({t}_{k}){S}^{\mathrm{T}}({t}_{k})$.Step 2.2 Calculate the state cubature point: ${X}_{i}({t}_{k})=S({t}_{k}){\xi}_{i}+\widehat{x}({t}_{k})$.Step 2.3 State cubature point propagation: $\left\{\begin{array}{l}\widehat{x}\prime (t)=F(X(t))\epsilon \\ \\ S\prime (t)=S(t)\Phi (B(t))\end{array}\right.$.Step 2.4 Calculate the state prediction value: ${\widehat{x}}_{k\left|k-1\right.}=\left({X}_{1}^{*}({t}_{k})+\cdots +{X}_{2n}^{*}({t}_{k})\right)/2n$.Step 2.5 Calculate the predicted square-root covariance: |

$S({t}_{k})=\left[\begin{array}{cccc}{S}_{1}({t}_{k})& {S}_{2}({t}_{k})& \cdots & {S}_{2n}({t}_{k})\end{array}\right],$ |

$\mathrm{where}\text{}{S}_{i}({t}_{k})$$\text{}\mathrm{represents}\text{}a\text{}\mathrm{sin}\mathrm{gle}\text{}\mathrm{column}\text{}\mathrm{vector},\text{}{S}_{i}({t}_{k})=\left({X}_{i}^{*}({t}_{k})-{\widehat{x}}_{k\left|k-1\right.}\right)/\sqrt{\mathrm{n}}$.Measurement updateStep 3.1 Calculate the state cubature point: ${S}_{k|k-1}^{}=S({t}_{k})$, ${X}_{i,k\left|k-1\right.}={S}_{k|k-1}^{}{\xi}_{i}+{\widehat{x}}_{k\left|k-1\right.}$.Step 3.2 Measure the spread of cubature points: ${Z}_{i,k\left|k-1\right.}=h({X}_{i,k\left|k-1\right.},k)$.Step 3.3 Calculate the predicted value of the measurement: ${\widehat{z}}_{k\left|k-1\right.}=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}{Z}_{i,k\left|k-1\right.}}$.Step 3.4 Construct the measurement weighted center matrix: |

${Z}_{k\left|k-1\right.}=\frac{1}{\sqrt{2n}}\left[{Z}_{1,k\left|k-1\right.}-{\widehat{z}}_{k\left|k-1\right.},\cdots ,{Z}_{2n,k\left|k-1\right.}-{\widehat{z}}_{k\left|k-1\right.}\right].$ |

Step 3.5 Calculate the innovation covariance matrix: ${P}_{\mathit{zz},k\left|k-1\right.}={Z}_{k\left|k-1\right.}^{}{Z}_{k\left|k-1\right.}^{T}+{R}_{k}^{}$.Step 3.6 Construct the state weighted center matrix: |

${X}_{k\left|k-1\right.}=\frac{1}{\sqrt{2n}}\left[{X}_{1,k\left|k-1\right.}-{\widehat{x}}_{k\left|k-1\right.},\cdots ,{X}_{2n,k\left|k-1\right.}-{\widehat{x}}_{k\left|k-1\right.}\right].$ |

Step 3.7 Calculate the cross covariance matrix: ${P}_{\mathit{xz},k\left|k-1\right.}^{}={X}_{k\left|k-1\right.}^{}{Z}_{k\left|k-1\right.}^{\mathrm{T}}$.Step 3.8 The continuous–discrete cubature gain is: ${K}_{k}^{}={P}_{\mathit{xz},k|k-1}^{}{P}_{\mathit{zz},k|k-1}^{-1}$.Step 3.9 Calculate the state estimate: ${\widehat{x}}^{}{}_{k\left|k\right.}={\widehat{x}}_{k\left|k-1\right.}+{K}_{k}^{}({z}_{k}-{\widehat{z}}_{k\left|k-1\right.})$.Step 3.10 Update the covariance matrix: ${P}_{k|k}^{}={P}_{k|k-1}^{}-{K}_{k}^{}{P}_{zz,k|k-1}{K}_{k}^{\mathrm{T}}$. |

**Remark**

**1.**

## 3. Robust Square-Root Continuous–Discrete Cubature Kalman Filter Algorithms

#### 3.1. Robust SRCD-CKF with Huber’s Method

Algorithm 2: HRSRCD-CKF. |

Step 1 Repeat Step 1 of the SRCD-CKF algorithm.Steps 2.1–3.7 are equivalent to Steps 2.1–3.7 of the SRCD-CKF algorithm.Step 3.8 The innovation covariance matrix is redefined: ${\overline{P}}_{\mathit{zz},k\left|k-1\right.}={Z}_{k\left|k-1\right.}^{}{Z}_{k\left|k-1\right.}^{\mathrm{T}}+{\overline{\mu}}_{k}^{-1}{R}_{k}^{}$.Step 3.9–3.11 are equivalent to Steps 3.8–3.10 of the SRCD-CKF algorithm. |

Algorithm 3: MRSRCD-CKF. |

Step 1 Repeat Step 1 of the SRCD-CKF algorithm.Steps 2.1–3.7 are equivalent to Steps 2.1–3.7 of the SRCD-CKF algorithm.Steps 3.8–3.11 are equivalent to Steps 3.8–3.11 of the HRSRCD-CKF algorithm, and ${\overline{\mathsf{\mu}}}_{k}$ is calculated by Equation (16). |

#### 3.2. RSRCD-MCCKF Algorithm Based on Maximum Correntropy Criterion

Algorithm 4: RSRCD-MCCKF. |

Step 1 Repeat Step 1 of the SRCD-CKF algorithm.Steps 2.1–3.7 are equivalent to Steps 2.1–3.7 of the SRCD-CKF algorithm.Step 3.8 Calculate ${\overline{H}}_{k}$$,\text{}{\overline{R}}_{k}$$,\text{}{\mathrm{L}}_{k}$ using Equations (21), (22), and (31).Step 3.9 Obtain the new Kalman filter gain ${\stackrel{=}{K}}_{k}$ using Equation (29).Step 3.10 Complete the estimation of the state value and the update of the error covariance matrix using Equations (28) and (30). |

#### 3.3. VBSRCD-CKF Algorithm Based on Variational Bayes Criterion

Algorithm 5: VBSRCD-CKF. |

Step 1 At Step 1 of the SRCD-CKF algorithm, the initialization of ${v}_{k}$ and ${V}_{k}$ are included. |

Steps 2.1–2.5 are equivalent to Steps 2.1–2.5 of the SRCD-CKF algorithm. |

Step 2.6 Calculate the parameters of the IW distribution of measurement noise covariance: ${v}_{k\left|k-1\right.}=\mathsf{\rho}\left({v}_{k-1}-n-1\right)+n+1$, ${V}_{k\left|k-1\right.}=C{V}_{k-1}{C}^{T}$, where $\mathsf{\rho}$ is a scale factor that 0 < $\mathsf{\rho}$ ≤ 1 and $C$ is a matrix that $0<\left|C\right|\le 1$ with a reasonable choice for the matrix $C=\sqrt{\mathsf{\rho}}{I}_{d}$. ${I}_{d}$ is an identity matrix, and d is the dimension of the measurement. |

Step 3.1 Before calculating the state cubature point, first let ${\widehat{x}}_{k}^{\left(0\right)}={\widehat{x}}_{k\left|k-1\right.}$, ${\mathit{V}}_{k}^{\left(0\right)}}={V}_{k\left|k-1\right.$, and ${v}_{k}=1+{v}_{k\left|k-1\right.}$. |

Steps 3.2–3.8 are equivalent to Steps 3.1–3.7 of the SRCD-CKF algorithm. |

Step 3.9 For j = 1:M, iterate the following steps (M is the times of algorithm iteration). |

Step 3.9.1 Calculate the measurement noise covariance matrix: ${R}_{k}^{\left(j\right)}={\left({v}_{k}-n-1\right)}^{-1}{V}_{k}^{\left(j-1\right)}$. |

Step 3.9.2 Update the innovation covariance matrix: ${P}_{\mathit{zz},k\left|k-1\right.}^{\left(j\right)}={Z}_{k\left|k-1\right.}^{}{Z}_{k\left|k-1\right.}^{\mathrm{T}}+{R}_{k}^{\left(j\right)}$. |

Step 3.9.3 Calculate the continuous–discrete filter gain: ${K}_{k}^{\left(j\right)}={P}_{\mathit{xz},k|k-1}^{}{\left({P}_{\mathit{zz},k|k-1}^{\left(j\right)}\right)}^{-1}$. |

Step 3.9.4 Calculate the state estimate and update the covariance: |

${\widehat{x}}^{\left(j\right)}{}_{k}={\widehat{x}}_{k\left|k-1\right.}+{K}_{k}^{\left(j\right)}({z}_{k}-{\widehat{z}}_{k\left|k-1\right.}),$ |

${P}_{k}^{\left(j\right)}={P}_{k|k-1}^{}-{K}_{k}^{\left(j\right)}{\mathit{P}}_{zz,k|k-1}^{\left(j\right)}{\left({K}_{k}^{\left(j\right)}\right)}^{\mathrm{T}}.$ |

Step 3.9.5 Calculate the updated parameter of the IW distribution of measurement noise covariance: |

${X}_{i,k}^{\left(j\right)}={S}_{k}^{\left(j\right)}{\xi}_{i}+{\widehat{x}}_{k}^{\left(j\right)},$ |

${V}_{k}^{\left(j\right)}={V}_{k\left|k-1\right.}+\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}\left({z}_{k}-h({X}_{i,k}^{\left(j\right)},k)\right){\left({z}_{k}-h({X}_{i,k}^{\left(j\right)},k)\right)}^{\mathrm{T}}}.$ |

Step 3.10 Until j = M, output ${\widehat{x}}_{k\left|k\right.}={\widehat{x}}^{\left(M\right)}$, ${P}_{k\left|k\right.}={P}_{k}^{\left(M\right)}$, and ${V}_{k}={V}_{k}^{\left(M\right)}$. |

## 4. Numerical Simulation

#### 4.1. Gaussian Noise

#### 4.2. Gaussian Mixture Noise

#### 4.3. Gaussian Noise Together with Shot Noise

#### 4.4. Computational Complexity

## 5. Conclusions

- (1)
- The effectiveness of the algorithms in this paper is evaluated through a remote distance passive target tracking scenario, and the simulation results demonstrate the better environmental adaptability of the proposed algorithms in solving continuous-discrete robust problems. What’s more, this paper aims to solve the slow tracking problem in remote distance passive target tracking, and the proposed algorithms can also be used in other navigation domains.
- (2)
- As a common tool for data processing, Huber’s estimation has a wide range of applications. The RSRCD-MCCKF algorithm and the VBSRCD-CKF algorithm proposed in this paper are better than the Huber’s estimator, which provides an alternative means for the field of robust estimation.
- (3)
- In the simulation results, we can see that the filtering performance of the algorithm without robustness is very poor. Therefore, when the system’s data are contaminated by outliers, the relevant robust estimation method is indispensable.

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Conflicts of Interest

## References

- He, R.K.; Chen, S.X.; Wu, H.; Chen, K.; Liu, J. Adaptive Covariance Feedback Cubature Kalman Filtering for Continuous-Discrete Bearings-Only Tracking System. IEEE Access
**2019**, 7, 2686–2694. [Google Scholar] [CrossRef] - Li, L.Q.; Wang, X.L.; Liu, Z.X.; Xie, W.X. Auxiliary truncated unscented Kalman filter for bearings-only maneuvering target tracking. Sensors
**2017**, 17, 972. [Google Scholar] [CrossRef] [PubMed] - Crouse, D. Basic tracking using nonlinear continuous-time dynamic models. IEEE Aerosp. Electron. Syst. Mag.
**2015**, 30, 4–41. [Google Scholar] [CrossRef] - Arasaratnam, I.; Haykin, S.; Hurd, T.R. Cubature Kalman Filtering for Continuous-Discrete Systems: Theory and Simulation. IEEE Trans. Signal Processing
**2010**, 58, 4977–4993. [Google Scholar] [CrossRef] - Crouse, D.F. Cubature Kalman Filters for Continuous-Time Dynamic Models Part I: Solutions Discretizing the Langevin Equation. In Proceedings of the 2014 IEEE Radar Conference, Cincinnati, OH, USA, 19–23 May 2014. [Google Scholar]
- Crouse, D.F. Cubature Kalman Filters for Continuous-Time Dynamic Models Part Ⅱ: A Solution Based on Moment Matching. In Proceedings of the 2014 IEEE Radar Conference, Cincinnati, OH, USA, 19–23 May 2014. [Google Scholar]
- Kulikov, G.Y.; Kulikova, M.V. Accurate Numerical Implementation of the Continuous-Discrete Extended Kalman Filter. IEEE Trans. Autom. Control
**2014**, 59, 273–279. [Google Scholar] [CrossRef] - Wang, J.L.; Zhang, D.X.; Shao, X.W. New version of continuous-discrete cubature Kalman filtering for nonlinear continuous-discrete systems. ISA Trans.
**2019**, 91, 174–783. [Google Scholar] [CrossRef] - Kulikov, G.Y.; Kulikova, M.V. NIRK-based Accurate Continuous-Discrete Extended Kalman filters for Estimating Continous-Time Stochastic Target Tracking Models. J. Comput. Appl. Math.
**2016**, 316, 260–270. [Google Scholar] [CrossRef] - He, R.K.; Chen, S.X.; Wu, H.; Chen, K. Stochastic feedback based continuous-discrete cubature Kalman filtering for bearings-only tracking. Sensors
**2018**, 18, 1959. [Google Scholar] [CrossRef] - Gordon, N.J.; Salmond, D.J.; Smith, A.F.M. Novel Approach to Nonlinear/Non-Gaussian Bayesian State Estimation. Radar Signal Processing IEE Proc. Part F
**1993**, 140, 107–113. [Google Scholar] [CrossRef] - Arulampalam, M.S.; Maskell, S.; Gordon, N.; Clapp, T. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking. IEEE Trans. Signal Processing
**2002**, 50, 174–188. [Google Scholar] [CrossRef] - Meinhold, R.J.; Singpurwalla, N.D. Robustification of Kalman filter methods. J. Am. Stat. Assoc.
**1989**, 84, 479–486. [Google Scholar] [CrossRef] - Huber, P.J.; Ronchetti, E.M. Robust Statistics, 2nd ed.; Wiley: Hoboken, NJ, USA, 2009. [Google Scholar]
- Jureckova, J.; Picek, J.; Schindler, M. Robust Statistical Methods with R, 2nd ed.; CRC Press: Boca Raton, FL, USA, 2019. [Google Scholar]
- Chang, J.B.; Liu, M. M-estimator-based robust Kalman filter for systems with process modeling errors and rank deficient measurement models. Nonlinear Dyn.
**2015**, 80, 1431–1449. [Google Scholar] [CrossRef] - Wu, H.; Chen, S.X.; Chen, K.; Yang, B.F. Robust cubature Kalman filter target tracking algorithm based on genernalized M-estiamtion. Acta Phys. Sin.
**2015**, 64, 456–463. [Google Scholar] [CrossRef] - Wu, H.; Chen, S.X.; Chen, K.; Yang, B.F. Robust Derivative-Free Cubature Kalman Filter for Bearings-Only Tracking. J. Guid. Control. Dyn.
**2016**, 39, 1865–1870. [Google Scholar] [CrossRef] - Särkkä, S.; Nummenmaa, A. Recursive noise adaptive Kalman filtering by variational Bayesian approximations. IEEE Trans. Autom. Control.
**2009**, 54, 596–600. [Google Scholar] [CrossRef] - Wang, G.Q.; Li, N.; Zhang, Y.G. Maximum correntropy unscented Kalman and information filters for non-Gaussian measurement noise. J. Frankl. Inst.
**2017**, 354, 8659–8677. [Google Scholar] [CrossRef] - Liu, X.; Qu, H.; Zhao, J.; Yue, P.; Wang, M. Maximum correntropy unscented Kalman filter for spacecraft relative state estimation. Sensors
**2016**, 16, 1530. [Google Scholar] [CrossRef] - Liu, X.; Chen, B.; Xu, B.; Wu, Z.; Honeine, P. Maximum correntropy unscented filter. Int. J. Syst. Sci.
**2017**, 48, 1607–1615. [Google Scholar] [CrossRef] - Särkkä, S.; Solin, A. On continuous-discrete cubature Kalman filtering. IFAC Proc. Vol.
**2012**, 45, 1221–1226. [Google Scholar] [CrossRef] - Särkkä, S. On unsented Kalman filtering for state estimation of continuous-time nonlinear systems. IEEE Trans. Autom. Control
**2007**, 52, 1631–1641. [Google Scholar] [CrossRef] - Chen, B.D.; Liu, X.; Zhao, H.Q.; Principe, J.C. Maximum correntropy Kalman filter. Automatica
**2017**, 76, 70–77. [Google Scholar] [CrossRef] - Särkkä, S.; Hartikainen, J. Non-linear noise adaptive Kalman filtering via variational Bayes. In Proceedings of the IEEE International Workshop on Machine Learning for Signal Processing (MLSP) Conference, Southampton, UK, 22–25 September 2013. [Google Scholar]
- He, R.K.; Chen, S.X.; Wu, H.; Zhang, F.Z.; Chen, K. Efficient extended cubature Kalman filtering for nonlinear target tracking. Int. J. Syst. Sci.
**2020**, 52, 392–406. [Google Scholar] [CrossRef]

**Figure 2.**The RMSE comparison of each algorithm under Gaussian noise. (

**a**) $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{p}\mathrm{o}\mathrm{s}}$ of each algorithm under Gaussian noise; (

**b**) $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{v}\mathrm{e}\mathrm{l}}$ of each algorithm under Gaussian noise.

**Figure 3.**The RMSE comparison of each algorithm under non-Gaussian noise. (

**a**) $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{p}\mathrm{o}\mathrm{s}}$ of each algorithm under non-Gaussian noise; (

**b**) $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{v}\mathrm{e}\mathrm{l}}$ of each algorithm under non-Gaussian noise.

**Figure 4.**The RMSE comparison of each algorithm under abnormal measurement. (

**a**) $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{p}\mathrm{o}\mathrm{s}}$ of each algorithm under abnormal measurement; (

**b**) $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{v}\mathrm{e}\mathrm{l}}$ of each algorithm under abnormal measurement.

Algorithms | $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{p}\mathrm{o}\mathrm{s}}$$\text{}\left[\mathrm{km}\right]$ | $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{v}\mathrm{e}\mathrm{l}}$$\text{}[\mathrm{km}\cdot {\mathrm{s}}^{-1}]$ |
---|---|---|

RSRCD-MCCKF ($\mathsf{\sigma}=0.1$) | — | — |

RSRCD-MCCKF ($\mathsf{\sigma}=1$) | 0.0528 | 0.000205 |

RSRCD-MCCKF ($\mathsf{\sigma}=2$) | 0.0449 | 0.000188 |

RSRCD-MCCKF ($\mathsf{\sigma}=5$) | 0.0482 | 0.000179 |

RSRCD-MCCKF ($\mathsf{\sigma}=10$) | 0.0645 | 0.000211 |

RSRCD-MCCKF ($\mathsf{\sigma}=50$) | 0.0771 | 0.000239 |

SRCD-CKF | 0.0784 | 0.000247 |

Algorithms | $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{p}\mathrm{o}\mathrm{s}}$$\text{}\left[\mathrm{km}\right]$ | $\mathrm{R}\mathrm{M}\mathrm{S}{\mathrm{E}}_{\mathrm{v}\mathrm{e}\mathrm{l}}$$\text{}[\mathrm{km}\cdot {\mathrm{s}}^{-1}]$ |
---|---|---|

RSRCD-MCCKF ($\mathsf{\sigma}=2$) | — | 0.0349 |

RSRCD-MCCKF ($\mathsf{\sigma}=3$) | 1.0829 | 0.0043 |

RSRCD-MCCKF ($\mathsf{\sigma}=5$) | 0.4716 | 0.0014 |

RSRCD-MCCKF ($\mathsf{\sigma}=7$) | 0.4813 | 0.0014 |

RSRCD-MCCKF ($\mathsf{\sigma}=10$) | 0.5375 | 0.0016 |

SRCD-CKF | 0.6679 | 0.0019 |

Algorithms | Computation Time (Scenario 4.1) | Computation Time (Scenario 4.3) |
---|---|---|

SRCD-CKF | 1 | 1 |

HRSRCD-CKF | 1.029 | 1.034 |

MRSRCD-CKF | 1.005 | 1.018 |

RSRCD-MCCKF | 1.076 | 1.080 |

VBSRCD-CKF | 1.068 | 1.071 |

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2022 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 (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Hu, H.; Chen, S.; Wu, H.; He, R.
Robust Estimation in Continuous–Discrete Cubature Kalman Filters for Bearings-Only Tracking. *Appl. Sci.* **2022**, *12*, 8167.
https://doi.org/10.3390/app12168167

**AMA Style**

Hu H, Chen S, Wu H, He R.
Robust Estimation in Continuous–Discrete Cubature Kalman Filters for Bearings-Only Tracking. *Applied Sciences*. 2022; 12(16):8167.
https://doi.org/10.3390/app12168167

**Chicago/Turabian Style**

Hu, Haoran, Shuxin Chen, Hao Wu, and Renke He.
2022. "Robust Estimation in Continuous–Discrete Cubature Kalman Filters for Bearings-Only Tracking" *Applied Sciences* 12, no. 16: 8167.
https://doi.org/10.3390/app12168167