IEKF and ISPKF

The extended Kalman filter uses the Jacobian of the observation function $h$ evaluated at $\hat{ \mathbf{x} }^{-}$, the prior state, and, using the observation, obtains the posterior state estimate.

In fact, this procedure is exactly one iteration of the Gauss–Newton method.

The number of iterations can be increased to obtain the class of iterative Kalman filters, which normally exhibit substantially better performance than their noniterative counterparts.

The only difference from the corresponding noniterative filters lies in the observation phase (see equation (3.32)), which is replaced by iterations of the form

$$\mathbf{x}_{i+1} = \hat{ \mathbf{x} } + \mathbf{K}(\mathbf{... ...thbf{x}_i) - \mathbf{H}_i( \hat{ \mathbf{x} } - \mathbf{x}_i))$$ (3.38)

with the gain $\mathbf{K}$ computed iteratively as

$$\mathbf{K} = \mathbf{P} \mathbf{H}_i^\top (\mathbf{H}_i \mathbf{P} \mathbf{H}_i^\top + \mathbf{R})^{-1}$$ (3.39)

and using $\mathbf{x}_0 = \hat{ \mathbf{x} }^{-}$ as the initial value for the minimization.

The value of $\mathbf{K}$ associated with the final iteration is then used to update the process covariance matrix.

The same procedure can be applied to the SPKF to obtain the Iterated Sigma Point Kalman Filter (SSM06), where the iteration for computing the state has the form

$$\mathbf{x}_{i+1} = \hat{ \mathbf{x} } + \mathbf{K} \left(\m... ...} \mathbf{P}^{-1} ( \hat{ \mathbf{x} } - \mathbf{x}_i) \right)$$ (3.40)