Revision 13:df1e8a650185, committed 2015-06-19

Comitter:

ojan

Date:

Fri Jun 19 01:08:35 2015 +0000

Parent:

12:0d978eb4d639

Child:

14:f85cb5340cb8

Commit message:

add scripts to avoid Spiral.; modify Kalman filter model.

Changed in this revision

--- a/Vector/Vector.cpp	Tue Jun 16 17:04:58 2015 +0000
+++ b/Vector/Vector.cpp	Fri Jun 19 01:08:35 2015 +0000
@@ -85,6 +85,10 @@
     components[dimNo-1] = val;
 }
 
+void Vector::SetComps(float* pComps) {
+    memcpy(components, pComps, sizeof(float) * dim);
+}
+
 float Vector::GetNorm() const {
     float norm = 0.0f;
     for (int i = 0; i < dim; i++) {

--- a/Vector/Vector.h	Tue Jun 16 17:04:58 2015 +0000
+++ b/Vector/Vector.h	Fri Jun 19 01:08:35 2015 +0000
@@ -18,6 +18,7 @@
     Vector& operator-=(const Vector& v);
 
     void SetComp(int dimNo, float val);
+    void SetComps(float* vals);
     float GetNorm() const;
     Vector Normalize() const;
     Vector GetParaCompTo(Vector v);

--- a/main.cpp	Tue Jun 16 17:04:58 2015 +0000
+++ b/main.cpp	Fri Jun 19 01:08:35 2015 +0000
@@ -16,11 +16,7 @@
 #define TRUE    1
 #define FALSE   0
 
-#define x       1
-#define y       2
-#define z       3
-
-const float dt = 0.1f;              // 割り込み周期(s)
+const float dt = 0.01f;              // 割り込み周期(s)
 const float ServoMax = 0.0023f;     // サーボの最大パルス長(s)
 const float ServoMin = 0.0006f;     // サーボの最小パルス長(s)
 const float PullMax  = 25.0f;       // 引っ張れる紐の最大量(mm)
@@ -76,9 +72,11 @@
 int pull_L = 0;                 // 左サーボの引っ張り量(mm：0~PullMax)
 int pull_R = 0;                 // 右サーボの引っ張り量(mm：0~PullMax)
 
-float yaw = 0.0f;               // 本体のヨー角（deg）
-float pitch = 0.0f;             // 本体のピッチ角（deg）
-float roll = 0.0f;              // 本体のロール角（deg）
+float yaw = 0.0f;               // 本体のヨー角（deg）z軸周り
+float pitch = 0.0f;             // 本体のピッチ角（deg）y軸周り
+float roll = 0.0f;              // 本体のロール角（deg）x軸周り
+
+float vrt_acc = 0.0f;           // 鉛直方向の加速度成分（落下検知に使用）
 
 /** config.txt **
 * #から始めるのはコメント行 
@@ -90,27 +88,27 @@
 
 /* ----- Kalman Filter ----- */
 // 地磁気ベクトル用
-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);
+Vector pri_x1(7);
+Matrix pri_P1(7, 7);
+Vector post_x1(7);
+Matrix post_P1(7, 7);
+Matrix F1(7, 7), H1(3, 7);
+Matrix R1(7, 7), Q1(3, 3);
+Matrix I1(7, 7);
+Matrix K1(7, 3);
 Matrix S1(3, 3), S_inv1(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);
+Vector pri_x2(5);
+Matrix pri_P2(5, 5);
+Vector post_x2(5);
+Matrix post_P2(5, 5);
+Matrix F2(5, 5), H2(3, 5);
+Matrix R2(5, 5), Q2(3, 3);
+Matrix I2(5, 5);
+Matrix K2(5, 3);
+Matrix S2(3, 3), S_inv2(3, 3);
 /* ----- ------------- ----- */
 
 Timer timer;
@@ -166,12 +164,12 @@
     raw_g.SetComp(3, 1.0f);
     
     // 機体に固定されたベクトルの初期化
-    b_f.SetComp(1, 1.0f);
+    b_f.SetComp(1, 0.0f);
     b_f.SetComp(2, 0.0f);
-    b_f.SetComp(3, 0.0f);
-    b_u.SetComp(1, 0.0f);
+    b_f.SetComp(3, -1.0f);
+    b_u.SetComp(1, 1.0f);
     b_u.SetComp(2, 0.0f);
-    b_u.SetComp(3, 1.0f);
+    b_u.SetComp(3, 0.0f);
     b_l = Cross(b_u, b_f);
     
     /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
@@ -182,10 +180,10 @@
         // 0.1秒おきにセンサーの出力をpcへ出力
         myled = 1; // LED is ON
         
-        INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
         
         // データ処理
         {
+            INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
             gms.read();                                     // GPSデータ取得
             UTC_t = (int)gms.time;
             
@@ -205,18 +203,16 @@
             float R_23 = r_l * b_u;             // 回転行列の(2,3)成分
             float R_33 = r_u * b_u;             // 回転行列の(3,3)成分
             
-            yaw = atan2(-R_12, R_11) * RAD_TO_DEG;
+            yaw = atan2(-R_12, R_11) * RAD_TO_DEG - MAG_DECLINATION;
             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\r\n", 
-                    yaw, roll, pitch);
-            */
+            pc.printf("%.3f, %.3f, %.3f\r\n", 
+                    //geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3));
+                    //yaw, pitch, roll);
+                    post_x2.GetComp(4), 
+                    post_x2.GetComp(5), 
+                    post_x1.GetComp(7));
             
             if(UTC_t - pre_UTC_t >= 1) {                    // GPSデータが更新されていたら
                 p.SetComp(1, gms.longitude * DEG_TO_RAD);
@@ -227,28 +223,53 @@
             
             pre_p = p;
             pre_UTC_t = UTC_t;
+        
+            float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
+            float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
+            
+            // データをmicroSDに保存し、XBeeでPCへ送信する
+            sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n", 
+                    g.GetComp(1), g.GetComp(2), g.GetComp(3), 
+                    geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), 
+                    press, gms.longitude, gms.latitude, 
+                    sv, lv, optSensor.read_u16());
+            //fprintf(fp, data);
+            //fflush(fp);
+            //xbee.printf(data);
+            
+            INT_flag = TRUE;            // 割り込み許可
         }
         
