Diff: main.cpp

Revision:

3:121647a7cddf

Parent:

2:756446014084

Child:

4:e50e18eac72b

diff -r 756446014084 -r 121647a7cddf main.cpp
--- a/main.cpp	Fri Oct 18 21:17:11 2019 +0000
+++ b/main.cpp	Fri Oct 18 21:49:59 2019 +0000
@@ -18,48 +18,7 @@
 
 Matrix<float, 8, 8> A;
 Matrix<float, 8, 1> b;
-// Matrix<float, 8, 8> I;
-
-/******************************************************************************/
-
-    void reset();
-    float get_est_state(uint8_t i);
-    void update(float gyro_x, float gyro_y, float accel_x, float accel_y);
-    
-    float s1;
-    float c1;
-    float s2;
-    float c2;
-    
-    float g;
-    float kv;
-    float Ts;
-    float rho;
-    Matrix <float, 2, 1>  var_gy;
-    Matrix <float, 6, 1>  var_fx; 
-
-    Matrix <float, 2, 1>  u;
-    Matrix <float, 2, 1>  y;
-    Matrix <float, 6, 1>  x;
-    Matrix <float, 6, 6>  F;
-    Matrix <float, 2, 6>  H;
-    Matrix <float, 6, 6>  Q;
-    Matrix <float, 2, 2>  R;
-    Matrix <float, 6, 6>  P;
-    Matrix <float, 6, 2>  K;
-    Matrix <float, 6, 6>  I;
-        
-    void update_angles();
-    void update_F();
-    void update_H();
-    void initialize_R();
-    void initialize_Q();
-    void update_Q();
-    
-    Matrix <float, 6, 1> fxd();
-    Matrix <float, 2, 1> gy();
-    
-/******************************************************************************/
+Matrix<float, 8, 8> I;
 
 int main()
 {
@@ -108,141 +67,20 @@
          8;
          
     // I.setIdentity();
-    
-/******************************************************************************/
-
-    // this->Ts = Ts;
-    Ts = 0.05;
-    /* [n_gyro; n_b_g; n_v] */
-    var_fx << 1000, 1000, 20, 20, 10, 10;
-    /* [n_acc] */
-    var_gy << 40, 40;
-    rho = 5000;
-    kv = 0.5;       /* k1/m */
-    g = 9.81;
-    reset();
-
-/******************************************************************************/
         
     while(1) {
         
-        Matrix<float, 8, 1> x = A.inverse() * b;
+        // Matrix<float, 8, 1> x = A.inverse() * b;
         
-        // ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
+        ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
         dt = timer.read();
         timer.reset();
         
         pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt);
-        pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2));
+        // pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt);
 
         counter++;
         
-        wait_us(1000000);
+        // wait_us(1000000);
     }
 }
-
-void reset()
-{   
-    u.setZero();
-    y.setZero();
-    x.setZero();
-    update_F();
-    update_H();
-    initialize_Q();
-    initialize_R();
-    P = Q;
-    K.setZero();
-    I.setIdentity();
-}
-
-void update(float gyro_x, float gyro_y, float accel_x, float accel_y)
-{
-    u << gyro_x, gyro_y;
-    y << accel_x, accel_y;
-    
-    update_F();
-    // update_H();  /* H remains constant */
-    update_Q();
-    
-    x = fxd();
-    P = F * P * F.transpose() + Q;
-    
-    K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
-    
-    x = x + K * (y - gy());
-    P = (I - K * H) * P;
-}
-
-float get_est_state(uint8_t i)
-{   
-    // x = [ang; b_g; v] 
-    return x(i);
-}
-
-void update_angles()
-{
-    s1 = sin(x(0));
-    c1 = cos(x(0));
-    s2 = sin(x(1));
-    c2 = cos(x(1));   
-}
-
-void update_F()
-{
-    F << (Ts*c1*s2*(u(1) - x(3)))/c2 + 1, -(Ts*s1*(u(1) - x(3)))/(s2*s2 - 1), -Ts, -(Ts*s1*s2)/c2,         0,         0,
-                    -Ts*s1*(u(1) - x(3)),                                  1,   0,         -Ts*c1,         0,         0,
-                                       0,                                  0,   1,              0,         0,         0,
-                                       0,                                  0,   0,              1,         0,         0,
-                                       0,                            Ts*c2*g,   0,              0, 1 - Ts*kv,         0,
-                             -Ts*c1*c2*g,                         Ts*g*s1*s2,   0,              0,         0, 1 - Ts*kv;
-}
-
-void update_H()
-{
-    H << 0, 0, 0, 0, -kv,   0,
-         0, 0, 0, 0,   0, -kv;
-}
-
-void initialize_R()
-{
-    R <<  rho*var_gy(0)/Ts,                0,
-                         0, rho*var_gy(1)/Ts;
-}
-
-void initialize_Q()
-{   
-    Q << Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2)), (Ts*c1*s1*s2*var_fx(1))/c2,            0,            0,            0,            0,
-                               (Ts*c1*s1*s2*var_fx(1))/c2,         Ts*c1*c1*var_fx(1),            0,            0,            0,            0,
-                                                        0,                          0, Ts*var_fx(2),            0,            0,            0,
-                                                        0,                          0,            0, Ts*var_fx(3),            0,            0,
-                                                        0,                          0,            0,            0, Ts*var_fx(4),            0,
-                                                        0,                          0,            0,            0,            0, Ts*var_fx(5);
-}
-
-void update_Q()
-{   
-    Q(0,0) = Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2));
-    Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2;
-    Q(1,0) = Q(0,1);
-    Q(1,1) = Ts*c1*c1*var_fx(1);
-}
-
-Matrix <float, 6, 1>  fxd()
-{
-    Matrix <float, 6, 1> retval;
-    retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2),
-                                      x(1) + Ts*c1*(u(1) - x(3)),
-                                                            x(2),
-                                                            x(3),
-                                      x(4) + Ts*(g*s2 - kv*x(4)),
-                                   x(5) - Ts*(kv*x(5) + c2*g*s1);
-    return retval;
-}
-
-Matrix <float, 2, 1> gy()
-{
-    Matrix <float, 2, 1> retval;
-    retval << -kv*x(4),
-              -kv*x(5);
-    return retval;
-}