Kalman Filter - How to combine data from sensors with different measurement rates?

Question

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

Answer 1

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