-
Notifications
You must be signed in to change notification settings - Fork 0
/
KalmanFilter.py
57 lines (45 loc) · 1.82 KB
/
KalmanFilter.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
import numpy as np
class KalmanFilter(object):
def __init__(self, dt, point):
self.dt = dt # delta t
# Initial State Matrix
self.E = np.matrix([[point[0]],
[point[1]],
[0],
[0]
])
# Transition Matrix
self.A = np.matrix([[1, 0, self.dt, 0],
[0, 1, 0, self.dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# Noise Matrix ( their are all independent from each other)
self.Q = np.matrix([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# Observation matrix (Only x and y coordinates are observed)
self.H = np.matrix([[1, 0, 0, 0],
[0, 1, 0, 0]
])
# Noise matrix for observation (that must be provided by used sensor manifactor company)
self.R = np.matrix([[1, 0],
[0, 1]
])
self.P = np.eye(self.A.shape[1])
def predict(self):
self.E = np.dot(self.A, self.E)
# Covariance of the error
self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q
return self.E
def update(self, z):
# Kalman gain
S = np.dot(self.H, np.dot(self.P, self.H.T)) + self.R
K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
# Correction / innovation
self.E = np.round(self.E + np.dot(K, (z - np.dot(self.H, self.E))))
I = np.eye(self.H.shape[1])
self.P = (I - (K*self.H))*self.P
return self.E