EKF Algorithm

0. Initialization
1. PREDICTION STEP:

$$\begin{array}{rl}& \\ & F_x={\frac{\partial f}{\partial x}|}_{\begin{array}{l}x=x(t+1)\end{array}}\\ & F_w={\frac{\partial f}{\partial w}|}_{\begin{array}{l}x=x(t+1)\end{array}}\\ & \hat{x}(t+1|t)=f(x(t),u(t),w(t))\\ & P(t+1|t)=F_{x}P(t|t)F_x^T+F_{v}Q{F}_v^T\end{array}$$

2. PREDICTION STEP:

$$\begin{array}{rl}& H_x={\frac{\partial h}{\partial x}|}_{\begin{array}{l}x=\hat{x}(t+1)\end{array}}\\ & H_v={\frac{\partial h}{\partial v}|}_{\begin{array}{l}x=\hat{x}(t+1)\end{array}}\\ & S=H_{x}P(t+1|t)H_x^T+H_{w}R{H}_w^T\\ & K=P(t+1|t)H_x^T{S}^{-1}\\ & x(t+1|t+1)=\hat{x}(t+1|t)+K\left(z(t+1)-h\left(\hat{x}(t+1|t)\right)\right)\\ & P(t+1|t+1)=\left[I-K{H}_x\right]P(t+1|t)\end{array}$$

Where:

• $z(t+1)$ is the measurement at time $t+1$.