/
run.py
43 lines (33 loc) · 1.11 KB
/
run.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
import numpy as np
import matplotlib.pyplot as plt
from collections import OrderedDict
from kalman import KalmanFilter
if __name__ == "__main__":
"""
Demo implementation of a Kalman Filter
For two hidden state variables x (position) and xv (velocity) with a constant acceleration
"""
# Initial estimates of position and velocity and (co)-variances
stateNaut = OrderedDict([
("position (m)", 4000),
("velocity (m/s)", 280)
])
processCovarianceNaut = np.diag([400.0, 25.0])
# Create the Kalman Filter with the initial state
# The filter implements the physics internally
KF = KalmanFilter(stateNaut, processCovarianceNaut)
# Add a list of observations to the filter
observations = np.array([
[4260, 282],
[4550, 285],
[4860, 286],
[5110, 290]
])
# And a covariance on the observations
observationCovariance = np.diag([625, 36])
for observation in observations:
KF.addObservation(observation, observationCovariance)
print(KF.currentState)
# Add observations as points
plt.plot(observations, marker="X", label="_nolegend_", linewidth=0)
KF.plot()