Need help with kalman-filter?
Click the “chat” button below for chat support from the developer who created it, or find similar developers for support.

About the developer

zziz
166 Stars 51 Forks 4 Commits 3 Opened issues

Description

Kalman Filter implementation in Python using Numpy only in 30 lines.

Services available

!
?

Need anything else?

Contributors list

# 635
Python
eccv
cvpr
4 commits

Implementation of Kalman filter in 30 lines using Numpy. All notations are same as in Kalman Filter Wikipedia Page.

It is a generic implementation of Kalman Filter, should work for any system, provided system dynamics matrices are set up properly. Included example is the prediction of position, velocity and acceleration based on position measurements. Synthetic data is generated for the purpose of illustration.

Running:

python kalman-filter.py
import numpy as np

class KalmanFilter(object): def init(self, F = None, B = None, H = None, Q = None, R = None, P = None, x0 = None):

    if(F is None or H is None):
        raise ValueError("Set proper system dynamics.")

    self.n = F.shape[1]
    self.m = H.shape[1]

    self.F = F
    self.H = H
    self.B = 0 if B is None else B
    self.Q = np.eye(self.n) if Q is None else Q
    self.R = np.eye(self.n) if R is None else R
    self.P = np.eye(self.n) if P is None else P
    self.x = np.zeros((self.n, 1)) if x0 is None else x0

def predict(self, u = 0):
    self.x = np.dot(self.F, self.x) + np.dot(self.B, u)
    self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
    return self.x

def update(self, z):
    y = z - np.dot(self.H, self.x)
    S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))
    K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
    self.x = self.x + np.dot(K, y)
    I = np.eye(self.n)
    self.P = np.dot(np.dot(I - np.dot(K, self.H), self.P), 
        (I - np.dot(K, self.H)).T) + np.dot(np.dot(K, self.R), K.T)

def example(): dt = 1.0/60 F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]]) H = np.array([1, 0, 0]).reshape(1, 3) Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]]) R = np.array([0.5]).reshape(1, 1)

x = np.linspace(-10, 10, 100)
measurements = - (x**2 + 2*x - 2)  + np.random.normal(0, 2, 100)

kf = KalmanFilter(F = F, H = H, Q = Q, R = R)
predictions = []

for z in measurements:
    predictions.append(np.dot(H,  kf.predict())[0])
    kf.update(z)

import matplotlib.pyplot as plt
plt.plot(range(len(measurements)), measurements, label = 'Measurements')
plt.plot(range(len(predictions)), np.array(predictions), label = 'Kalman Filter Prediction')
plt.legend()
plt.show()

if name == 'main': example()

Output

Result

We use cookies. If you continue to browse the site, you agree to the use of cookies. For more information on our use of cookies please see our Privacy Policy.