# Adaptive cubature Kalman filter based on the variance-covariance components estimation

- Ya Zhang
^{1}, - Jianguo Wang
^{2}, - Qian Sun
^{3}Email author and - Wei Gao
^{1}

**15**:1

https://doi.org/10.1186/s41445-017-0006-z

© The Author(s) 2017

**Received: **18 October 2016

**Accepted: **21 February 2017

**Published: **9 March 2017

## Abstract

Although the Kalman filter (KF) is widely used in practice, its estimated results are optimal only when the system model is linear and the noise characteristics of the system are already exactly known. However, it is extremely difficult to satisfy such requirement since the uncertainty caused by the inertial instrument and the external environment, for instance, in the aided inertial navigation. In practice almost all of the systems are nonlinear. So the nonlinear filter and the adaptive filter should be considered together. To improve the filter accuracy, a novel adaptive filter based on the nonlinear Cubature Kalman filter (CKF) and the Variance-Covariance Components Estimation (VCE) was proposed in this paper. Here, the CKF was used to solve the nonlinear issue while the VCE method was used for the noise covariance matrix of the nonlinear system real-time estimation. The simulation and experiment results showed that better estimated states can be obtained with this proposed adaptive filter based on the CKF.

## Keyword

## Introduction

In modern navigation and data fusion, the Kalman filter (KF) has become one of the most widely used estimation methods due to its advantages of being more simple and useful (Chen 2012; Han & Wang 2012; Feng et al. 2013; Santos 2015). However, the KF method still has some limitations (Tang et al. 2014; Wang et al. 2010; Wang 2009). For example, it has been developed on the base of linear systems while almost all of the systems are nonlinear actually. So if the KF is applied in practice, the nonlinearity may lead to large errors or even to the filter divergence. Thus, the nonlinear filters have been a hot area of the state estimation.

In practical engineering, two nonlinear methods named the extended Kalman filter (EKF) and the unscented Kalman filter (UKF) are most widely used (Gao et al. 2014; Dini et al. 2011; Vaccarella et al. 2013; Masazade et al. 2012). In the EKF, the nonlinear system can be linearized utilizing the Taylor series expansion for variance propagation while the prediction of the state vector and measurement vector are conducted using the nonlinear system (Gao et al. 2014; Arasaratnam & Haykin 2009). Although this method is used in many nonlinear systems for its simplicity, the precision is limited in the systems with strong nonlinearity and the fussy Jacobian matrix should be calculated which will inevitably increase the computational load. With the Unscented Transformation (UT), the UKF method can approximate the mean and the variance of the Gaussian state distribution using the nonlinear function to avoid the local linearization and the calculation of the Jacobian matrix effectively (Gao et al. 2014; Dini et al. 2011). However, the covariance matrix sometimes is easy non-positive in high-dimensional systems which will lead to filtering divergence.

To solve the above mentioned problems, Arasaratnam et al. proposed the Cubature Kalman filter (CKF) method based on the Cubature Transform to conduct the prediction using the Cubature points which have the same weight (Arasaratnam & Haykin 2009; Arasaratnam & Haykin 2010; Zhang et al. 2015). Similar with UKF, CKF avoids linearization to the nonlinear system by utilizing point sets to predict state vectors and covariance matrix. However, CKF has strictly theoretical derivation based on Bayesian and Spherical-Radial Cubature principles while UKF does not. In addition, the point set in CKF is acquired by integration and all weights are positive values and equal while the weights in UKF are negative values easily in high dimensional systems, which will reduce the filtering accuracy and stability. So the CKF is more widely used in practical to solve the nonlinear problem.

In above filters, the estimations are the optimal only when the mathematic model is exactly known and the system process and measurement noises are the White Gaussian Noise. However, this cannot easily be satisfied in practical aided inertial navigation applications, for example, because of the drift errors of the inertial components, the dynamic errors caused by the carrier’s maneuver, and the existence of the uncertainty in the external environment (Wang et al. 2010; Jin et al. 2014; Von Chong & Caballero 2014). The system model and its associated statistical characteristics of the noises are obtained based on the priori knowledge, which definitely have some errors compared with the true values. So the adaptive filter should be considered to improve the estimated accuracy.

