1
$\begingroup$

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.

$\endgroup$
1
  • $\begingroup$ 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). $\endgroup$ – link Nov 10 '20 at 17:04
0
$\begingroup$

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)}$$

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.