-        float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
-        float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
-        
-        sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n", 
-                g.GetComp(1), g.GetComp(2), g.GetComp(3), 
-                geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), 
-                press, gms.longitude, gms.latitude, 
-                sv, lv, optSensor.read_u16());
-        //fprintf(fp, data);
-        //fflush(fp);
-        //xbee.printf(data);
-        
-        INT_flag = TRUE;            // 割り込み許可
         
         // 制御ルーチン
         {
-            pull_L = (pull_L+5)%30;
-            pull_R = (pull_R+5)%30;
+            
+            if(fabs(roll) > 40.0f) {
+                // スパイラル回避行動
+                if(roll > 0) {
+                    pull_L = PullMax;
+                    pull_R = 0;
+                } else {
+                    pull_L = 0;
+                    pull_R = PullMax;
+                }
+            } else {
+                
+            }
+                
+            //pull_L = (pull_L+5)%PullMax;
+            //pull_R = (pull_R+5)%PullMax;
             //pull_L = 0;
             //pull_R = 30;
+            
+            
+            
+            // 指定された引っ張り量だけ紐を引っ張る
+            if(pull_L < 0) pull_L = 0;
+            else if(pull_L > PullMax) pull_L = PullMax;
+            if(pull_R < 0) pull_R = 0;
+            else if(pull_R > PullMax) pull_R = PullMax;
+            
             servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
             servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
             
@@ -311,119 +332,122 @@
     // 重力
     {
         // 誤差共分散行列の値を決める（対角成分のみ）
-        float alpha_R2 = 0.001f;
+        float alpha_R2 = 0.002f;
         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[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
+        // 観測方程式のヤコビアンの値を設定（時間変化無し）
+        float h2[15] = {
+            1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+            0.0f, 1.0f, 0.0f, 0.0f, 0.0f,  
+            0.0f, 0.0f, 1.0f, 0.0f, 0.0f
         };
         
-        F2.SetComps(f);
-        
-        // 観測方程式のヤコビアンの値を設定（時間変化無し）
-        float h[8] = {
-            1.0f, 0.0f, 0.0f, 0.0f,  
-            0.0f, 1.0f, 0.0f, 0.0f
-        };
-        
-        H2.SetComps(h);
+        H2.SetComps(h2);
     }
     
     // 地磁気
     {
         // 誤差共分散行列の値を決める（対角成分のみ）
-        float alpha_R1 = 500.0f;
-        float alpha_Q1 = 1000.0f;
+        float alpha_R1 = 0.002f;
+        float alpha_Q1 = 0.5f;
         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
+        // 観測方程式のヤコビアンの値を設定（時間変化無し）
+        float h1[21] = {
+            1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
+            0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+            0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.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);
+        H1.SetComps(h1);
     }
 }
 
 void KalmanUpdate() {
+    // 重力
+    
     {
-        // 入力ベクトル（角速度）
-        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);
+        // ヤコビアンの更新
+        float f2[25] = {
+            1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, post_x2.GetComp(3)*dt, 
+            -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, -post_x2.GetComp(3)*dt, 0.0f, 
+            (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, post_x2.GetComp(2)*dt, -post_x2.GetComp(1)*dt, 
+            0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 
+            0.0f, 0.0f, 0.0f, 0.0f, 1.0f
+        };
+        
+        F2.SetComps(f2);
         
         // 事前推定値の更新
-        pri_x2 = F2 * post_x2;
+        //pri_x2 = F2 * post_x2;
+        
+        float pri_x2_vals[5] = {
+            post_x2.GetComp(1) + post_x2.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x2.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt, 
+            post_x2.GetComp(2) + post_x2.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x2.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt, 
+            post_x2.GetComp(3) + post_x2.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x2.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt, 
+            post_x2.GetComp(4), 
+            post_x2.GetComp(5)
+        };
+        
+        pri_x2.SetComps(pri_x2_vals);
+        
         // 事前誤差分散行列の更新
         pri_P2 = F2 * post_P2 * F2.Transpose() + R2;
         
         // カルマンゲインの計算
         S2 = Q2 + H2 * pri_P2 * H2.Transpose();
-        float det;
+        static 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;    
         
-        // 観測ベクトル（重力加速度ベクトルから算出した角度）
-        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 * (alpha - H2 * pri_x2);
+        post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2);
         // 事後誤差分散行列の更新
         post_P2 = (I2 - K2 * H2) * pri_P2;
     }
     
+    
     // 地磁気
     {
         // ヤコビアンの更新
-        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
+        float f1[49] = {
+            1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, 0.0f, 0.0f, -post_x1.GetComp(2) * dt, 
+            -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 0.0f, 0.0f, 0.0f, post_x1.GetComp(1) * dt, 
+            (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, 0.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, 0.0f, 1.0f, 0.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, 0.0f, 1.0f
         };
         
-        F1.SetComps(f);
+        F1.SetComps(f1);
         
         // 事前推定値の更新
-        pri_x1 = F1 * post_x1;
+        //pri_x1 = F1 * post_x1;
+        float pri_x1_vals[7] = {
+            post_x1.GetComp(1) + post_x1.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x1.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt, 
+            post_x1.GetComp(2) + post_x1.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x1.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt, 
+            post_x1.GetComp(3) + post_x1.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x1.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt, 
+            post_x1.GetComp(4), 
+            post_x1.GetComp(5), 
+            post_x1.GetComp(6), 
+            post_x1.GetComp(7)
+        };
+        
+        pri_x1.SetComps(pri_x1_vals);
+        
         // 事前誤差分散行列の更新
         pri_P1 = F1 * post_P1 * F1.Transpose() + R1;
         
         // カルマンゲインの計算
         S1 = Q1 + H1 * pri_P1 * H1.Transpose();
-        float det;
+        static float det;
         if((det = S1.Inverse(S_inv1)) >= 0.0f) {
             pc.printf("E:%.3f\r\n", det);
             return;     // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
@@ -432,6 +456,19 @@
         
         // 事後推定値の更新
         post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
+        // 地磁気ベクトルの大きさに拘束条件を与える
+        /*{
+            Vector gm(3);
+            gm.SetComp(1, post_x1.GetComp(1));
+            gm.SetComp(2, post_x1.GetComp(2));
+            gm.SetComp(3, post_x1.GetComp(3));
+            
+            gm = MAG_MAGNITUDE * gm.Normalize();
+            
+            post_x1.SetComp(1, gm.GetComp(1));
+            post_x1.SetComp(2, gm.GetComp(2));
+            post_x1.SetComp(3, gm.GetComp(3));
+        }*/
         // 事後誤差分散行列の更新
         post_P1 = (I1 - K1 * H1) * pri_P1;
     }
@@ -440,13 +477,13 @@
 float distance(Vector p1, Vector p2) {
     if(p1.GetDim() != p2.GetDim()) return 0.0f;
     
-    float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
-    float s_mu_y = sin(mu_y);
-    float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
-    float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
-    float n = GPS_A / w;
-    float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
-    float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
+    static float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
+    static float s_mu_y = sin(mu_y);
+    static float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
+    static float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
+    static float n = GPS_A / w;
+    static float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
+    static float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
     
     return sqrt(d1 * d1 + d2 * d2);
 }
@@ -486,6 +523,8 @@
         gyro = raw_gyro;
         press = raw_press;
         
+        vrt_acc = raw_acc * raw_g;
+        
     }
 }
 

--- a/myConstants.h	Tue Jun 16 17:04:58 2015 +0000
+++ b/myConstants.h	Fri Jun 19 01:08:35 2015 +0000
@@ -27,6 +27,7 @@
 #define MAG_LSB_TO_GAUSS    0.00092f                // Gauss/LSB
 #define MAG_MAGNITUDE       0.46f                   // Magnitude of GeoMagnetism (Gauss)
 #define MAG_SIN             -0.754709580f           // Sin-Value of Inclination
+#define MAG_DECLINATION     7.5f                    // declination (deg)
 
 /* ADC */
 #define ADC_LSB_TO_V        0.000050354f            // 3.3(V)/65535(LSB)
\ No newline at end of file