EKF_obsolete.h

00001 #include "Mat.h"
00002 
00003 template<typename T>
00004 class EKF
00005 {
00006     private :
00007     
00008     T dt;   /* par default : = 0.005 */
00009     int nbr_state;
00010     int nbr_ctrl;
00011     int nbr_obs;
00012     
00013     Mat<T> dX;  /*desired state*/
00014     Mat<T>* _X; /*previous state*/
00015     Mat<T>* X;      /*states*/
00016     Mat<T>* X_; /*derivated states or next states... (continuous/discrete)*/
00017     Mat<T>* u;      /*controls*/
00018     Mat<T>* z;      /*observations/measurements*/
00019     
00020     Mat<T>* Ki;
00021     Mat<T>* Kp;
00022     Mat<T>* Kd;
00023     Mat<T>* Kdd;    
00024     
00025     Mat<T>* A;      /*linear relation matrix between states and derivated states.*/
00026     /*par default : X_ = A * X + B * u / x_i = x_i + dt * x_i_ + b_i * u_i... */
00027     Mat<T>* B;      /*linear relation matrix between derivated states and control.*/
00028     /*par default : B = 1 */
00029     Mat<T>* C;      /*linear relation matrix between states and observation.*/
00030     /*par default : C = [1 0], on observe les positions, non leurs dérivées. */
00031 
00032     /*Noise*/
00033     T std_noise;    /*par defaut : 0.0005*/
00034     Mat<T>* Sigma;  /*covariance matrix*/
00035     Mat<T>* Q;      /*process noise*/
00036     Mat<T>* R;      /*measurement noise*/
00037     
00038     /*Prediction*/
00039     Mat<T> K;       // Kalman Gain...
00040     Mat<T> Sigma_p;
00041     
00042     /*Others*/
00043     Mat<T>* Identity;
00044     
00045     
00046     /*Extended*/
00047     bool extended;
00048     Mat<T> (*ptrMotion)(Mat<T> state, Mat<T> command, T dt);
00049     Mat<T> (*ptrSensor)(Mat<T> state, Mat<T> command, Mat<T> d_state, T dt);
00050     Mat<T> G;
00051     Mat<T> H;
00052     Mat<T> (*ptrJMotion)(Mat<T> state, Mat<T> command, T dt);
00053     Mat<T> (*ptrJSensor)(Mat<T> state, Mat<T> command, Mat<T> d_state, T dt);
00054     
00055     
00056     public :
00057     
00058     EKF(int nbr_state_, int nbr_ctrl_, int nbr_obs_, T dt_, T std_noise_, Mat<T> currentState, bool ext = false)
00059     {
00060         /*extension*/
00061         extended = ext;
00062         ptrMotion = NULL;
00063         ptrSensor = NULL;
00064         ptrJMotion = NULL;
00065         ptrJSensor = NULL;
00066         G = Mat<T>((T)0, nbr_state_, nbr_state_);
00067         H = Mat<T>((T)0, nbr_obs_, nbr_state_);
00068         
00069         /*----------------*/
00070         
00071         dt = dt_;
00072         nbr_state = nbr_state_;
00073         nbr_ctrl = nbr_ctrl_;
00074         nbr_obs = nbr_obs_;
00075         
00076         _X = new Mat<T>((T)0, nbr_state, (int)1);       /*previous state*/
00077         X = new Mat<T>(currentState);                   /*states*/
00078         X_ = new Mat<T>((T)0, nbr_state, (int)1);       /*derivated states*/
00079         u = new Mat<T>((T)0, nbr_ctrl, (int)1);         /*controls*/
00080         z = new Mat<T>((T)0, nbr_obs, (int)1);          /*observations*/
00081         A = new Mat<T>((T)0, nbr_state, nbr_state);     /*linear relation or jacobian matrix between states and derivated states.*/
00082         B = new Mat<T>((T)0, nbr_state, nbr_ctrl);      /*linear relation matrix between derivated states and control.*/
00083         C = new Mat<T>((T)0, nbr_obs, nbr_state);       /*linear relation or jacobian matrix between states and observation.*/
00084     
00085         Ki = new Mat<T>((T)0.08, nbr_ctrl, nbr_state);
00086         Kp = new Mat<T>((T)0.08, nbr_ctrl, nbr_state);
00087         Kd = new Mat<T>((T)0.08, nbr_ctrl, nbr_state);
00088         Kdd = new Mat<T>((T)0.08, nbr_ctrl, nbr_state);
00089     
00090     
00091         std_noise = std_noise_;
00092         Sigma = new Mat<T>((T)0, nbr_state, nbr_state);
00093         Q = new Mat<T>((T)0, nbr_state, nbr_state);
00094         R = new Mat<T>((T)0, nbr_obs, nbr_obs/*1*/);
00095     
00096         /*Initialize Covariance matrix as the identity matrix.*/
00097         for(int i=1;i<=nbr_state;i++)
00098         {
00099             Sigma->set((T)1, i,i);
00100             R->set( (T)1, i,i);
00101             /*
00102             for(int j=1;j<=nbr_state;j++)
00103             {
00104                 Sigma->set((T)1, i, j);
00105             
00106                 //if(i<=nbr_obs && j==1)
00107                 //  R->set(std_noise*std_noise, i, j);
00108                 
00109             }
00110             */
00111         }
00112     
00113         Identity = new Mat<T>(*Sigma);
00114         *Q = (std_noise*std_noise)*(*Identity);
00115         *R = (std_noise*std_noise)*(*R);
00116     
00117     }
00118 
00119     ~EKF()
00120     {
00121         delete _X;
00122         delete X;
00123         delete X_;
00124         delete u;
00125         delete z;
00126         delete A;
00127         delete B;
00128         delete C;
00129     
00130         delete Ki;
00131         delete Kp;
00132         delete Kd;
00133         delete Kdd;
00134     
00135         delete Sigma;
00136         delete Q;
00137         delete R;
00138     
00139         delete Identity;
00140     }
00141 
00142 /*-------------------------------------------------------------*/
00143 /*-------------------------------------------------------------*/
00144 /*-------------------------------------------------------------*/
00145 
00146 
00147     int initA( Mat<T> A_)
00148     {
00149         if(A_ == *Identity)
00150         {
00151             for(int i=1;i<=(int)(nbr_state/2);i++)
00152             {
00153                 A->set( dt, i, i+(int)(nbr_state/2));
00154             }
00155         
00156             return 1;
00157         }
00158         else
00159         {
00160             if(A_.getColumn() == nbr_state && A_.getLine() == nbr_state)
00161             {
00162                 *A = A_;
00163                 return 1;
00164             }
00165             else
00166             {
00167                 cout << "ERREUR : mauvais format de matrice d'initialisation de A." << endl;
00168                 return 0;
00169             }
00170         }
00171     }
00172 
00173 
00174     int initB( Mat<T> B_)
00175     {   
00176         if(B_.getColumn() == nbr_ctrl && B_.getLine() == nbr_state)
00177         {
00178             *B = B_;
00179             return 1;
00180         }
00181         else
00182         {
00183             cout << "ERREUR : mauvais format de matrice d'initialisation de B." << endl;
00184             return 0;
00185         }
00186     
00187     }
00188     
00189     
00190     
00191     int initC( Mat<T> C_)
00192     {   
00193         if(C_.getColumn() == nbr_state && C_.getLine() == nbr_obs)
00194         {
00195             *C = C_;
00196             return 1;
00197         }
00198         else
00199         {
00200             cout << "ERREUR : mauvais format de matrice d'initialisation de C." << endl;
00201             return 0;
00202         }
00203     
00204     }
00205     
00206     /*extension*/
00207     void initMotion( Mat<T> motion(Mat<T>, Mat<T>, T) )
00208     {
00209         ptrMotion = motion;
00210     }
00211     
00212     
00213     
00214     void initSensor( Mat<T> sensor(Mat<T>, Mat<T>, Mat<T>, T) )
00215     {   
00216         ptrSensor = sensor; 
00217     }
00218     
00219     void initJMotion( Mat<T> jmotion(Mat<T>, Mat<T>, T) )
00220     {
00221         ptrJMotion = jmotion;
00222     }
00223     
00224     
00225     
00226     void initJSensor( Mat<T> jsensor(Mat<T>, Mat<T>, Mat<T>, T) )
00227     {   
00228         ptrJSensor = jsensor;   
00229     }
00230     
00231     
00232 /*-------------------------------------------------------------*/
00233 /*-------------------------------------------------------------*/
00234 /*-------------------------------------------------------------*/
00235 
00236 
00237     int setKi( Mat<T> Ki_)
00238     {
00239         if(Ki_.getColumn() == nbr_state && Ki_.getLine() == nbr_ctrl)
00240         {
00241             *Ki = Ki_;
00242             return 1;
00243         }
00244         else
00245         {
00246             cout << "ERREUR : mauvais format de vecteur d'initialisation de Ki." << endl;
00247             return 0;
00248         }
00249     }
00250     
00251     int setKp( Mat<T> Kp_)
00252     {
00253         if(Kp_.getColumn() == nbr_state && Kp_.getLine() == nbr_ctrl)
00254         {
00255             *Kp = Kp_;
00256             return 1;
00257         }
00258         else
00259         {
00260             cout << "ERREUR : mauvais format de vecteur d'initialisation de Kp." << endl;
00261             return 0;
00262         }
00263     }
00264     
00265     
00266     
00267     int setKd( Mat<T> Kd_)
00268     {
00269         if(Kd_.getColumn() == nbr_state && Kd_.getLine() == nbr_ctrl)
00270         {
00271             *Kd = Kd_;
00272             return 1;
00273         }
00274         else
00275         {
00276             cout << "ERREUR : mauvais format de vecteur d'initialisation de Kd." << endl;
00277             return 0;
00278         }
00279 
00280     }
00281     
00282     int setKdd( Mat<T> Kdd_)
00283     {
00284         if(Kdd_.getColumn() == nbr_state && Kdd_.getLine() == nbr_ctrl)
00285         {
00286             *Kdd = Kdd_;
00287             return 1;
00288         }
00289         else
00290         {
00291             cout << "ERREUR : mauvais format de vecteur d'initialisation de Kdd." << endl;
00292             return 0;
00293         }
00294 
00295     }
00296     
00297     
00298     void setdt( float dt_)
00299     {
00300         dt = dt_;
00301     }
00302     
00303 /*-------------------------------------------------------------*/
00304 /*-------------------------------------------------------------*/
00305 /*-------------------------------------------------------------*/   
00306     
00307     
00308     Mat<T> getCommand()
00309     {
00310         return *u;
00311     }   
00312     
00313     Mat<T> getX()
00314     {
00315         return *X;
00316     }
00317 
00318     Mat<T> getSigma()
00319     {
00320         return *Sigma;
00321     }   
00322 
00323 
00324 /*-------------------------------------------------------------*/
00325 /*-------------------------------------------------------------*/
00326 /*-------------------------------------------------------------*/       
00327     
00328     
00329     Mat<T> predictState()       /*return the computed predicted state*/
00330     {
00331         return (!extended ? (*A)*(*X)+(*B)*(*u) : ptrMotion(*X, *u, dt) );
00332     }
00333     
00334     
00335     Mat<T> predictCovariance()  /*return the predicted covariance matrix.*/
00336     {
00337         if(extended)
00338             G = ptrJMotion(*X, *u, dt);
00339         
00340         return ( (!extended ? ((*A)*(*Sigma))* transpose(*A) : G*(*Sigma)*transpose(G) ) + *Q);
00341     }
00342     
00343         
00344     Mat<T> calculateKalmanGain()    /*return the Kalman Gain K = C*Sigma_p * (C*Sigma_p*C.T +R).inv */
00345     {
00346         Sigma_p = predictCovariance();  
00347     
00348         if(extended)
00349             H = ptrJSensor(*X, *u, dX,dt);
00350     
00351         return ( !extended ? Sigma_p * transpose(*C) * invGJ( (*C) * Sigma_p * transpose(*C) + *R)  : Sigma_p * transpose(H) * invGJ( H * Sigma_p * transpose(H) + *R) );
00352     }
00353     
00354         
00355     Mat<T> correctState()       /*update X */
00356     {
00357         Mat<T> X_p( predictState());        
00358     
00359         *_X = *X;
00360         *X = X_p + K*( (*z) - (!extended ? (*C)*X_p  : ptrSensor(X_p, *u, dX, dt) ) );
00361     
00362         return *X;
00363     }
00364     
00365         
00366     Mat<T> correctCovariance()  /*update Sigma*/
00367     {
00368         *Sigma = (*Identity - K* (!extended ? (*C) : H) ) *Sigma_p;
00369     
00370         return *Sigma;
00371     }
00372     
00373     
00374     void state_Callback()       /* Update all... */
00375     {
00376         if( extended && (ptrMotion == NULL || ptrSensor == NULL || ptrJMotion == NULL || ptrJSensor == NULL) )
00377         {
00378             //~EKF();
00379             throw("ERREUR : les fonctions ne sont pas initialisees...");
00380         }       
00381         
00382         K = calculateKalmanGain();          
00383         correctState();
00384         correctCovariance();
00385     }
00386     
00387     void measurement_Callback(Mat<T> measurements, Mat<T> dX_)
00388     {
00389         if( extended && (ptrMotion == NULL || ptrSensor == NULL || ptrJMotion == NULL || ptrJSensor == NULL) )
00390         {
00391             //~EKF();
00392             throw("ERREUR : les fonctions ne sont pas initialisees...");
00393         }
00394         
00395         dX = dX_;
00396         
00397         *z = (!extended ? measurements : ptrSensor(*X,*u, dX, dt) );    
00398     }
00399     
00400     void measurement_Callback(Mat<T> measurements)
00401     {
00402         if( extended && (ptrMotion == NULL || ptrSensor == NULL || ptrJMotion == NULL || ptrJSensor == NULL) )
00403         {
00404             //~EKF();
00405             throw("ERREUR : les fonctions ne sont pas initialisees...");
00406         }
00407         
00408         
00409         *z = (!extended ? measurements : ptrSensor(*X,*u, dX, dt) );    
00410     }
00411     
00412     
00413     void computeCommand( Mat<T> desiredX, T dt_, int mode)
00414     {
00415         if(dt_ != (T)0 && mode != -1)
00416         {
00417             *u = (*Kp)*(desiredX - (*X));
00418             if(mode >= 1)
00419                 *u = *u + (T)(dt_)*(*Ki)*(desiredX - (*X));
00420             if(mode >= 2)
00421                 *u = *u + (T)((double)1.0/dt_)*(*Kd)*(desiredX - (*X));
00422             if(mode >= 3)
00423                 *u = *u + (T)((double)1.0/(dt_*dt_))*(*Kdd)*(desiredX - (*X));                      
00424         }       
00425         else if(mode !=-1)
00426         {
00427             *u = (*Kp)*(desiredX - (*X));       
00428         }
00429             
00430         if(mode <= -1)
00431         {
00432             
00433             Mat<double> bicycle((double)0, 3,1);
00434             bicycle.set((double)70, 1,1);  /*radius*/
00435             bicycle.set((double)70, 2,1);
00436             bicycle.set((double)260, 3,1);   /*entre-roue*/
00437             
00438             double rho = (double)sqrt(z->get(1,1)*z->get(1,1) + z->get(2,1)*z->get(2,1));
00439             double alpha = atan21( dX.get(2,1)-X->get(2,1), dX.get(1,1) - X->get(1,1) );
00440             double beta = alpha + X->get(3,1) + dX.get(3,1);
00441             
00442             cout << "alpha = " << alpha << endl;
00443             
00444             double krho = (rho > 1 ? rho*3 : 1)/100;
00445             double kalpha = rho*300/100;
00446             double kbeta = 4*beta/(rho+1)/100;
00447             
00448             double vd = krho*rho;
00449             double wd = kalpha*alpha +  kbeta*beta;
00450             
00451             double phi_max = 100;
00452             
00453             double phi_r = bicycle.get(3,1)/bicycle.get(1,1)*(wd+vd/bicycle.get(3,1));
00454             double phi_l = bicycle.get(3,1)/bicycle.get(2,1)*(wd+vd/bicycle.get(3,1));
00455         
00456             phi_r = (phi_r <= phi_max ? phi_r : phi_max);
00457             phi_l = (phi_l <= phi_max ? phi_l : phi_max);
00458             
00459             vd = bicycle.get(1,1)/2*(phi_r+phi_l);
00460             wd = bicycle.get(1,1)/(2*bicycle.get(3,1))*(phi_r-phi_l);
00461             
00462             u->set( vd, 1,1);
00463             u->set( wd, 2,1);
00464             
00465         }
00466     }   
00467     
00468 
00469 };