00001 #ifndef actcalKALMAN_H
00002 #define actcalKALMAN_H
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #define _POSIX_SOURCE 1
00021
00022 #include "ccs.h"
00023 #include "actcalDefines.h"
00024
00026 class actcalKALMAN {
00027 private:
00028 vltDOUBLE sigmaXp, sigmaXv, sigmaNoise;
00029 vltDOUBLE sigmaXp_init, sigmaXv_init;
00030
00031 vltDOUBLE Xv_init;
00032 vltDOUBLE T_init;
00033
00034 vltDOUBLE Qpp, Qvv, pee;
00035 vltLOGICAL first_iteration;
00036
00037 vltDOUBLE Xp_new, Xv_new, Xp_err;
00038 vltDOUBLE Ppp_new, Ppv_new, Pvp_new, Pvv_new;
00039 vltDOUBLE time_old;
00040
00041 public:
00042 actcalKALMAN();
00043 ~actcalKALMAN();
00044
00045 ccsCOMPL_STAT Configure(vltDOUBLE inpSigmaXp_init,
00046 vltDOUBLE inpSigmaXv_init,
00047 vltDOUBLE inpSigmaXp,
00048 vltDOUBLE inpSigmaXv,
00049 vltDOUBLE inpSigmaNoise);
00050 ccsCOMPL_STAT Init();
00051 ccsCOMPL_STAT Calculate(vltDOUBLE time_mea, vltDOUBLE Xp_mea);
00052 vltDOUBLE GetPosition();
00053 vltDOUBLE GetSqrError();
00054 };
00055
00057 class actcalKALMANFILTER : public eccsERROR_CLASS {
00058 private:
00059 actcalKALMAN kalmanXmode [actcalMAX_NUM_MODES];
00060 actcalKALMAN kalmanYmode [actcalMAX_NUM_MODES];
00061 vltINT32 symmetry[actcalMAX_NUM_MODES];
00062 actcalM1_CORR_PAR *pM1CorrPar;
00063
00064 public:
00065 actcalKALMANFILTER();
00066 ~actcalKALMANFILTER();
00067
00068 ccsCOMPL_STAT Init();
00069 ccsCOMPL_STAT Calculate();
00070 };
00071
00072
00073
00074
00075 #endif