Revision 12:0d978eb4d639, committed 2015-06-16

Comitter:

ojan

Date:

Tue Jun 16 17:04:58 2015 +0000

Parent:

11:083c8c9a5b84

Child:

13:df1e8a650185

Commit message:

add angle estimation with Kalman filter (unsuccessful)

Changed in this revision

--- a/main.cpp	Tue Jun 16 15:31:38 2015 +0000
+++ b/main.cpp	Tue Jun 16 17:04:58 2015 +0000
@@ -102,15 +102,15 @@
 
 // 重力ベクトル用
 // ジャイロのバイアスも同時に推定する
-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);
+Vector pri_x2(4);
+Matrix pri_P2(4, 4);
+Vector post_x2(4);
+Matrix post_P2(4, 4);
+Matrix F2(4, 4), H2(2, 4);
+Matrix R2(4, 4), Q2(2, 2);
+Matrix I2(4, 4);
+Matrix K2(4, 2);
+Matrix S2(2, 2), S_inv2(2, 2);
 /* ----- ------------- ----- */
 
 Timer timer;
@@ -208,11 +208,15 @@
             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\r\n", 
+                    gyro.GetComp(1), post_x2.GetComp(3), 
+                    gyro.GetComp(2), post_x2.GetComp(4));
+            */
             
-            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));
+            /*pc.printf("%.3f, %.3f, %.3f\r\n", 
+                    yaw, roll, pitch);
+            */
             
             if(UTC_t - pre_UTC_t >= 1) {                    // GPSデータが更新されていたら
                 p.SetComp(1, gms.longitude * DEG_TO_RAD);
@@ -307,28 +311,27 @@
     // 重力
     {
         // 誤差共分散行列の値を決める（対角成分のみ）
-        float alpha_R2 = 60.0f;
-        float alpha_Q2 = 100.0f;
+        float alpha_R2 = 0.001f;
+        float alpha_Q2 = 0.5f;
         R2 *= alpha_R2;
+        R2.SetComp(3, 3, 0.003f);
+        R2.SetComp(4, 4, 0.003f);
         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
+        // 状態方程式のヤコビアンの初期値を代入（時間変化無し）
+        float f[16] = {
+            1.0f, 0.0f, -dt, 0.0f, 
+            0.0f, 1.0f, 0.0f, -dt, 
+            0.0f, 0.0f, 1.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
+        float h[8] = {
+            1.0f, 0.0f, 0.0f, 0.0f,  
+            0.0f, 1.0f, 0.0f, 0.0f
         };
         
         H2.SetComps(h);
@@ -367,17 +370,12 @@
 
 void KalmanUpdate() {
     {
-        // ヤコビアンの更新
-        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);
+        // 入力ベクトル（角速度）
+        Vector u(4);
+        u.SetComp(1, raw_gyro.GetComp(1) * dt);
+        u.SetComp(2, raw_gyro.GetComp(2) * dt);
+        u.SetComp(3, 0.0f);
+        u.SetComp(4, 0.0f);
         
         // 事前推定値の更新
         pri_x2 = F2 * post_x2;
@@ -393,8 +391,13 @@
         }
         K2 = pri_P2 * H2.Transpose() * S_inv2;    
         
+        // 観測ベクトル（重力加速度ベクトルから算出した角度）
+        Vector alpha(2);
+        alpha.SetComp(1, -atan2(raw_acc.GetComp(2), raw_acc.GetComp(3)));
+        alpha.SetComp(2, -atan2(raw_acc.GetComp(1), raw_acc.GetComp(3)));
+        
         // 事後推定値の更新
-        post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2);
+        post_x2 = pri_x2 + K2 * (alpha - H2 * pri_x2);
         // 事後誤差分散行列の更新
         post_P2 = (I2 - K2 * H2) * pri_P2;
     }