PINEAL.ME   Archives  About

Extended Kalman Filter

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 $$

Robotics   Math