Algorithm 1 CKF algorithm

Require:  $k=0$, ${\widehat{\mathit{x}}}_{0|0}$, ${\widehat{\mathit{P}}}_{0|0}$, $\mathit{Q}$, $\mathit{R}$ Ensure:  ${\widehat{\mathit{x}}}_{k|k}$, ${\widehat{\mathit{P}}}_{k|k}$   1: if $k\ge 1$ then   2:    Cholesky decomposition of ${\mathit{P}}_{k-1|k-1}$;   3:    Calculate the Cubature point set ${\mathit{X}}_{i,k-1|k-1}(i=1,2,\dots ,2N)$;   4:    Propagate the Cubature point by using the state equation ${\mathit{X}}_{i,k|k-1}^{*}$;   5:    Calculate the prediction of the state ${\widehat{\mathit{x}}}_{k|k-1}$;   6:    Calculate the prediction of the state covariance matrix ${\mathit{P}}_{k|k-1}$;   7:    Cholesky decomposition of ${\mathit{P}}_{k|k-1}$;   8:    Calculate the Cubature point set again ${\mathit{Y}}_{i,k|k-1}(i=1,2,\dots ,2N)$;   9:    Propagate the Cubature point by using the measurement equation ${\mathit{Y}}_{i,k|k-1}^{*}$;   10:    Calculate the prediction of the measurement ${\widehat{\mathit{y}}}_{k|k-1}$;   11:    Calculate the autocorrelation matrix ${\mathit{P}}_{k|k-1}^{zz}$;   12:    Calculate the cross-correlation matrix ${\mathit{P}}_{k|k-1}^{xz}$;   13:    Calculate the filtering gain ${\mathit{K}}_k$;   14:    Calculate the estimation of the state ${\widehat{\mathit{x}}}_{k|k-1}$;   15:    Calculate the estimation of the state covariance matrix ${\mathit{P}}_{k|k}$;   16: end if