2 # -*- coding: iso-8859-15 -*-
4 # Implementation of simple Kalman in python based on
5 # Xilinx DSP Magazine #1 (2005) - http://www.xilinx.com/publications/
10 def __init__(self, x_est, P_est, Q, R):
11 # Convert arguments to vectors/matrices if not given
12 if type(x_est) != type(np.array([0])): x_est = np.array(x_est)
13 if type(P_est) != type(np.matrix([0])): P_est = np.matrix(P_est)
14 if type(Q) != type(np.matrix([0])): Q = np.matrix(Q)
15 if type(R) != type(np.matrix([0])): R = np.matrix(R)
17 self.x_est = x_est # Systemzustand
18 self.P_est = P_est # Fehlerkovarianz
19 self.Q = Q # Systemrauschen
20 self.R = R # Varianz des Messfehlers
21 self.I = np.eye(P_est.shape[0])
24 # Korrektur mit der Messung
25 # (1) Berechnung der Kalman Verstärkung
26 K = self.P_est * (self.R + self.P_est).I
27 # (2) Korrektur der Schätzung mit der Messung y
28 x = self.x_est + K*(y - self.x_est)
29 # (3) Korrektur der Fehlerkovarianzmatrix
30 P = (self.I-K)*self.P_est
33 # (1) Prädiziere den Systemzustand
35 # (2) Präzidiere die Fehlerkovarianzmatrix
36 self.P_est = P + self.Q
38 if x.shape == (1,1): return x.item(0,0)
42 if __name__ == '__main__':
44 from matplotlib.pyplot import *
49 orig = [0.5 for i in range(100)]
50 y = [orig[i] + (random.random()-0.5)/5 for i in range(0, len(orig))]
53 x_est = 0.0 # Systemzustand
54 P_est = 0.1 # Fehlerkovarianz
55 Q = 10e-6 # Systemrauschen
56 R = 0.0035 # Varianz des Messfehlers
57 p = simple_kalman(x_est, P_est, Q, R)
59 for i in range(0, 100):
63 plot(orig, label="Orig")
64 plot(range(100), y, 'o', label="Noise")
65 plot(x, label="Filtered")
73 orig1 = [0.5 for i in range(100)]
74 orig2 = [1.5 for i in range(100)]
75 y1 = [orig1[i] + (random.random()-0.5)/5 for i in range(0, len(orig1))]
76 y2 = [orig2[i] + (random.random()-0.5)/5 for i in range(0, len(orig2))]
80 x_est = np.array([[0.0, 0.0]]).T # Systemzustand
81 P_est = 0.1 * np.eye(2) # Fehlerkovarianz
82 Q = 10e-6 * np.eye(2) # Systemrauschen
83 R = np.matrix([[0.0035, 0.0], [0.0, 0.0035]]) # Varianz des Messfehlers
84 p = simple_kalman(x_est, P_est, Q, R)
86 for i in range(0, 100):
88 a = p.run(np.array([[y1[i], y2[i]]]).T)
89 x1.append(a.item((0, 0)))
90 x2.append(a.item((1, 0)))
92 plot(orig1, label="Orig")
93 plot(orig2, label="Orig")
94 plot(range(100), y1, 'o', label="Noise")
95 plot(range(100), y2, 'o', label="Noise")
96 plot(x1, label="Filtered")
97 plot(x2, label="Filtered")