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)
41 def set_measure_cov(self, R):
42 if type(R) != type(np.matrix([0])): R = np.matrix(R)
43 if np.all(R >= np.zeros([2,2])):
47 if __name__ == '__main__':
49 from matplotlib.pyplot import *
54 orig = [0.5 for i in range(100)]
55 y = [orig[i] + (random.random()-0.5)/5 for i in range(0, len(orig))]
58 x_est = 0.0 # Systemzustand
59 P_est = 0.1 # Fehlerkovarianz
60 Q = 10e-6 # Systemrauschen
61 R = 0.0035 # Varianz des Messfehlers
62 p = simple_kalman(x_est, P_est, Q, R)
64 for i in range(0, 100):
68 plot(orig, label="Orig")
69 plot(range(100), y, 'o', label="Noise")
70 plot(x, label="Filtered")
78 orig1 = [0.5 for i in range(100)]
79 orig2 = [1.5 for i in range(100)]
80 y1 = [orig1[i] + (random.random()-0.5)/5 for i in range(0, len(orig1))]
81 y2 = [orig2[i] + (random.random()-0.5)/5 for i in range(0, len(orig2))]
85 x_est = np.array([[0.0, 0.0]]).T # Systemzustand
86 P_est = 0.1 * np.eye(2) # Fehlerkovarianz
87 Q = 10e-6 * np.eye(2) # Systemrauschen
88 R = np.matrix([[0.0035, 0.0], [0.0, 0.0035]]) # Varianz des Messfehlers
89 p = simple_kalman(x_est, P_est, Q, R)
91 for i in range(0, 100):
93 result = p.run(np.array([[y1[i], y2[i]]]).T)
94 x1.append(result.item((0, 0)))
95 x2.append(result.item((1, 0)))
97 plot(orig1, label="Orig")
98 plot(orig2, label="Orig")
99 plot(range(100), y1, 'o', label="Noise")
100 plot(range(100), y2, 'o', label="Noise")
101 plot(x1, label="Filtered")
102 plot(x2, label="Filtered")