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         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])):
  44                         self.R = R
  45
  46
  47 if __name__ == '__main__':
  48         import random
  49         from matplotlib.pyplot import *
  50
  51         #
  52         # 1d Test
  53         #
  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))]
  56         x = []
  57
  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)
  63
  64         for i in range(0, 100):
  65                 # Messwert
  66                 x.append(p.run(y[i]))
  67
  68         plot(orig, label="Orig")
  69         plot(range(100), y, 'o', label="Noise")
  70         plot(x, label="Filtered")
  71         ylim(0, 2)
  72         legend()
  73         show()
  74
  75         #
  76         # 2d Test
  77         #
  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))]
  82         x1 = []
  83         x2 = []
  84
  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)
  90
  91         for i in range(0, 100):
  92                 # Messwert
  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)))
  96
  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")
 103         ylim(0, 2)
 104         legend()
 105         show()