Source Code for Module pyvision.surveillance.kalman

  1  # PyVision License 
  2  # 
  3  # Copyright (c) 2010 David S. Bolme 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions 
  8  # are met: 
  9  #  
 10  # 1. Redistributions of source code must retain the above copyright 
 11  # notice, this list of conditions and the following disclaimer. 
 12  #  
 13  # 2. Redistributions in binary form must reproduce the above copyright 
 14  # notice, this list of conditions and the following disclaimer in the 
 15  # documentation and/or other materials provided with the distribution. 
 16  #  
 17  # 3. Neither name of copyright holders nor the names of its contributors 
 18  # may be used to endorse or promote products derived from this software 
 19  # without specific prior written permission. 
 20  #  
 21  #  
 22  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 23  # ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 24  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 
 25  # A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR 
 26  # CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
 27  # EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
 28  # PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
 29  # PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
 30  # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
 31  # NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
 32  # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 
 33   
 34  ''' 
 35  Created on Sep 27, 2010 
 36   
 37  @author: bolme 
 38  ''' 
 39   
 40  import numpy as np 
 41   
 42 -class KalmanFilter: 
 43       
 44 -    def __init__(self,x_0,P_0,F,Q,H,R,B=None): 
 45          ''' 
 46          Based on wikipedia.   
 47           
 48          Warning: Controls have not been tested. 
 49           
 50          @param x_0: initial state 
 51          @param P_0: initial state covariance estimate 
 52          @param F: the state transistion model 
 53          @param B: control input model 
 54          @param Q: process noise covariance 
 55          @param z_k: measurement 
 56          @param H: observation model - transform state to observation 
 57          @param R: measurement noise covariance 
 58          not needed 
 59          @param w: process noise 
 60          @param v_k: measurement noise 
 61          ''' 
 62           
 63          self.F = np.array(F,dtype=np.float64) 
 64          d = self.F.shape[0] 
 65          assert self.F.shape == (d,d) 
 66           
 67          self.P_k = np.array(P_0,dtype=np.float64) 
 68          assert self.P_k.shape == (d,d) 
 69   
 70          if B == None: 
 71              B = np.zeros((d,1)) 
 72          self.B = np.array(B,dtype=np.float64) 
 73          b = self.B.shape[1] 
 74          assert self.B.shape == (d,b) 
 75   
 76          self.Q = np.array(Q,dtype=np.float64) 
 77          assert self.Q.shape == (d,d) 
 78   
 79          self.H = np.array(H,dtype=np.float64) 
 80          m = self.H.shape[0] 
 81          assert self.H.shape == (m,d) 
 82   
 83          self.R = np.array(R,dtype=np.float64) 
 84          assert self.R.shape == (m,m) 
 85   
 86          self.x_k = np.array(x_0,dtype=np.float64).reshape(d,1) 
 87           
 88          self.d = d 
 89          self.b = b 
 90          self.m = m 
 91           
 92          #self.history = [] 
 93           
 94           
 95 -    def predict(self, u=None): 
 96          # Check and convert inputs 
 97          #m = self.m 
 98          b = self.b 
 99          #d = self.d 
100                   
101          if u == None: 
102              u = np.zeros((b,)) 
103          u = np.array(u,dtype=np.float64).reshape(b,1) 
104   
105          x_k = np.dot(self.F,self.x_k) + np.dot(self.B,u) 
106          return x_k 
107           
108                   
109 -    def update(self, z=None, u=None): 
110          ''' 
111          @param z: measurement 
112          @param u: control 
113          ''' 
114          # Check and convert inputs 
115          m = self.m 
116          b = self.b 
117          d = self.d 
118           
119           
120          if u == None: 
121              u = np.zeros((b,)) 
122          u = np.array(u,dtype=np.float64).reshape(b,1) 
123           
124          # Predict step 
125          x_k = np.dot(self.F,self.x_k) + np.dot(self.B,u) 
126          #print "Pred X:\n",x_k 
127           
128          P_k = np.dot(np.dot(self.F,self.P_k),self.F.T) + self.Q 
129          #print "Pred P:\n",P_k 
130   
131          if z == None: 
132              self.x_k = x_k 
133              self.P_k = P_k 
134              return self.x_k 
135   
136          z = np.array(z,dtype=np.float64).reshape(m,1) 
137           
138          # Update Step 
139          y_k = z - np.dot(self.H,x_k) 
140          #print "Innovation Resid:\n",y_k 
141           
142          S_k = np.dot(np.dot(self.H,P_k),self.H.T) + self.R 
143          #print "Innonvation Cov:\n",S_k 
144           
145          K = np.dot(np.dot(P_k,self.H.T),np.linalg.inv(S_k)) 
146          #print "Kalman Gain:\n",K 
147   
148          x_k = x_k + np.dot(K,y_k) 
149          #print "Postieriori state:\n",x_k 
150           
151          P_k = np.dot(np.eye(d) - np.dot(K,self.H),P_k) 
152          #print "Posteriori Cov:",P_k 
153           
154          self.x_k = x_k 
155          self.P_k = P_k 
156           
157          return self.x_k 
158       
159 -    def state(self): 
160          ''' Return the current estimate of state. ''' 
161          return self.x_k 
162       
163 -    def setState(self,x_k): 
164          self.x_k = np.array(x_k,np.float64) 
165       
166 -    def __str__(self): 
167          return "KalmanFilter<%s>"%self.state().flatten() 
168