Extended Kalman Filter

xt+1=f(xt,ut,wt)wtN(0,W)yt=g(xt,vt)vtN(0,V)\begin{align*} x_{t+1}&= f(x_{t},u_{t},w_{t})&w_{t}\sim \mathcal{N}(0,W)\\ y_{t}&= g(x_{t},v_{t})&v_{t}\sim \mathcal{N}(0,V) \end{align*} Given the previous dynamics, we linearize the motion and observation models about the current state estimate mean: f(xt,ut,wt)xˇt+Ft(xtx^t)+Ltwth(xt,vt)yˇt+Ht(xtxˇt)+Mtvt\begin{align*} f(x_{t},u_{t},w_{t})&\approx \check{x}_{t}+F_{t}(x_{t}-\hat{x}_{t})+L_{t}w_{t}\\ h(x_{t},v_{t})&\approx \check{y}_{t}+H_{t}(x_{t}-\check{x}_{t})+M_{t}v_{t} \end{align*}where Ft:=f(xt,ut,wt)xt(xt,ut,wt)=(m~t,ut,0)Lt:=f(xt,ut,wt)wt(xt,ut,wt)=(m~t,ut,0)Ht:=h(xt,vt)xt(xt,vt)=(mt,0)Mt:=g(xt,vt)vt(xt,vt)=(mt,0)\begin{align*} F_{t}:&= \left.\frac{ \partial f(x_{t},u_{t},w_{t}) }{ \partial x_{t} }\right|_{(x_{t},u_{t},w_{t})=(\tilde{m}_{t},u_{t},0)}& L_{t}:&= \left.\frac{ \partial f(x_{t},u_{t},w_{t}) }{ \partial w_{t} } \right|_{(x_{t},u_{t},w_{t})=(\tilde{m}_{t},u_{t},0)}\\ H_{t}:&= \left.\frac{ \partial h(x_{t},v_{t}) }{ \partial x_{t} } \right|_{(x_{t},v_{t})=(m_{t},0)}& M_{t}:&= \left.\frac{ \partial g(x_{t},v_{t}) }{ \partial v_{t} } \right|_{(x_{t},v_{t})=(m_{t},0)} \end{align*}