# Iterated Orthogonal Simplex Cubature Kalman Filter and Its Applications in Target Tracking
## 4 Iterated Orthogonal Simplex Cubature Kalman Filter
### Iterated Measurement Update Process  
The prior state \(\hat{\mathbf{x}}_{k}^{-}\) and covariance matrix \(\mathbf{P}_{k}^{-}\) are used in the measurement update process to calculate the posterior state \(\hat{\mathbf{x}}_{k}^{+}\) and covariance matrix \(\mathbf{P}_{k}^{+}\), which are revised using the latest measurement \(\mathbf{z}_{k}\) via Kalman gain \(\mathbf{K}_{k}\). This means an approximation accuracy of \(\hat{\mathbf{x}}_{k}^{+}\) to the real state is bound to be better than \(\hat{\mathbf{x}}_{k}^{-}\), and the filtering accuracy will be improved if we can use \(\hat{\mathbf{x}}_{k}^{+}\) and \(\mathbf{P}_{k}^{+}\) directly in the measurement update. Therefore, to iteratively make use of the measurement information, the Gauss-Newton iteration is introduced into the updating process of the mean and covariance of the state. Under the assumption of Gaussian variable independence, there exist \(\mathbf{x}_{k}:N(\hat{\mathbf{x}}_{k}^{-},\mathbf{P}_{k}^{-})\) and \(\mathbf{z}_{k}:N(\mathbf{h}(\mathbf{x}_{k}),\mathbf{R}_{k})\), and the likelihood function of \(\mathbf{x}_{k}\) and \(\mathbf{z}_{k}\) is defined as follows [33].  
\[p(\mathbf{x}_{k}|\mathbf{z}_{k})=C\exp\biggl{\{}-\frac{1}{2}\Bigl{[}\bigl{(}\mathbf{x}_{k }-\hat{\mathbf{x}}_{k}^{-}\bigr{)}^{\mathsf{T}}\bigl{(}\mathbf{P}_{k}^{-}\bigr{)}^{-1} \bigl{(}\mathbf{x}_{k}-\hat{\mathbf{x}}_{k}^{-}\bigr{)}+(\mathbf{z}_{k}-\mathbf{h}(\mathbf{x}_{k} ))^{\mathsf{T}}\mathbf{R}_{k}^{-1}(\mathbf{z}_{k}-\mathbf{h}(\mathbf{x}_{k}))\Bigr{]}\biggr{\}} \tag{26}\]  
where \(C\) is a constant, and the cost function is defined as follows.  
\[J(\mathbf{x}_{k})=\bigl{(}\mathbf{x}_{k}-\hat{\mathbf{x}}_{k}^{-}\bigr{)}^{\mathsf{T}} \bigl{(}\mathbf{P}_{k}^{-}\bigr{)}^{-1}\bigl{(}\mathbf{x}_{k}-\hat{\mathbf{x}}_{k}^{-} \bigr{)}+[\mathbf{z}_{k}-\mathbf{h}(\mathbf{x}_{k})]^{\mathsf{T}}\mathbf{R}_{k}^{-1}[\mathbf{z}_{k }-\mathbf{h}(\mathbf{x}_{k})] \tag{27}\]  
The maximum likelihood estimate of Equation (26) is equivalent to the minimum value of the cost function (27), which is usually solved by the nonlinear iteration method. The linearized measurement equation and the Gauss-Newton iteration are used to achieve the iterative formula for solving the minimum value of \(J_{xz}\) as follows:  
\[\hat{\mathbf{x}}_{k}^{i+1}=\hat{\mathbf{x}}_{k}^{-}+\mathbf{P}_{k}^{-}\Bigl{(}\mathbf{H}_{k} ^{i}\Bigr{)}^{\mathsf{T}}\biggl{[}\mathbf{H}_{k}^{i}\mathbf{P}_{k}^{-}\Bigl{(}\mathbf{H}_ {k}^{i}\Bigr{)}^{\mathsf{T}}+\mathbf{R}_{k}\biggr{]}^{-1}\biggl{[}\mathbf{z}_{k}-\mathbf{ h}\Bigl{(}\hat{\mathbf{x}}_{k}^{i}\Bigr{)}-\mathbf{H}_{k}^{i}\Bigl{(}\hat{\mathbf{x}}_{k}^{-}- \hat{\mathbf{x}}_{k}^{i}\Bigr{)}\biggr{]} \tag{28}\]  
where \(\mathbf{H}_{k}^{i}=\left.\partial\mathbf{h}(\mathbf{x}_{k})/\partial\mathbf{x}_{k}\right|_{ \mathbf{x}_{k}=\hat{\mathbf{x}}_{k}^{i}}\) denotes the Jacobian matrix, and \(i\) represents the iteration index.  
The analogous representation that \(\mathbf{P}_{xz}=\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{\mathsf{T}}\) and \(\mathbf{P}_{z}=\mathbf{H}_{k}\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{\mathsf{T}}+\mathbf{R}_{k}\) is obtained using the statistical linear error approach, which is used instead of the Jacobian matrix to reduce the computational complexity and avoid the linearization error caused by the Taylor first-order expansion. Furthermore, the enhanced Gauss-Newton iteration is displayed as follows.  
\[\hat{\mathbf{x}}_{k}^{i+1}=\hat{\mathbf{x}}_{k}^{-}+\mathbf{P}_{xz}^{i}\Bigl{(}\mathbf{P}_{z}^ {i}\Bigr{)}^{-1}\biggl{[}\mathbf{z}_{k}-\mathbf{h}\Bigl{(}\hat{\mathbf{x}}_{k}^{i}\Bigr{)} -\Bigl{(}\mathbf{P}_{xz}^{i}\Bigr{)}^{\mathsf{T}}\bigl{(}\mathbf{P}_{k}^{-}\bigr{)}^{-1 }\Bigl{(}\hat{\mathbf{x}}_{k}^{-}-\hat{\mathbf{x}}_{k}^{i}\Bigr{)}\biggr{]} \tag{29}\]