FIND ME ON

GitHub

LinkedIn

Extended Kalman Filter

🌱

xt+1=f(xt,ut,wt)wt∼N(0,W)yt=g(xt,vt)vt∼N(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(xtāˆ’x^t)+Ltwth(xt,vt)ā‰ˆyˇt+Ht(xtāˆ’xˇ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*}

Linked from