EKF_RP.cpp

00001 #include "EKF_RP.h"
00002 
00003 using namespace std;
00004 using namespace Eigen;
00005 
00006 EKF_RP::EKF_RP(float Ts)
00007 {
00008     this->Ts = Ts;
00009     /* [n_gyro; n_b_g; n_v] */
00010     var_fx << 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f;
00011     /* [n_acc] */
00012     var_gy << 40.0f, 40.0f;
00013     rho = 1.0f;
00014     // kv = 0.5f;       /* QK3 k1/m */
00015     kv = 0.26f;       /* QK4 k1/m */
00016     g = 9.81f;
00017     scale_P0 = 1000.0f;
00018     reset();
00019 }
00020 
00021 EKF_RP::~EKF_RP() {}
00022 
00023 void EKF_RP::reset()
00024 {   
00025     u.setZero();
00026     y.setZero();
00027     x.setZero();
00028     update_angles();
00029     calc_F();
00030     calc_H();
00031     initialize_Q();
00032     initialize_R();
00033     K.setZero();
00034     I.setIdentity();
00035     e.setZero();
00036     P = scale_P0 * I;
00037 }
00038 
00039 void EKF_RP::increase_diag_P()
00040 {
00041     P = P + scale_P0 * I;      
00042 }
00043 
00044 float EKF_RP::get_est_state(uint8_t i)
00045 {   
00046     /* x = [ang; v; b_g] = [0: phi
00047                             1: theta
00048                             2: vx
00049                             3: vy
00050                             4: b_gx
00051                             5: b_gy] */
00052     return x(i);
00053 }
00054 
00055 void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y)
00056 {
00057     u << gyro_x, gyro_y;
00058     y << accel_x, accel_y;
00059     update_angles();
00060     
00061     calc_F();
00062     // calc_H(); /* H remains constant */
00063     calc_Q();
00064     
00065     x = fxd();
00066     P = F * P * F.transpose() + Q;
00067     e = y - gy();
00068     
00069     /* inversion faster 184 mus < 207 mus recursion */
00070     K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
00071     x = x + K * e;
00072     P = (I - K * H) * P;
00073     
00074     /* only valid if R is diagonal (uncorrelated noise) */
00075     /*
00076     for(uint8_t i = 0; i < 2; i++)  {
00077         K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
00078         x = x + K.col(i) * e(i);
00079         P = (I - K.col(i)*H.row(i)) * P;
00080     }
00081     */
00082 }
00083 
00084 void EKF_RP::update_angles()
00085 {
00086     s1 = sinf(x(0));
00087     c1 = cosf(x(0));
00088     s2 = sinf(x(1));
00089     c2 = cosf(x(1));   
00090 }
00091 
00092 void EKF_RP::calc_F()
00093 {
00094     F << Ts*c1*s2*(u(1) - x(3))/c2 + 1.0f, Ts*s1*(u(1) - x(3))/(c2*c2),  -Ts, -Ts*s1*s2/c2,         0.0f,         0.0f,
00095                      -Ts*s1*(u(1) - x(3)),                        1.0f, 0.0f,       -Ts*c1,         0.0f,         0.0f,
00096                                      0.0f,                        0.0f, 1.0f,         0.0f,         0.0f,         0.0f,
00097                                      0.0f,                        0.0f, 0.0f,         1.0f,         0.0f,         0.0f,
00098                                      0.0f,                     Ts*c2*g, 0.0f,         0.0f, 1.0f - Ts*kv,         0.0f,
00099                               -Ts*c1*c2*g,                  Ts*g*s1*s2, 0.0f,         0.0f,         0.0f, 1.0f - Ts*kv;
00100 }
00101 
00102 void EKF_RP::calc_H()
00103 {
00104     H << 0.0f, 0.0f, 0.0f, 0.0f,  -kv, 0.0f,
00105          0.0f, 0.0f, 0.0f, 0.0f, 0.0f,  -kv;
00106 }
00107 
00108 void EKF_RP::initialize_R()
00109 {
00110     R <<  rho*var_gy(0)/Ts,             0.0f,
00111                       0.0f, rho*var_gy(1)/Ts;
00112 }
00113 
00114 void EKF_RP::initialize_Q()
00115 {   
00116     Q << Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2)), Ts*c1*s1*s2*var_fx(1)/c2,         0.0f,         0.0f,         0.0f,         0.0f,
00117                                Ts*c1*s1*s2*var_fx(1)/c2,       Ts*c1*c1*var_fx(1),         0.0f,         0.0f,         0.0f,         0.0f,
00118                                                    0.0f,                     0.0f, Ts*var_fx(2),         0.0f,         0.0f,         0.0f,
00119                                                    0.0f,                     0.0f,         0.0f, Ts*var_fx(3),         0.0f,         0.0f,
00120                                                    0.0f,                     0.0f,         0.0f,         0.0f, Ts*var_fx(4),         0.0f,
00121                                                    0.0f,                     0.0f,         0.0f,         0.0f,         0.0f, Ts*var_fx(5);
00122 }
00123 
00124 void EKF_RP::calc_Q()
00125 {   
00126     Q(0,0) = Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2));
00127     Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2;
00128     Q(1,0) = Q(0,1);
00129     Q(1,1) = Ts*c1*c1*var_fx(1);
00130 }
00131 
00132 Matrix <float, 6, 1>  EKF_RP::fxd()
00133 {
00134     Matrix <float, 6, 1> retval;
00135     retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2),
00136                                       x(1) + Ts*c1*(u(1) - x(3)),
00137                                                             x(2),
00138                                                             x(3),
00139                                       x(4) + Ts*(g*s2 - kv*x(4)),
00140                                    x(5) - Ts*(kv*x(5) + c2*g*s1);
00141     return retval;
00142 }
00143 
00144 Matrix <float, 2, 1> EKF_RP::gy()
00145 {
00146     Matrix <float, 2, 1> retval;
00147     retval << -kv*x(4),
00148               -kv*x(5);
00149     return retval;
00150 }
00151