Coupled observations example

[danni]

Hi, I've been trying to build a coupled observations example, and I'm not entirely sure I have the dimensionality of the parameters correct, is this correct? If so it might be worth including as an example.

dt= 0.1  # 10 Hz samples
A = np.array([[1, dt], [0, 1]])
Q = np.array([[1.0, 0.1], [0, 0.1]])
H = np.diag([1, 1])
R = np.diag([0.5, 1.0])

kf = simdkalman.KalmanFilter(state_transition=A,
                             process_noise=Q,
                             observation_model=H,
                             observation_noise=R)

# Given speed and accel from independent sensors (doppler GPS and IMU)
# which are the same length and sampling rate.
#
# Mask out speeds that have high gradients as likely errors
speed[np.abs(np.gradient(speed, 1/10)) > 3] = np.nan

# Prepare an array of shape (1, n, 2)
data = np.array([np.array([speed, accel]).T])
data = kf.smooth(data, initial_value=data[0,0,:], initial_covariance=np.eye(2) * 0.1)
speed, accel = data.observations.mean[0].T

Masking out the bad speeds feels like it shouldn't be required, that coupling the processes together should result in matching acceleration and velocity processes. What am I doing wrong?

[oseiskar]

Modeling with Kalman filters is a bit of an art and there's probably no single right answer to your problem. However, a few things stand out.

The dimensionality of the parameters look correct, but some of the values look a bit odd. In particular, the matrix Q must always be symmetric. In this case, it most likely should be np.diag([a, b])**2 for some a, b.

To understand the parameters better, it usually helps to think of the underlying dynamic and measurement models. In this case, the state is x = [speed, accel]^T and the dynamic model changes it as

x -> A * x + [q_vel, q_acc]^T

where q_vel and q_acc are random variables whose variances are given by Q. So if Q = diag([s_v, s_acc])**2, then q_vel has standard deviation s_v.

With A = np.array([[1, dt], [0, 1]]), you get

speed -> speed + accel * dt + q_vel
accel -> accel + q_acc

the first one looks good. Due to physics, the noise level s_v should be zero and could be set to a very small number (like 1e-9). The second one looks a bit weird though. I don't see an obvious reason why acceleration should remain at a steady level. You could simply try changing this to A = np.array([[1, dt], [0, 0]]) so you would get the dynamics

accel -> q_acc

where the standard deviation s_acc would constrain the momentary accelerations you expect in the model.

The observation model (with H = I)

y = x + [r_vel, r_acc]^T

looks fine. If R = np.diag([s_vel_meas, s_acc_meas]**2), this means that you expect the velocity measurement to be the true velocity plus random noise whose standard deviation is s_vel_meas and the same thing for acceleration. Suitable values for the measurement error levels depend on the errors you expect in the measurements (and usually some extra for model inaccuracies).

So I recommend trying with

s_acc = 0.1
s_vel_meas = 0.5
s_acc_meas = 1.0

dt = 0.1  # 10 Hz samples
A = np.array([[1, dt], [0, 0]])
Q = np.diag([1e-6, s_acc**2])
H = np.diag([1, 1])
R = np.diag([s_vel_meas**2, s_acc_meas**2])

and tweaking the s_ parameters.

I hope this helps :)

[danni]

Thank you so much, you have just fixed some mistakes in my understanding of Kalman. My next cut looks much closer to coupled.

Which looks believable given the data.

Of course it falls apart a bit because the measurement noise in the speed sensor is time variant as a function of angular velocity (actually all sensors get funky as a function of angular velocity). Is it possible to provide a series for observation noise measurements as a function of time? Alternatively would you set up another state inside the filter and feed in the sensor most strongly correlated to the noise?

[oseiskar]

If the noise or transition matrices vary from sample to sample, then you can use the primitives module. (see example), where you have to iterate over the samples manually. For smoothing, this is a bit inconvenient since you also have to first do a forward pass storing the states and then a backward pass. Currently there are no good examples for this (only the implementation of kf.smooth, which is not very readable), but I'll try to come up with something.

[oseiskar]

Added a new example which shows how to use the simdkalman.primitives module for smoothing if some of the matrices vary from sample to sample.

Also closing the ticket as this discussion is more about modeling with Kalman filters in general and not so much about this library.

[danni]

Really appreciate this, thanks.