summaryrefslogtreecommitdiffhomepage
path: root/filter-kalman
diff options
context:
space:
mode:
authorDaMichel <mw.pub@welter-4d.de>2016-07-09 10:38:12 +0200
committerStanislaw Halik <sthalik@misaki.pl>2016-07-10 05:59:02 +0200
commitef900b344578249aab915625e33d991865cc3d08 (patch)
tree63acb45eccb87f6e127d585a1f4792c9d2b1d582 /filter-kalman
parent0f2806bb5bc5ccd9a4d6772fe57563f981f76e18 (diff)
simple sim in python for playing with kalman filtering
Diffstat (limited to 'filter-kalman')
-rw-r--r--filter-kalman/kalman_simulation.py76
1 files changed, 76 insertions, 0 deletions
diff --git a/filter-kalman/kalman_simulation.py b/filter-kalman/kalman_simulation.py
new file mode 100644
index 00000000..7a320426
--- /dev/null
+++ b/filter-kalman/kalman_simulation.py
@@ -0,0 +1,76 @@
+# -*- coding: utf-8 -*-
+import numpy as np
+import numpy.matlib as mt
+import matplotlib.pyplot as plt
+
+M = 2
+N = 500
+dt = 1.0 / 30.
+sigma_measure = 0.1
+# x = 1/2 a t**2
+# assume x = 0.5 in 0.5, i.e. head turns half maximal movement range in 0.5 sec.
+# so a = 2 x / t**2 = 4.0
+sigma_accel = 4.0
+
+x = np.zeros(N, dtype = np.float)
+x[N/3:] = 1.0 # step input
+measurement = x.copy()
+measurement += np.random.normal(0., sigma_measure, N)
+
+A = np.matrix([
+ [ 1. , dt ],
+ [ 0. , 1. ]
+])
+
+R = np.matrix([[ sigma_measure**2 ]])
+
+dv = sigma_accel * dt
+dp = sigma_accel * 0.5 * dt * dt
+Q = np.matrix([
+ [ dp*dp, dp*dv ],
+ [ dv*dp, dv*dv ]
+])
+
+H = np.matrix([
+ [ 1., 0. ],
+])
+
+I = mt.identity(M, dtype = np.float)
+
+def arrayOfMatrices(n, shape):
+ return np.asarray([mt.zeros(shape, dtype = np.float) for i in xrange(n)])
+
+# Base on the scipy-cookbook http://scipy-cookbook.readthedocs.io/items/KalmanFiltering.html
+sz_state = (M, 1)
+sz_cov = (M, M)
+sz_K = (M, 1)
+xhat= arrayOfMatrices(N, sz_state) # a posteri estimate of x
+P=arrayOfMatrices(N, sz_cov) # a posteri error estimate
+xhatminus=arrayOfMatrices(N, sz_state) # a priori estimate of x
+Pminus=arrayOfMatrices(N, sz_cov) # a priori error estimate
+K=arrayOfMatrices(N, sz_K) # gain or blending factor
+
+P[0] = mt.ones((M,M)) * 100
+xhat[0] = measurement[0]
+
+for k in range(1,N):
+ # time update
+ xhatminus[k] = A * xhat[k-1]
+ Pminus[k] = A * P[k-1] + Q
+
+ # measurement update
+ K[k] = Pminus[k] * H.T * np.linalg.inv( H * Pminus[k] * H.T + R )
+ xhat[k] = xhatminus[k] + K[k] * (measurement[k] - H * xhatminus[k])
+ P[k] = ( I - K[k] ) * Pminus[k]
+
+t = np.arange(N) * dt
+plt.figure()
+plt.subplot(2,1,1)
+plt.plot(t, measurement,'k+',label='noisy measurements')
+plt.plot(t, xhat[:,0,0],'b-',label='position estimate')
+plt.plot(t, x, 'r-', label='ground truth')
+
+plt.subplot(2,1,2)
+plt.plot(t, xhat[:,1,0],'g-',label='velocity estimate')
+
+plt.show() \ No newline at end of file