Go to the documentation of this file.00001
00002
00003
00004
00005 #ifndef _KF_h_DEFINED
00006 #define _KF_h_DEFINED
00007
00008
00009
00010 #include <stdio.h>
00011 #include <art_map/Matrix.h>
00012 #include <art_map/MQMath.h>
00013
00014 #define KF_CRASH 0 // Matrix dimensions error, check your code!
00015 #define KF_NUMERICS 1 // Bad error matrix, reset
00016 #define KF_OUTLIER 2 // Outlier rejected
00017 #define KF_SUCCESS 3 // Success!
00018
00019 #define S_D_RANGE_REJECT 2
00020
00021 struct KFStruct {
00022
00023 float R;
00024 float Y;
00025 float Ybar;
00026 bool rejectOutliers;
00027 float outlierSD;
00028 bool mainFilterAngleUpdate;
00029 bool ingoreLongRangeUpdate;
00030 float deadzoneSize;
00031 float dist;
00032 bool ambigObject;
00033 bool changeAlpha;
00034 };
00035
00036 class KF
00037 {
00038 public:
00039 KF();
00040 ~KF() {};
00041
00042 bool Start(short numStates, Matrix& uncert, Matrix& intStates);
00043 bool Restart();
00044 bool TimeUpdate(Matrix& A, Matrix& B, Matrix& U, Matrix& Q,
00045 bool mainFilterUpdate);
00046 bool TimeUpdateExtended(Matrix& A, Matrix& Xbar, Matrix& Q);
00047 int MeasurementUpdate(Matrix& C, float R, float Y, bool rejectOutliers,
00048 float outlierError, bool mainFilterAngleUpdate);
00049 int MeasurementUpdateExtended(Matrix& C, float R, float Y, float Ybar,
00050 bool rejectOutliers, float outlierError,
00051 bool mainFilterAngleUpdate,
00052 bool ignoreLongRangeUpdate,
00053 float deadzoneSize, float dist,
00054 bool ambigObject, bool changeAlpha);
00055
00056 int MeasurementUpdateExtended(Matrix &C,KFStruct s);
00057
00058 void Reset();
00059 Matrix GetStates();
00060 void SetStates(Matrix Xbar);
00061 float GetState(short n);
00062 void SetState(short n, float x);
00063 void NormaliseState(short n);
00064 Matrix GetErrorMatrix();
00065 void SetErrorMatrix(Matrix Pbar);
00066 float GetCovariance(short m, short n);
00067 float GetVariance(short n);
00068 Matrix GetXchanges();
00069 float GetXchange(short n);
00070 void CompilerError(const char* str);
00071
00072 void Deadzone(float* R, float* innovation, float CPC, float eps);
00073
00074 short numStates;
00075 Matrix I;
00076 Matrix initX;
00077 Matrix initP;
00078 Matrix X;
00079 Matrix P;
00080 Matrix Xchange;
00081
00082
00083
00084
00085 bool active;
00086 bool activate;
00087 float alpha;
00088 };
00089
00090 #endif // _KF_h_DEFINED
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