/*! \file \brief A test for Kalman with unknown Q \author Vaclav Smidl. ----------------------------------- BDM++ - C++ library for Bayesian Decision Making under Uncertainty Using IT++ for numerical operations ----------------------------------- */ #include #include #include #include "pmsm.h" using namespace bdm; //!Extended Kalman filter with unknown \c Q class EKF_unQ : public EKFCh , public BMcond { public: //! Default constructor EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {}; void condition ( const vec &Q0 ) { Q.setD ( Q0,0 ); //from EKF preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); }; }; int main() { // Kalman filter int Ndat = 10000; // cout << KF; // internal model IMpmsm fxu; // Rs Ls dt Fmag(Ypm) kp p J Bf(Mz) fxu.set_parameters ( 0.28, 0.003465, 20*1e-6, 0.1989, 1.5 ,4.0, 0.04, 0.0 ); // observation model OMpmsm hxu; vec mu0= "100 100 100 1"; vec Qdiag ( "0.1 0.1 0.01 0.00001" ); vec Rdiag ( "0.02 0.02" ); vec vQ = "0.01:0.01:100"; chmat Q ( Qdiag ); chmat R ( Rdiag ); RV rQ ( "{Q}","2" ); EKF_unQ KFE ( rx,ry,ru,rQ ); KFE.set_parameters ( &fxu,&hxu,Q,R ); KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) ); mgamma evolQ ( rQ,rQ ); //evolQ.set_parameters ( 10000.0 ); //sigma = 1/10 mu MPF M ( rx,rQ,evolQ,evolQ,100,KFE ); const epdf& KFEep = KFE.posterior(); const epdf& Mep = M.posterior(); // initialize evolQ.set_parameters ( 1.0 ); //sigma = 1/10 mu evolQ.condition ( "0.5 0.5" ); const epdf& pfinit=evolQ.posterior(); M.set_est ( pfinit ); evolQ.set_parameters ( 1000.0 ); //sigma = 1/10 mu //simulator values vec dt ( 2 ); // output (isa isb) vec wt ( 2 ); // noise on dt vec ut ( 2 ); // vec xt=mu0; // initial state vec et ( 4 ); // noise on xt mat Xt=zeros ( 4,Ndat ); // True trajetory of xt mat XtE=zeros ( 4,Ndat ); // Estimate of xt using EKF (known Q) mat XtM=zeros ( 6,Ndat ); // Estimate of xt using EKF-MPF Xt.set_col ( 0,mu0 ); XtM.set_col ( 0,Mep.mean() ); for ( int t=1;t