Iterative Extended Kalman Filter

Extension of Extended Kalman Filter where in the update step, we just keep correcting until we reach some threshold.

Update ONLY

Compute the first order approximation of the non-linear measurement function using our predicted state prior.

$${\mathbf{H}}_k=\frac{\partial h}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_{k|k-1}}$$

Compute the Kalman Gain

$${\mathbf{K}}_k=\frac{{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T}{{\mathbf{H}}_k{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T+{\mathbf{R}}_k}$$

Update the priors with the measurement using the Kalman Gain (this becomes the posterior)

$${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k({\mathbf{z}}_k-\underset{\begin{array}{c}\text{We use}\\ \text{non-linear}\\ \text{here}\end{array}}{\underbrace{h({\hat{\mathbf{x}}}_{k|k-1})}})$$ $${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{H}}_k{\mathbf{K}}_k){\mathbf{P}}_{k|k-1}$$ $${\mathbf{x}}_{op,k}\leftarrow {\hat{\mathbf{x}}}_{k|k}$$

second iteration

Compute the first order approximation of the non-linear measurement function using our predicted state prior.

$${\mathbf{H}}_k=\frac{\partial h}{\partial \mathbf{x}}{|}_{{\mathbf{x}}_{op,k}}$$

Compute the Kalman Gain

$${\mathbf{K}}_k=\frac{{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T}{{\mathbf{H}}_k{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T+{\mathbf{R}}_k}$$

Update the priors with the measurement using the Kalman Gain (this becomes the posterior)

$${\hat{\mathbf{x}}}_{k|k}={\mathbf{x}}_{op,k}+{\mathbf{K}}_k({\mathbf{z}}_k-\underset{\begin{array}{c}\text{We use}\\ \text{non-linear}\\ \text{here}\end{array}}{\underbrace{h({\mathbf{x}}_{op,k})}})$$ $${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{H}}_k{\mathbf{K}}_k){\mathbf{P}}_{k|k-1}$$ $${\mathbf{x}}_{op,k}\leftarrow {\hat{\mathbf{x}}}_{k|k}$$

repeat… keep doing this until $\Delta {\mathbf{x}}_{op,k}<threshold$