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

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{P}_{k-1|k-1} + \boldsymbol{L}_{k-1} \boldsymbol{Q}_{k-1}\boldsymbol{L}^{T}_{k-1} $$

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