User talk:ForrestVoight/EKF

System

${\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1},{\boldsymbol {w}}_{k-1})$

${\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k},{\boldsymbol {v}}_{k})$

Discrete-time predict and update equations

Predict

${\hat {\boldsymbol {x}}}_{k|k-1}=f({\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1})$

${\color {Red}{\boldsymbol {F}}_{k-1}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}$

${{\boldsymbol {L}}_{k-1}}=\left.{\frac {\partial f}{\partial {\boldsymbol {w}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}$

${\boldsymbol {P}}_{k|k-1}={{\boldsymbol {F}}_{k-1}}{\boldsymbol {P}}_{k-1|k-1}{{\boldsymbol {F}}_{k-1}^{\top }}+{\boldsymbol {L}}_{k-1}{\boldsymbol {Q}}_{k-1}{\boldsymbol {L}}_{k-1}^{T}$

Update

${\tilde {\boldsymbol {y}}}_{k}={\boldsymbol {z}}_{k}-h({\hat {\boldsymbol {x}}}_{k|k-1})$

${\color {Red}{\boldsymbol {H}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}$

${{\boldsymbol {M}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {v}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}$

${\boldsymbol {S}}_{k}={{\boldsymbol {H}}_{k}}{\boldsymbol {P}}_{k|k-1}{{\boldsymbol {H}}_{k}^{\top }}+{\boldsymbol {M}}_{k}{\boldsymbol {R}}_{k}{\boldsymbol {M}}_{k}^{T}$

${\boldsymbol {K}}_{k}={\boldsymbol {P}}_{k|k-1}{\color {Red}{\boldsymbol {H}}_{k}^{\top }}{\boldsymbol {S}}_{k}^{-1}$

${\hat {\boldsymbol {x}}}_{k|k}={\hat {\boldsymbol {x}}}_{k|k-1}+{\boldsymbol {K}}_{k}{\tilde {\boldsymbol {y}}}_{k}$

${\boldsymbol {P}}_{k|k}=({\boldsymbol {I}}-{\boldsymbol {K}}_{k}{\color {Red}{\boldsymbol {H}}_{k}}){\boldsymbol {P}}_{k|k-1}$