Mathematical Steps to Obtain the EKF Algorithm

0. Starting Knowledge:
1. Taylor expansion on the $f$ function:
2. Neglect the last term and we obtain: Where: $F(t)=\frac{\partial f}{\partial x}$ and $G(t)=\frac{\partial f}{\partial w}$ are Jacobean matrices So we can re-write $x(t+1)$ as:
3. Calculate the estimate $\hat{x}(t+1|t)$ of $x(t+1)$:
4. We have defined $P(t+1|t)$ as the covariance matrix of the error of the prediction $\hat{x}(t+1|t)$ and true value $x(t+1)$, so:

Exactly like in the linear case, the only difference is that $F$ and $G$ are now time varying matrices, so because $\tilde{x}$ and $w$ are uncorrelated we can write:

$$=E\left[F(t)\tilde{x}(t|t){\tilde{x}}^T(t|t)F^T(t)\right]+E\left[G(t)w(t)w^T(t)G^T(t)\right]$$

From the definition we gave to $P(t|t)$ we have that:

$$\begin{array}{rl}& P(t|t):=E\left[\left(x(t)-\hat{x}(t|t)\right)\cdot {\left(x(t)-\hat{x}(t|t)\right)}^T\right]\\ & \tilde{x}(t|t)=\left(x(t)-\hat{x}(t|t)\right)\\ & \Rightarrow E\left[\tilde{x}(t|t)\cdot {\tilde{x}}^T(t|t)\right]=P(t|t)\end{array}$$

And from the definition of Covariance Matrix, and knowing that the mean of $w$ is $0$:

$$\begin{array}{rl}& Q(t):=E\left[\left(w(t)-\bar{w}(t)\right)\cdot {\left(w(t)-\bar{w}(t)\right)}^T\right]\\ & \bar{w}(t)=0\\ & \Rightarrow E\left[w(t)\cdot w^T(t)\right]=Q(t)\end{array}$$

We can conclude that:

And obtain the 2nd formula of the EKF algorithm: 5. We can now start with the correction step: The following step will be very similar to the previous ones. 6. Taylor expansion of the $h$ function: So we have:

$$\begin{array}{rl}& d(t+1):=x(t+1)-\hat{x}(t+1|t)\\ & m(t+1):=y(t+1)-h\left(\hat{x}(t+1|t)\right)\\ & h(t+1)\simeq h\left(\hat{x}(t+1|t)\right)+H(t+1)d(t+1)\\ & \Rightarrow m(t+1)=H(t+1)d(t+1)+v(t+1)\end{array}$$

7. After declaring $d(t+1)$ we need to find its estimate:

$$\begin{array}{rl}& \hat{d}(t+1):=E[d(t+1)]\\ & d(t+1)=x(t+1)-\hat{x}(t+1|t)\\ & \Rightarrow E[d(t+1)]=\hat{x}(t+1|t)-\hat{x}(t+1|t)\\ & \Rightarrow \hat{d}(t+1)=0\end{array}$$

8. Its covariance:
9. Then we can say that, using the same formulas from the KF algorithm: Obtaining the 3rd formula of the EKF algorithm:
10. Where the Kalman Gain, $K(t+1)$, is given by: (4th formula of the EKF algorithm, taken from the KF algorithm)
11. Also taken from the KF algorithm, the 5th formula of the EKF algorithm: