Algorithm 1 The procedures of the UKF algorithm

1. Initialization (k = 0): ${\widehat{\mathbf{X}}}_0=\mathbf{E}({\mathbf{X}}_{\mathbf{0}})$, ${\mathbf{P}}_0=\mathbf{E}\left[({\mathbf{X}}_0-{\widehat{\mathbf{X}}}_0){({\mathbf{X}}_0-{\widehat{\mathbf{X}}}_0)}^T\right]$ 2. Time update: ${\widehat{\mathbf{X}}}_{k|k-1}=\mathbf{A}{\widehat{\mathbf{X}}}_{k-1|k-1}$,   ${\mathbf{P}}_{k|k-1}=\mathbf{A}{\mathbf{P}}_{k-1|k-1}{\mathbf{A}}^T+\mathbf{\Phi }$ 3. Sigma points and weights calculation: (1) $\{{\mathbf{X}}_{k|k-1}^{(0)}={\widehat{\mathbf{X}}}_{k|k-1},i=0$; ${\mathbf{X}}_{k|k-1}^{(i)}={\widehat{\mathbf{X}}}_{k|k-1}+(\sqrt{{(n+\lambda )\mathbf{P})}_i},i=1\dots n$; ${\mathbf{X}}_{k|k-1}^{(i)}={\widehat{\mathbf{X}}}_{k|k-1}-(\sqrt{{(n+\lambda )\mathbf{P})}_i},i=n+1\dots 2n$ where ${(\mathbf{P})}^T(\mathbf{P})=\mathbf{P}$, ${(\mathbf{P})}_i$ is the ith column of the square root of the matrix $\mathbf{P}$. (2) $w_m^{(0)}=\lambda /n+\lambda$; $w_c^{(0)}=\lambda /n+\lambda +(1-{\tau }^2+\beta )$; $w_m^{(i)}=w_c^{(i)}=\lambda /2(n+\lambda ),i=1\dots 2n$, where $\lambda ={\tau }^2(n+\kappa )$ is the scaling parameter, and the other parameters are generally set to ${\tau }^2=0.01,\beta =0,(n+\lambda )\mathbf{P}$ = 3 4. Measurement update $V_{k|k-1}^{(i)}=h\left[{\mathbf{X}}_{k|k-1}^{(i)}\right]$ ${\overline{V}}_{k|k-1}={\sum }_{i=0}^{2n}{w}_m^{(i)}{V}_{k|k-1}^{(i)}$ $P_{V_k{V}_k}={\sum }_{i=0}^{2n}{w}_c^{(i)}[V_{k|k-1}^{(i)}-{\overline{V}}_{k|k-1}]{[V_{k|k-1}^{(i)}-{\overline{V}}_{k|k-1}]}^T+R$ ${\mathbf{P}}_{{\mathbf{X}}_k{V}_k}={\sum }_{i=0}^{2n}{w}_c^{(i)}[{\mathbf{X}}_{k|k-1}^{(i)}-{\widehat{\mathbf{X}}}_{k|k-1}]{[V_{k|k-1}^{(i)}-{\overline{V}}_{k|k-1}]}^T$ ${\mathbf{K}}_k={\mathbf{P}}_{{\mathbf{X}}_k{V}_k}{P}_{V_k{V}_k}^{-1}$ ${\widehat{\mathbf{X}}}_{k|k}={\widehat{\mathbf{X}}}_{k|k-1}+{\mathbf{K}}_k[V_k-{\overline{V}}_k]$ ${\mathbf{P}}_{k|k}={\mathbf{P}}_{k|k-1}-P_{V_k{V}_k}{\mathbf{K}}_k{\mathbf{K}}_k^T$