MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 12:0d978eb4d639
- Parent:
- 11:083c8c9a5b84
- Child:
- 13:df1e8a650185
--- a/main.cpp Tue Jun 16 15:31:38 2015 +0000 +++ b/main.cpp Tue Jun 16 17:04:58 2015 +0000 @@ -102,15 +102,15 @@ // 重力ベクトル用 // ジャイロのバイアスも同時に推定する -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); +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); /* ----- ------------- ----- */ Timer timer; @@ -208,11 +208,15 @@ 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\r\n", + gyro.GetComp(1), post_x2.GetComp(3), + gyro.GetComp(2), post_x2.GetComp(4)); + */ - 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)); + /*pc.printf("%.3f, %.3f, %.3f\r\n", + yaw, roll, pitch); + */ if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら p.SetComp(1, gms.longitude * DEG_TO_RAD); @@ -307,28 +311,27 @@ // 重力 { // 誤差共分散行列の値を決める(対角成分のみ) - float alpha_R2 = 60.0f; - float alpha_Q2 = 100.0f; + float alpha_R2 = 0.001f; + 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[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 + // 状態方程式のヤコビアンの初期値を代入(時間変化無し) + 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 }; 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 + float h[8] = { + 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, 0.0f }; H2.SetComps(h); @@ -367,17 +370,12 @@ void KalmanUpdate() { { - // ヤコビアンの更新 - 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); + // 入力ベクトル(角速度) + 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); // 事前推定値の更新 pri_x2 = F2 * post_x2; @@ -393,8 +391,13 @@ } 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 * (raw_acc - H2 * pri_x2); + post_x2 = pri_x2 + K2 * (alpha - H2 * pri_x2); // 事後誤差分散行列の更新 post_P2 = (I2 - K2 * H2) * pri_P2; }