# Extended Kalman Filter formulation

For a nonlinear system, \begin{align} &{\boldsymbol x}(k+1)={\boldsymbol f}({\boldsymbol x}(k),{\boldsymbol u}(k),{\boldsymbol w}(k)) \\ &{\boldsymbol y}(k)={\boldsymbol h}({\boldsymbol x}(k),{\boldsymbol v}(k)) \end{align}

I would like to find the Extended Kalman Filter (EKF), having a look at wikipedia, I have reached

\begin{align} & \hat {\boldsymbol x}(k+1|k)={\boldsymbol f}(\hat {\boldsymbol x}(k|k),{\boldsymbol u}(k|k)) \\ & {\boldsymbol P}(k+1|k)={\boldsymbol F}(k){\boldsymbol P}(k|k){\boldsymbol F}^T(k)+{\boldsymbol L}(k) {\boldsymbol Q}(k) {\boldsymbol L}^T(k) \end{align}

and

\begin{align} &{\boldsymbol S}(k+1)={\boldsymbol H}(k+1){\boldsymbol P}(k+1|k){\boldsymbol H}^T(k+1)+{\boldsymbol M}(k+1){\boldsymbol R}(k+1){\boldsymbol M}^T(k+1) \\ & {\boldsymbol K}(k+1)={\boldsymbol P}(k+1|k) {\boldsymbol H}^T (k+1){\boldsymbol S}^{-1}(k+1) \\ & \hat{\boldsymbol x}(k+1|k+1)=\hat{\boldsymbol x}(k+1|k)+{\boldsymbol K}(k+1)[y(k+1)-{\boldsymbol h}(\hat {\boldsymbol x}(k+1|k))] \\ & {\boldsymbol P}(k+1|k+1)k=[{\boldsymbol I}-{\boldsymbol K}(k+1){\boldsymbol H}(k+1)]{\boldsymbol P}(k+1|k) \end{align}

where

\begin{align} & {\boldsymbol F}(k)=\frac{\partial \boldsymbol f}{\partial \boldsymbol x}|_{\hat {\boldsymbol x}(k|k),{\boldsymbol u}(k)} \\ & {\boldsymbol H}(k+1)=\frac{\partial \boldsymbol h}{\partial \boldsymbol x}|_{\hat {\boldsymbol x}(k+1|k)}\\ &{\boldsymbol L}(k)=\frac{\partial\boldsymbol f}{\partial\boldsymbol w}|_{\hat {\boldsymbol x}(k|k),{\boldsymbol u}(k)}\\ &{\boldsymbol M}(k+1)=\frac{\partial\boldsymbol h}{\partial\boldsymbol v}|_{\hat {\boldsymbol x}(k+1|{\color{red}{k+1}})} \end{align}

Is this obtained formulation correct?

My main concerns are about the whole timing indexes especially the last one.

• Maybe you could give some background what you want to use the EKF for? More often than not your model looks more like $x(k+1)=f(x(k),u(k))+w(k)$ and $y(k)=h(x(k)) +v(k)$ or at least this is a good approximation which yields a much simpler representation of the equations, e.g., in that case $M(k+1)=\frac{\partial h}{\partial v}=I$ no matter what index. The same holds true for L(k) in that case. A very good resource for Kalman Filtering is "Optimal State Estimation: Kalman, H-Infinity, and Nonlinear Approaches." by Simon, D. (2006). – link Nov 10 '20 at 17:04

I believe that the formulation is correct apart from $$M(k+1)$$ which should be $$M(k+1)=\frac{\partial \boldsymbol{h}_k}{\partial \boldsymbol{v}}\bigg\vert_{\hat{\boldsymbol{x}}(k+1|k)}$$