MPU6050のサンプルプログラム2
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 11:083c8c9a5b84
- Parent:
- 10:8ee11e412ad7
- Child:
- 12:0d978eb4d639
diff -r 8ee11e412ad7 -r 083c8c9a5b84 main.cpp --- a/main.cpp Mon Jun 15 13:29:36 2015 +0000 +++ b/main.cpp Tue Jun 16 15:31:38 2015 +0000 @@ -89,15 +89,28 @@ 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); +// 地磁気ベクトル用 +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); +Matrix S1(3, 3), S_inv1(3, 3); + +// 重力ベクトル用 +// ジャイロのバイアスも同時に推定する +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); /* ----- ------------- ----- */ Timer timer; @@ -122,7 +135,7 @@ if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 - + /* //Config読み取り LoadConfig(); @@ -138,7 +151,7 @@ fp = fopen(filename, "w"); fprintf(fp, "log data\r\n"); xbee.printf("log data\r\n"); - + */ servoL.period(0.020f); // サーボの信号の周期は20ms servoR.period(0.020f); @@ -153,12 +166,12 @@ raw_g.SetComp(3, 1.0f); // 機体に固定されたベクトルの初期化 - b_f.SetComp(1, 0.0f); + b_f.SetComp(1, 1.0f); b_f.SetComp(2, 0.0f); - b_f.SetComp(3, -1.0f); - b_u.SetComp(1, 1.0f); + b_f.SetComp(3, 0.0f); + b_u.SetComp(1, 0.0f); b_u.SetComp(2, 0.0f); - b_u.SetComp(3, 0.0f); + b_u.SetComp(3, 1.0f); b_l = Cross(b_u, b_f); /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */ @@ -181,6 +194,7 @@ r_f = geomag.GetPerpCompTo(g).Normalize(); r_l = Cross(r_u, r_f); + // 回転行列を経由してオイラー角を求める // オイラー角はヨー・ピッチ・ロールの順に回転したものとする // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。 @@ -191,9 +205,14 @@ 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; + 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, %.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)); if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら p.SetComp(1, gms.longitude * DEG_TO_RAD); @@ -214,28 +233,32 @@ geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), press, gms.longitude, gms.latitude, sv, lv, optSensor.read_u16()); - fprintf(fp, data); - xbee.printf(data); + //fprintf(fp, data); + //fflush(fp); + //xbee.printf(data); INT_flag = TRUE; // 割り込み許可 // 制御ルーチン { - //pull_L = (pull_L+5)%30; - //pull_R = (pull_R+5)%30; - pull_L = 0; - pull_R = 30; + 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 + /*pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", + post_x.GetComp(4), post_x.GetComp(5), post_x.GetComp(6), + yaw, pitch, roll, + geomag.GetNorm());*/ // ループはきっかり0.2秒ごと while(timer.read_ms() < 200); - pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f); } @@ -281,66 +304,134 @@ } void KalmanInit() { - - // 誤差共分散行列の値を決める(対角成分のみ) - float alpha_R = 60.0f; - float alpha_Q = 100.0f; - R *= alpha_R; - Q *= alpha_Q; + // 重力 + { + // 誤差共分散行列の値を決める(対角成分のみ) + float alpha_R2 = 60.0f; + float alpha_Q2 = 100.0f; + R2 *= alpha_R2; + 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 + }; + + 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 + }; + + H2.SetComps(h); + } - // 状態方程式のヤコビアンの初期値を代入(時間変化あり) - 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); + // 地磁気 + { + // 誤差共分散行列の値を決める(対角成分のみ) + float alpha_R1 = 500.0f; + float alpha_Q1 = 1000.0f; + 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 + }; + + 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); + } } 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); + { + // ヤコビアンの更新 + 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); + + // 事前推定値の更新 + pri_x2 = F2 * post_x2; + // 事前誤差分散行列の更新 + pri_P2 = F2 * post_P2 * F2.Transpose() + R2; + + // カルマンゲインの計算 + S2 = Q2 + H2 * pri_P2 * H2.Transpose(); + 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; + + // 事後推定値の更新 + post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2); + // 事後誤差分散行列の更新 + post_P2 = (I2 - K2 * H2) * pri_P2; + } - // 事前推定値の更新 - 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; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 + // 地磁気 + { + // ヤコビアンの更新 + 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 + }; + + F1.SetComps(f); + + // 事前推定値の更新 + pri_x1 = F1 * post_x1; + // 事前誤差分散行列の更新 + pri_P1 = F1 * post_P1 * F1.Transpose() + R1; + + // カルマンゲインの計算 + S1 = Q1 + H1 * pri_P1 * H1.Transpose(); + float det; + if((det = S1.Inverse(S_inv1)) >= 0.0f) { + pc.printf("E:%.3f\r\n", det); + return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 + } + K1 = pri_P1 * H1.Transpose() * S_inv1; + + // 事後推定値の更新 + post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1); + // 事後誤差分散行列の更新 + post_P1 = (I1 - K1 * H1) * pri_P1; } - K = pri_P * H.Transpose() * inv; - - // 事後推定値の更新 - post_x = pri_x + K * (raw_geomag - H * pri_x); - // 事後誤差分散行列の更新 - post_P = (I - K * H) * pri_P; } float distance(Vector p1, Vector p2) { @@ -386,7 +477,7 @@ if(INT_flag != FALSE) { g = raw_g; for(int i=0; i<3; i++) { - geomag.SetComp(i+1, post_x.GetComp(i+1)); + geomag.SetComp(i+1, post_x1.GetComp(i+1)); } acc = raw_acc; gyro = raw_gyro;