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])
coords.head(3)
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()