KF.cc
Go to the documentation of this file.
00001 //Note : only do single value updates, do not update with a matrix of values!!!
00002 
00003 #include <iostream>
00004 #include <algorithm>
00005 
00006 #include <art/conversions.h>
00007 #include <art_map/KF.h>
00008 
00009 using namespace std;
00010 
00011 // The constructor defualts to 1x1 matrices. This is meant to be a
00012 // somewhat generic KF class, so the start method redefines the size
00013 // of the matrix
00014 KF::KF() {
00015   numStates = 0;
00016   I = Matrix(1,1,false);
00017   initP = Matrix(1,1,false);
00018   initX = Matrix(1,1,false);
00019   P = Matrix(1,1,false);
00020   X = Matrix(1,1,false);
00021   Xchange = Matrix(1,1,false);
00022 
00023   active = false;    // Is the model currrently in use ?
00024   activate = false;
00025   alpha = 1.0;  
00026 }
00027 
00028 // Basically this method is called at the start. The inputs define the size of
00029 // the KF and the initial values of the matrices.
00030 bool KF::Start(short n, Matrix& uncert, Matrix& initStates) {
00031   if (uncert.getm() != n || uncert.getn() != n || initStates.getm() != n || initStates.getn() != 1) {
00032     printf("Incorrect matrix dimensions in method Start()");
00033     return false;
00034   }
00035   //E.g. uncert is 5x5, initStates = 5x1
00036   numStates = n;
00037   I = Matrix(n, n, true);
00038   initP = uncert;
00039   initX = initStates;
00040 
00041   alpha = 1.0;
00042   active = false;    // Is the model currrently in use ? By Default it is not .. 
00043   activate = false;
00044   return Restart();
00045   return true;
00046 }
00047 
00048 // Restarts the KF. Either called at the beginning or at some other point (i.e. a maths problem occured)
00049 bool KF::Restart() {
00050   P = initP;
00051   X = initX;
00052   Xchange = Matrix(numStates, 1, false);
00053   // Anything else that needs to be reset should be done here
00054   return true;
00055 }
00056 
00057 
00058 bool KF::TimeUpdate(Matrix& A, Matrix& B, Matrix& U, Matrix& Q, bool mainFilterUpdate) { 
00059   if (A.getm() != numStates || A.getn() != numStates || B.getm() != numStates || B.getn() != U.getm() || U.getn() != 1 || Q.getm() != numStates || Q.getn() != numStates) {
00060     printf("Incorrect matrix dimensions in method TimeUpdate()");
00061     return false;
00062   }
00063 
00064   X = A*X + B*U;
00065   if (mainFilterUpdate) X[2][0] = Normalise_PI(X[2][0]);
00066   P = A*P*A.transp() + Q;
00067   Xchange = Matrix(numStates, 1, false);
00068   for (int  i = 0; i < numStates; i++) Xchange[i][0] = 0;
00069   return true;
00070 }
00071 
00072 bool KF::TimeUpdateExtended(Matrix& A, Matrix& Xbar, Matrix& Q) { //A = df/dx|x=x(k) where Xbar = f(x(k))
00073   if (A.getm() != numStates || A.getn() != numStates || Xbar.getm() != numStates || Xbar.getn() != 1 || Q.getm() != numStates || Q.getn() != numStates) {
00074     printf("Incorrect matrix dimensions in method TimeUpdateExtended()");
00075     return false;
00076   }
00077   //E.g. A is 5x5, X is 5x1, & Q is 5x5
00078   X = Xbar;
00079   P = A*P*A.transp() + Q;
00080   Xchange = Matrix(numStates, 1, false);
00081   for (int  i = 0; i < numStates; i++) Xchange[i][0] = 0;
00082   return true;
00083 }
00084 
00085 int KF::MeasurementUpdate(Matrix& C, float R, float Y, bool rejectOutliers, float outlierSD, bool mainFilterAngleUpdate) { // Set mainFilterAngleUpdate to false unless this is an angle update operation and X[2][0] is the orientation of the robot
00086   if (C.getn() != numStates || C.getm() != 1) {
00087     CompilerError("Incorrect matrix dimensions in method MeasurementUpdate()");
00088     return KF_CRASH;
00089   }
00090   //E.g. C is 1x5
00091   float HX = convDble(C*X);
00092   float innovation = Y - HX;
00093   if (mainFilterAngleUpdate) innovation = Normalise_PI(innovation);
00094   Xchange = Xchange - X;
00095   float posVar = convDble(C*P*C.transp());
00096   if (posVar < 0.0) {
00097     Reset();
00098     posVar = convDble(C*P*C.transp());
00099     cout << "KF reset due to negative variance" << endl << flush;
00100   }
00101   float varPredError = posVar + R;
00102   if (rejectOutliers && (fabs(innovation) > pow(outlierSD,2)*sqrt(varPredError))) return KF_OUTLIER;
00103   Matrix J = P*C.transp()/varPredError; //J is now X.M x Y.M e.g. 5x1
00104   Matrix newP = (I - J*C)*P;
00105   for (int i = 0; i < numStates; i++) {
00106     if (newP[i][i] <= 0) {
00107       cout << "Numerics error"<< endl << flush;
00108       Reset();
00109       return MeasurementUpdate(C, R, Y, rejectOutliers, outlierSD, mainFilterAngleUpdate);
00110     }
00111     for (int j = i+1; j < numStates; j++)
00112       if (newP[i][j]*newP[i][j] > newP[i][i]*newP[j][j]) {
00113         cout << "Numerics error" <<  endl << flush;
00114         Reset();
00115         return MeasurementUpdate(C, R, Y, rejectOutliers, outlierSD, mainFilterAngleUpdate);
00116       }
00117   }
00118   X = X + J*innovation;
00119   P = newP;
00120   Xchange = Xchange + X;
00121   return KF_SUCCESS;
00122 }
00123 
00124 
00125 int KF::MeasurementUpdateExtended(Matrix& C,KFStruct s) {
00126         return MeasurementUpdateExtended(C,s.R, s.Y, s.Ybar, s.rejectOutliers, s.outlierSD, s.mainFilterAngleUpdate, s.ingoreLongRangeUpdate, s.deadzoneSize, s.dist, s.ambigObject, s.changeAlpha);
00127 }
00128 
00129 
00130 int KF::MeasurementUpdateExtended(Matrix& C, float R, float Y, float Ybar, bool rejectOutliers, float outlierSD, bool mainFilterAngleUpdate, bool ignoreLongRangeUpdate, float deadzoneSize, float dist, bool ambigObj, bool changeAlpha) { // Set mainFilterAngleUpdate to false unless this is an angle update operation and X[2][0] is the orientation of the robot
00131   if (C.getn() != numStates || C.getm() != 1) {
00132     CompilerError("Incorrect matrix dimensions in method MeasurementUpdateExtended()");
00133     cout << "Incorrect matrix dimensions in method MeasurementUpdateExtended()" << endl << flush;
00134     return KF_CRASH;
00135   }
00136   //E.g. C is 1x5
00137   float innovation = Y - Ybar;
00138   float posVar = convDble(C*P*C.transp());
00139 
00140   if (mainFilterAngleUpdate) {
00141     innovation = Normalise_PI(innovation);
00142   }
00143 
00144    if (mainFilterAngleUpdate) {
00145     R += SQUARE((P[0][0]+P[1][1])/(dist*dist));    // Moved from object update into here on 22/3/2007
00146   }
00147 
00148   Xchange = Xchange - X;
00149   
00150   if (posVar < 0.0) {
00151     Reset();
00152     posVar = convDble(C*P*C.transp());
00153     cout << "KF reset due to negative variance" << endl << flush;
00154   }
00155   // add in deadzone calculations: RHM 1/6/06
00156   Deadzone(&R, &innovation, posVar, deadzoneSize);
00157 
00158   float varPredError = posVar + R;
00159   if (ignoreLongRangeUpdate && (innovation > S_D_RANGE_REJECT*sqrt(varPredError))) {
00160     cout << "Ignore Long range update" << endl << flush;
00161     // R = R*4;
00162         alpha *= 0.5; //RHM 7/7/07
00163     return KF_SUCCESS;
00164   }
00165   if (rejectOutliers && (pow(innovation,2) > pow(outlierSD,2)*varPredError)) {
00166     alpha*=0.5; //RHM 7/7/07  
00167     return KF_OUTLIER;
00168   }
00169 // RHM 7/7/07: Shifted alpha changes to here
00170   if (changeAlpha)
00171     {
00172       if (ambigObj)
00173         {
00174           alpha *= std::max(((R)/(R+innovation*innovation)), 0.01f); //0.1);
00175         }
00176       else
00177         {
00178           alpha *= (R)/(R+innovation*innovation);
00179         }
00180     }
00181 
00182   
00183   Matrix J = P*C.transp()/varPredError; //J is now X.M x Y.M e.g. 5x1
00184 //  if (!mainFilterAngleUpdate) J[2][0] = 0;
00185   Matrix Xbar = X;
00186   Matrix newP = (I - J*C)*P;
00187   for (int i = 0; i < numStates; i++) {
00188     if (newP[i][i] <= 0) {
00189       //cout << "Numerics error" << endl << flush;
00190       Reset();
00191       return MeasurementUpdateExtended(C, R, Y, Ybar, rejectOutliers, outlierSD, mainFilterAngleUpdate, ignoreLongRangeUpdate, deadzoneSize, dist, ambigObj,changeAlpha);
00192     }
00193     for (int j = i+1; j < numStates; j++)
00194       if (newP[i][j]*newP[i][j] > newP[i][i]*newP[j][j]) {
00195         //cout << "Numerics error" << ", KF reset" << endl << flush;
00196         Reset();
00197         return MeasurementUpdateExtended(C, R, Y, Ybar, rejectOutliers, outlierSD, mainFilterAngleUpdate, ignoreLongRangeUpdate, deadzoneSize, dist, ambigObj,changeAlpha);
00198       }
00199   }
00200   X = Xbar + J*innovation;
00201   P = newP;
00202   Xchange = Xchange + X;
00203   return KF_SUCCESS;
00204 }
00205 
00206 // Resets the P matrix, basically increases the location uncertainty. 
00207 // This sovles problems like the 'kidnappend robot' scenario.
00208 void KF::Reset() {
00209   P = initP;
00210 }
00211 
00212 Matrix KF::GetStates() {
00213   return X;
00214 }
00215 
00216 void KF::SetStates(Matrix Xbar) {
00217   X = Xbar;
00218 }
00219 
00220 float KF::GetState(short n) {
00221   return X[n][0];
00222 }
00223 
00224 void KF::SetState(short n, float x) {
00225   X[n][0] = x;
00226 }
00227 
00228 void KF::NormaliseState(short n) {
00229   X[n][0] = Normalise_PI(X[n][0]);
00230 }
00231 
00232 Matrix KF::GetErrorMatrix() {
00233   return P;
00234 }
00235 
00236 void KF::SetErrorMatrix(Matrix Pbar) {
00237   P = Pbar;
00238 }
00239 
00240 float KF::GetCovariance(short m, short n) {
00241   return P[m][n];
00242 }
00243 
00244 float KF::GetVariance(short n) {
00245   return P[n][n];
00246 }
00247 
00248 Matrix KF::GetXchanges() {
00249   return Xchange;
00250 }
00251 
00252 float KF::GetXchange(short n) {
00253   return Xchange[n][0];
00254 }
00255 
00256 void KF::CompilerError(const char* str) {
00257   cout << str << endl << flush;
00258 }
00259 
00260 void KF::Deadzone(float* R, float* innovation, float CPC, float eps)
00261 {
00262         float invR;
00263         // R is the covariance of the measurement (altered by this procedure)
00264         // innovation is the prediction error (altered by this procedure)
00265         // CPC is the variance of the predicted y value (not altered)
00266         // eps is the value of the deadzone
00267         // all vars except innovation must e positive for this to work
00268 
00269         //Return if eps or CPC or R non-positive
00270         if ((eps<1.0e-08) || (CPC<1.0e-08) || (*R<1e-08))
00271         return;
00272 
00273         if (ABS(*innovation)>eps)
00274         { //RHM 5/6/06 ?extra fix to Deadzones
00275         invR=(ABS(*innovation)/eps-1)/CPC; // adjust R when outside the deadzone to not jump too far
00276         //return;
00277         }
00278         else
00279         {
00280         // If we get to here, then valid parameters passed, and we
00281         // are within the specified accuracy
00282 
00283         //cout << "Deadzone  " << eps << "   " << *innovation << "\n" << flush; 
00284         *innovation=0.0; //Set to zero to force no update of parameters
00285         invR=0.25/(eps*eps)-1.0/CPC;
00286         }
00287 
00288         if (invR<1.0e-08) //RHM 5/6/06 decrease from 1.0e-06
00289                 invR=1e-08; 
00290 
00291         // only ever increase R
00292         if ( *R < 1.0/invR )
00293         *R=1.0/invR;
00294         // R is adjusted (when in the deadzone) so that the a-posteriori variance of the 
00295         // prediction is 4*eps*eps;
00296         // or, (if we are outside the deadzone), so that the new prediction at most just reaches
00297         // the deadzone.
00298 }


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34