-
Notifications
You must be signed in to change notification settings - Fork 0
/
kalman_filter.py
36 lines (26 loc) · 903 Bytes
/
kalman_filter.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
# Write a program that will iteratively update and
# predict based on the location measurements
# and inferred motions shown below.
def update(mean1, var1, mean2, var2):
new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)
new_var = 1/(1/var1 + 1/var2)
return [new_mean, new_var]
def predict(mean1, var1, mean2, var2):
new_mean = mean1 + mean2
new_var = var1 + var2
return [new_mean, new_var]
measurements = [5., 6., 7., 9., 10.]
motion = [1., 1., 2., 1., 1.]
measurement_sig = 4.
motion_sig = 2.
mu = 0
sig = 10000
for n in range(len(measurements)):
[mu, sig] = update(mu, sig, measurements[n], measurement_sig)
print 'Update: ', [mu,sig]
[mu, sig] = predict(mu, sig, motion[n], motion_sig)
print 'Predict: ', [mu,sig]
#Please print out ONLY the final values of the mean
#and the variance in a list [mu, sig].
# Insert code here
print [mu, sig]