UKF Sensor Fusion with multiple update frequencies? OR How to not write zero to measurement_vector? Solved! You may want to extend your UKF documentation with this!

[StohanzlMart]

I tried implementing GNSS and IMU fusion with filterpy.

Almost finished, but a "small" problem:

When I update one sensor, how to output a measurement vector that does not put a measurement of zero into unmeasured variables?

I looked a bit at your source code, but have enough for today... is it enough to put unmeasured variables to [None]?

e.g. hx_imu(): return [None, None, None, None, None, None, ax, ay, az ,...] if I only have data for [6:9]?

Please notice me Senpai @rlabbe :]

[StohanzlMart]

I read various of your answers and have difficulties understanding them.

For example, could you go into a bit more detail what you mean with your pseudo code here?

I quote:

#get sensor 1
F = compute_f(dt) # is this the state transition function?
kf.predict()
kf.update(z1, R=R1, H=H1) # why u no pass F here as fx=???

#get sensor 2
F = compute_f(dt) # ok now you use the same function again to write the same variable (that you do not use)?
kf.predict()
kf.update(z2, R=R2, H=H2) # ok, but what to return from H2?

For example: I have the following state transition function (not in matrix form for my sanity).

def state_transition_function(state, dt):
    # Unpack the state vector [px, py, pz, vx, vy, vz, ax, ay, az, θx, θy, θz].T
    px, py, pz, vx, vy, vz, ax, ay, az, thetax, thetay, thetaz = state

    # Predict the next state based on current state and time elapsed
    next_px = px + vx * dt + 0.5 * ax * dt**2
    next_py = py + vy * dt + 0.5 * ay * dt**2
    next_pz = pz + vz * dt + 0.5 * az * dt**2

    next_vx = vx + ax * dt
    next_vy = vy + ay * dt
    next_vz = vz + az * dt

    return [next_px, next_py, next_pz, next_vx, next_vy, next_vz, ax, ay, az, thetax, thetay, thetaz]

Is the H_imu ok this way so it only affects the wanted state variables? I'd do ukf.update([0,0,0,0,0,0,ax,ay,az,θx,θy,θz], hx=H_imu). Or do I have to use the approach mentioned in this issue, or is it possible by modifying your pseudo code? Please be specific in your answer, because I cannot fill in the blanks in my understanding of this highly mathematical topic.

def H_imu():
    # Creating a 12x12 matrix filled with zeros
    H = np.zeros((12, 12))

    # Filling 7,7 8,8 up to position 12,12 with ones >>> HERE H is only zeros from row 1 to 6. Is this bad?
    for i in range(7, 13):
        H[i-1, i-1] = 1

    return H

def H_gnss():
    # Creating a 5x12 matrix filled with zeros
    H = np.zeros((5, 12))

    # Filling 1,1 2,2 up to position 5,5 with ones
    for i in range(1, 6):
        H[i-1, i-1] = 1

    return H

Thank you!

[StohanzlMart]

I found a solution, maybe @rlabbe wants to extend his book by this? Annoying 👺, how long I had to work for this simple UKF with a well made python library, just because the documentation/book is missing this more realistic problem yet.

def state_transition_function(x, dt):
    # State vector x = [px, vx, ax, py, vy, ay, θz].T
    F = np.array([
        [1, dt, 0.5*dt**2, 0,  0,         0, 0],
        [0,  1,        dt, 0,  0,         0, 0],
        [0,  0,         1, 0,  0,         0, 0],
        [0,  0,         0, 1, dt, 0.5*dt**2, 0],
        [0,  0,         0, 0,  1,        dt, 0],
        [0,  0,         0, 0,  0,         1, 0],
        [0,  0,         0, 0,  0,         0, 1]
        ], dtype=np.float64)
    return F @ x

def H_imu(x):
    H = np.array([
       [0, 0, 1, 0, 0, 0, 0],
       [0, 0, 0, 0, 0, 1, 0],
       [0, 0, 0, 0, 0, 0, 1]
       ], dtype=np.float64)
    return H @ x

def H_gnss(x):
    H = np.array([
       [1, 0, 0, 0, 0, 0, 0],
       [0, 1, 0, 0, 0, 0, 0],
       [0, 0, 0, 1, 0, 0, 0],
       [0, 0, 0, 0, 1, 0, 0],
       [0, 0, 0, 0, 0, 0, 1]
       ],dtype=np.float64)
    return H @ x

def main():
    # Create the UKF
    ukf = UnscentedKalmanFilter(dim_x=7, dim_z=5, dt=dt, fx=state_transition_function,
                                hx=H_imu, points=points)

    # Process noise
    q = block_diag(np.eye(2) * std_pos**2, np.eye(2) * std_vel**2, 
                   np.eye(2) * std_acc**2, np.eye(1) * std_gyro**2)
    ukf.Q = q

    # TODO extract and set initial points and CoVar

    while/for ... data_point in data:
        ukf.predict()
        if "imu" in data_point:
                imu_measurement = extract_imu_measurement(data_point)
                # Measurement noise TODO: Scale this with sampling covariance
                R_imu = block_diag(np.eye(2) * std_acc**2, np.eye(1) * std_gyro**2)
                ukf.update(z=imu_measurement, hx=H_imu, R=R_imu)
        if "gnss" in data_point:
                gnss_measurement =extract_gnss_measurement(data_point, ref_lat, ref_lon)
                # Measurement noise TODO: Scale this with sampling covariance and uncertainty of GNSS measurement
                R_gnss = block_diag(np.eye(2) * std_pos**2, np.eye(2) * std_vel**2, np.eye(1) * std_gyro**2)
                ukf.update(z= gnss_measurement, hx=H_gnss, R=R_gnss) # do the update
        # Get filtered data out
        lat, lon = local_to_latlon(ukf.x[0], ukf.x[3], ref_lat, ref_lon)
        filtered_data.append({'timestamp': data_point['timestamp'], 'latitude': lat, 'longitude': lon})

Note: Make sure to use SI-units everywhere! (especially for dt!!!)

Your book is nice, although the problems you solve there are very limited. One has to search across two chapters (8 & 10) and many examples to piece together how your stuff is supposed to work. If only in chapter 8 your "Sensor fusion: Different Data Rates" Example wasn't such a toy example (different sizes of measurements?) one would easier see how to use H to switch for sensors. Or you could even use one example like this to expand your UKF chapter 10?

Kind regards and I'm looking forward to your answer! 🐣

[StohanzlMart]

Are you open to a collab regarding that bonus documentation?

All the best, *eat the rest ;]