Changeset 737 for trunk

Timestamp:

05/09/10 13:17:28 (21 months ago)

Author:

kleinjt

Message:

added work in progress kalman filter

Files:

• trunk/software/rb/kalman.py (modified) (6 diffs)

trunk/software/rb/kalman.py

@@ -1 @@
-# Copyright (C) 2009 Thomas W. Most
-#
+# Copyright (C) 2010 Thomas W. Most, Jon Klein
+# 
 # This program is free software: you can redistribute it and/or modify
 # it under the terms of the GNU General Public License as published by
@@ -14 +14 @@
 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
+# ___________________________________ 
+#/ I am wearing a hat, so I'm only   \
+#\ probably a stegosaurus.           /
+# ----------------------------------- 
+#\                             .       .
+# \                           / `.   .' " 
+#  \                  .---.  <    > <    >  .---.
+#   \                 |    \  \ - ~ ~ - /  /    |
+#         _____          ..-~             ~-..-~
+#        |     |   \~~~\.'                    `./~~~/
+#       ---------   \__/                        \__/
+#      .'  O    \     /               /       \  " 
+#     (_____,    `._.'               |         }  \/~~~/
+#      `----.          /       }     |        /    \__/
+#            `-.      |       /      |       /      `. ,~~|
+#                ~-.__|      /_ - ~ ^|      /- _      `..-'   
+#                     |     /        |     /     ~-.     `-. _  _  _
+#                     |_____|        |_____
+
+# for the theory beind this file, and if you want to know what the stegosaurus is talking about, 
+# see Mobile Robot Localization using Kalman Filters by Jon Klein, Trenton Tabor
+# http://tarsonis.dhcp.rose-hulman.edu/robot_kalman.pdf
+
 
 from thread import start_new_thread
@@ -19 +42 @@
 from rb.utils import meter_offset
 
+import pylab
+from numpy import *
+
+# constants for state matrix
+LONGITUDE = 0
+LATITUDE = 1
+LOCATION = 0:2
+LINEAR_VELOCITY = 2
+ORIENTATION = 3
+ANGULAR_VELOCITY = 4
+
+STATE_SIZE = 5                   # number of dimensions tracked in the state matrix
+GPS_TIMESTEP = 1                 # update rate for GPS
+COMPASS_TIMESTEP = .1            # update rate for compass
+MAX_LINEAR_ACCELERATION = 9.8    # maximum possible linear acceleration
 
 class KalmanFilter(object):
@@ -44 +82 @@
                 self.gps_zero = None
                 
-                # Raw sensor data for use before the filter has actually
-                # been implemented
-                self._x_y = (0, 0)
-                self._orient = 0
-                # TODO
+                # state matrix, see (10), initialize states to zero
+      self.state_matrix = mat(zeros((STATE_SIZE,1)))
+      self.state_matrix[LOCATION] = 0
+      self.state_matrix[LINEAR_VELOCITY] = 0
+      self.state_matrix[ANGULAR_VELOCITY] = 0
+      self.state_matrix[ORIENTATION] = 0
+      
+      # constant modeling error matrix Q, see (11-15)
+      self.modeling_error = mat(zeros((STATE_SIZE,STATE_SIZE)))
+      location_error = (GPS_TIMESTEP ** 2) * MAX_LINEAR_ACCELERATION / 2
+      orientation_error = (COMPASS_TIMESTEP ** 2) * MAX_ANGULAR_ACCELERATION / 2
+      self.modeling_error[LATITUDE,LATITUDE] = location_error
+      self.modeling_error[LONGITUDE,LONGITUDE] = location_error
+      self.modeling_error[LINEAR_VELOCITY,LINEAR_VELOCITY] = ENCODER_TIMESTEP * MAX_LINEAR_ACCELERATION
+      self.modeling_error[ANGULAR_VELOCITY, ANGULAR_VELOCITY] = ENCODER_TIMESTEP * MAX_ANGULAR_ACCELERATION
+      self.modeling_error[ORIENTATION, ORIENTATION] = orientation_error 
+      
+      # noise model matrix R, let's assume we begin with no noise, see (19)
+      self.measurement_noise = mat(zeros((STATE_SIZE,STATE_SIZE)))
+      
+      # boring, identity matrix
+      self.identity = mat(eye(STATE_SIZE,STATE_SIZE))
+
+      # transistion matrix, see (16)
+      self.transistion = mat(eye(STATE_SIZE, STATE_SIZE);
+      
+   def _update_(self) 
+   def _update_transistion_matrx(self)
+      '''
+      Update the transistion matrix to take into accout changes in one dimension effecting changes in another dimension
+      See robot model, (4.4)
+      '''
+
+      self.transistion(ORIENTATION,ANGULAR_VELOCITY) = ENCODER_TIMESTEP
+      self.transistion(LATITUDE, VELOCITY) = ENCODER_TIMESTEP * cos(self.state_matrix(ANGULAR_VELOCITY))
+      self.transistion(LONGITUDE, VELOCITY) = ENCODER_TIMESTEP * sin(self.state_matrix(ANGULAR_VELOCITY))
 
         def update_target_speed(self, data):
@@ -54 +123 @@
                 Update filter with the target speed.
                 '''
-        
+      
         def update_encoders(self, data):
                 '''
                 Update the filter with encoder data.
+                data.left is change in distance, in meters, from the left encoder
+                data.right is the change in distance, in meters, from the right encoder
                 '''
-        
+      # updates the angular and linear velocity
+      # see (2.1.3),
+      
+      H = mat(zeros((STATE_SIZE,1)));
+      H[LINEAR_VELOCITY] = 1
+      H[ANGULAR_VELOCITY] = 1
+
         def update_microstrain(self, accel):
                 '''
                 Update the filter with data from the Microstrain.
                 '''
-                self._orient = accel.orient[2]
+      
+      H = mat(zeros((STATE_SIZE,1)))
+      H[ORIENTATION] = 1
+                
+                self._orient = accel.orient[2] 
         
         def update_gps(self, coords):
@@ -70 +151 @@
                 Update the filter with data from the GPS.
                 '''
+                
+      H = mat(zeros((STATE_SIZE,1)));
+      H[LONGITUDE] = 1
+      H[LATITUDE] = 1
+     
                 if self.gps_zero is None:
                         # Use the robot's initial location as zero
                         self.gps_zero = coords.lat_lon
                 self._x_y = meter_offset(self.gps_zero, coords.lat_lon)
-        
+
+   def measurement_update(self, H)
+      '''
+      
+      '''
+
+
         def update_drive_speed(self, (lspeed, rspeed)):
                 '''
                 Update the filter with data based on the drive motors.  Calculate the
                 rotational velocity and change in position based on dead reckoning.
+                Not actually used for anything, you may not want to call this function. . 
                 '''
                 pass # TODO