I built a toy Kalman filter to estimate velocity and acceleration of a system (1-dimensional to start), and I'm surprised that I can't get better location estimates than by calculating location without any filter at all.
As far as I can tell, I'm following some Kalman filter examples by-the-book. However, these examples don't explicitly show that the Kalman filter made any improvement over simpler processing alternatives, so I'm not sure if this is a scenario in which a Kalman filter should perform well.
Toy Kalman filter
- Fake location generated, e.g. 1000 samples over a period of a sine wave
- Fake velocity data derived from fake location data, Gaussian noise added
- Fake acceleration data derived from fake location data, Gaussian noise added
- Kalman filter estimates state [ acceleration velocity ]
- F = [ [ 1 0 ] [ dt 1 ] ]
- Q = covariance( acceleration data, velocity data )
- R = covariance( acceleration noise, velocity noise )
Evaluation: estimating final location
Location is integrated using $x_k = x_{k-1} + \frac{v_k+v_{k-1}}{2}dt + \frac{1}{2}\frac{a_k+a_{k-1}}{2}dt^2$. I'm comparing two final location estimates to the true final location: the estimate using $a$ and $v$ from the Kalman filter, and the estimate using the noisy $a$ and $v$ data. In this comparison, at best I can get the Kalman filter location error to equal the simple no-filter error.
Things I've tried
- making the noise asymmetric -- no change in Kalman performance over simpler process
- changing amplitude of noise, either and both of acc. and vel. -- again, no change
- making the noise scale time-dependent -- no change
- changing process matrices R and Q -- Kalman worsens with any change
- applying to 2-D, using a fake yaw rate measurement. The thought is that symmetrical velocity and yaw errors won't cancel in two dimensions, so estimates averaging closer to truth will perform better -- no improvement by Kalman
Sub-questions (which may answer the Question):
- Do I need to have a direct measurement method of the desired state (location) for Kalman to be useful (does integration undo the filtering)?
- Does Kalman only work when I have a known feedback? E.g., if this were a car, the filter only performs better than a simple algorithm if the throttle is a measured input?
Here's an example of the data I'm generating in python:
import numpy as np
sz = 1000 # size of data
dt = 1 # time interval
t = np.arange(sz*dt, step=dt) # time array
loc = -0.1*sz*np.sin(t*10/sz) + sz*np.sin(t/sz) # 1-D location vs. time
vel = -np.cos(t*10/sz) + np.cos(t/sz) # velocity (speed in 1-D for now)
acc = 10/sz*np.sin(t*10/sz) + -1/sz*np.sin(t/sz) # acceleration
# acceleration data with noise
acc_noise = 1e-2
acc_data = acc + np.random.normal(loc=0., scale=acc_noise, size=sz)
# velocity data with noise
vel_noise = 1e-1
vel_data = vel + np.random.normal(loc=0., scale=vel_noise, size=sz)
