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

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:

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

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

- $\rho$ is the radial distance from the origin to the object
- $\varphi$ is the angle between the object-origin ray and x direction
- $\dot{\rho}$ is the velocity along this ray

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: