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

Available items

The developer of this repository has not created any items for sale yet. Need a bug fixed? Help with integration? A different license? **Create a request here**:

Readme

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