Revision 11:083c8c9a5b84, committed 2015-06-16

Comitter:

ojan

Date:

Tue Jun 16 15:31:38 2015 +0000

Parent:

10:8ee11e412ad7

Child:

12:0d978eb4d639

Commit message:

add Gravity vector estimation with Kalman filter (unsuccessful)

Changed in this revision

--- a/main.cpp	Mon Jun 15 13:29:36 2015 +0000
+++ b/main.cpp	Tue Jun 16 15:31:38 2015 +0000
@@ -89,15 +89,28 @@
 float target_x, target_y;
 
 /* ----- Kalman Filter ----- */
-Vector pri_x(6);
-Matrix pri_P(6, 6);
-Vector post_x(6);
-Matrix post_P(6, 6);
-Matrix F(6, 6), H(3, 6);
-Matrix R(6, 6), Q(3, 3);
-Matrix I(6, 6);
-Matrix K(6, 3);
-Matrix S(3, 3), inv(3, 3);
+// 地磁気ベクトル用
+Vector pri_x1(6);
+Matrix pri_P1(6, 6);
+Vector post_x1(6);
+Matrix post_P1(6, 6);
+Matrix F1(6, 6), H1(3, 6);
+Matrix R1(6, 6), Q1(3, 3);
+Matrix I1(6, 6);
+Matrix K1(6, 3);
+Matrix S1(3, 3), S_inv1(3, 3);
+
+// 重力ベクトル用
+// ジャイロのバイアスも同時に推定する
+Vector pri_x2(6);
+Matrix pri_P2(6, 6);
+Vector post_x2(6);
+Matrix post_P2(6, 6);
+Matrix F2(6, 6), H2(3, 6);
+Matrix R2(6, 6), Q2(3, 3);
+Matrix I2(6, 6);
+Matrix K2(6, 3);
+Matrix S2(3, 3), S_inv2(3, 3);
 /* ----- ------------- ----- */
 
 Timer timer;
@@ -122,7 +135,7 @@
     
     if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
     if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
-    
+    /*
     //Config読み取り
     LoadConfig();
     
@@ -138,7 +151,7 @@
     fp = fopen(filename, "w");
     fprintf(fp, "log data\r\n");
     xbee.printf("log data\r\n");
-    
+    */
     servoL.period(0.020f);                // サーボの信号の周期は20ms
     servoR.period(0.020f);
     
@@ -153,12 +166,12 @@
     raw_g.SetComp(3, 1.0f);
     
     // 機体に固定されたベクトルの初期化
-    b_f.SetComp(1, 0.0f);
+    b_f.SetComp(1, 1.0f);
     b_f.SetComp(2, 0.0f);
-    b_f.SetComp(3, -1.0f);
-    b_u.SetComp(1, 1.0f);
+    b_f.SetComp(3, 0.0f);
+    b_u.SetComp(1, 0.0f);
     b_u.SetComp(2, 0.0f);
-    b_u.SetComp(3, 0.0f);
+    b_u.SetComp(3, 1.0f);
     b_l = Cross(b_u, b_f);
     
     /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
@@ -181,6 +194,7 @@
             r_f = geomag.GetPerpCompTo(g).Normalize();
             r_l = Cross(r_u, r_f);
             
+            
             // 回転行列を経由してオイラー角を求める
             // オイラー角はヨー・ピッチ・ロールの順に回転したものとする
             // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
@@ -191,9 +205,14 @@
             float R_23 = r_l * b_u;             // 回転行列の(2,3)成分
             float R_33 = r_u * b_u;             // 回転行列の(3,3)成分
             
