-
Notifications
You must be signed in to change notification settings - Fork 1
/
kf.py
65 lines (46 loc) · 1.5 KB
/
kf.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
58
59
60
61
62
63
import numpy as np
class KF:
def __init__(self, initial_x: float,
initial_v: float,
accel_variance: float ) -> None:
# moyenne de l'état GRV
self._x = np.array([initial_x, initial_v])
self.accel_variance = accel_variance
#covariance de l'état GRV
self._P = np.eye(2)
def predict(self, dt: float) -> None:
# x = F*x
# P = F * P * Ft + G * Gt *a
F = np.array([[1, dt], [0,1]])
new_x = F.dot(self._x)
G = np.array([0.5*dt**2,dt]).reshape((2,1))
new_P = F.dot(self._P).dot(F.T) + G.dot(G.T) * self.accel_variance
self._P = new_P
self._x = new_x
def update(self, meas_value: float, meas_variance: float):
# y = z-Hx
# S = H P Ht + R
# K = P Ht S⁻1
# P = (I - KH) * P
H = np.array([1,0]).reshape((1,2))
z = np.array([meas_value])
R = np.array([meas_variance])
y = z - H.dot(self._x)
S = H.dot(self._P).dot(H.T) + R
K = self._P.dot(H.T).dot(np.linalg.inv(S))
new_x = self._x + K.dot(y)
new_P = (np.eye(2)- K.dot(H)).dot(self._P)
self._P = new_P
self._x = new_x
@property
def cov(self) -> np.array:
return self._P
@property
def mean(self) -> np.array:
return self._x
@property
def pos(self)-> float:
return self._x[0]
@property
def vel(self)-> float:
return self._x[1]