I'm trying to implement a Kalman filter for tracking the position of a vehicle with the help of position data from GPS and Odometry measurements. The GPS data (WGS84 format collected from an app on an iPhone) provides a reading approximately every 1 second and contains information about the latitude, longitude, elevation and timestamp. The Odometry (Data in HDF5 format) is assumed to provide a reading once every 200ms and contains information about the vehicle position in x and y, the timestamp (given as a counter that does not increment in equal steps), step id and vehicle position angle.
- How do we combine the position data coming at different measurement rates from these sensors and provide it as a measurement input to the Kalman?
- Is it possible to sample the measurement data coming from both sensors? How can we call the Kalman to update measurement whenever a new data is received from the sensors?
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.
\begin{align} x_{k+1} &= x_k + \dot{x}_k\,\Delta t \\ \dot{x}_{k+1} &= \dot{x}_k \end{align}
Therefore, the state transition matrix would be (Considering 2D positioning (x,y) with latitude and longitude coordinates):
F = [[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]]
CODE
import h5py
import numpy as np
from tkinter import *
import matplotlib.pyplot as plt
import gpxpy
import pandas as pd
import utm
"Code for reading the HDF5 data"
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()
"Code for reading GPX file"
with open('my_run_001.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)
"Converting Lat Long to 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
"Kalman F and H matrix definition"
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
"Calling Kalman"
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()
Files:
GPX Reference: 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