-            yaw = atan2(R_11, -R_12) * RAD_TO_DEG;
-            roll = atan2(R_33, -R_23) * RAD_TO_DEG;
-            pitch = atan2(R_11/cos(yaw), R_13) * RAD_TO_DEG;
+            yaw = atan2(-R_12, R_11) * RAD_TO_DEG;
+            roll = atan2(-R_23, R_33) * RAD_TO_DEG;
+            pitch = asin(R_13) * RAD_TO_DEG;
+            
+            pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", 
+                    gyro.GetComp(1), post_x2.GetComp(4), 
+                    gyro.GetComp(2), post_x2.GetComp(5), 
+                    gyro.GetComp(3), post_x2.GetComp(6));
             
             if(UTC_t - pre_UTC_t >= 1) {                    // GPSデータが更新されていたら
                 p.SetComp(1, gms.longitude * DEG_TO_RAD);
@@ -214,28 +233,32 @@
                 geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), 
                 press, gms.longitude, gms.latitude, 
                 sv, lv, optSensor.read_u16());
-        fprintf(fp, data);
-        xbee.printf(data);
+        //fprintf(fp, data);
+        //fflush(fp);
+        //xbee.printf(data);
         
         INT_flag = TRUE;            // 割り込み許可
         
         // 制御ルーチン
         {
-            //pull_L = (pull_L+5)%30;
-            //pull_R = (pull_R+5)%30;
-            pull_L = 0;
-            pull_R = 30;
+            pull_L = (pull_L+5)%30;
+            pull_R = (pull_R+5)%30;
+            //pull_L = 0;
+            //pull_R = 30;
             servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
             servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
             
         }
         
         myled = 0; // LED is OFF
+        /*pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", 
+                post_x.GetComp(4), post_x.GetComp(5), post_x.GetComp(6), 
+                yaw, pitch, roll, 
+                geomag.GetNorm());*/
         
         // ループはきっかり0.2秒ごと
         while(timer.read_ms() < 200);
         
-        pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
         
     }
     
