> For the complete documentation index, see [llms.txt](https://calvinfeng.gitbook.io/probabilistic-robotics/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://calvinfeng.gitbook.io/probabilistic-robotics/basics/gaussian-filters/02-extended-kalman-filter.md).

# Extended Kalman Filter

The problem with standard Kalman filter is that it assumes linear state transitions and measurements. In reality, state transitions and measurements are rarely linear, e.g. a robot that moves with constant transnational and rotational velocity will move in a circular path, which is not linear. The *extended Kalman filter* relaxes the linearity assumption.

We suppose that state transition probability and measurement probability are governed by nonlinear functions. 

$$
x\_{t} = g(u\_{t}, x\_{t-1}) + \epsilon\_{t} \\;\\
z\_{t} = h(x\_{t}) + \delta\_{t}
$$

The function $$g$$replaces the matrices $$A\_{t}$$ and $$B\_{t}$$, and the function $$h$$ replaces the matrix $$C\_{t}$$. Unfortunately, with arbitrary non-linear functions, the belief is no longer a Gaussian. In fact, performing the belief update exactly is usually impossible in this case. The EKF (extended Kalman filter) needs to calculate a Gaussian approximation to the true belief via Monte Carlo methods. 

## Linearization

The key idea underlying the EKF approximation is called **linearization**.  If we were to use Monte Carlo methods, we'd have to pass hundred thousands of points to $$g$$followed by the computation of their mean and covariance. This is extremely inefficient. We need a way to hack around it and approximates $$g$$ and $$h$$ as linear functions using first order Taylor expansion. 

### Taylor Expansion

Taylor expansion constructs a linear approximation to a function $$g$$ from $$g$$'s value and slope. The slope is given by the following expression.

$$
g\prime(u\_{t}, x\_{t-1}) = \frac{\partial g}{\partial x\_{t-1}}
$$

The value and slope of $$g$$are dependent on the arguments $$u\_{t}$$ and $$x\_{t-1}$$. We should choose the most probable states to perform Taylor expansion with. Since the most probable state of $$X\_{t}$$ is $$\mu\_{t-1}$$, $$g$$ is approximated by its values at $$\mu\_t{-1}$$ and $$u\_{t}$$. 

$$
g(u\_{t}, x\_{t-1}) \approx g(u\_{t}, \mu\_{t-1}) + g\prime(u\_{t}, \mu\_{t-1})(x\_{t-1} - \mu\_{t-1}) = g(u\_{t}, \mu\_{t-1}) + G\_{t}(x\_{t-1} - \mu\_{t-1})
$$

Notice that $$G\_{t}$$ is a n by n matrix, with n denoting the dimension of the state. This matrix is often called the Jacobian. The value of Jacobian is dependent on $$u\_{t}$$ and $$\mu\_{t-1}$$, hence the matrix is time dependent. EKF implements the exact same linearization for the measurement function $$h$$. The calculation must perform after prediction state, hence $$\overline{\mu}\_{t}$$.

$$
h(x\_{t}) \approx h(\overline{\mu}*{t}) + h\prime(\overline{\mu}*{t})(x\_{t} - \overline{\mu}*{t}) = h(\overline{\mu}*{t}) + H\_{t}(x\_{t} - \overline{\mu}\_{t})
$$

## EKF Algorithm

Now if we put everything together, we have the EKF algorithm. Equation (1) performs the prediction steps.

$$
\tag{1} \overline{\mu}*{t} = g(u*{t}, \mu\_{t-1}) \\;\\
\overline{\Sigma}*{t} = G*{t}\Sigma\_{t-1}G^{T}*{t} + R*{r}t
$$

Equation (2) performs the Kalman gain calculation.

$$
\tag{2} K\_{t} = \overline{\Sigma}*{t}H*{t}^{T}(H\_{t}\overline{\Sigma}H\_{t}^{T} + Q\_{t})^{-1}
$$

Equation (3) performs the measurement incorporation steps.

$$
\mu\_{t} = \overline{\mu}*{t} + K*{t}(z\_{t} - h(\overline{\mu}*{t})) \\;\\
\Sigma*{t} = (I - K\_{t}H\_{t})\overline{\Sigma}\_{t}
$$

Finally,

$$
\text{return};\mu\_{t}, \Sigma\_{t}
$$