Revision 10:8ee11e412ad7, committed 2015-06-15

Comitter:

ojan

Date:

Mon Jun 15 13:29:36 2015 +0000

Parent:

7:0ec343d29641

Parent:

9:6d4578dcc1ed

Child:

11:083c8c9a5b84

Commit message:

merge iori's program and onaka's program.; add config file loader and calculation of Euler angles.

Changed in this revision

--- a/Vector/Vector.cpp	Mon Jun 15 00:50:28 2015 +0000
+++ b/Vector/Vector.cpp	Mon Jun 15 13:29:36 2015 +0000
@@ -31,6 +31,16 @@
     return *this;
 }
 
+Vector Vector::operator+() {
+    return *this;
+}
+
+Vector Vector::operator-() {
+    Vector retVec(*this);
+    retVec *= -1;
+    return retVec;
+}
+
 Vector& Vector::operator*=(float c) {
     for (int i = 0; i < dim; i++) {
         components[i] *= c;
@@ -93,6 +103,15 @@
     return temp;
 }
 
+Vector Vector::GetParaCompTo(Vector v) {
+    Vector norm_v = v.Normalize();
+    return (*this * norm_v) * norm_v;
+}
+
+Vector Vector::GetPerpCompTo(Vector v) {
+    return (*this - this->GetParaCompTo(v));
+}
+
 void Vector::CleanUp() {
     float maxComp = 0.0f;
     for (int i = 0; i < dim; i++) {

--- a/Vector/Vector.h	Mon Jun 15 00:50:28 2015 +0000
+++ b/Vector/Vector.h	Mon Jun 15 13:29:36 2015 +0000
@@ -10,6 +10,8 @@
     Vector(const Vector& v);
 
     Vector& operator=(const Vector& v);
+    Vector operator+();
+    Vector operator-();
     Vector& operator*=(float c);
     Vector& operator/=(float c);
     Vector& operator+=(const Vector& v);
@@ -18,6 +20,8 @@
     void SetComp(int dimNo, float val);
     float GetNorm() const;
     Vector Normalize() const;
+    Vector GetParaCompTo(Vector v);
+    Vector GetPerpCompTo(Vector v);
 
     inline int GetDim() const {
         return dim;

--- a/main.cpp	Mon Jun 15 00:50:28 2015 +0000
+++ b/main.cpp	Mon Jun 15 13:29:36 2015 +0000
@@ -15,45 +15,70 @@
 /********** private define    **********/
 #define TRUE    1
 #define FALSE   0
+
+#define x       1
+#define y       2
+#define z       3
+
+const float dt = 0.1f;              // 割り込み周期(s)
+const float ServoMax = 0.0023f;     // サーボの最大パルス長(s)
+const float ServoMin = 0.0006f;     // サーボの最小パルス長(s)
+const float PullMax  = 25.0f;       // 引っ張れる紐の最大量(mm)
 /********** private macro     **********/
 
 /********** private typedef   **********/
 
 /********** private variables **********/
-DigitalOut      myled(LED1);                 // デバッグ用LEDのためのデジタル出力
-I2C             i2c(PB_9, PB_8);            // I2Cポート
-MPU6050         mpu(&i2c);                  // 加速度・角速度センサ
-HMC5883L        hmc(&i2c);                  // 地磁気センサ
-LPS25H          lps(&i2c);                  // 気圧センサ
-Serial          gps(PA_11, PA_12);                // GPS通信用シリアルポート
-Serial          pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
-GMS6_CR6        gms(&gps, &pc);             // GPS
-Ticker          INT_timer;                      // 割り込みタイマー
+DigitalOut      myled(LED1);                        // デバッグ用LEDのためのデジタル出力
+I2C             i2c(PB_9, PB_8);                    // I2Cポート
+MPU6050         mpu(&i2c);                          // 加速度・角速度センサ
+HMC5883L        hmc(&i2c);                          // 地磁気センサ
+LPS25H          lps(&i2c);                          // 気圧センサ
+Serial          gps(PA_11, PA_12);                  // GPS通信用シリアルポート
+Serial          pc(SERIAL_TX, SERIAL_RX);           // PC通信用シリアルポート
+GMS6_CR6        gms(&gps, &pc);                     // GPS
 SDFileSystem    sd(PB_5, PB_4, PB_3, PB_10, "sd");  // microSD
 BufferedSerial  xbee(PA_9, PA_10, PC_1);            // Xbee
 ConfigFile      cfg;                                //ConfigFile
-PwmOut          servoL(PC_7), servoR(PB_6);
-AnalogIn        rf(PC_0);
-AnalogIn        servoVcc(PA_0);
-AnalogIn        logicVcc(PA_1);
+PwmOut          servoL(PB_6), servoR(PC_7);         // サーボ用PWM出力
+AnalogIn        optSensor(PC_0);                           // 照度センサ用アナログ入力
+AnalogIn        servoVcc(PA_0);                     // バッテリー電圧監視用アナログ入力(サーボ用)
+AnalogIn        logicVcc(PA_1);                     // バッテリー電圧監視用アナログ入力(ロジック用)
+Ticker          INT_timer;                          // 割り込みタイマー
 
-
-const float dt = 0.1f;           // 割り込み周期（s）
+int lps_cnt = 0;                // 気圧センサ読み取りカウント
+uint8_t INT_flag = TRUE;        // 割り込み可否フラグ
+Vector raw_acc(3);              // 加速度(m/s^2)  生
+Vector raw_gyro(3);             // 角速度(deg/s)  生
+Vector raw_geomag(3);           // 地磁気(?)  生
+float raw_press;                // 気圧(hPa)  生
+Vector acc(3);                  // 加速度(m/s^2)
+Vector gyro(3);                 // 角速度(deg/s)
+Vector geomag(3);               // 地磁気(?)
+float press;                    // 気圧(hPa)
 
-int lps_cnt = 0;                    // 気圧センサ読み取りカウント
-uint8_t INT_flag = TRUE;            // 割り込み可否フラグ
-Vector raw_acc(3);                       // 加速度(m/s^2)  生
-Vector raw_gyro(3);                      // 角速度(deg/s)  生
-Vector raw_geomag(3);                    // 地磁気(?)  生
-float raw_press;                        // 気圧(hPa)  生
-Vector acc(3);                       // 加速度(m/s^2)
-Vector gyro(3);                      // 角速度(deg/s)
-Vector geomag(3);                    // 地磁気(?)
-float press;                        // 気圧(hPa)
+Vector raw_g(3);                // 重力ベクトル  生
+Vector g(3);                    // 重力ベクトル
+Vector p(2);                    // 位置情報(経度, 緯度)
+Vector pre_p(2);                // 過去の位置情報(経度, 緯度)
+int UTC_t = 0;                  // UTC時刻
+int pre_UTC_t = 0;              // 前のUTC時刻
+//Vector n(3);                  // 地磁気ベクトル
 
-Vector raw_g(3);                        // 重力ベクトル  生
-Vector g(3);                        // 重力ベクトル  生
-//Vector n(3);                        // 地磁気ベクトル
+Vector b_f(3);                  // 機体座標に固定された、機体前方向きのベクトル（x軸）
+Vector b_u(3);                  // 機体座標に固定された、機体上方向きのベクトル（z軸）
+Vector b_l(3);                  // 機体座標に固定された、機体左方向きのベクトル（y軸）
+
+Vector r_f(3);                  // 参照座標に固定された、北向きのベクトル（X軸）
+Vector r_u(3);                  // 参照座標に固定された、上向きのベクトル（Z軸）
+Vector r_l(3);                  // 参照座標に固定された、西向きのベクトル（Y軸）
+
+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）
 
 /** config.txt **
 * #から始めるのはコメント行 
@@ -80,11 +105,12 @@
 char data[512] = {};
 
 /********** private functions **********/
-void LoadConfig();                  // config読み取り
-int find_last();                    // SDカード初期化用関数
-void KalmanInit();                  // カルマンフィルタ初期化
-void KalmanUpdate();                // カルマンフィルタ更新
-void INT_func();                    // 割り込み用関数
+void LoadConfig();                      // config読み取り
+int find_last();                        // SDカード初期化用関数
+void KalmanInit();                      // カルマンフィルタ初期化
+void KalmanUpdate();                    // カルマンフィルタ更新
+float distance(Vector p1, Vector p2);   // 緯度・経度の差から2点間の距離を算出(m)
+void INT_func();                        // 割り込み用関数
 void toString(Matrix& m);
 void toString(Vector& v);
 
@@ -113,6 +139,9 @@
     fprintf(fp, "log data\r\n");
     xbee.printf("log data\r\n");
     
+    servoL.period(0.020f);                // サーボの信号の周期は20ms
+    servoR.period(0.020f);
+    
     //カルマンフィルタ初期化
     KalmanInit();
     
@@ -123,6 +152,15 @@
     raw_g.SetComp(2, 0.0f);
     raw_g.SetComp(3, 1.0f);
     
+    // 機体に固定されたベクトルの初期化
+    b_f.SetComp(1, 0.0f);
+    b_f.SetComp(2, 0.0f);
+    b_f.SetComp(3, -1.0f);
+    b_u.SetComp(1, 1.0f);
+    b_u.SetComp(2, 0.0f);
+    b_u.SetComp(3, 0.0f);
+    b_l = Cross(b_u, b_f);
+    
     /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
     while(1) {
         timer.stop();
@@ -130,11 +168,44 @@
         timer.start();
         // 0.1秒おきにセンサーの出力をpcへ出力
         myled = 1; // LED is ON
-        wait(0.05f); // 50 ms
-        myled = 0; // LED is OFF
         
         INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
         
+        // データ処理
+        {
+            gms.read();                                     // GPSデータ取得
+            UTC_t = (int)gms.time;
+            
+            // 参照座標系の基底を求める
+            r_u = g;
+            r_f = geomag.GetPerpCompTo(g).Normalize();
+            r_l = Cross(r_u, r_f);
+            
+            // 回転行列を経由してオイラー角を求める
+            // オイラー角はヨー・ピッチ・ロールの順に回転したものとする
+            // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
+            
+            float R_11 = r_f * b_f;             // 回転行列の(1,1)成分
+            float R_12 = r_f * b_l;             // 回転行列の(1,2)成分
+            float R_13 = r_f * b_u;             // 回転行列の(1,3)成分
+            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;
+            
+            if(UTC_t - pre_UTC_t >= 1) {                    // GPSデータが更新されていたら
+                p.SetComp(1, gms.longitude * DEG_TO_RAD);
+                p.SetComp(2, gms.latitude * DEG_TO_RAD);  
+            } else {                                        // 更新されていなかったら
+                
+            }
+            
+            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;
         
@@ -142,21 +213,29 @@
                 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, gms.Ns);
+                sv, lv, optSensor.read_u16());
         fprintf(fp, data);
         xbee.printf(data);
         
         INT_flag = TRUE;            // 割り込み許可
         
-        pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
-        
         // 制御ルーチン
         {
+            //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
         
-        // ループはきっかり1秒ごと
-        while(timer.read_ms() < 1000);
+        // ループはきっかり0.2秒ごと
+        while(timer.read_ms() < 200);
+        
+        pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
         
     }
     
@@ -264,6 +343,22 @@
     post_P = (I - K * H) * pri_P;
 }
 
+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));
+    
+    return sqrt(d1 * d1 + d2 * d2);
+}
+
+/* -------------------- 割り込み関数 -------------------- */
+
 void INT_func() {
     // センサーの値を更新
     mpu.read();
@@ -300,6 +395,8 @@
     }
 }
 