@@ -281,66 +304,134 @@
 }
 
 void KalmanInit() {
-    
-    // 誤差共分散行列の値を決める（対角成分のみ）
-    float alpha_R = 60.0f;
-    float alpha_Q = 100.0f;
-    R *= alpha_R;
-    Q *= alpha_Q;
+    // 重力
+    {
+        // 誤差共分散行列の値を決める（対角成分のみ）
+        float alpha_R2 = 60.0f;
+        float alpha_Q2 = 100.0f;
+        R2 *= alpha_R2;
+        Q2 *= alpha_Q2;
+        
+        // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
+        float f[36] = {
+            1.0f, (raw_gyro.GetComp(3) - post_x2.GetComp(6))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, post_x2.GetComp(3), -post_x2.GetComp(2), 
+            -(raw_gyro.GetComp(3) - post_x2.GetComp(6))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, -post_x2.GetComp(3), 0.0f, post_x2.GetComp(1), 
+            (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, post_x2.GetComp(2), -post_x2.GetComp(1), 0.0f, 
+            0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
+            0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
+            0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
+        };
+        
+        F2.SetComps(f);
+        
+        // 観測方程式のヤコビアンの値を設定（時間変化無し）
+        float h[18] = {
+            1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,  
+            0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,  
+            0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f
+        };
+        
+        H2.SetComps(h);
+    }
     
-    // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
-    float f[36] = {
-        1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
-        -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
-        raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
-        0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
-        0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
-        0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
-    };
-    
-    F.SetComps(f);
-    
-    // 観測方程式のヤコビアンの値を設定（時間変化無し）
-    float h[18] = {
-        1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,  
-        0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
-        0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
-    };
-    
-    H.SetComps(h);
+    // 地磁気
+    {
+        // 誤差共分散行列の値を決める（対角成分のみ）
+        float alpha_R1 = 500.0f;
+        float alpha_Q1 = 1000.0f;
+        R1 *= alpha_R1;
+        Q1 *= alpha_Q1;
+        
+        // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
+        float f[36] = {
+            1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
+            -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
+            raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
+            0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
+            0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
+            0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
+        };
+        
+        F1.SetComps(f);
+        
+        // 観測方程式のヤコビアンの値を設定（時間変化無し）
+        float h[18] = {
+            1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,  
+            0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
+            0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
+        };
+        
+        H1.SetComps(h);
+    }
 }
 
 void KalmanUpdate() {
-    // ヤコビアンの更新
-    float f[36] = {
-        1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
-        -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
-        raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
-        0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
-        0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
-        0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
-    };
-    
-    F.SetComps(f);
+    {
+        // ヤコビアンの更新
+        float f[36] = {
+            1.0f, (raw_gyro.GetComp(3) - post_x2.GetComp(6))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, post_x2.GetComp(3), -post_x2.GetComp(2), 
+            -(raw_gyro.GetComp(3) - post_x2.GetComp(6))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, -post_x2.GetComp(3), 0.0f, post_x2.GetComp(1), 
+            (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, post_x2.GetComp(2), -post_x2.GetComp(1), 0.0f, 
+            0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
+            0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
+            0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
+        };
+        
+        F2.SetComps(f);
+        
+        // 事前推定値の更新
+        pri_x2 = F2 * post_x2;
+        // 事前誤差分散行列の更新
+        pri_P2 = F2 * post_P2 * F2.Transpose() + R2;
+        
+        // カルマンゲインの計算
+        S2 = Q2 + H2 * pri_P2 * H2.Transpose();
+        float det;
+        if((det = S2.Inverse(S_inv2)) >= 0.0f) {
+            pc.printf("E:%.3f\r\n", det);
+            return;     // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
+        }
+        K2 = pri_P2 * H2.Transpose() * S_inv2;    
+        
+        // 事後推定値の更新
+        post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2);
+        // 事後誤差分散行列の更新
+        post_P2 = (I2 - K2 * H2) * pri_P2;
+    }
     
-    // 事前推定値の更新
-    pri_x = F * post_x;
-    // 事前誤差分散行列の更新
-    pri_P = F * post_P * F.Transpose() + R;
-    
-    // カルマンゲインの計算
-    S = Q + H * pri_P * H.Transpose();
-    float det;
-    if((det = S.Inverse(inv)) >= 0.0f) {
-        pc.printf("E:%.3f\r\n", det);
-        return;     // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
+    // 地磁気
+    {
+        // ヤコビアンの更新
+        float f[36] = {
+            1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
+            -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
+            raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
+            0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
+            0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
+            0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
+        };
+        
+        F1.SetComps(f);
+        
+        // 事前推定値の更新
+        pri_x1 = F1 * post_x1;
+        // 事前誤差分散行列の更新
+        pri_P1 = F1 * post_P1 * F1.Transpose() + R1;
+        
+        // カルマンゲインの計算
+        S1 = Q1 + H1 * pri_P1 * H1.Transpose();
+        float det;
+        if((det = S1.Inverse(S_inv1)) >= 0.0f) {
+            pc.printf("E:%.3f\r\n", det);
+            return;     // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
+        }
+        K1 = pri_P1 * H1.Transpose() * S_inv1;    
+        
+        // 事後推定値の更新
+        post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
+        // 事後誤差分散行列の更新
+        post_P1 = (I1 - K1 * H1) * pri_P1;
     }
-    K = pri_P * H.Transpose() * inv;    
-    
-    // 事後推定値の更新
-    post_x = pri_x + K * (raw_geomag - H * pri_x);
-    // 事後誤差分散行列の更新
-    post_P = (I - K * H) * pri_P;
 }
 
 float distance(Vector p1, Vector p2) {
@@ -386,7 +477,7 @@
     if(INT_flag != FALSE) {
         g = raw_g;
         for(int i=0; i<3; i++) {
-            geomag.SetComp(i+1, post_x.GetComp(i+1));
+            geomag.SetComp(i+1, post_x1.GetComp(i+1));
         }
         acc = raw_acc;
         gyro = raw_gyro;