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

Question

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()

Answer 1

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: