3
$\begingroup$

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.

  1. 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?
  2. 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

$\endgroup$

1 Answer 1

3
$\begingroup$

You can model your system as a linear time varying, where only the measurement matrix $H_k$ varies in time

\begin{align} x_{k+1} &= F\,x_k, \\ y_k &= H_k\,x_k. \end{align}

Namely in your case you can consider $y_k^i=H^i\,x_k$ ($i$ is just an index, not a power) to be the output of the $i$th sensor. So at a time $k$ when only sensor 1 is active you have $H_k=H^1$. Similar when only sensor 2 is active you have $H_k=H^2$. When both sensors are active you get

$$ H_k = \begin{bmatrix} H^1 \\ H^2 \end{bmatrix}. $$

When none of the sensors are active you have $H_k\in\mathbb{R}^{0\times n}$ (in your case $n=4$), which basically comes down to only doing the prediction step and not also the correction step of the Kalman filter.

When using this $H_k$, which varies in size over time, you also need to use noise covariance matrices of the appropriate size. I believe in your code this would be $R$ ($Q$ can stay the same size).

$\endgroup$
5
  • $\begingroup$ The measurement matrix values changes based on the sensor input. So, in coding terms, it would essentially mean like an if statement to check if measurement is from sensor 1 or 2 and then apply, the Kalman filter (measurement step at that instance (H.x)) to it? Is this understanding correct? And when there are no measurements, just run the prediction step? $\endgroup$
    – surajr
    Sep 6, 2019 at 14:34
  • $\begingroup$ I'm a bit confused as to how to model and code the condition when both sensors are active. Since we already have all the data with us in files and are just loading it to the measurement step, how would we define the time to consider each measurement from the sensors at some time interval? Should we define some function to call each measuremen? And since both give the position information, wouldn't we have twice the number of states for Xk (One for each sensor)? $\endgroup$
    – surajr
    Sep 6, 2019 at 14:38
  • $\begingroup$ @surajr Yes to your first comment. And for your second comment remember that both sensors are measuring the same state so the state dimension should remain the same. Namely in that case the correction step of the Kalman filter also acts as sensor fusion. I do assume that your electronics is running at a fixed sample time (so most of the time steps none of the sensors would give measurements). Another option would be to calculate the $F$ matrix depending on how much time has passed (set $\Delta t$ to this) but this will probably also require you to change the noise covariance matrices. $\endgroup$
    – fibonatic
    Sep 6, 2019 at 14:45
  • $\begingroup$ Since both sensors have timestamp information that are different to one another (Counter for Odometry and HH:MM:SS timestamp for GPS), we would need to first synchronise the start of both measurements before considering the sample time for each sensor (400ms for Odometry and 1s for GPS). Is this correct? What do you mean with changing the F matrix based on delta t? Is it changing the value of delta t based on the sampling time of the sensor? $\endgroup$
    – surajr
    Sep 6, 2019 at 15:16
  • $\begingroup$ @surajr No, essentially save the time you received the last measurement. Once you receive a measurement of any of the sensors calculate the $\Delta t$ from the difference between it and the current time and use that to calculate $F$. The other option would be, assuming that the sensors are "synchronized" (once in a while both sensors measurement at the same time) and use the greatest common divisor of the sample times as fixed time step, so $\Delta t$=200 ms. $\endgroup$
    – fibonatic
    Sep 6, 2019 at 16:33

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.