The adaptive filter can estimate and correct the model parameters and the noise characteristics at the same time to continually reduce the errors of the state estimation. Then the estimated accuracy can be improved availably (Hu et al. 2003; Chang & Liu 2015; Mundla et al. 2012).

At present, there are various adaptive filters. One of the outstanding adaptive filters named the Sage-Husa adaptive filter can estimate variance matrix of the process noise or the one of the observation noise in real-time when the variance matrix of the observation noise or the one of the process noise is known (Mundla et al. 2012). In other words, if we want to estimate the process (or observation) noise variance matrix with the Sage-Husa adaptive filter, variance matrix of the observation noise (or the process noise) should be already known. Thus, these two variance matrices cannot be estimated simultaneously. It has limitations when the process and observation noises are both unknown.

By adjusting or restraining the impacts of the current observations on the parameter estimation using the fading factor, an adaptive filter makes full use of the current observations to improve the filtering accuracy (Chang & Liu 2015). Nevertheless, it is difficult to obtain the optimal fading factor and the computational complexity will accordingly be increased as well. Furthermore, the weights of the a-priori reliable information would be decreased to a certain extent if the current observation is abnormal, and then the filtering accuracy would also be degraded.

To estimate the parameters of the system correctly, the weights of different observations should be known. Thus, the Variance-Covariance Components Estimation (VCE) was proposed (Wang et al. 2010; Moghtased-Azar et al. 2014). At present, various VCE methods have widely be used in statistics and geodesy. And one of the famous VCE methods is a posteriori VCE method after Helmert, in which the weights of different observations can be calculated by the adaptive iteration. Since the matrix will be negative sometimes during the calculation, some improved methods were proposed. Wang et al. proposed an adaptive Kalman filter (AKF) based on the VCE method, and verified its effectiveness using the actual experiments (Wang et al. 2010; Wang 2009; Wang et al. 2009). Although the adaptive filter based on the VCE has many advantages in the filter estimations, its application in nonlinear systems has rarely been studied.

Thus, by combining the CKF and the VCE method, an improved adaptive filter was proposed in this paper. Since the adaptive filter and the nonlinear filter were engaged at the same time, this novel method can not only estimate the statistical property of the system’s noise, but also be applied in various nonlinear systems. Hence, the accuracy and the applicability of the new filter can be further improved. The rest of this paper was organized as follows. The description of the CKF and the VCE method was presented in Basic Knowledge. An improved adaptive filter based on the CKF proposed the novel adaptive filter based on the CKF. Numerical examples and experiment along with specific analysis were given in Simulations and Experiments. Conclusions concluded this manuscript.

## Basic knowledge

### Adaptive filter based on the VCE method

The adaptive filter based on the VCE method can estimate the variance components of the process noise and the measurement noise vectors in real time using the residual vectors to decompose the system innovation vector (Wang et al. 2010; Wang 2009; Moghtased-Azar et al. 2014). On the basis of the estimated variance components, the weighting matrices of the process noise and the measurement noise vectors can be adjusted and then their effects on the state vector can be adjusted accordingly.

**x**(

*k*) and

**z**(

*k*) are the state vector and the measurement vector, respectively;

**Φ**(

*k*,

*k*− 1),

**Γ**(

*k*) and

**H**(

*k*) are the state-transition matrix, the coefficient matrix of the process noise vector and the design matrix, respectively;

**w**(

*k*) and

**Δ**(

*k*) denote the process noise vector and the measurement noise vector, respectively. Further,

**w**(

*k*) and

**Δ**(

*k*) are the zero mean Gaussian noises:

where **Q**(*k*) and **R**(*k*) are positive definite matrices.

Thus, the two-step update process of the Kalman filter is as follows:

**G**(

*k*) is the gain matrix and

**d**(

*k*) is the system innovation vector:

