Review of Kalman filter
Distribution of Gausion is not Gaussian, it becomes non-linear
Extented Kalman filter uses a linear approximation of h(x) Here we use first order taylor expansion to
Given a function f(x), a taylor series expansion could be expressed:
$$f(x) \approx \frac{\partial{f(\mu)} }{\partial{x}}(x - \mu)$$
Multivariate Taylor Series
Design Kalman Filter for 1D tracking problem
We need to define two linear functions: 1. state transition function 2. measurement function
State transition function
$$ x’ = F * x + noise $$
where,
$$F = \begin{pmatrix} 1 & \Delta{t} \\ 0 & 1 \end{pmatrix}$$
$$x = \begin{pmatrix} p \\ v\end{pmatrix}$$
postion $p$ is linear motion model, calculation is:
$$p’ = p + v * \Delta{t}$$
Thus We can express it in a matrix form:
$$
\begin{pmatrix} p’ \\ v’ \end{pmatrix}
\begin{pmatrix} 1 & \Delta{t} \\ 0 & 1 \end{pmatrix} \begin{pmatrix} p \\ v\end{pmatrix} $$
Measurement Update function
At time $t$, the belief is represented by the mean $\mu_t$ and the covariance $\Sigma_t$.
Process Model
The state transition probability $p(x_t \mid ut, x{t-1})$ must be a linear functoin in its arguments with added Gaussian noise. This is expressed by the following equation:
$$x_t = Atx{t-1} + B_tu_t + \epsilon_t$$
Measurement Model
The measurement probability $p(z_t \mid x_t)$ must also be linear in its arguments, with added Gaussian noise:
$$z_t = C_tx_t + \delta_t$$
Kalman Filter Algorithm
$$\bar\mu_t = At\mu{t-1} + B_tu_t$$
$$\bar\Sigma_t = At\Sigma{t-1} + R_t $$