Logo Questions Linux Laravel Mysql Ubuntu Git Menu
 

kalman 2d filter in python

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!

like image 648
Noam Peled Avatar asked Dec 16 '12 13:12

Noam Peled


People also ask

What is 2d Kalman filter?

In 2-D Kalman filter, we suppose that the measurement positions and are both independent, so we can ignore any interaction between them so that the covariance and is 0. We look at only the variance in the and the variance in the .

How do I use Kalman filter in Python?

A Kalman Filtering is carried out in two steps: Prediction and Update. Each step is investigated and coded as a function with matrix input and output. These different functions are explained and an example of a Kalman Filter application for the localization of mobile in wireless networks is given.

How do you implement extended Kalman filter in Python?

The estimation process for the Extended Kalman Filter is performed in two steps. First, the Kalman Gain is computed. Second, the state vector and state covariance for the new measurement time is computed with the Kalman Gain. Both of these steps require the use of an H matrix.


1 Answers

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() 

enter image description here

The red dots show the noisy position measurements, the green line shows the Kalman predicted positions.

like image 192
unutbu Avatar answered Nov 05 '22 02:11

unutbu