KalmanFilter.C

Go to the documentation of this file.
00001 /*!@file Image/KalmanFilter.C implementation of a 2nd order linear Kalman Filter
00002  */
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2002   //
00005 // by the University of Southern California (USC) and the iLab at USC.  //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Dirk Walther <walther@caltech.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Image/KalmanFilter.C $
00035 // $Id: KalmanFilter.C 4899 2005-07-12 20:21:11Z itti $
00036 //
00037 
00038 #include "Image/KalmanFilter.H"
00039 
00040 #include "Image/IO.H"
00041 #include "Image/MathOps.H"
00042 #include "Image/MatrixOps.H"
00043 #include "Util/Assert.H"
00044 #include "Util/log.H"
00045 #include <cmath>
00046 #include <iostream>
00047 
00048 // ######################################################################
00049 inline float sqr(float x) {
00050   return (x * x);
00051 }
00052 
00053 // ######################################################################
00054 KalmanFilter::KalmanFilter()
00055   : initialized(false)
00056 {}
00057 
00058 // ######################################################################
00059 KalmanFilter::KalmanFilter(float initialM, float pNoise,
00060                            float mNoise, float timeStep)
00061   : initialized(true)
00062 {
00063   init(initialM, pNoise, mNoise, timeStep);
00064 }
00065 
00066 
00067 // ######################################################################
00068 void KalmanFilter::init(float initialM, float pNoise,
00069                         float mNoise, float timeStep)
00070 {
00071   // set the noise
00072   itsPNoise = pNoise;
00073   itsMNoise2 = mNoise * mNoise;
00074 
00075   // populate H
00076   H.resize(3,1,ZEROS);
00077   H.setVal(0,0,1.0F);
00078   H.setVal(1,0,0.0F);
00079   H.setVal(2,0,0.0F);
00080   HT = transpose(H);
00081 
00082 
00083   // set the inner state to the initial measurement
00084   //x = HT * initialM;
00085   x.resize(1,3,ZEROS);
00086   x.setVal(0,0,initialM);
00087   x.setVal(0,1,0.0F);
00088   x.setVal(0,2,0.0F);
00089 
00090   // set the identity matrix
00091   I = eye<float>(3);
00092 
00093   // fill P with some large numbers on the diagonal
00094   P = I * 1000.0F;
00095 
00096   // generate powers of timeStep
00097   std::vector<float> ts(6);
00098   ts[0] = 1.0F;
00099   for (uint i = 1; i <= 5; ++i)
00100     ts[i] = ts[i-1] * timeStep;
00101 
00102   // populate Phi
00103   Phi = I;
00104   Phi.setVal(1,0,ts[1]); Phi.setVal(2,0,ts[2]/2.0F); Phi.setVal(2,1,ts[1]);
00105   PhiT = transpose(Phi);
00106 
00107   // populate Q
00108   Q.resize(3,3,NO_INIT);
00109   Q.setVal(0,0,ts[5]/20.0F); Q.setVal(1,0,ts[4]/8.0F); Q.setVal(2,0,ts[3]/6.0F);
00110   Q.setVal(0,1,ts[4]/ 8.0F); Q.setVal(1,1,ts[3]/3.0F); Q.setVal(2,1,ts[2]/2.0F);
00111   Q.setVal(0,2,ts[3]/ 6.0F); Q.setVal(1,2,ts[2]/2.0F); Q.setVal(2,2,ts[1]);
00112   Q *= pNoise;
00113 
00114   // run a first update for M, K, and P
00115   updateFilter();
00116 
00117   // LINFO("x =  [%g %g %g]; 1st prediction: %g; Phi(0,0) = %g",
00118   //    x.getVal(0,0),x.getVal(0,1),x.getVal(0,2),
00119   //    getXEstimate().getVal(0,0),Phi.getVal(0,0));
00120 
00121   initialized = true;
00122 }
00123 
00124 // ######################################################################
00125 void KalmanFilter::updateFilter()
00126 {
00127   //writeImageToStream(std::cout,H);
00128 
00129   // first, update the covariance matrix before update M
00130   // M = Phi*P*PhiT + Q
00131   M = matrixMult(matrixMult(Phi,P),PhiT) + Q;
00132 
00133   // now update the Kalman marix itself
00134   // K = M*HT*(H*M*HT + R)^(-1)
00135   Image<float> HMHT = matrixMult(matrixMult(H,M),HT);
00136   ASSERT(HMHT.getSize() == 1);
00137   K = matrixMult(M,HT) / (HMHT.getVal(0,0) + itsMNoise2);
00138 
00139   // finally, update the covariance matrix after update P
00140   // P = (I-K*H)*M
00141   P = matrixMult(I - matrixMult(K,H),M);
00142 }
00143 
00144 // ######################################################################
00145 void KalmanFilter::writeToStream(std::ostream& os) const
00146 {
00147   if (initialized) os << "1\n";
00148   else os << "0\n";
00149 
00150   if (initialized)
00151     {
00152       os << itsPNoise << ' ' << itsMNoise2 << '\n';
00153 
00154       writeImageToStream(os,x);
00155       writeImageToStream(os,I);
00156       writeImageToStream(os,M);
00157       writeImageToStream(os,K);
00158       writeImageToStream(os,P);
00159       writeImageToStream(os,H);
00160       writeImageToStream(os,HT);
00161       writeImageToStream(os,Phi);
00162       writeImageToStream(os,PhiT);
00163       writeImageToStream(os,Q);
00164     }
00165 }
00166 
00167 // ######################################################################
00168 void KalmanFilter::readFromStream(std::istream& is)
00169 {
00170   int i; is >> i;
00171   initialized = (i == 1);
00172 
00173   if (initialized)
00174     {
00175       is >> itsPNoise;
00176       is >> itsMNoise2;
00177 
00178       readImageFromStream(is,x);
00179       readImageFromStream(is,I);
00180       readImageFromStream(is,M);
00181       readImageFromStream(is,K);
00182       readImageFromStream(is,P);
00183       readImageFromStream(is,H);
00184       readImageFromStream(is,HT);
00185       readImageFromStream(is,Phi);
00186       readImageFromStream(is,PhiT);
00187       readImageFromStream(is,Q);
00188     }
00189 }
00190 
00191 // ######################################################################
00192 Image<float> KalmanFilter::getXEstimate() const
00193 {
00194   return matrixMult(Phi,x);
00195 }
00196 
00197 // ######################################################################
00198 Image<float> KalmanFilter::getXEstimate(float z) const
00199 {
00200   Image<float> Px = getXEstimate();
00201   return (Px + K * (z - matrixMult(H,Px).getVal(0,0)));
00202 }
00203 
00204 // ######################################################################
00205 float KalmanFilter::getEstimate() const
00206 {
00207   ASSERT(initialized);
00208   float est = getXEstimate().getVal(0,0);
00209   //LINFO("currVal = %g; est = %g",x.getVal(0,0),est);
00210   return est;
00211 }
00212 
00213 // ######################################################################
00214 float KalmanFilter::getEstimate(float measurement) const
00215 {
00216   ASSERT(initialized);
00217   float est = getXEstimate(measurement).getVal(0,0);
00218   //LINFO("currVal = %g; est = %g",x.getVal(0,0),est);
00219   return est;
00220 }
00221 
00222 // ######################################################################
00223 float KalmanFilter::getSpeed() const
00224 {
00225   ASSERT(initialized);
00226   return x.getVal(0,1);
00227 }
00228 
00229 // ######################################################################
00230 float KalmanFilter::getCost(float measurement) const
00231 {
00232   ASSERT(initialized);
00233 
00234   float cost =  sqr(getEstimate(measurement) - measurement);
00235 
00236   if (!(cost == cost))
00237     {
00238       writeToStream(std::cout);
00239       LFATAL("Kalman Filter error");
00240     }
00241 
00242   return cost;
00243 }
00244 
00245 // ######################################################################
00246 float KalmanFilter::update()
00247 {
00248   ASSERT(initialized);
00249   x = getXEstimate();
00250   updateFilter();
00251 
00252   if (!(x.getVal(0,0) == x.getVal(0,0)))
00253     {
00254       writeToStream(std::cout);
00255       LFATAL("Kalman Filter error");
00256     }
00257 
00258   return x.getVal(0,0);
00259 }
00260 
00261 // ######################################################################
00262 float KalmanFilter::update(float measurement)
00263 {
00264   ASSERT(initialized);
00265   x = getXEstimate(measurement);
00266   updateFilter();
00267 
00268   if (!(x.getVal(0,0) == x.getVal(0,0)))
00269     {
00270       writeToStream(std::cout);
00271       LFATAL("Kalman Filter error");
00272     }
00273 
00274   return x.getVal(0,0);
00275 }
00276 
00277 // ######################################################################
00278 Image<float> KalmanFilter::getStateVector() const
00279 {
00280   ASSERT(initialized);
00281   return x;
00282 }
00283 
00284 // ######################################################################
00285 Image<float> KalmanFilter::getCovariances() const
00286 {
00287   ASSERT(initialized);
00288   return P;
00289 }
00290 
00291 // ######################################################################
00292 bool KalmanFilter::isInitialized() const
00293 {
00294   return initialized;
00295 }
00296 
00297 // ######################################################################
00298 /* So things look consistent in everyone's emacs... */
00299 /* Local Variables: */
00300 /* indent-tabs-mode: nil */
00301 /* End: */
Generated on Sun May 8 08:40:55 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3