# Error in using Kalman Filter for 2D Position Estimation in Python

This is my first question on DSP Stack exchange, so I apologise if it is poorly worded. I have some positioning data from a vehicle (GPX Format, collected through Strava) and want to use a Kalman filter as a first step to track the position of the car and determine the accuracy of the GPS measurement. However, the output estimate of the Kalman filter from my implementation seems completely wrong and absolutely does not match the position information and plot from the data. Can someone help me figure out what is wrong with this implementation and provide some methods to fix this problem? Appreciate all the help in advance. Thanks!

As a reference, I used the code given in the below link and correspondingly modified it based on my requirements: https://stackoverflow.com/questions/13901997/kalman-2d-filter-in-pythonn. Background: I only have a basic understanding of the working of the Kalman filter and am a new user to Python, but for this implementation, I've considered a constant velocity model with states as Position and Velocity, time step is assumed to be 1 (Considering GPS updates at 1Hz), the measurement matrix only considers the position information and the actual measurement gives the corresponding longitude and latitude values. The test GPX file was obtained from the following link: https://github.com/stevenvandorpe/testdata/blob/master/gps_coordinates/gpx/my_run_001.gpx

My implementation in Python:

import gpxpy
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

with open('test3.gpx') as fh:
gpx_file = gpxpy.parse(fh)
segment = gpx_file.tracks[0].segments[0]
coords = pd.DataFrame([
{'lat': p.latitude,
'lon': p.longitude,
'ele': p.elevation,
'time': p.time} for p in segment.points])
plt.plot(coords.lon[::36], coords.lat[::36], 'ro')
plt.show()

def kalman_xy(x, P, measurement, R,
Q = np.array(np.eye(4))):

return kalman(x, P, measurement, R, Q,
F=np.array([[1.0, 0.0, 1.0, 0.0],
[0.0, 1.0, 0.0, 1.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]]),
H=np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0]]))

def kalman(x, P, measurement, R, Q, F, H):

y = np.array(measurement).T - np.dot(H,x)
S = H.dot(P).dot(H.T) + R  # residual convariance
K = np.dot((P.dot(H.T)), np.linalg.pinv(S))
x = x + K.dot(y)
I = np.array(np.eye(F.shape[0]))  # identity matrix
P = np.dot((I - np.dot(K,H)),P)

# PREDICT x, P
x = np.dot(F,x)
P = F.dot(P).dot(F.T) + Q

return x, P

def demo_kalman_xy():
x = np.array([[100.0, 0.0, 0.0, 100.0]]).T
P = np.array(np.eye(4))*1000 # initial uncertainty
plt.plot(coords.lon[::36], coords.lat[::36], 'ro')
result = []
R = 0.01**2
for meas in zip(coords.lon, coords.lat):
x, P = kalman_xy(x, P, meas, R)
result.append((x[:2]).tolist())
kalman_x, kalman_y = zip(*result)
plt.plot(kalman_x, kalman_y, 'g-')
plt.show()

demo_kalman_xy()

• I don't think you can use lat long position directly into the Kalman filter. You need to convert lat and long degrees to kms, and then use the kms in the Kalman filter. – Maxtron Aug 27 '19 at 2:51
• I definitely didn't consider the necessary conversion of angular coordinates (Lat long) to Cartesian (x,y). Thanks for the hint! I have 2 additional queries to this conversion of data. 1. What is the accuracy of this projection transformation? 2. I also read about directly converting the data into Cartesian coordinates (Considering the Earth is a sphere). Which technique would be best suited for this application? Reference: stackoverflow.com/questions/1185408/… – surajr Aug 27 '19 at 18:50
• 1. If it is a one-to-one mapping, it should be fairly accurate. Also, it makes an assumption that the Earth is spherical and not ellipsoid. 2. I don't know. – Maxtron Aug 27 '19 at 19:17

The main reason why your Kalman filter is not working is because you are not converting lat and lon values to kms. In the code below, I defined a new function called lat_lon_posx_posy which converts lat and lon values to px and py values in mts. You will need to make the following changes to your code.

• Include the following function

import utm

def lat_log_posx_posy(coords):

px, py = [], []
for i in range(len(coords.lat)):
dx = utm.from_latlon(coords.lat[i], coords.lon[i])
px.append(dx[0])
py.append(dx[1])
return px, py

• Change demo_kalman_xy() to the following:

def demo_kalman_xy():

px, py = lat_log_posx_posy(coords)
plt.plot(px[::18], py[::18], 'ro')
plt.show()

x = np.array([px[0], py[0], 0.01, 0.01]).T
P = np.array(np.eye(4))*1000 # initial uncertainty
result = []
R = 0.01**2
for meas in zip(px, py):
x, P = kalman_xy(x, P, meas, R)
result.append((x[:2]).tolist())
kalman_x, kalman_y = zip(*result)
plt.plot(px[::18], py[::18], 'ro')
plt.plot(kalman_x, kalman_y, 'g-')
plt.show()

• Final Plot:

• Perfect! Thank you :) I want to clarify one interesting point that I observed with regards to the plot data. Before converting the lat long position data to UTM (Plot defined after the data frame), the plot is defined with Latitude on the X-axis and Longitude on the Y-Axis. After conversion, in order to obtain the same plot, we see that x-axis of the plot is px which is derived from latitude data and y-axis is py which is derived from the longitude data. Is there a particular reason for the axis elements being switched? – surajr Aug 27 '19 at 18:47
• Can you mark the answer as correct? Thanks. If you refer to the utm package, you should have an answer. Please refer to the link below stackoverflow.com/questions/6778288/… – Maxtron Aug 27 '19 at 19:14
• Thanks for your help :) – surajr Aug 27 '19 at 19:18