EKF.cpp

00001 #include "EKF.h"
00002 
00003 using namespace std;
00004 
00005 EKF::EKF(float Ts) : x(6,1,0.0),x_km(6,1,0.0),P(6,0.001),Q(6),R(2,0.00045),F(6,6,0.0),H(2,6,0.0),K(6,2,0.0),E(6)
00006 {
00007     
00008     g = -9.81;  // -1 wege Drehung des z-Vektors (z nach oben)
00009     tau_g = 2.0;
00010     itau_g = 1.0f/tau_g;
00011     k1 = 0.55;
00012     m = 1.1;
00013     kdm = k1/m;
00014     this->Ts = Ts;
00015     
00016     F.put_entry(0,2,-1);
00017     F.put_entry(1,3,-1);
00018     F.put_entry(2,2,-1/tau_g);
00019     F.put_entry(3,3,-1/tau_g);
00020     F.put_entry(4,1,-g);
00021     F.put_entry(5,0,g);
00022     F.put_entry(4,4,-k1/m);
00023     F.put_entry(5,5,-k1/m); // F is continuous Linearized!!
00024     H.put_entry(0,4,-kdm);
00025     H.put_entry(1,5,-kdm);
00026     F.scale(Ts);
00027     F.mcopy(&(F+E));
00028     Q.mcopy(&(F/=F));   // Q=A*A'
00029     Q.scale(1e-11);
00030     //H.printout();
00031     //P.printout();
00032     }
00033     
00034     
00035 EKF::~EKF(void) {
00036     printf("EKF is being deleted\r\n");
00037     }
00038 
00039 void EKF::dgl(matrix *Z)
00040 {
00041     
00042     x_km.a[0][0]= x.a[0][0]+Ts*(Z->a[0][0]-x.a[2][0]+tan(x.a[1][0])*my_sin(x.a[0][0])*(Z->a[1][0]-x.a[3][0]));
00043     x_km.a[1][0]= x.a[1][0]+Ts*(my_cos(x.a[0][0])*(Z->a[1][0]-x.a[3][0]));
00044     x_km.a[2][0]= x.a[2][0]+Ts*(-itau_g*x.a[2][0]);
00045     x_km.a[3][0]= x.a[3][0]+Ts*(-itau_g*x.a[3][0]);
00046     x_km.a[4][0]= x.a[4][0]+Ts*(-g*my_sin(x.a[1][0])-kdm*x.a[4][0]);
00047     x_km.a[5][0]= x.a[5][0]+Ts*(g*my_cos(x.a[1][0])*my_sin(x.a[0][0])-kdm*x.a[5][0]);
00048     }
00049 // Just map gx ... to measurement matrix (to be consistent with Mahony etc.
00050 void EKF::update(float gx,float gy,float gz,float ax,float ay,float az){
00051     matrix measurement(6, 1, 0.0f);
00052     measurement.put_entry(0, 0, gx);
00053     measurement.put_entry(1, 0, gy);
00054     measurement.put_entry(3, 0, ax);
00055     measurement.put_entry(4, 0, ay);
00056     loop(&measurement);
00057     }  
00058 // 
00059 void EKF::loop(matrix *Z)
00060 {
00061     // F_km1 is constant
00062 
00063     dgl(Z);                          // here x_km is calculated
00064     P.mcopy(&(((F*P)/=F)+Q));                        //P_km = F_km1*P_km*F_km1'+Q_k
00065 
00066     matrix dum1(2);
00067     dum1.mcopy(&(((H*P)/=H)+R));
00068     K.mcopy(&((P/=H)*dum1.inv_2x2()));  // K_k=(P_km*H_k')*inv(H_k*P_km*H_k'+R_k); % Kalman-Gain
00069                                         // x_k = x_km + K_k*(Z(r,[4 5])'-copter3D_calc_h_ekf(x_km,kin)); 
00070     matrix dum2(2,1,0.0);
00071     dum2.put_entry(0,0,Z->a[3][0]+kdm*x_km.a[4][0]);
00072     dum2.put_entry(1,0,Z->a[4][0]+kdm*x_km.a[5][0]);     // calculates Z-h h is -k/m*...
00073     x_km += (K*dum2);
00074     x.mcopy(&x_km);
00075                                         // P_km = (eye(6)-K_k*H_k)*P_km; % Kovar
00076     P.mcopy(&((E-(K*H))*P));
00077   
00078     }
00079 float EKF::get_est_state(uint8_t i)
00080 {
00081     return x_km.a[i][0];
00082     }