Previous: Feedback Control with Linear Quadratic Regulator (LQR)

At the very beginning of the Robotik series, we discussed about the state estimation and Bayes filter. In this post we will talk about perhaps one of the most used state estimation techniques called the Kalman filter. I am not going to go over the math in this post because it can be daunting and non-intuitive. My goal in this post is so that we can first build our intuition on why we need Kalman filter and how we can use it to solve our problem. I have to say understanding the math is also important though, so hopefully we can do that in the near future :)

Kalman Filter

The goal of using Kalman filter is to improve state estimation by combining multiple sources of state estimates. For example, this can be combining the prediction from the dynamics model of our robot with the state measurement that we may get from some sensors. In the real world, we may already know the dynamics of the robot that allows us to track how the state of the robot is supposed to change over time given the initial state and control signals applied to the robot at each time step. We might think that we can just use this model to know the state of the robot at each time step. Unfortunately, the dynamics model that we have is never perfect (e.g., due to imperfect parts, etc.), so relying only on our dynamics model will only give us a noisy estimate of the state. Okay, but robots typically have various sensors, can we use the measurements from these sensors to know the state of the robot? Well, sensors are also not perfect and can only provide a rough estimate of the robot state.

This is where the Kalman filter can help us. If we have additional information about the state of the robot, as long as they are somewhat informative, we can combine them with what we already have to have a better estimation! In the example above with dynamics model and a sensor measurement, we have two different ways to get the state estimates, each with known uncertainty. The intuition is that the more likely state is probably the state that falls within the intersection of the dynamics and sensor measurement.

There are three main assumptions that Kalman filter makes:

1. Linear dynamics with Gaussian noise

\[\mathbf{x}_{t} = A \mathbf{x}_{t-1} + B \mathbf{u}_{t-1} + \mathbf{w}_{t}, \ \textrm{where} \ \mathbf{w}_{t} \sim \mathcal{N}(0, Q)\]

2. Linear measurement model with Gaussian noise

\[\mathbf{z}_t = H \mathbf{x}_t + \mathbf{n}_t, \ \textrm{where} \ \mathbf{n}_t \sim \mathcal{N}(0, R)\]

3. Gaussian prior for the initial state

\[bel(\mathbf{x}_0) \sim \mathcal{N}(\mu_0, \Sigma_0)\]

Of course, these assumptions sometimes are not realistic, but they allow us to derive the equations for the Kalman filter. That said, it turns out that even with these assumptions, Kalman filter is still a powerful technique that we can often rely on in real world applications!

There are two main steps that we need to perform when doing Kalman filtering: prediction and update steps. The goal of prediction step is to predict the state of the robot based on the dynamics model after applying a control signal. Since the model is Gaussian, the prediction of the state belief is also Gaussian. And since Gaussian is parametrized by the mean and covariance, we want to compute the new mean and the new covariance after applying the control signal. The equations we need to perform the prediction step are then:

1. Compute predicted mean

\[\mu_{t|t-1} = A\mu_{t-1|t-1} + B\mathbf{u}_{t-1}\]

2. Compute predicted covariance

\[\Sigma_{t|t-1} = A\Sigma_{t-1|t-1}A^T + Q\]

After we do the prediction step, we assume to receive a measurement \(\mathbf{z}_{t}\) from our sensor. The goal of the update step is to update the belief that we just computed from the prediction step by considering the measurement that we just received from our sensor. The uncertainty in the belief should then decrease as the result of the update step. Now, this is where I am going to start giving you the Kalman filter equations without actually deriving them. The equations we need to perform the update step are:

1. Compute mean and covariance of the prediction residual

\[\delta_{\mu} = \mathbf{z}_{t} - H\mu_{t|t-1}\] \[\delta_{\Sigma} = H \Sigma_{t|t-1} H^T + R\]

2. Compute Kalman gain

\[K_{t} = \Sigma_{t|t-1} H^T \delta_{\Sigma}^{-1}\]

3. Compute mean and covariance of the belief

\[\mu_{t | t} = \mu_{t | t-1} + K\delta_{\mu}\] \[\Sigma_{t | t} = \Sigma_{t|t-1} - KH\Sigma_{t|t-1}\]

Again, we are not going to go through the derivation that gives us all the equations for the update step in this post. But I hope this can still help us to at least understand how Kalman filter can be used and what is actually needed to implement it.

Now, let’s take a look at a concrete example to see how we can use these equations.

Example

Consider a robot moving in a room without obstacles. The robot is equipped with two sensors to measure distance between the robot and the walls, which allows the robot to measure the location of the robot in the room. These sensors are not perfect, however the manufacturer provide us with the information that tells us how innacurate these sensors are (i.e., we are given the measurement model). In addition, we also know the dynamics of the robot we are using. This means that we are given the \(A\), \(B\), \(Q\), \(H\) and \(R\) matrices.

Say the state of the robot is its \(x\) and \(y\) position in the room (i.e., \(\mathbf{x}_t = \begin{bmatrix} x \\ y \end{bmatrix}\)), and the control inputs are the velocity in each direction \(v_x\) and \(v_y\) (i.e., \(\mathbf{u}_t = \begin{bmatrix} v_x \\ v_y \end{bmatrix}\)). The initial belief of the robot is

\[\small \mu_0 = \begin{bmatrix} 0 \\ 0 \end{bmatrix} \ \ \ \ \Sigma_0 = \begin{bmatrix} 0.01 & 0 \\ 0 & 0.01 \end{bmatrix}\]

and the robot moves with constant control inputs \(v_x = v_y = 1\), in other words:

\[\small \mathbf{u}_t = \begin{bmatrix} 1 \\ 1 \end{bmatrix}\]

At each time step, after applying a control signal, the robot can take a measurement using the sensors to have an idea where the robot currently is. For the sake of simplicity, assume the sensors to directly return the noisy measurement of \((x,y)\) location in the room.

Given:

\[\small A = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \ \ \ \ B = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \ \ \ \ Q = \begin{bmatrix} 0.3 & 0 \\ 0 & 0.3 \end{bmatrix}\] \[\small H = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \ \ \ \ R = \begin{bmatrix} 0.75 & 0 \\ 0 & 0.6 \end{bmatrix}\]

What is the state estimation after 1 time step if \(\mathbf{z}_{t=1} = \begin{bmatrix}0.93 \\ 1.77 \end{bmatrix}\) (this is the measurement the robot gets after applying the control command \(\mathbf{u}_t\))?

To answer this, we just need to use all the given information to perform the prediction step and then the update step. First, let’s do the prediction step:

\[\small \begin{eqnarray} \mu_{t|t-1} &=& A\mu_{t-1|t-1} + B\mathbf{u}_{t-1} &=& \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ 0 \end{bmatrix} + \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 1 \\ 1 \end{bmatrix} &=& \begin{bmatrix} 1 \\ 1 \end{bmatrix} \end{eqnarray}\] \[\small \begin{eqnarray} \Sigma_{t|t-1} &=& A\Sigma_{t-1|t-1}A^T + Q &=& \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0.01 & 0 \\ 0 & 0.01 \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}^T + \begin{bmatrix} 0.3 & 0 \\ 0 & 0.3 \end{bmatrix} &=& \begin{bmatrix} 0.31 & 0 \\ 0 & 0.31 \end{bmatrix} \end{eqnarray}\]

We can then take the measurement \(\mathbf{z}_{t=1}\) and perform the update step:

\[\small \begin{eqnarray} \delta_{\mu} &=& \mathbf{z}_{t} - H\mu_{t|t-1} &=& \begin{bmatrix} 0.93 \\ 1.77 \end{bmatrix} - \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 1 \\ 1 \end{bmatrix} &=& \begin{bmatrix} -0.07 \\ 0.77 \end{bmatrix} \end{eqnarray}\] \[\small \begin{eqnarray} \delta_{\Sigma} &=& H \Sigma_{t|t-1} H^T + R &=& \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0.31 & 0 \\ 0 & 0.31 \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}^T + \begin{bmatrix} 0.75 & 0 \\ 0 & 0.6 \end{bmatrix} &=& \begin{bmatrix} 1.06 & 0 \\ 0 & 0.91 \end{bmatrix} \end{eqnarray}\] \[\small \begin{eqnarray} K_{t} &=& \Sigma_{t|t-1} H^T \delta_{\Sigma}^{-1} &=& \begin{bmatrix} 0.31 & 0 \\ 0 & 0.31 \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}^T \begin{bmatrix} 0.31 & 0 \\ 0 & 0.31 \end{bmatrix}^{-1} &=& \begin{bmatrix} 0.29 & 0 \\ 0 & 0.34 \end{bmatrix} \end{eqnarray}\] \[\small \begin{eqnarray} \mu_{t | t} &=& \mu_{t | t-1} + K\delta_{\mu} &=& \begin{bmatrix} 1 \\ 1 \end{bmatrix} + \begin{bmatrix} 0.29 & 0 \\ 0 & 0.34 \end{bmatrix} \begin{bmatrix} -0.07 \\ 0.77 \end{bmatrix} &=& \begin{bmatrix} 0.98 \\ 1.26 \end{bmatrix} \end{eqnarray}\] \[\small \begin{eqnarray} \Sigma_{t | t} &=& \Sigma_{t|t-1} - KH\Sigma_{t|t-1} &=& \begin{bmatrix} 0.31 & 0 \\ 0 & 0.31 \end{bmatrix} - \begin{bmatrix} 0.29 & 0 \\ 0 & 0.34 \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0.31 & 0 \\ 0 & 0.31 \end{bmatrix} &=& \begin{bmatrix} 0.22 & 0 \\ 0 & 0.20 \end{bmatrix} \end{eqnarray}\]

That’s it! You see, once we know the equations, implementing Kalman filter is not too complicated as we just need to plug the numbers into the equations. Concretely, we just need to write two functions for the prediction and update steps. Once we have the updated belief, we replace the prior with it, and repeat the process for every new \(\mathbf{u}_t\) and \(\mathbf{z}_{t}\). If we have more than one measurement, we can treat them as independent measurement and perform update step multiple times.

Summary

To summarize, given linear dynamics and measurement models with Gaussian noise, Kalman filter works as following:

  1. Run prediction step

  2. Get measurement \(\mathbf{z}_{t}\)

  3. Run update step

  4. Use the results of update step as the new prior and repeat the process

With Kalman filter, we can have a better state estimation by combining information from the dynamics model with measurement from our sensor. As long as the assumption that we make with Kalman filter holds, the state estimation results will be better compared to just relying only on the dynamics model or sensor measurement.

That is all for the intro to Kalman filter! As always, please feel free to send me an email if you have questions, suggestions, or if you found some mistakes in this article :)

References

David Meger. Kalman Filters. PowerPoint Presentation, 2020. URL https://www.cim.mcgill.ca/~dmeger/comp765/index2020.html.

Next: State Estimation with Particle Filter