September 2021
I worked through quite a few different resources in order to build a basic Kalman filter. Every time I would work through one of them, whether a book, video, or a web tutorial, I would reach a point where I could not understand how to implement the next step. I worked through Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Second Edition by Paul D. Groves, video tutorials from MATLAB, and a video tutorial from Google Engineer, Scott Lobdell. In the end, the most straightforward tutorial was How a Kalman Filter Works, In Pictures by Tim Babb. However, even that tutorial left me with some critical gaps to figure out.
This article answers those things that perplexed me (and perhaps perplexes you) when building a Kalman filter. Perhaps the answer that you are looking for is in this article.
If you are looking to build a Kalman filter for the first time, I recommend that you work through Tim Babb’s tutorial and visit this article for the portions that get you stuck.
First, take a case that involves input from one sensor: Let’s say we are building a Kalman filter that utilizes information from solely the GPS and no other sensor. With sensor fusion, you would typically utilize information from inertial sensors, LiDAR, camera, or another sensor, but this simple configuration allows us to focus on resolving our gaps of understanding.
Second, let’s say that we want to utilize the GPS in such a way that outputs a track of positions more accurately than its default output of positions, which typically looks like a wiggly track like this:
The track (blue line) without a Kalman filter shows a "wiggle" due to GPS drift
GPS provides us position, speed, and course. Regarding the accuracy of speed, J. Meguro, et al. says,
“the accuracy of the estimated velocity derived from GPS Doppler processing is better than that using the distances between absolute positions based on pseudo range.”
Therefore, we will leverage the speed and course from the GPS in a Kalman filter to improve the positions. This technique is called doppler smoothing. Since we are using a smartphone, we will use course instead of heading.
In many sources, we see some definition of the position and velocity, like the following. However, when the time comes to sit down and either write out the equations, or write the computer code that implements them, we are perplexed as to what exactly these variables mean.
$$ \begin{align*}\hat x=\left[ {\begin{array}{c}p\\v\end{array} } \right]\tag{1}\end{align*} $$
We understand that this expression represents the state of the GPS system by its position, $p$, and its velocity $v$, but these are vectors with $x$ and $y$ components, not scalars! (We will ignore the $z$ component, which represents altitude.) How do we represent the components in this expression?
Well, the answer is very simple: you split apart the $x$ and $y$ components so that instead of a 2x1 matrix of vectors, you have a 4x1 matrix of scalars:
$$ \begin{align*}\hat x=\left[ {\begin{array}{c}p_x\\p_y\\v_x\\v_y\end{array} } \right]\tag{2}\end{align*} $$
From here, you can create this matrix using the native libraries in your framework, MATLAB, or other platform, but we won’t get into those coding specifics here.
Now, we have a state equation (2) represented by $x$ and $y$ components of position and velocity, but our GPS gives us position in Geodetic coordinates (latitude, longitude), speed in Cartesian coordinates (meters per second), and course in degrees from North. Which coordinate system should we use?