+/* -------------------- デバッグ用関数 -------------------- */
+
 void toString(Matrix& m) {
     
     for(int i=0; i<m.GetRow(); i++) {

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Mon Jun 15 13:29:36 2015 +0000
@@ -0,0 +1,321 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "HMC5883L.h"
+#include "LPS25H.h"
+#include "GMS6_CR6.h"
+#include "ErrorLogger.h"
+#include "Vector.h"
+#include "Matrix.h"
+#include "Vector_Matrix_operator.h"
+#include "myConstants.h"
+#include "SDFileSystem.h"
+#include "BufferedSerial.h"
+#include "ConfigFile.h"
+
+/********** private define    **********/
+#define TRUE    1
+#define FALSE   0
+/********** private macro     **********/
+
+/********** private typedef   **********/
+
+/********** private variables **********/
+DigitalOut      myled(LED1);                 // デバッグ用LEDのためのデジタル出力
+I2C             i2c(PB_9, PB_8);            // I2Cポート
+MPU6050         mpu(&i2c);                  // 加速度・角速度センサ
+HMC5883L        hmc(&i2c);                  // 地磁気センサ
+LPS25H          lps(&i2c);                  // 気圧センサ
+Serial          gps(PA_11, PA_12);                // GPS通信用シリアルポート
+Serial          pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
+GMS6_CR6        gms(&gps, &pc);             // GPS
+Ticker          INT_timer;                      // 割り込みタイマー
+SDFileSystem    sd(PB_5, PB_4, PB_3, PB_10, "sd");  // microSD
+BufferedSerial  xbee(PA_9, PA_10, PC_1);            // Xbee
+ConfigFile      cfg;                                //ConfigFile
+PwmOut          servoL(PC_7), servoR(PB_6);
+AnalogIn        rf(PC_0);
+AnalogIn        servoVcc(PA_0);
+AnalogIn        logicVcc(PA_1);
+
+
+const float dt = 0.1f;           // 割り込み周期（s）
+
+int lps_cnt = 0;                    // 気圧センサ読み取りカウント
+uint8_t INT_flag = TRUE;            // 割り込み可否フラグ
+Vector raw_acc(3);                       // 加速度(m/s^2)  生
+Vector raw_gyro(3);                      // 角速度(deg/s)  生
+Vector raw_geomag(3);                    // 地磁気(?)  生
+float raw_press;                        // 気圧(hPa)  生
+Vector acc(3);                       // 加速度(m/s^2)
+Vector gyro(3);                      // 角速度(deg/s)
+Vector geomag(3);                    // 地磁気(?)
+float press;                        // 気圧(hPa)
+
+Vector raw_g(3);                        // 重力ベクトル  生
+Vector g(3);                        // 重力ベクトル  生
+//Vector n(3);                        // 地磁気ベクトル
+
+/** config.txt **
+* #から始めるのはコメント行 
+* #イコールの前後に空白を入れない
+* target_x=111.222
+* target_y=33.444
+*/
+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);
+/* ----- ------------- ----- */
+
+Timer timer;
+
+char data[512] = {};
+
+/********** private functions **********/
+void LoadConfig();                  // config読み取り
+int find_last();                    // SDカード初期化用関数
+void KalmanInit();                  // カルマンフィルタ初期化
+void KalmanUpdate();                // カルマンフィルタ更新
+void INT_func();                    // 割り込み用関数
+void toString(Matrix& m);
+void toString(Vector& v);
+
+/********** main function     **********/
+
+int main() {
+    
+    i2c.frequency(400000);          // I2Cの通信速度を400kHzに設定
+    
+    if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
+    if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
+    
+    //Config読み取り
+    LoadConfig();
+    
+    //SDカード初期化
+    FILE *fp;
+    char filename[15];
+    int n = find_last();
+    if(n < 0){
+        pc.printf("Could not read a SD Card.\n");
+        return 0;
+    }
+    sprintf(filename, "/sd/log%03d.csv", n+1);
+    fp = fopen(filename, "w");
+    fprintf(fp, "log data\r\n");
+    xbee.printf("log data\r\n");
+    
+    //カルマンフィルタ初期化
+    KalmanInit();
+    
+    INT_timer.attach(&INT_func, dt);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
+    
+    //重力ベクトルの初期化
+    raw_g.SetComp(1, 0.0f);
+    raw_g.SetComp(2, 0.0f);
+    raw_g.SetComp(3, 1.0f);
+    
+    /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
+    while(1) {
+        timer.stop();
+        timer.reset();
+        timer.start();
+        // 0.1秒おきにセンサーの出力をpcへ出力
+        myled = 1; // LED is ON
+        wait(0.05f); // 50 ms
+        myled = 0; // LED is OFF
+        
+        INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
+        
+        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, gms.Ns);
+        fprintf(fp, data);
+        xbee.printf(data);
+        
+        INT_flag = TRUE;            // 割り込み許可
+        
+        pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
+        
+        // 制御ルーチン
+        {
+        }
+        
+        
+        // ループはきっかり1秒ごと
+        while(timer.read_ms() < 1000);
+        
+    }
+    
+    /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
+    //fclose(fp);
+}
+
+void LoadConfig(){
+    char value[20];
+    //Read a configuration file from a mbed.
+    if (!cfg.read("/sd/config.txt")){
+        pc.printf("Config file does not exist\n");
+    }else{    
+        //Get values
+        if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value);
+        else{
+            pc.printf("Failed to get value for target_x\n");
+        }
+        if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value);
+        else{
+            pc.printf("Failed to get value for target_y\n");
+        }
+    }
+}
+
+int find_last() {
+    int i, n = 0;
+    char c;
+    DIR *dp;
+    struct dirent *dirst;
+    dp = opendir("/sd/");
+    if (!dp){
+        pc.printf("Could not open directry.\n");
+        return -1;
+    }
+    while((dirst = readdir(dp)) != NULL) {
+        if(sscanf(dirst->d_name, "log%03d.csv%c", &i, &c) == 1 && i>n) {
+            n = i;
+        }
+    }
+    closedir(dp);
+    return n;
+}
+
+void KalmanInit() {
+    
+    // 誤差共分散行列の値を決める（対角成分のみ）
+    float alpha_R = 60.0f;
+    float alpha_Q = 100.0f;
+    R *= alpha_R;
+    Q *= alpha_Q;
+    
+    // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
+    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);
+}
+
+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);
+    
+    // 事前推定値の更新
+    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 * (raw_geomag - H * pri_x);
+    // 事後誤差分散行列の更新
+    post_P = (I - K * H) * pri_P;
+}
+
+void INT_func() {
+    // センサーの値を更新
+    mpu.read();
+    hmc.read();
+    
+    for(int i=0; i<3; i++) {
+        raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
+        raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
+        raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
+    }
+        
+    Vector delta_g = Cross(raw_gyro, raw_g);                            // Δg = ω × g
+    raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize();   // 相補フィルタ
+    raw_g = raw_g.Normalize();
+       
+    KalmanUpdate();
+        
+    // LPS25Hによる気圧の取得は10Hz
+    lps_cnt = (lps_cnt+1)%10;
+    if(lps_cnt == 0) {
+        raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
+    }
+    //pc.printf("%d(us)\r\n", timer.read_us());
+    
+    if(INT_flag != FALSE) {
+        g = raw_g;
+        for(int i=0; i<3; i++) {
+            geomag.SetComp(i+1, post_x.GetComp(i+1));
+        }
+        acc = raw_acc;
+        gyro = raw_gyro;
+        press = raw_press;
+        
+    }
+}
+
+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

--- a/myConstants.h	Mon Jun 15 00:50:28 2015 +0000
+++ b/myConstants.h	Mon Jun 15 13:29:36 2015 +0000
@@ -18,6 +18,11 @@
 /* Pressure Sensor */
 #define PRES_LSB_TO_HPA     0.000244140625f         // hPa/LSB (1/4096
 
+/* GPS */
+#define GPS_SQ_E                0.00669437999f      // （第一離心率）^2
+#define GPS_A                   6378137.0f          // 長半径(赤道半径)(m)
+#define GPS_B                   6356752.3f          // 短半径(極半径)(m)
+
 /* Geomagnetic Sensor */
 #define MAG_LSB_TO_GAUSS    0.00092f                // Gauss/LSB
 #define MAG_MAGNITUDE       0.46f                   // Magnitude of GeoMagnetism (Gauss)