2
$\begingroup$

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()
$\endgroup$
3
  • $\begingroup$ 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. $\endgroup$
    – Maxtron
    Aug 27, 2019 at 2:51
  • $\begingroup$ 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/… $\endgroup$
    – surajr
    Aug 27, 2019 at 18:50
  • $\begingroup$ 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. $\endgroup$
    – Maxtron
    Aug 27, 2019 at 19:17

1 Answer 1

3
$\begingroup$

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:

enter image description here

$\endgroup$
4
  • $\begingroup$ 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? $\endgroup$
    – surajr
    Aug 27, 2019 at 18:47
  • $\begingroup$ 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/… $\endgroup$
    – Maxtron
    Aug 27, 2019 at 19:14
  • $\begingroup$ Thanks for your help :) $\endgroup$
    – surajr
    Aug 27, 2019 at 19:18
  • $\begingroup$ @Maxtron, why is it necesary to convert to UTM? for kalman filter it shouldn't make any difference whether its in decimal form or UTM form am I right? if so then why? is that solely because of the plotting section then? $\endgroup$
    – Hossein
    Feb 12, 2022 at 10:34

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.