# Kalman Filter
Recall that the key steps in implementing a model are:
* represent the probability distribution (analytically or using sampling)
* model the motion (or state transition): $$p(x\_t|u\_t, x\_{t-1})$$
* model the measurement: $$p(z\_t|x\_t)$$
## Kalman Filter
In the standard Kalman Filter algorithm, the state transition is modeled as:
$$
x\_t = A\_tx\_{t-1} + B\_t u\_t + \epsilon\_t
$$
where $$\epsilon\_t \sim \mathcal{N}(0, R\_t)$$. The covariance matrix $$R\_t$$ represents the uncertainty or noise. To obtain the analytic form of $$p(x\_t|u\_t,x\_{t-1})$$, we just need to observer $$x\_t - A\_tx\_{t-1} - B\_tu\_{t}$$ follows the normal distribution.
Similarly, the measurement in the standard Kalman filter is modeled as:
$$
z\_t = C\_tx\_t + \delta\_t
$$
where $$\delta\_t \sim \mathcal{N}(0, Q\_t)$$ describing the measurement noise.
It can be shown that in the standard Kalman filter, the states $$x\_t$$follows normal distribution. i.e.:
$$
x\_t \sim \mathcal{N}(\mu\_t, \Sigma\_t)
$$
and the algorithm is given as
```
\begin{algorithm}
\renewcommand{\thealgorithm}{}
\begin{algorithmic}[1]
\Function{Kalman\_Filter}{$\mu_{t-1}, \Sigma_{t-1}, u_t, z_t$}
\State $\bar{\mu}_t = A_t \mu_{t-1} + B_t u_t$
\State $\bar{\Sigma}_t = A_t\Sigma_{t-1}A_t^T + R_t$
\State $K_t = \bar{\Sigma}_t C_t^T (C_t \bar{\Sigma}_t C_t^T + Q_t)^{-1}$ \Comment{This is called Kalman gain}
\State $\mu_t = \bar{\mu}_t + K_t(z_t - C_t \bar{\mu}_t)$
\State $\Sigma_t = (I - K_tC_t) \bar{\Sigma}_t$
\State $\textbf{return} \;\;\; \mu_t, \Sigma_t$
\EndFunction
\end{algorithmic}
\end{algorithm}
```
## Extended Kalman Filter
In EKF, the state transition and measurement model take more general form:
$$
\begin{align\*}
x\_t & = g(u\_t, x\_{t-1}) + \epsilon\_t \\
z\_t & = h(x\_t) + \delta\_t
\end{align\*}
$$
The problem is that for arbitrary functions, it may not be possible to obtain an analytic form of the distribution of state variable $$x\_t$$. One way to get around this problem is linearization using Taylor expansion.
Before we look into the detail of the Taylor expansion, let's take one step back and review what we already have. One of the important things is not to confuse parameters with known values.
Recall that the ultimate goal is to calculate $$p(x\_t|u\_t, x\_{t-1})$$and $$p(z\_t|x\_t)$$. Although they are conditional probability and $$(u\_t, x\_{t-1})$$ and $$x\_t$$ are conditions in the two expressions respectively, all of them are parameters (or function arguments). If we forget about the probability context for a moment, it's quite obvious $$p(x\_t|u\_t, x\_{t-1})$$ is a mapping from $$(x\_t, u\_t, x\_{t-1})$$to a value.
We also recall that in the standard Kalman filter, the distribution of the states is tracked by $$\mathcal{N}(\mu\_t, \Sigma\_t)$$ so at time $$t$$, $$\mu\_1, \mu\_2, ..., \mu\_{t-1}$$ are known values.
Now, we can get back to the Taylor expansion. For the motion model, we perform the linearization around the $$\mu\_{t-1}$$ because this is our estimate of the state at $$t-1$$ and it should be close to $$x\_{t-1}$$. Therefore, we have
$$
\begin{align\*}
g(u\_t, x\_{t-1}) & \approx g(u\_t, \mu\_{t-1}) + \frac{\partial g}{\partial x\_{t-1}}(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})
\end{align\*}
$$
where $$\frac{\partial g}{\partial x\_{t-1}}(u\_t,\mu\_{t-1})$$ means the partial derivative with respect to the second variable evaluated at $$(u\_t, \mu\_{t-1})$$.
Similarly, we can write
$$
h(x\_t) \approx h(\bar{\mu}\_t) + H\_t(x\_t - \bar{\mu}\_t)
$$
where $$\bar{\mu}*t = g(u\_t, \mu*{t-1})$$.
The extended Kalman filter algorithm is given as:
```
\begin{algorithm}
\renewcommand{\thealgorithm}{}
\begin{algorithmic}[1]
\Function{EKF}{$\mu_{t-1}, \Sigma_{t-1}, u_t, z_t$}
\State $\bar{\mu}_t = g(u_t, \mu_{t-1})$
\State $\bar{\Sigma}_t = G_t\Sigma_{t-1}G_t^T + R_t$
\State $K_t = \bar{\Sigma}_t H_t^T (H_t \bar{\Sigma}_t H_t^T + Q_t)^{-1}$
\State $\mu_t = \bar{\mu}_t + K_t(z_t - h(\bar{\mu}_t))$
\State $\Sigma_t = (I - K_tH_t) \bar{\Sigma}_t$
\State $\textbf{return} \;\;\; \mu_t, \Sigma_t$
\EndFunction
\end{algorithmic}
\end{algorithm}
```
---
# Agent Instructions: Querying This Documentation
If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.
Perform an HTTP GET request on the current page URL with the `ask` query parameter:
```
GET https://www.learnros2.com/probabilistic-robotics/kalman-filter.md?ask=
```
The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.
Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.