When **Q**(*k*) and **R**(*k*) are already known, the estimated state vector is optimal. However, in practice they cannot easily be obtained. So it would be great if they could be estimated in real time in the adaptive filter based on the VCE method.

**Δ**(

*k*), the system process noise

**w**(

*k*) and the noise on the predicted state vector \( \widehat{\mathbf{x}}\left( k/ k-1\right) \) brought by \( \widehat{\mathbf{x}}\left( k\hbox{-} 1\right) \) though the propagation of {

**Δ**(1), ⋯,

**Δ**(

*k*− 1)} and {

**w**(1), ⋯,

**w**(

*k*‐ 1)}. Thus, according to noise sources, we can define three independent (pseudo-)observation groups as follows (Wang 2009):

where l_{
x
}(*k*) is the pseudo-observation related to the predicted state vector, l_{
w
}(*k*) is the pseudo-observation related to the system process noise, and l_{z}(*k*) is the pseudo-observation related to the observation noise.

So we can estimate the covariance matrix of the system as long as we calculate the covariance matrix of the residual vectors.

Here the innovation vector is epochwise projected into three residual vectors associated with the three groups of the measurements. Hence, we can estimate the variance factors.

**l**

_{ z }(

*k*) and

**l**

_{ w }(

*k*) are uncorrelated, so both of the

**R**(

*k*) and

**Q**(

*k*) become diagonal. In this case, the redundancy index of each measurement noise factor is given by (Wang 2009)

*k*, the individual variance factors of l

_{ z }(

*k*) can be calculated by:

**l**

_{ w }(

*k*) and the variance matrix

**Q**(

*k*) can be calculated by the equations (17) and (18):

### CKF algorithm

Since the CKF is not only simple and easy to be implemented, but also high precise and convergent well, it is widely used in nonlinear estimations.

where x_{
k
} and z_{
k
} are the state vector and the measurement vector, respectively, f(⋅) and h(⋅) are the nonlinear state and measurement vector functions; **w** ∼ N(0, Q) and **Δ** ∼ N(0, R) represent the process noise and the measurement noise vectors, respectively.

*n*Cubature points which are equal of weight were selected to calculate the Gaussian distribution, and then the CKF can be implemented through the time and measurement updates. And the cubature points are set as:

where *n* is the dimension of the state vector.

And the estimation process is as below (Arasaratnam & Haykin 2009; Arasaratnam & Haykin 2010):

where d_{
k
} = z_{
k
} − ẑ_{
k|k − 1} is the system innovation vector of the nonlinear system.

## Methods

### An improved adaptive filter based on the CKF

To solve the nonlinear problem and improve the stochastic model simultaneously, we here proposed a new improved adaptive filter by combining the CKF and the VCE adaptive method.

Here, the covariance factors and the variance matrices for the nonlinear system can be calculated after the equations (15) ~ (18).

## Results and discussion

### Simulations and experiments

In order to show the advantages of the improved CKF, this section presents the solution comparison between the normal and the improved CKF methods through two nonlinear models: a bearing-only tracking model and a target tracking model, using the simulated data against their known reference solutions. A further experiment with the real data was also conducted and summarized.

### Bearing-only tracking method

**w**

_{ k }~

*N*(

**0**,

**Q**) with

*k*), sin(

*k*)) at

*k*moment. And the measurement equation is as follows:

where the measurement noise is *v*
_{
k
} ~ N(0, *R*) with *R* = 0.005.

In order to evaluate the performance of the improved CKF, the PDFs of the estimated state errors resulted from the simulation data were calculated (Figs. 2 and 3), which showed that the PDF with the improved CKF much better fits the normal distribution which tells that the estimated states were more close to the true states than the once from the normal CKF. So the proposed novel method can estimate the stochastic model more accurately. By the way, from the results shown in Fig. 1, the MSEs of estimated states with the normal CKF and the improved CKF are not that different, but the results from the improved CKF method behaved better, although one should not expect to have the smaller variances if the errors are distributed more reasonably in general.

### Target tracking method

*x*,

