September 2021
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 minimize the number of inputs and the number of hardware components.
Next, 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.
Our state will include position and velocity, like the following:
$$ \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$, and these are vectors with $x$ and $y$ components. We will ignore the $z$ component, which represents altitude, which gives a 4x1 matrix:
$$ \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?
The answer is to convert each position from Geodetic coordinates to Cartesian coordinates. Oleg Katkov blogs [2] about this, and points us to a Wikipedia article about Great Circle Distance, but he doesn’t get into specifics about its implementation, so I explain it here:
We want our positions in Cartesian coordinates, more specifically, local north-east-down (NED) coordinates where $+x$ is East, $+y$ is North, and $+z$ is down, with respect to the first position. Ignoring the height off of the surface of the earth, $z$, the first position would have the coordinates $(0, 0)$, and if the next position is 1-meter north, the NED coordinates would be $(1, 0)$, and so on.
https://www.mathworks.com/help/map/choose-a-3-d-coordinate-system.html
Our step-by-step process to compute NED coordinates is the following: