1

I'm using data from GPS and from vehicle sensors (Ultrasonic) separately to track the position of a vehicle using Kalman filter. I want to implement sensor fusion for the received measurement data and check for improvement of filter performance. However, I have a problem with regards to combining the measurements since they are of different types.

The GPS data (GPX file exported from an app) gives information about the Latitude, Longitude, Elevation and Timestamp (Format: YYYY-MM-DD HH.MM.SS) and is then converted to UTM. 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).

How is it possible to combine/synchronise position data from 1 coordinate system (GPS - x,y UTM data) with the position coordinates of another sensor system?

Assumptions: GPS updates at 1 Hz and data received from Ultrasonic sensors every 500ms.

Reference:

GPX file:
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

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