examples with different sensor types fused together? about filterpy

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