kalman 2d filter in python
Here is my implementation of the Kalman filter based on the equations given on wikipedia. Please be aware that my understanding of Kalman filters is very rudimentary so there are most likely ways to improve this code. (For example, it suffers from the numerical instability problem discussed here. As I understand it, this only affects the numerical stability when Q
, the motion noise, is very small. In real life, the noise is usually not small, so fortunately (at least for my implementation) in practice the numerical instability does not show up.)
In the example below, kalman_xy
assumes the state vector is a 4-tuple: 2 numbers for the location, and 2 numbers for the velocity.
The F
and H
matrices have been defined specifically for this state vector: If x
is a 4-tuple state, then
new_x = F * x
position = H * x
It then calls kalman
, which is the generalized Kalman filter. It is general in the sense it is still useful if you wish to define a different state vector -- perhaps a 6-tuple representing location, velocity and acceleration. You just have to define the equations of motion by supplying the appropriate F
and H
.
import numpy as np
import matplotlib.pyplot as plt
def kalman_xy(x, P, measurement, R,
motion = np.matrix('0. 0. 0. 0.').T,
Q = np.matrix(np.eye(4))):
"""
Parameters:
x: initial state 4-tuple of location and velocity: (x0, x1, x0_dot, x1_dot)
P: initial uncertainty convariance matrix
measurement: observed position
R: measurement noise
motion: external motion added to state vector x
Q: motion noise (same shape as P)
"""
return kalman(x, P, measurement, R, motion, Q,
F = np.matrix('''
1. 0. 1. 0.;
0. 1. 0. 1.;
0. 0. 1. 0.;
0. 0. 0. 1.
'''),
H = np.matrix('''
1. 0. 0. 0.;
0. 1. 0. 0.'''))
def kalman(x, P, measurement, R, motion, Q, F, H):
'''
Parameters:
x: initial state
P: initial uncertainty convariance matrix
measurement: observed position (same shape as H*x)
R: measurement noise (same shape as H)
motion: external motion added to state vector x
Q: motion noise (same shape as P)
F: next state function: x_prime = F*x
H: measurement function: position = H*x
Return: the updated and predicted new values for (x, P)
See also http://en.wikipedia.org/wiki/Kalman_filter
This version of kalman can be applied to many different situations by
appropriately defining F and H
'''
# UPDATE x, P based on measurement m
# distance between measured and current position-belief
y = np.matrix(measurement).T - H * x
S = H * P * H.T + R # residual convariance
K = P * H.T * S.I # Kalman gain
x = x + K*y
I = np.matrix(np.eye(F.shape[0])) # identity matrix
P = (I - K*H)*P
# PREDICT x, P based on motion
x = F*x + motion
P = F*P*F.T + Q
return x, P
def demo_kalman_xy():
x = np.matrix('0. 0. 0. 0.').T
P = np.matrix(np.eye(4))*1000 # initial uncertainty
N = 20
true_x = np.linspace(0.0, 10.0, N)
true_y = true_x**2
observed_x = true_x + 0.05*np.random.random(N)*true_x
observed_y = true_y + 0.05*np.random.random(N)*true_y
plt.plot(observed_x, observed_y, 'ro')
result = []
R = 0.01**2
for meas in zip(observed_x, observed_y):
x, P = kalman_xy(x, P, meas, R)
result.append((x[:2]).tolist())
kalman_x, kalman_y = zip(*result)
plt.plot(kalman_x, kalman_y, 'g-')
plt.show()
demo_kalman_xy()
The red dots show the noisy position measurements, the green line shows the Kalman predicted positions.
Noam Peled
I'm an instructor in Harvard medical school and the MGH/HST Martinos Center for Biomedical Imaging. I'm mainly interested in analyzing neuroimaging data from multi-modalities: fMRI, MEG, iEEG, etc, and how each modality relates to the others. I'm also interested in decoding human behavior using neuroimaging data and their facial expressions.
Updated on July 09, 2022Comments
-
Noam Peled almost 2 years
My input is 2d (x,y) time series of a dot moving on a screen for a tracker software. It has some noise I want to remove using Kalman filter. Does someone can point me for a python code for Kalman 2d filter? In scipy cookbook I found only a 1d example: http://www.scipy.org/Cookbook/KalmanFiltering I saw there is implementation for Kalman filter in OpenCV, but couldn't find code examples. Thanks!
-
Sophia about 5 yearsTo calculate the initial uncertainty, as in
P = np.matrix(np.eye(4))*1000
. Why do you multiply by 1000? -
Sandu Ursu over 4 yearsShouldn't there be matrix multiplication in the many places where you use "*"?
-
LudvigH over 3 yearsyour example is 1d only. the question asks specifically for 2d data
-
Marco Cerliani over 3 yearsIn my case, x is the time (assumed to be increasing) and y the values reached by the series, so 2D... it's the same case reported in the answer above where observed_x and observed_y are two increasing quantity
-
hyperspasm over 3 years@SanduUrsu when the arguments to the * operator are of type np.matrix (as opposed to np.array) matrix multiplication is performed. To be safe though, the @ operator can be used to ensure matrix multiplication in either case.
-
hyperspasm over 3 yearsIn the example
demo_kalman_xy()
R should really be a 2x2 (measurement noise covariance) matrix, for exampleR = np.matrix([[r,0], [0,r]])
. The default value of Q inkalman_xy()
is also probably too high to easily see the effects of adjusting R.