*y*), \( \left(\overset{.}{x},\overset{.}{y}\right) \) and Ω are the position, the velocity and the angular velocity of the carrier, respectively, and

*T*

_{ Δ }represents the sampling interval. The process noise is r

_{ k }∼ N(0, Q) with

_{ k }and the azimuth

*θ*

_{ k }from the observation system to the carrier. So, the observation equation is:

where v_{
k
} is the observation noise vector, v_{
k
} ∼ *N*(0, R) with \( \operatorname{R}= diag\left(\left[\begin{array}{cc}\hfill {\sigma}_r^2\hfill & \hfill {\sigma}_{\theta}^2\hfill \end{array}\right]\right) \).

Simulation Parameters

Parameters | Set values |
---|---|

| 1 |

| 0.1 |

| 1.75 × 10 |

| 10 |

| \( \sqrt{10} mrad \) |

Q | 3 Q |

R | 3 R |

where *n*
_{exp} is the times of the Monte Carlo simulations.

From above, the RMSEs are almost the same with these two methods. And the PDFs with the improved CKF are much more fits the standardized normal distribution curve too, which is similar with Bearing-only tracking method. Thus, the feasibility and the superiority of the improved adaptive filter based on the CKF can be verified.

### Experiment and analysis

Performance Parameters of FOG

Parameters | Settings |
---|---|

Dynamic Range | ± 100 |

Bias Stability | ≤ 0.01 |

Bias Repeatability | ≤ 0.01 |

Angle Random Walk (ARW) | ≤ 0.005 |

Scale Factor Error | ≤ 20 |

Temperature Range | −40 °C ~ +60 °C |

Performance Parameters of accelerometer

Parameters | Settings |
---|---|

Measuring Range | ± 20 |

Bias Stability | ≤ 0.05 |

Bias Repeatability | ≤ 0.05 |

Velocity Random Walk (VRW) | ≤ 0.01 |

Resolution | ≤ 5 |

Scale Factor Error | ≤ 30 |

From the above figure, we can see that the trajectory can be both estimated availably with the improved CKF and normal CKF. Since we don’t have the true trajectory, the PDFs cannot provide here. It still can verify the feasibility of the improved adaptive filter based on CKF.

## Conclusions

This paper proposed a novel adaptive filter based on the VCE adaptive method and the nonlinear CKF method to improve the stochastic model due to the unknown system noises in the nonlinear system. By utilizing the frame structure of the nonlinear CKF, this proposed method can estimate the variances of the process noises and the measurement noises through variance component estimations in real time, describing the system more accurately. And the simulation and experiment results and the comparisons showed that the noise’s characteristics can be estimated effectively with the improved adaptive filter based on the CKF and VCE. Therefore, the effectiveness and superiority of the improved adaptive filter can be proved.

## Declarations

### Competing interests

The authors declare that they have no competing interests.

### Authors’ contribution

YZ wrote this paper and analyzed the results of the experiments; JW proposed the initial idea and revised this manuscript; QS carried out the simulations and revised this manuscript; WG conceived of the experiments. All authors read and approved the final manuscript.

**Open Access**This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.

## Authors’ Affiliations

## References

