https://github.com/zziz/kalman-filter

Kalman Filter implementation in Python using Numpy only in 30 lines.
https://github.com/zziz/kalman-filter

dynamic-systems filter kalman kalman-filter numpy python

Last synced: 1 day ago
JSON representation

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

• Host: GitHub
• URL: https://github.com/zziz/kalman-filter
• Owner: zziz
• Created: 2018-09-05T12:45:03.000Z (over 6 years ago)
• Default Branch: master
• Last Pushed: 2022-09-14T03:13:14.000Z (over 2 years ago)
• Last Synced: 2025-01-13T20:11:55.615Z (9 days ago)
• Topics: dynamic-systems, filter, kalman, kalman-filter, numpy, python
• Language: Python
• Homepage:
• Size: 73.2 KB
• Stars: 369
• Watchers: 9
• Forks: 86
• Open Issues: 5
• Metadata Files:
  • Readme: README.md

Awesome Lists containing this project

README

        
Implementation of Kalman filter in 30 lines using Numpy. All notations are same as in Kalman Filter [Wikipedia Page](https://en.wikipedia.org/wiki/Kalman_filter).   

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```

```python

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](asset/Figure_1.png)