examples with different sensor types fused together?

[gch]

As noted in one supporting notebook from your book , a very common use case for Kalman Filters is the ability to fuse information from different sensors, for example using GPS to directly measure position whilst using an accelerometer to measure accelerations.

From playing around with filterpy it seems like things support different sensor updates, but I'm having a hard time getting something working. Note, I set up a filter with a default hx function (i.e. hx_sensor1), and call:

update(sensor1_measurement)

Then I found if I did the following, I could at least get the second sensor to start to update:

kf.dim_z = SENSOR2_DIMS
kf.update(sensor2_measurement, R=sensor2_R, hx=hx_sensor2)

But things rapidly start to break down.

Are there any code samples showing something like this in operation @rlabbe ?

[gch]

I think I can get this working by simply having two KFs, never predicting on one of them, transferring state between them, then calling update individually based upon different sensor inputs. Still need to experiment if I can do this with one filter using the hx=hx_sensor2 route.

Fully working example (I think), based off of your example code in UKF.py:

import numpy as np
from filterpy.kalman import UnscentedKalmanFilter
from filterpy.kalman import MerweScaledSigmaPoints
from filterpy.common import Q_discrete_white_noise

def fx(x, dt):
    F = np.array([[1, 0, dt, 0],
                  [0, 1, 0, dt],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]], dtype=float)
    return np.dot(F, x)

def hx_pos(x):
    return x[0], x[1]

def hx_vel(x):
    return x[2], x[3]

dt = 0.1

# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1)

kf_pos = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx_pos, points=points)
kf_vel = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx_vel, points=points)

kf_pos.x = np.array([-1., 1., -1., 1]) # initial state
kf_pos.P *= 0.2 # initial uncertainty
z_std = 0.1
kf_pos.R = np.diag([z_std**2, z_std**2])
zvel_std = 0.1*10
kf_vel.R = np.diag([zvel_std**2, zvel_std**2])

kf_pos.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2)
kf_vel.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2)

zs = [[i+np.random.randn()*z_std, i+np.random.randn()*z_std] for i in range(50)] # measurements
zvels = [[10 + np.random.randn()*zvel_std, 10 + np.random.randn()*zvel_std] for i in range(50)]
for z, zvel in zip(zs, zvels):
    kf_pos.predict() ### add a dt if different from default
    kf_pos.update(z)
    kf_vel.x = kf_pos.x
    kf_vel.P = kf_pos.P
    kf_vel.sigmas_f = kf_pos.sigmas_f
    kf_vel.update(zvel)
    kf_pos.x = kf_vel.x
    kf_pos.P = kf_vel.P
    kf_pos.sigmas_f = kf_vel.sigmas_f
    print('POS:', kf_pos.x, 'log-likelihood', kf_pos.log_likelihood)
    print('VEL:', kf_vel.x, 'log-likelihood', kf_vel.log_likelihood)

[StohanzlMart]

Is this the intended use for fusing different sensors in filterpy? @rlabbe
If yes, please consider adding this to the documentation.