00001
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
00012
00013
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;
00024 activate = false;
00025 alpha = 1.0;
00026 }
00027
00028
00029
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
00036 numStates = n;
00037 I = Matrix(n, n, true);
00038 initP = uncert;
00039 initX = initStates;
00040
00041 alpha = 1.0;
00042 active = false;
00043 activate = false;
00044 return Restart();
00045 return true;
00046 }
00047
00048
00049 bool KF::Restart() {
00050 P = initP;
00051 X = initX;
00052 Xchange = Matrix(numStates, 1, false);
00053
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) {
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
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) {
00086 if (C.getn() != numStates || C.getm() != 1) {
00087 CompilerError("Incorrect matrix dimensions in method MeasurementUpdate()");
00088 return KF_CRASH;
00089 }
00090
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;
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) {
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
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));
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
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
00162 alpha *= 0.5;
00163 return KF_SUCCESS;
00164 }
00165 if (rejectOutliers && (pow(innovation,2) > pow(outlierSD,2)*varPredError)) {
00166 alpha*=0.5;
00167 return KF_OUTLIER;
00168 }
00169
00170 if (changeAlpha)
00171 {
00172 if (ambigObj)
00173 {
00174 alpha *= std::max(((R)/(R+innovation*innovation)), 0.01f);
00175 }
00176 else
00177 {
00178 alpha *= (R)/(R+innovation*innovation);
00179 }
00180 }
00181
00182
00183 Matrix J = P*C.transp()/varPredError;
00184
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
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
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
00207
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
00264
00265
00266
00267
00268
00269
00270 if ((eps<1.0e-08) || (CPC<1.0e-08) || (*R<1e-08))
00271 return;
00272
00273 if (ABS(*innovation)>eps)
00274 {
00275 invR=(ABS(*innovation)/eps-1)/CPC;
00276
00277 }
00278 else
00279 {
00280
00281
00282
00283
00284 *innovation=0.0;
00285 invR=0.25/(eps*eps)-1.0/CPC;
00286 }
00287
00288 if (invR<1.0e-08)
00289 invR=1e-08;
00290
00291
00292 if ( *R < 1.0/invR )
00293 *R=1.0/invR;
00294
00295
00296
00297
00298 }