Diff: main.cpp

Revision:

3:5358a691a100

Parent:

2:d2b60a1d0cd9

Child:

4:45dc5590abc0

--- a/main.cpp	Sun May 24 17:44:11 2015 +0000
+++ b/main.cpp	Sat May 30 18:08:34 2015 +0000
@@ -5,7 +5,10 @@
 #include "GMS6_CR6.h"
 #include "ErrorLogger.h"
 #include "Vector.h"
+#include "Matrix.h"
+#include "Vector_Matrix_operator.h"
 #include "myConstants.h"
+#include "Log.h"
 
 /********** private define    **********/
 #define TRUE    1
@@ -15,30 +18,50 @@
 /********** private typedef   **********/
 
 /********** private variables **********/
-I2C         i2c(I2C_SDA, I2C_SCL);      // I2Cポート
+I2C         i2c(PB_9, PB_8);      // I2Cポート
 MPU6050     mpu(&i2c);                  // 加速度・角速度センサ
 HMC5883L    hmc(&i2c);                  // 地磁気センサ
 LPS25H      lps(&i2c);                  // 気圧センサ
-Serial      gps(D8, D2);             // GPS通信用シリアルポート
+Serial      gps(PA_11, PA_12);                // GPS通信用シリアルポート
 Serial      pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
 GMS6_CR6    gms(&gps, &pc);             // GPS
-Ticker      timer;                      // 割り込みタイマー
+Ticker      INT_timer;                      // 割り込みタイマー
+//Log         logger(PA_9, PA_10, PB_5, PB_4, PB_3, PB_10, "sd");    // ロガー（microSD、XBee）
 
-const float Freq = 0.01f;           // 割り込み周期（s）
+const float dt = 0.1f;           // 割り込み周期（s）
 
 int lps_cnt = 0;                    // 気圧センサ読み取りカウント
-uint8_t INT_flag = FALSE;            // 割り込み可否フラグ
+uint8_t INT_flag = TRUE;            // 割り込み可否フラグ
 Vector acc(3);                       // 加速度(m/s^2)
 Vector gyro(3);                      // 角速度(deg/s)
 Vector geomag(3);                    // 地磁気(?)
 float press;                        // 気圧(hPa)
 
 Vector g(3);                        // 重力ベクトル
-Vector n(3);                        // 地磁気ベクトル
-Vector bias(3);                     // 地磁気センサのバイアスベクトル
+//Vector n(3);                        // 地磁気ベクトル
+
+/* ----- 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);
+/* ----- ------------- ----- */
+
+Timer timer;
+
+char data[1024] = {};
 
 /********** private functions **********/
+void KalmanInit();
+void KalmanUpdate();
 void INT_func();                    // 割り込み用関数
+void toString(Matrix& m);
+void toString(Vector& v);
 
 /********** main function     **********/
 
@@ -49,7 +72,12 @@
     if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
     if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
     
-    timer.attach(&INT_func, Freq);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
+    //char* title = "log data\r\n";                                       // ログのタイトル行
+    //if(!logger.initialize_sdlog(title)) return 0;                       // ログファイル初期設定
+    
+    KalmanInit();
+    
+    INT_timer.attach(&INT_func, dt);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
     
     //重力ベクトルの初期化
     g.SetComp(1, 0.0f);
