Lidar & Radar Fusion using Kalman Filter

Sep 22 2019

Kalman Filter

Here’re some notations in this blog. We assume that our algorithm runs on a 2-dimensional system.

$x = \begin{bmatrix}p_x\\p_y\\v_x\\v_y\end{bmatrix}$ is state vector
$P = Cov(x) = \begin{bmatrix}\sigma_{p_x}^2&\sigma_{p_x, p_y}&\sigma_{p_x, v_x}&\sigma_{p_x, v_y}\\\sigma_{p_y, p_x}&\sigma_{p_y}^2&\sigma_{p_y, v_x}&\sigma_{p_y, v_y}\\\sigma_{v_x, p_x}&\sigma_{v_x, p_y}&\sigma_{v_x}^2&\sigma_{v_x, v_y}\\\sigma_{v_y, p_x}&\sigma_{v_y, p_y}&\sigma_{v_y, v_x}&\sigma_{v_y}^2\end{bmatrix}$ is state covariance matrix
$F$ is state transition matrix
$u$ is control variable matrix
$w$ is process noise
$Q$ is process noise covariance matrix
$K$ is kalman gain
$z$ is measurement data
$H$ is measurement transition matrix
$R$ is measurement covariance matrix
$y$ is measurement residual

Prediction Process:

Let’s review the Kinematic Equations Formula:

Rewrite them into matrix format:

Hence,

As there might be some noises in the environment, we model the process noise as $w_k$, which follows Gaussian Distribution $w_k\sim {\mathcal {N}}(0, Q_k)$.

Combine them and get the following equations:

In order to compare the in-coming measurement data and our preivous predict data, we need to transform our $x_k$ to the same format of $z_k$, so we need the measurement transition matrix $H_k$

So, the prediction process predict the measurement data has following mean and variance:

Measurement Process:

In the measurement process, there also have some noises. We model the measurement noise as $v_k$, which also follows Gaussian Distribution $v_k\sim {\mathcal{N}}(0, R_k)$.

So the measurement data has following mean and variance:

Now, the problem convert into how we could combine these two $\mu$ and $\sigma^2$. We know we can combine two Gaussian random variable as followings:

So we have

We eliminate the common factor in both sides

We have measurement residual $y_k = z_k - H_kx_k$, and we define kalman gain as $K_k = \frac{P_kH_k^T}{H_kP_kH_k^T + R_k}$ (like a weight factor between the prediction and measurement)

Finally we have

Extended Kalman Filter

In normal Kalman Filter, $x$ has a linear transformation to $z$, which use measurement transition matrix $H$.

But what if $x$ cannot be linearly transformed to the format of $z$? Let’s see an example: radar.

Radar sees the world different from lidar. Instead of a 2D pose $(p_x, p_y)$, the radar can directly measure the object range $\rho$, angle $\varphi$ and velocity $\dot{\rho}$, so the new measurement vector $z$ is:

where

So now we cannot use a transition matrix $H$ to tranfrom $x$ to $z$, but we can use a funtion $h(x)$ to do so.

Since this $h(x)$ is a nonlinear funtion, $h(x)$ will not follow Gaussian Distribution after the convertion. So we need to linearize $h(x)$. We can use a method called first order Taylor expansion:

The derivative of $h(x)$ with respect to $x$ are called Jacobians:

So now the equations of measurement process become:

Measurement Process: