RootEKF.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef ROOTEKF_H
00039 #define ROOTEKF_H
00040
00041 #include <Spaces.h>
00042 #include <math.h>
00043 #include <PDF.h>
00044
00045 #define DEBUG
00046
00047 using namespace MipBaselib;
00048 using namespace arma;
00049
00050
00051 namespace MipAlgorithms
00052 {
00054
00055
00059 class RootEKFvars
00060 {
00061 public:
00062 RootEKFvars(){};
00063 virtual void evalA(DCol input, DCol state, Decimal sTime) = 0;
00064 virtual void evalW(DCol input, DCol state, Decimal sTime) = 0;
00065 virtual void evalV(DCol state) = 0;
00066 virtual void evalH(DCol state) = 0;
00067 virtual DCol projectState(DCol input, DCol state, Decimal sTime) = 0;
00068 virtual DCol predictMeas(DCol state) = 0;
00069 inline DMat getA() const {return A;};
00070 inline DMat getV() const {return V;};
00071 inline DMat getH() const {return H;};
00072 inline DMat getW() const {return W;};
00073 protected:
00074 DMat A;
00075 DMat V;
00076 DMat H;
00077 DMat W;
00078 };
00079
00083 class RootEKFparams
00084 {
00085 public:
00086 RootEKFparams(){};
00087
00088 inline DCol getInitialEst() const {return initEst;}
00089 inline DMat getInitialCov() const {return initCov;}
00090 inline DMat getInitialProcNoise() const {return initProcNoise;}
00091 inline DMat getInitialMeasNoise() const {return initMeasNoise;}
00092 protected:
00093 DCol initEst;
00094 DMat initCov;
00095 DMat initProcNoise;
00096 DMat initMeasNoise;
00097 };
00098
00102 class RootEKF
00103 {
00104 public:
00108 RootEKF(RootEKFparams* myParam, RootEKFvars* myVars);
00110 DCol getStateEst() const;
00112 DMat getStateCov() const;
00113 inline DMat getProcNoise() const {return procNoise;};
00114 inline void setProcNoise(DMat value){procNoise = value;};
00115 inline DMat getMeasNoise() const {return measNoise;};
00116 inline void setMeasNoise(DMat value){measNoise = value;};
00117 inline DMat getK(){return K;};
00121 void doTimeUpd(DCol input, Decimal sTime);
00125 void doMeasUpd(DCol measure, Decimal sTime);
00130 void doUpd(DCol input, DCol measure, Decimal sTime){doTimeUpd(input, sTime); doMeasUpd(measure, sTime);}
00135 void doRevUpd(DCol input, DCol measure, Decimal sTime){doMeasUpd(measure, sTime); doTimeUpd(input, sTime);}
00136 private:
00137 RootEKFparams* _param;
00138 RootEKFvars* _vars;
00139 DCol stateEst;
00140 DMat stateCov;
00141 DMat procNoise;
00142 DMat measNoise;
00143 DMat K;
00144 void projectCov();
00145 void evalK();
00146 void updState(DCol measure);
00147 void updCov();
00148 void CovSymmetryCheck();
00149 };
00150 };
00151
00152
00153
00154 #endif // ROOTEKF_H