@@ -59,23 +87,40 @@
     /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
     while(1) {
         
-        // 1秒おきにセンサーの出力をpcへ出力
-        wait(1.0f);
+        // 0.1秒おきにセンサーの出力をpcへ出力
+        wait(0.1f);
         
         INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
         
         // センサ類の全出力値をPCに送信
-        pc.printf("%f,", acc.GetComp(1));
-        pc.printf("%f,", acc.GetComp(2));
-        pc.printf("%f,", acc.GetComp(3));
-        pc.printf("%f,", gyro.GetComp(1));
-        pc.printf("%f,", gyro.GetComp(2));
-        pc.printf("%f,", gyro.GetComp(3));
-        pc.printf("%f,", geomag.GetComp(1));
-        pc.printf("%f,", geomag.GetComp(2));
-        pc.printf("%f,", geomag.GetComp(3));
-        pc.printf("%f\r\n", press);
+        /*
+        pc.printf("%.3f,", acc.GetComp(1));
+        pc.printf("%.3f,", acc.GetComp(2));
+        pc.printf("%.3f,", acc.GetComp(3));
+        pc.printf("%.3f,", gyro.GetComp(1));
+        pc.printf("%.3f,", gyro.GetComp(2));
+        pc.printf("%.3f,", gyro.GetComp(3));
+        pc.printf("%.3f,", geomag.GetComp(1));
+        pc.printf("%.3f,", geomag.GetComp(2));
+        pc.printf("%.3f\r\n", geomag.GetComp(3));
+        */
+        //pc.printf("%.3f\r\n", press);
+        
         
+        pc.printf("%.3f,", geomag.GetComp(1));
+        pc.printf("%.3f,", geomag.GetComp(2));
+        pc.printf("%.3f,", geomag.GetComp(3));
+        pc.printf("%.3f,", post_x.GetComp(1));
+        pc.printf("%.3f,", post_x.GetComp(2));
+        pc.printf("%.3f\r\n", post_x.GetComp(3));
+        
+        /*
+        sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", 
+                acc.GetComp(1), acc.GetComp(2), acc.GetComp(3), 
+                gyro.GetComp(1), gyro.GetComp(2), gyro.GetComp(3), 
+                geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), press);
+        logger.puts(data);
+        */
         INT_flag = TRUE;            // 割り込み許可
         
     }
@@ -83,9 +128,74 @@
     /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
 }
 
+void KalmanInit() {
+    
+    // 誤差共分散行列の値を決める（対角成分のみ）
+    float alpha_R = 60.0f;
+    float alpha_Q = 100.0f;
+    R *= alpha_R;
+    Q *= alpha_Q;
+    
+    // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
+    float f[36] = {
+        1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
+        -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
+        gyro.GetComp(2)*dt, -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);
+}
+
+void KalmanUpdate() {
+    // ヤコビアンの更新
+    float f[36] = {
+        1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
+        -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
+        gyro.GetComp(2)*dt, -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);
+    
+    // 事前推定値の更新
+    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;     // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
+    }
+    K = pri_P * H.Transpose() * inv;    
+    
+    // 事後推定値の更新
+    post_x = pri_x + K * (geomag - H * pri_x);
+    // 事後誤差分散行列の更新
+    post_P = (I - K * H) * pri_P;
+}
+
 void INT_func() {
-    if(INT_flag == FALSE) {
+    if(INT_flag != FALSE) {
         
+        timer.reset();
+        timer.start();
         // センサーの値を更新
         mpu.read();
         hmc.read();
@@ -97,17 +207,40 @@
         }
         
         Vector delta_g = Cross(gyro, g);                            // Δg = ω × g
-        g = 0.9f * (g - delta_g * Freq) + 0.1f * acc.Normalize();   // 相補フィルタ
+        g = 0.9f * (g - delta_g * dt) + 0.1f * acc.Normalize();   // 相補フィルタ
         g = g.Normalize();
         
-        Vector delta_n = Cross(gyro,n);
-        n = 0.9f * (n - delta_n * Freq) + 0.1f * geomag.Normalize();
-        n = n.Normalize();
+        
+        KalmanUpdate();
+        
         
         // LPS25Hによる気圧の取得は10Hz
         lps_cnt = (lps_cnt+1)%10;
         if(lps_cnt == 0) {
             press = (float)lps.pressure() * PRES_LSB_TO_HPA;
         }
+        
+        timer.stop();
+        //pc.printf("%d(us)\r\n", timer.read_us());
     }
+}
+
+void toString(Matrix& m) {
+    
+    for(int i=0; i<m.GetRow(); i++) {
+        for(int j=0; j<m.GetCol(); j++) {
+            pc.printf("%.6f\t", m.GetComp(i+1, j+1));
+        }
+        pc.printf("\r\n");
+    }
+    
+}
+
+void toString(Vector& v) {
+    
+    for(int i=0; i<v.GetDim(); i++) {
+        pc.printf("%.6f\t", v.GetComp(i+1));
+    }
+    pc.printf("\r\n");
+    
 }
\ No newline at end of file