# hbcbh1999/kalman-filter forked from zziz/kalman-filter

Kalman Filter implementation in Python using Numpy only in 30 lines.
This branch is even with zziz:master.
Fetching latest commit…
Cannot retrieve the latest commit at this time.
Type Name Latest commit message Commit time
Failed to load latest commit information.
asset
kalman-filter.py

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

You can’t perform that action at this time.