hiroya taura / Mbed 2 deprecated LAURUS_program

MPU6050のサンプルプログラム２

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@3:5358a691a100, 2015-05-30 (annotated)

Committer:
ojan
Date:
Sat May 30 18:08:34 2015 +0000
Revision:
3:5358a691a100
Parent:
2:d2b60a1d0cd9
Child:
4:45dc5590abc0
estimate geomagnetism vector with Kalman Filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojan 0:bc6f14fc60c7 1 #include "mbed.h"
ojan 0:bc6f14fc60c7 2 #include "MPU6050.h"
ojan 0:bc6f14fc60c7 3 #include "HMC5883L.h"
ojan 0:bc6f14fc60c7 4 #include "LPS25H.h"
ojan 1:6cd6d2760856 5 #include "GMS6_CR6.h"
ojan 0:bc6f14fc60c7 6 #include "ErrorLogger.h"
ojan 0:bc6f14fc60c7 7 #include "Vector.h"
ojan 3:5358a691a100 8 #include "Matrix.h"
ojan 3:5358a691a100 9 #include "Vector_Matrix_operator.h"
ojan 0:bc6f14fc60c7 10 #include "myConstants.h"
ojan 3:5358a691a100 11 #include "Log.h"
ojan 0:bc6f14fc60c7 12
ojan 0:bc6f14fc60c7 13 /********** private define **********/
ojan 0:bc6f14fc60c7 14 #define TRUE 1
ojan 0:bc6f14fc60c7 15 #define FALSE 0
ojan 0:bc6f14fc60c7 16 /********** private macro **********/
ojan 1:6cd6d2760856 17
ojan 0:bc6f14fc60c7 18 /********** private typedef **********/
ojan 1:6cd6d2760856 19
ojan 0:bc6f14fc60c7 20 /********** private variables **********/
ojan 3:5358a691a100 21 I2C i2c(PB_9, PB_8); // I2Cポート
ojan 1:6cd6d2760856 22 MPU6050 mpu(&i2c); // 加速度・角速度センサ
ojan 1:6cd6d2760856 23 HMC5883L hmc(&i2c); // 地磁気センサ
ojan 1:6cd6d2760856 24 LPS25H lps(&i2c); // 気圧センサ
ojan 3:5358a691a100 25 Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
ojan 1:6cd6d2760856 26 Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
ojan 1:6cd6d2760856 27 GMS6_CR6 gms(&gps, &pc); // GPS
ojan 3:5358a691a100 28 Ticker INT_timer; // 割り込みタイマー
ojan 3:5358a691a100 29 //Log logger(PA_9, PA_10, PB_5, PB_4, PB_3, PB_10, "sd"); // ロガー（microSD、XBee）
ojan 0:bc6f14fc60c7 30
ojan 3:5358a691a100 31 const float dt = 0.1f; // 割り込み周期（s）
ojan 0:bc6f14fc60c7 32
ojan 0:bc6f14fc60c7 33 int lps_cnt = 0; // 気圧センサ読み取りカウント
ojan 3:5358a691a100 34 uint8_t INT_flag = TRUE; // 割り込み可否フラグ
ojan 1:6cd6d2760856 35 Vector acc(3); // 加速度(m/s^2)
ojan 1:6cd6d2760856 36 Vector gyro(3); // 角速度(deg/s)
ojan 1:6cd6d2760856 37 Vector geomag(3); // 地磁気(?)
ojan 0:bc6f14fc60c7 38 float press; // 気圧(hPa)
ojan 1:6cd6d2760856 39
ojan 1:6cd6d2760856 40 Vector g(3); // 重力ベクトル
ojan 3:5358a691a100 41 //Vector n(3); // 地磁気ベクトル
ojan 3:5358a691a100 42
ojan 3:5358a691a100 43 /* ----- Kalman Filter ----- */
ojan 3:5358a691a100 44 Vector pri_x(6);
ojan 3:5358a691a100 45 Matrix pri_P(6, 6);
ojan 3:5358a691a100 46 Vector post_x(6);
ojan 3:5358a691a100 47 Matrix post_P(6, 6);
ojan 3:5358a691a100 48 Matrix F(6, 6), H(3, 6);
ojan 3:5358a691a100 49 Matrix R(6, 6), Q(3, 3);
ojan 3:5358a691a100 50 Matrix I(6, 6);
ojan 3:5358a691a100 51 Matrix K(6, 3);
ojan 3:5358a691a100 52 Matrix S(3, 3), inv(3, 3);
ojan 3:5358a691a100 53 /* ----- ------------- ----- */
ojan 3:5358a691a100 54
ojan 3:5358a691a100 55 Timer timer;
ojan 3:5358a691a100 56
ojan 3:5358a691a100 57 char data[1024] = {};
ojan 1:6cd6d2760856 58
ojan 0:bc6f14fc60c7 59 /********** private functions **********/
ojan 3:5358a691a100 60 void KalmanInit();
ojan 3:5358a691a100 61 void KalmanUpdate();
ojan 0:bc6f14fc60c7 62 void INT_func(); // 割り込み用関数
ojan 3:5358a691a100 63 void toString(Matrix& m);
ojan 3:5358a691a100 64 void toString(Vector& v);
ojan 1:6cd6d2760856 65
ojan 0:bc6f14fc60c7 66 /********** main function **********/
ojan 0:bc6f14fc60c7 67
ojan 0:bc6f14fc60c7 68 int main() {
ojan 0:bc6f14fc60c7 69
ojan 0:bc6f14fc60c7 70 i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
ojan 0:bc6f14fc60c7 71
ojan 0:bc6f14fc60c7 72 if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化
ojan 0:bc6f14fc60c7 73 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化
ojan 0:bc6f14fc60c7 74
ojan 3:5358a691a100 75 //char* title = "log data\r\n"; // ログのタイトル行
ojan 3:5358a691a100 76 //if(!logger.initialize_sdlog(title)) return 0; // ログファイル初期設定
ojan 3:5358a691a100 77
ojan 3:5358a691a100 78 KalmanInit();
ojan 3:5358a691a100 79
ojan 3:5358a691a100 80 INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
ojan 1:6cd6d2760856 81
ojan 1:6cd6d2760856 82 //重力ベクトルの初期化
ojan 1:6cd6d2760856 83 g.SetComp(1, 0.0f);
ojan 1:6cd6d2760856 84 g.SetComp(2, 0.0f);
ojan 1:6cd6d2760856 85 g.SetComp(3, 1.0f);
ojan 0:bc6f14fc60c7 86
ojan 2:d2b60a1d0cd9 87 /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
ojan 0:bc6f14fc60c7 88 while(1) {
ojan 0:bc6f14fc60c7 89
ojan 3:5358a691a100 90 // 0.1秒おきにセンサーの出力をpcへ出力
ojan 3:5358a691a100 91 wait(0.1f);
ojan 1:6cd6d2760856 92
ojan 0:bc6f14fc60c7 93 INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
ojan 0:bc6f14fc60c7 94
ojan 0:bc6f14fc60c7 95 // センサ類の全出力値をPCに送信
ojan 3:5358a691a100 96 /*
ojan 3:5358a691a100 97 pc.printf("%.3f,", acc.GetComp(1));
ojan 3:5358a691a100 98 pc.printf("%.3f,", acc.GetComp(2));
ojan 3:5358a691a100 99 pc.printf("%.3f,", acc.GetComp(3));
ojan 3:5358a691a100 100 pc.printf("%.3f,", gyro.GetComp(1));
ojan 3:5358a691a100 101 pc.printf("%.3f,", gyro.GetComp(2));
ojan 3:5358a691a100 102 pc.printf("%.3f,", gyro.GetComp(3));
ojan 3:5358a691a100 103 pc.printf("%.3f,", geomag.GetComp(1));
ojan 3:5358a691a100 104 pc.printf("%.3f,", geomag.GetComp(2));
ojan 3:5358a691a100 105 pc.printf("%.3f\r\n", geomag.GetComp(3));
ojan 3:5358a691a100 106 */
ojan 3:5358a691a100 107 //pc.printf("%.3f\r\n", press);
ojan 3:5358a691a100 108
ojan 0:bc6f14fc60c7 109
ojan 3:5358a691a100 110 pc.printf("%.3f,", geomag.GetComp(1));
ojan 3:5358a691a100 111 pc.printf("%.3f,", geomag.GetComp(2));
ojan 3:5358a691a100 112 pc.printf("%.3f,", geomag.GetComp(3));
ojan 3:5358a691a100 113 pc.printf("%.3f,", post_x.GetComp(1));
ojan 3:5358a691a100 114 pc.printf("%.3f,", post_x.GetComp(2));
ojan 3:5358a691a100 115 pc.printf("%.3f\r\n", post_x.GetComp(3));
ojan 3:5358a691a100 116
ojan 3:5358a691a100 117 /*
ojan 3:5358a691a100 118 sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
ojan 3:5358a691a100 119 acc.GetComp(1), acc.GetComp(2), acc.GetComp(3),
ojan 3:5358a691a100 120 gyro.GetComp(1), gyro.GetComp(2), gyro.GetComp(3),
ojan 3:5358a691a100 121 geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), press);
ojan 3:5358a691a100 122 logger.puts(data);
ojan 3:5358a691a100 123 */
ojan 0:bc6f14fc60c7 124 INT_flag = TRUE; // 割り込み許可
ojan 0:bc6f14fc60c7 125
ojan 0:bc6f14fc60c7 126 }
ojan 2:d2b60a1d0cd9 127
ojan 2:d2b60a1d0cd9 128 /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
ojan 0:bc6f14fc60c7 129 }
ojan 0:bc6f14fc60c7 130
ojan 3:5358a691a100 131 void KalmanInit() {
ojan 3:5358a691a100 132
ojan 3:5358a691a100 133 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 3:5358a691a100 134 float alpha_R = 60.0f;
ojan 3:5358a691a100 135 float alpha_Q = 100.0f;
ojan 3:5358a691a100 136 R *= alpha_R;
ojan 3:5358a691a100 137 Q *= alpha_Q;
ojan 3:5358a691a100 138
ojan 3:5358a691a100 139 // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
ojan 3:5358a691a100 140 float f[36] = {
ojan 3:5358a691a100 141 1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 142 -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 143 gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 144 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 145 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 146 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 147 };
ojan 3:5358a691a100 148
ojan 3:5358a691a100 149 F.SetComps(f);
ojan 3:5358a691a100 150
ojan 3:5358a691a100 151 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 3:5358a691a100 152 float h[18] = {
ojan 3:5358a691a100 153 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 154 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 155 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 156 };
ojan 3:5358a691a100 157
ojan 3:5358a691a100 158 H.SetComps(h);
ojan 3:5358a691a100 159 }
ojan 3:5358a691a100 160
ojan 3:5358a691a100 161 void KalmanUpdate() {
ojan 3:5358a691a100 162 // ヤコビアンの更新
ojan 3:5358a691a100 163 float f[36] = {
ojan 3:5358a691a100 164 1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 165 -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 166 gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 167 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 168 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 169 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 170 };
ojan 3:5358a691a100 171
ojan 3:5358a691a100 172 F.SetComps(f);
ojan 3:5358a691a100 173
ojan 3:5358a691a100 174 // 事前推定値の更新
ojan 3:5358a691a100 175 pri_x = F * post_x;
ojan 3:5358a691a100 176 // 事前誤差分散行列の更新
ojan 3:5358a691a100 177 pri_P = F * post_P * F.Transpose() + R;
ojan 3:5358a691a100 178
ojan 3:5358a691a100 179 // カルマンゲインの計算
ojan 3:5358a691a100 180 S = Q + H * pri_P * H.Transpose();
ojan 3:5358a691a100 181 float det;
ojan 3:5358a691a100 182 if((det = S.Inverse(inv)) >= 0.0f) {
ojan 3:5358a691a100 183 pc.printf("E:%.3f\r\n", det);
ojan 3:5358a691a100 184 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 3:5358a691a100 185 }
ojan 3:5358a691a100 186 K = pri_P * H.Transpose() * inv;
ojan 3:5358a691a100 187
ojan 3:5358a691a100 188 // 事後推定値の更新
ojan 3:5358a691a100 189 post_x = pri_x + K * (geomag - H * pri_x);
ojan 3:5358a691a100 190 // 事後誤差分散行列の更新
ojan 3:5358a691a100 191 post_P = (I - K * H) * pri_P;
ojan 3:5358a691a100 192 }
ojan 3:5358a691a100 193
ojan 0:bc6f14fc60c7 194 void INT_func() {
ojan 3:5358a691a100 195 if(INT_flag != FALSE) {
ojan 0:bc6f14fc60c7 196
ojan 3:5358a691a100 197 timer.reset();
ojan 3:5358a691a100 198 timer.start();
ojan 1:6cd6d2760856 199 // センサーの値を更新
ojan 0:bc6f14fc60c7 200 mpu.read();
ojan 0:bc6f14fc60c7 201 hmc.read();
ojan 0:bc6f14fc60c7 202
ojan 0:bc6f14fc60c7 203 for(int i=0; i<3; i++) {
ojan 1:6cd6d2760856 204 acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
ojan 1:6cd6d2760856 205 gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 1:6cd6d2760856 206 geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
ojan 0:bc6f14fc60c7 207 }
ojan 0:bc6f14fc60c7 208
ojan 1:6cd6d2760856 209 Vector delta_g = Cross(gyro, g); // Δg = ω × g
ojan 3:5358a691a100 210 g = 0.9f * (g - delta_g * dt) + 0.1f * acc.Normalize(); // 相補フィルタ
ojan 1:6cd6d2760856 211 g = g.Normalize();
ojan 1:6cd6d2760856 212
ojan 3:5358a691a100 213
ojan 3:5358a691a100 214 KalmanUpdate();
ojan 3:5358a691a100 215
ojan 1:6cd6d2760856 216
ojan 0:bc6f14fc60c7 217 // LPS25Hによる気圧の取得は10Hz
ojan 0:bc6f14fc60c7 218 lps_cnt = (lps_cnt+1)%10;
ojan 0:bc6f14fc60c7 219 if(lps_cnt == 0) {
ojan 0:bc6f14fc60c7 220 press = (float)lps.pressure() * PRES_LSB_TO_HPA;
ojan 0:bc6f14fc60c7 221 }
ojan 3:5358a691a100 222
ojan 3:5358a691a100 223 timer.stop();
ojan 3:5358a691a100 224 //pc.printf("%d(us)\r\n", timer.read_us());
ojan 0:bc6f14fc60c7 225 }
ojan 3:5358a691a100 226 }
ojan 3:5358a691a100 227
ojan 3:5358a691a100 228 void toString(Matrix& m) {
ojan 3:5358a691a100 229
ojan 3:5358a691a100 230 for(int i=0; i<m.GetRow(); i++) {
ojan 3:5358a691a100 231 for(int j=0; j<m.GetCol(); j++) {
ojan 3:5358a691a100 232 pc.printf("%.6f\t", m.GetComp(i+1, j+1));
ojan 3:5358a691a100 233 }
ojan 3:5358a691a100 234 pc.printf("\r\n");
ojan 3:5358a691a100 235 }
ojan 3:5358a691a100 236
ojan 3:5358a691a100 237 }
ojan 3:5358a691a100 238
ojan 3:5358a691a100 239 void toString(Vector& v) {
ojan 3:5358a691a100 240
ojan 3:5358a691a100 241 for(int i=0; i<v.GetDim(); i++) {
ojan 3:5358a691a100 242 pc.printf("%.6f\t", v.GetComp(i+1));
ojan 3:5358a691a100 243 }
ojan 3:5358a691a100 244 pc.printf("\r\n");
ojan 3:5358a691a100 245
ojan 0:bc6f14fc60c7 246 }