1
$\begingroup$

I'm using Kalman filter to track the position of a vehicle and receive position data from 2 sensors: A GPS sensor and an Ultrasonic sensor for which I want to implement sensor fusion into the Kalman. However, the data formats between the 2 sensors have different timestamps and hence, I cannot directly consider it together without first synchronising them. Does anyone have any ideas or suggestions as to how this can be implemented in Python? All suggestions are welcome. Thanks!

The GPS data (GPX file exported from an app) collected gives information about the Latitude, Longitude, Elevation and Timestamp (Format: YYYY-MM-DD HH.MM.SS). The HDF5 data collected from a vehicle (HDF5 Format) gives information about the Vehicle Position X, Vehicle Position Y, and a timestamp that is updated like a counter. (Present in file step_S). I'm able to import the GPS and HDF5 files and read its contents but not able to synchronise them.

GPX file reference in below link:
https://github.com/stevenvandorpe/testdata/blob/master/gps_coordinates/gpx/my_run_001.gpx
HDF5 data:
https://github.com/surishell/Kalman-HDF5/blob/master/TestRoute.hdf5

One idea that I had would be checking the timestamp corresponding to the first non-zero value of X and Y positions of the HDF5 file and setting it as a base index against the timestamp of the first position at which GPS value changes. (Assumption: GPS updates at 1 Hz and the Ultrasonic at 500 ms. Once the index is known, the sensor values can be separated and synchronised by corresponding time step). However, I'm not sure of the implementation and could really use some help from the community.

Code for reading the HDF5 data:

import h5py
import numpy as np
from tkinter import *
import matplotlib.pyplot as plt

f = h5py.File(
    "C:\Users\Suraj\Desktop\TestRoute.hdf5","r")

with f:

    st = f.__getitem__("daste_step_S")
    t = list(zip(*st[()]))
    step_time = t[0]
    step_id = t[1]
    step_map_in_index = t[2]
    step_map_out_index = t[3]
    step_v_pos_x = t[4]
    step_v_pos_y = t[5]
    step_v_pos_angle = t[6]

    print(step_v_pos_x)
    test1 = [t - s for s, t in zip(step_v_pos_x, step_v_pos_x[1:])]
    print(test1)
    ax = plt.axes(projection="3d")
    ax.plot3D(step_v_pos_x, step_v_pos_y, step_time, 'gray')
    plt.show()

UPDATE

Kalman Filter:

I've considered a standard motion model: Constant Velocity (Assuming that acceleration plays no effect on this vehicle's position estimation) and therefore, my states consist of only position and velocity.
𝑥𝑘+1 = 𝑥𝑘 + 𝑥˙𝑘 Δ𝑡
𝑥˙𝑘+1 = 𝑥˙𝑘

Therefore, the state transition matrix would be (Considering 2D positioning (x,y) with latitude and longitude coordinates):

A = [[1.0, 0.0, Δ𝑡, 0.0],
     [0.0, 1.0, 0.0, Δ𝑡],
     [0.0, 0.0, 1.0, 0.0],
     [0.0, 0.0, 0.0, 1.0]]

Since we only have position measurement data available, we can correspondingly write the measurement matrix as:

H = [[1.0, 0.0, 0.0, 0.0],
     [0.0, 1.0, 0.0, 0.0]]

Code for reading GPS data and implementing Kalman filter for Position tracking

import gpxpy
import pandas as pd
import numpy as np
import utm
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[::18], coords.lat[::18],'ro')
plt.show()
#plt.plot(coords.lon, coords.lat)

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

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

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

demo_kalman_xy()  
$\endgroup$
  • $\begingroup$ How is the ultrasonic sensor measuring the x and y positions of the the vehicle? Is the vehicle in a fixed box and the ultrasonic sensors are measuring the sides? Can the vehicle rotate? This will get more complicated if the vehicle can change the way it's facing. $\endgroup$ – holmeski Aug 28 at 21:03
  • $\begingroup$ You mention that you are tracking a vehicle. Do you have a model for how the vehicle moves? $\endgroup$ – holmeski Aug 28 at 23:42
  • $\begingroup$ Synchronizing the measurements is not necessary. You can trigger Kalman filter measurement updates whenever the sensor(s) generate a new measurement. $\endgroup$ – holmeski Aug 28 at 23:47
  • $\begingroup$ Thanks for your comments. Unfortunately, I do not have a definite answer for all the queries. With regards to the first comment, the position data from ultrasonic sensors is purely made from an assumption after reading the extracted HDF5 data file (Column: Veh_Pos_X and Veh_Pos_Y). We obtained this dataset for the university from an external source and the only information we have is that the data was collected from a test run on public roads and that, there are a collection of 8 ultrasonic sensors present on the vehicle (4 in the front and 4 in the back). $\endgroup$ – surajr Aug 28 at 23:57
  • $\begingroup$ Yes I do. I will update the question with my current code used for the Kalman filter (Only GPS implemented in this). The vehicle model that I consider is a Constant Velocity model. Have posted another query on Signal Processing site with regards to the parameters that can maybe shed a light on this implementation. dsp.stackexchange.com/questions/60311/… $\endgroup$ – surajr Aug 29 at 0:02

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.