Open gch opened 2 years ago
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)
Is this the intended use for fusing different sensors in filterpy? @rlabbe If yes, please consider adding this to the documentation.
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 defaulthx
function (i.e.hx_sensor1
), and call:Then I found if I did the following, I could at least get the second sensor to start to update:
But things rapidly start to break down.
Are there any code samples showing something like this in operation @rlabbe ?