KF.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*- */
00002 
00003 // $Id: KF.h 85 2010-03-25 23:54:08Z jack.oquin $
00004 
00005 #ifndef _KF_h_DEFINED
00006 #define _KF_h_DEFINED
00007 
00008 //#define DEBUGFILTER
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   //Matrix &C;
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     // ------ New Stuff for multiple models
00083 
00084     // Control and model Evaluation
00085     bool active;    // Is the model currrently in use ?
00086     bool activate; 
00087     float alpha;   // The probability that the model is correct (0->1)
00088 };
00089 
00090 #endif // _KF_h_DEFINED
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:41:51