simple_kalman.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 # Implementation of simple Kalman in python based on
   5 # Xilinx DSP Magazine #1 (2005) - http://www.xilinx.com/publications/
   6
   7 import numpy as np
   8
   9 class simple_kalman:
  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)
  16
  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])
  22
  23         def run(self, y):
  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
  31                 #
  32                 # Prädiktion
  33                 # (1) Prädiziere den Systemzustand
  34                 self.x_est = x
  35                 # (2) Präzidiere die Fehlerkovarianzmatrix
  36                 self.P_est = P + self.Q
  37
  38                 if x.shape == (1,1): return x.item(0,0)
  39                 return x
  40
  41
  42 if __name__ == '__main__':
  43         import random
  44         from matplotlib.pyplot import *
  45
  46         #
  47         # 1d Test
  48         #
  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))]
  51         x = []
  52
  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)
  58
  59         for i in range(0, 100):
  60                 # Messwert
  61                 x.append(p.run(y[i]))
  62
  63         plot(orig, label="Orig")
  64         plot(range(100), y, 'o', label="Noise")
  65         plot(x, label="Filtered")
  66         ylim(0, 2)
  67         legend()
  68         show()
  69
  70         #
  71         # 2d Test
  72         #
  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))]
  77         x1 = []
  78         x2 = []
  79
  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)
  85
  86         for i in range(0, 100):
  87                 # Messwert
  88                 result = p.run(np.array([[y1[i], y2[i]]]).T)
  89                 x1.append(result.item((0, 0)))
  90                 x2.append(result.item((1, 0)))
  91
  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")
  98         ylim(0, 2)
  99         legend()
 100         show()