How to synchronise data for fusion in Kalman from multiple sensors with different timestamp information?

Question

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

Answer 1

Kalman filter consists of two steps: Time Update and Measurement Update. For asynchronous measurement case you will have variable sampling time which you will calculate using timestamp. For each sensor data(should come with a timestamp information) you might calculate the time difference between the last iteration and the current iteration. Then, you will use this time difference into the time update step. After that, measurement update will be performed with the corresponding measurement.

For each iteration:
    dt = timestamp - last_time
    TimeUpdate(dt)
    MeasurementUpdate()
    last_time = timestamp