- Arasaratnam I, Haykin S (2009) Cubature Kalman filter. IEEE Trans Autom Control 54(6):1254–1269View ArticleGoogle Scholar
- Arasaratnam I, Haykin S (2010) Cubature Kalman filtering for continuous-dicrete systems: theory and simulations. IEEE Trans Signal Process 58(10):4977–4993View ArticleGoogle Scholar
- Chang G, Liu M (2015) An adaptive fading Kalman filter based on Mahalanobis distance. J Aerosp Eng 229(6):1114–1123Google Scholar
- Chen SY (2012) Kalman filter for robot vision: a survey. IEEE Trans Ind Electron 59(11):4409–4420View ArticleGoogle Scholar
- Dini DH, Mandic DP, Julier SJ (2011) A widely linear complex unscented Kalman filter. IEEE Signal Processing Letters 18(11):623–626View ArticleGoogle Scholar
- Feng J, Wang Z, Zeng M (2013) Distributed weighted robust Kalman filter fusion for uncertain systems with autocorrelated and cross-correlated noises. Information Fusin 4(1):78–86View ArticleGoogle Scholar
- Gao W, Zhang Y, Wang J (2014) A straapdown inertial navigation system/beidou/doppler velocity Log integrated navigation algorithm based on a cubature Kalman filter. Sensors 14(1):1511–1527View ArticleGoogle Scholar
- Han S, Wang J (2012) Integrated GPS/INS navigation system with dual-rate Kaman filter. GPS Solutions 16(3):389–404View ArticleGoogle Scholar
- Hu C, Wu C, Chen Y, Liu D (2003) Adaptive Kalman filter for vejicle navigation. J Global Position System 2(1):42–47View ArticleGoogle Scholar
- Jin M, Zhao J, Jin J, Yu G, Li W (2014) The adaptive Kalman filter based on fuzzy logic for inertial motion capture system. Measurement 49:196–204View ArticleGoogle Scholar
- Julier SJ, Uhlman JK (1997) A New extension of the Kalman filter to nonlinear systems. Proceedings of the Society of Photo-Optical Instrumentation Engineers 3068:182–193Google Scholar
- Kotecha JH, Djuric PA (2003) Gaussian particle filtering. IEEE Trans Signal Process 51(10):2592–2601View ArticleGoogle Scholar
- Masazade E, Fardad M, Varshney PK (2012) Sparsity-promoting extended Kalman filtering for target tracking in wireless sensor networks. IEEE Signal Processing Letters 19(12):845–848View ArticleGoogle Scholar
- Moghtased-Azar K, Tehranchi R, Amiri-Simkooei AR (2014) An alternative method for Non-negative estimation of variance components. J Geod 88(5):47–439View ArticleGoogle Scholar
- Mundla N, Rangababu P, Samrat L et al (2012) A modified sage-husa adaptive Kalman filter for denoising fiber optical gyroscope signal. In Proceedings of the 2012 Annual IEEE India Conference (INDICON), Kochi, pp 1266–1271Google Scholar
- Santos MCP (2015) Estimating and Controlling UAV Position using RGB-D/IMU data Fusion with Decentralized Information/Kalman Filter. Proceedings in the 2015th IEEE International Conference on Industrial Technology (ICIT), Seville, pp 22–239Google Scholar
- Tang C, Ao Z, Zhang K, Wang Y (2014) A multi-sensor data fusion algorithm based on improved Kalman filter. Mechatronics and Automatic Control Systems 237:219–229View ArticleGoogle Scholar
- Vaccarella A, de Momi E, Enquobahrie A, Ferrigno G (2013) Unscented Kalman filter based sensor fusion for robust optical and electromagnetic tracking in surgical navigation. IEEE Trans Instrum Meas 62(7):2067–2081View ArticleGoogle Scholar
- Von Chong A, Caballero R (2014) Adaptive Kalman filtering for the estimation of orientation and displacement in submarine systems. Proceedings of the 2014 IEEE Central America and Panama Convention (CONCAPAN XXXIV), Panama city, pp 1–6Google Scholar
- Wang J (2009) Reliability analysis in Kalman filtering. J Global Positioning Systems 8(1):101–111View ArticleGoogle Scholar
- Wang J, Gopaul N, Scherzinger B (2009) Simplified algorithms of variance component estimation for static and kinematic GPS single point positioning. J Global Positioning Systems 8(1):43–52View ArticleGoogle Scholar
- Wang J, Gopaul SN, Guo J (2010) Adaptive Kalman filtering based on posteriori variance-covariance components estimation. CPGPS 2010 Technical Forum, Shanghai, pp 1–11Google Scholar
- Zhang C, Zhao M, Yu X, Cui M, Zhou Y, Wang X (2015) Cubature Kalman filter based on strong tracking. The Proceedings of the Third International Conference on Communications, Signal Processing, and Systems 322:131–138View ArticleGoogle Scholar