hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@36:94dc027e05cd, 2015-12-07 (annotated)

Committer:
taurin
Date:
Mon Dec 07 02:11:56 2015 +0000
Revision:
36:94dc027e05cd
Parent:
34:4bda9af9a0cd 
MPU?HMC??????ver

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 "Vector.h"
ojan 3:5358a691a100 7 #include "Matrix.h"
ojan 3:5358a691a100 8 #include "Vector_Matrix_operator.h"
ojan 0:bc6f14fc60c7 9 #include "myConstants.h"
onaka 7:0ec343d29641 10 #include "SDFileSystem.h"
onaka 7:0ec343d29641 11 #include "BufferedSerial.h"
onaka 7:0ec343d29641 12 #include "ConfigFile.h"
ojan 0:bc6f14fc60c7 13
ojan 14:f85cb5340cb8 14 const float dt = 0.01f; // 割り込み周期(s)
ojan 1:6cd6d2760856 15
ojan 25:4c72d7420d8a 16 enum Direction {LEFT, RIGHT, NEUTRAL};
ojan 25:4c72d7420d8a 17
ojan 14:f85cb5340cb8 18 /****************************** private macro ******************************/
ojan 14:f85cb5340cb8 19 /****************************** private typedef ******************************/
ojan 14:f85cb5340cb8 20 /****************************** private variables ******************************/
ojan 10:8ee11e412ad7 21 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
taurin 36:94dc027e05cd 22 I2C i2c(p9, p8); // I2Cポート
ojan 10:8ee11e412ad7 23 MPU6050 mpu(&i2c); // 加速度・角速度センサ
ojan 10:8ee11e412ad7 24 HMC5883L hmc(&i2c); // 地磁気センサ
ojan 10:8ee11e412ad7 25 LPS25H lps(&i2c); // 気圧センサ
taurin 36:94dc027e05cd 26 Serial pc(USBTX,USBRX); // PC通信用シリアルポート
ojan 19:ad8ff2de68f5 27 FILE * fp; // ログファイルのポインタ
ojan 14:f85cb5340cb8 28 ConfigFile cfg; // ConfigFile
ojan 20:00afba164688 29 Ticker ticker; // 割り込みタイマー
ojan 14:f85cb5340cb8 30 Timer timer; // 時間計測用タイマー
ojan 25:4c72d7420d8a 31 Direction dir = NEUTRAL; // 旋回方向
ojan 0:bc6f14fc60c7 32
ojan 34:4bda9af9a0cd 33 volatile int lps_cnt = 0; // 気圧センサ読み取りカウント
ojan 34:4bda9af9a0cd 34 volatile bool INT_flag = true; // 割り込み可否フラグ
ojan 17:03b45055ca05 35 /* こちらの変数群はメインループでは参照しない */
ojan 14:f85cb5340cb8 36 Vector raw_acc(3); // 加速度(m/s^2) 生
ojan 14:f85cb5340cb8 37 Vector raw_gyro(3); // 角速度(deg/s) 生
ojan 14:f85cb5340cb8 38 Vector raw_geomag(3); // 地磁気(?) 生
ojan 14:f85cb5340cb8 39 float raw_press; // 気圧(hPa) 生
ojan 23:79cdc1432160 40 float raw_temp; // 温度(℃)　生
ojan 17:03b45055ca05 41 /* メインループ内ではこちらを参照する */
ojan 14:f85cb5340cb8 42 Vector acc(3); // 加速度(m/s^2)
ojan 14:f85cb5340cb8 43 Vector gyro(3); // 角速度(deg/s)
ojan 14:f85cb5340cb8 44 Vector geomag(3); // 地磁気(?)
ojan 24:8838be99cec3 45 float p0; // 気圧の初期値
ojan 14:f85cb5340cb8 46 float press; // 気圧(hPa)
ojan 23:79cdc1432160 47 float temp; // 温度(℃)
ojan 24:8838be99cec3 48 float height; // 高さ(m)
ojan 1:6cd6d2760856 49
ojan 14:f85cb5340cb8 50 Vector raw_g(3); // 重力ベクトル 生
ojan 14:f85cb5340cb8 51 Vector g(3); // 重力ベクトル
ojan 14:f85cb5340cb8 52 Vector target_p(2); // 目標情報（経度、緯度）（rad）
ojan 25:4c72d7420d8a 53 Vector p(2); // 現在の位置情報(補完含む)(経度, 緯度)（rad）
ojan 25:4c72d7420d8a 54 Vector new_p(2); // 最新の位置情報(経度, 緯度)（rad）
ojan 14:f85cb5340cb8 55 Vector pre_p(2); // 過去の位置情報(経度, 緯度)（rad）
ojan 14:f85cb5340cb8 56 int UTC_t = 0; // UTC時刻
ojan 14:f85cb5340cb8 57 int pre_UTC_t = 0; // 前のUTC時刻
ojan 30:fb310564097b 58 int ss = 0; // 時刻の秒数の小数部分
ojan 8:602865d8fca3 59
ojan 14:f85cb5340cb8 60 Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル（x軸）
ojan 14:f85cb5340cb8 61 Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル（z軸）
ojan 14:f85cb5340cb8 62 Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル（y軸）
ojan 9:6d4578dcc1ed 63
ojan 14:f85cb5340cb8 64 Vector r_f(3); // 世界座標に固定された、北向きのベクトル（X軸）
ojan 14:f85cb5340cb8 65 Vector r_u(3); // 世界座標に固定された、上向きのベクトル（Z軸）
ojan 14:f85cb5340cb8 66 Vector r_l(3); // 世界座標に固定された、西向きのベクトル（Y軸）
ojan 14:f85cb5340cb8 67
ojan 34:4bda9af9a0cd 68 float pull_L = 10.0f; // 左サーボの引っ張り量（目標値）(mm：0~PullMax)
ojan 34:4bda9af9a0cd 69 float pull_R = 10.0f; // 右サーボの引っ張り量（目標値）(mm：0~PullMax)
ojan 9:6d4578dcc1ed 70
ojan 14:f85cb5340cb8 71 float yaw = 0.0f; // 本体のヨー角（deg）z軸周り
ojan 14:f85cb5340cb8 72 float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り
ojan 14:f85cb5340cb8 73 float roll = 0.0f; // 本体のロール角（deg）x軸周り
ojan 9:6d4578dcc1ed 74
ojan 14:f85cb5340cb8 75 float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用）
ojan 13:df1e8a650185 76
ojan 34:4bda9af9a0cd 77 volatile int step = 0; // シーケンス制御のステップ
ojan 20:00afba164688 78 char data[512] = {}; // 送信データ用配
ojan 20:00afba164688 79 int loopTime = 0; // 1ループに掛かる時間（デバッグ用
ojan 25:4c72d7420d8a 80 float sv = 0.0f; // サーボ電源電圧
ojan 25:4c72d7420d8a 81 float lv = 0.0f; // ロジック電源電圧
ojan 3:5358a691a100 82
ojan 34:4bda9af9a0cd 83 #ifdef RULE3_1
ojan 34:4bda9af9a0cd 84 float beta = 0.0f; // β変数 （制御則3_1で使用
ojan 34:4bda9af9a0cd 85 #endif
onaka 7:0ec343d29641 86 /** config.txt **
ojan 15:d14d385d37e2 87 * #から始めるのはコメント行
onaka 7:0ec343d29641 88 * #イコールの前後に空白を入れない
onaka 7:0ec343d29641 89 * target_x=111.222
onaka 7:0ec343d29641 90 * target_y=33.444
onaka 7:0ec343d29641 91 */
ojan 23:79cdc1432160 92 float target_x, target_y;
onaka 7:0ec343d29641 93
ojan 14:f85cb5340cb8 94 /* ---------- Kalman Filter ---------- */
ojan 11:083c8c9a5b84 95 // 地磁気ベクトル用
ojan 14:f85cb5340cb8 96 // ジャイロのz軸周りのバイアスも推定
ojan 13:df1e8a650185 97 Vector pri_x1(7);
ojan 13:df1e8a650185 98 Matrix pri_P1(7, 7);
ojan 13:df1e8a650185 99 Vector post_x1(7);
ojan 13:df1e8a650185 100 Matrix post_P1(7, 7);
ojan 13:df1e8a650185 101 Matrix F1(7, 7), H1(3, 7);
ojan 13:df1e8a650185 102 Matrix R1(7, 7), Q1(3, 3);
ojan 13:df1e8a650185 103 Matrix I1(7, 7);
ojan 13:df1e8a650185 104 Matrix K1(7, 3);
ojan 11:083c8c9a5b84 105 Matrix S1(3, 3), S_inv1(3, 3);
ojan 11:083c8c9a5b84 106
ojan 11:083c8c9a5b84 107 // 重力ベクトル用
ojan 14:f85cb5340cb8 108 // ジャイロのx軸、y軸周りのバイアスも同時に推定
ojan 13:df1e8a650185 109 Vector pri_x2(5);
ojan 13:df1e8a650185 110 Matrix pri_P2(5, 5);
ojan 13:df1e8a650185 111 Vector post_x2(5);
ojan 13:df1e8a650185 112 Matrix post_P2(5, 5);
ojan 13:df1e8a650185 113 Matrix F2(5, 5), H2(3, 5);
ojan 13:df1e8a650185 114 Matrix R2(5, 5), Q2(3, 3);
ojan 13:df1e8a650185 115 Matrix I2(5, 5);
ojan 13:df1e8a650185 116 Matrix K2(5, 3);
ojan 13:df1e8a650185 117 Matrix S2(3, 3), S_inv2(3, 3);
ojan 14:f85cb5340cb8 118 /* ---------- ------------- ---------- */
ojan 3:5358a691a100 119
ojan 1:6cd6d2760856 120
ojan 14:f85cb5340cb8 121 /****************************** private functions ******************************/
ojan 23:79cdc1432160 122 void SensorsInit(); // 各種センサーの初期化
ojan 17:03b45055ca05 123 void KalmanInit(); // カルマンフィルタ初期化
ojan 20:00afba164688 124 void DataProcessing(); // データ処理関数
ojan 14:f85cb5340cb8 125 void KalmanUpdate(); // カルマンフィルタ更新
ojan 20:00afba164688 126 void DataUpdate(); // データ取得&更新関数
ojan 1:6cd6d2760856 127
ojan 25:4c72d7420d8a 128 /** 小さい値を返す関数
ojan 25:4c72d7420d8a 129 * @param a: 1つ目の入力値
ojan 25:4c72d7420d8a 130 * @param b: 2つ目の入力値
taurin 36:94dc027e05cd 131 *
ojan 25:4c72d7420d8a 132 * @return a,bのうち、小さい方の値
ojan 25:4c72d7420d8a 133 */
ojan 15:d14d385d37e2 134 inline float min(float a, float b)
ojan 15:d14d385d37e2 135 {
ojan 14:f85cb5340cb8 136 return ((a > b) ? b : a);
ojan 14:f85cb5340cb8 137 }
ojan 14:f85cb5340cb8 138
ojan 14:f85cb5340cb8 139 /****************************** main function ******************************/
ojan 0:bc6f14fc60c7 140
ojan 15:d14d385d37e2 141 int main()
ojan 15:d14d385d37e2 142 {
ojan 15:d14d385d37e2 143
ojan 14:f85cb5340cb8 144 i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
onaka 7:0ec343d29641 145 //カルマンフィルタ初期化
ojan 3:5358a691a100 146 KalmanInit();
ojan 15:d14d385d37e2 147
ojan 20:00afba164688 148 ticker.attach(&DataUpdate, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
taurin 36:94dc027e05cd 149
ojan 15:d14d385d37e2 150
ojan 15:d14d385d37e2 151
ojan 14:f85cb5340cb8 152 /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
ojan 0:bc6f14fc60c7 153 while(1) {
ojan 4:45dc5590abc0 154 timer.stop();
ojan 4:45dc5590abc0 155 timer.reset();
ojan 4:45dc5590abc0 156 timer.start();
ojan 14:f85cb5340cb8 157 myled = 1; // LED is ON
taurin 36:94dc027e05cd 158
ojan 24:8838be99cec3 159 INT_flag = false; // 割り込みによる変数書き換えを阻止
ojan 14:f85cb5340cb8 160 /******************************* データ処理 *******************************/
ojan 20:00afba164688 161 DataProcessing();
taurin 36:94dc027e05cd 162 INT_flag = true;
ojan 0:bc6f14fc60c7 163 }
ojan 14:f85cb5340cb8 164 /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
onaka 7:0ec343d29641 165 }
onaka 7:0ec343d29641 166
ojan 20:00afba164688 167 /** データ処理関数
ojan 20:00afba164688 168 *
ojan 20:00afba164688 169 */
ojan 20:00afba164688 170 void DataProcessing()
ojan 20:00afba164688 171 {
ojan 27:a26ff85bba23 172 static float R_11; // 回転行列(1,1)成分
ojan 27:a26ff85bba23 173 static float R_12; // 回転行列(1,2)成分
ojan 27:a26ff85bba23 174 static float r_cos; // ロール角のcos値
ojan 27:a26ff85bba23 175 static float r_sin; // ロール角のsin値
ojan 27:a26ff85bba23 176 static float p_cos; // ピッチ角のcos値
ojan 27:a26ff85bba23 177 static float p_sin; // ピッチ角のsin値
taurin 36:94dc027e05cd 178
ojan 20:00afba164688 179
ojan 20:00afba164688 180 // 参照座標系の基底を求める
ojan 20:00afba164688 181 r_u = g;
ojan 20:00afba164688 182 r_f = geomag.GetPerpCompTo(g).Normalize();
ojan 20:00afba164688 183 r_l = Cross(r_u, r_f);
ojan 20:00afba164688 184
ojan 20:00afba164688 185 // 回転行列を経由してオイラー角を求める
ojan 20:00afba164688 186 // オイラー角はヨー・ピッチ・ロールの順に回転したものとする
ojan 20:00afba164688 187 // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
ojan 20:00afba164688 188
ojan 25:4c72d7420d8a 189 R_11 = r_f * b_f; // 回転行列の(1,1)成分
ojan 25:4c72d7420d8a 190 R_12 = r_f * b_l; // 回転行列の(1,2)成分
ojan 20:00afba164688 191 yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION;
ojan 25:4c72d7420d8a 192 r_cos = g.GetPerpCompTo(b_f).Normalize() * b_u;
ojan 25:4c72d7420d8a 193 r_sin = Cross(g.GetPerpCompTo(b_f).Normalize(), b_u) * b_f;
ojan 25:4c72d7420d8a 194 if(r_sin > 0.0f) {
ojan 25:4c72d7420d8a 195 roll = acos(r_cos) * RAD_TO_DEG;
ojan 25:4c72d7420d8a 196 } else {
ojan 25:4c72d7420d8a 197 roll = -acos(r_cos) * RAD_TO_DEG;
ojan 25:4c72d7420d8a 198 }
taurin 36:94dc027e05cd 199
ojan 25:4c72d7420d8a 200 p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u;
ojan 25:4c72d7420d8a 201 p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l;
ojan 25:4c72d7420d8a 202 if(p_sin > 0.0f) {
ojan 25:4c72d7420d8a 203 pitch = acos(p_cos) * RAD_TO_DEG;
ojan 25:4c72d7420d8a 204 } else {
ojan 25:4c72d7420d8a 205 pitch = -acos(p_cos) * RAD_TO_DEG;
ojan 25:4c72d7420d8a 206 }
ojan 20:00afba164688 207
ojan 20:00afba164688 208 if(yaw > 180.0f) yaw -= 360.0f; // ヨー角を[-π, π]に補正
ojan 30:fb310564097b 209 else if(yaw < -180.0f) yaw += 360.0f; // ヨー角を[-π, π]に補正
ojan 20:00afba164688 210 }
ojan 20:00afba164688 211
ojan 20:00afba164688 212
ojan 23:79cdc1432160 213 /** 各種センサーの初期化を行う関数
ojan 17:03b45055ca05 214 *
ojan 17:03b45055ca05 215 */
ojan 23:79cdc1432160 216 void SensorsInit()
ojan 20:00afba164688 217 {
taurin 36:94dc027e05cd 218
ojan 23:79cdc1432160 219 if(!mpu.init()) error("mpu6050 Initialize Error !!"); // mpu6050初期化
ojan 23:79cdc1432160 220 if(!hmc.init()) error("hmc5883l Initialize Error !!"); // hmc5883l初期化
taurin 36:94dc027e05cd 221
ojan 17:03b45055ca05 222 //重力ベクトルの初期化
ojan 17:03b45055ca05 223 raw_g.SetComp(1, 0.0f);
ojan 17:03b45055ca05 224 raw_g.SetComp(2, 0.0f);
ojan 17:03b45055ca05 225 raw_g.SetComp(3, 1.0f);
ojan 17:03b45055ca05 226
ojan 17:03b45055ca05 227 // 機体に固定されたベクトルの初期化
ojan 17:03b45055ca05 228 b_f.SetComp(1, 0.0f);
ojan 17:03b45055ca05 229 b_f.SetComp(2, 0.0f);
ojan 33:56163d4e2c53 230 b_f.SetComp(3, -1.0f);
ojan 31:2f88240999fe 231 b_u.SetComp(1, 1.0f);
ojan 17:03b45055ca05 232 b_u.SetComp(2, 0.0f);
ojan 17:03b45055ca05 233 b_u.SetComp(3, 0.0f);
ojan 17:03b45055ca05 234 b_l = Cross(b_u, b_f);
ojan 20:00afba164688 235
ojan 17:03b45055ca05 236 // 目標座標をベクトルに代入
ojan 17:03b45055ca05 237 target_p.SetComp(1, target_x * DEG_TO_RAD);
ojan 17:03b45055ca05 238 target_p.SetComp(2, target_y * DEG_TO_RAD);
ojan 17:03b45055ca05 239 }
ojan 17:03b45055ca05 240
ojan 15:d14d385d37e2 241 void KalmanInit()
ojan 15:d14d385d37e2 242 {
ojan 11:083c8c9a5b84 243 // 重力
ojan 11:083c8c9a5b84 244 {
ojan 11:083c8c9a5b84 245 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 13:df1e8a650185 246 float alpha_R2 = 0.002f;
ojan 12:0d978eb4d639 247 float alpha_Q2 = 0.5f;
ojan 11:083c8c9a5b84 248 R2 *= alpha_R2;
ojan 11:083c8c9a5b84 249 Q2 *= alpha_Q2;
ojan 15:d14d385d37e2 250
ojan 13:df1e8a650185 251 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 13:df1e8a650185 252 float h2[15] = {
ojan 13:df1e8a650185 253 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
ojan 15:d14d385d37e2 254 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 255 0.0f, 0.0f, 1.0f, 0.0f, 0.0f
ojan 11:083c8c9a5b84 256 };
ojan 15:d14d385d37e2 257
ojan 13:df1e8a650185 258 H2.SetComps(h2);
ojan 11:083c8c9a5b84 259 }
ojan 15:d14d385d37e2 260
ojan 11:083c8c9a5b84 261 // 地磁気
ojan 11:083c8c9a5b84 262 {
ojan 11:083c8c9a5b84 263 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 13:df1e8a650185 264 float alpha_R1 = 0.002f;
ojan 13:df1e8a650185 265 float alpha_Q1 = 0.5f;
ojan 11:083c8c9a5b84 266 R1 *= alpha_R1;
ojan 11:083c8c9a5b84 267 Q1 *= alpha_Q1;
ojan 15:d14d385d37e2 268
ojan 13:df1e8a650185 269 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 13:df1e8a650185 270 float h1[21] = {
ojan 13:df1e8a650185 271 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 272 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 273 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f
ojan 11:083c8c9a5b84 274 };
ojan 15:d14d385d37e2 275
ojan 13:df1e8a650185 276 H1.SetComps(h1);
ojan 11:083c8c9a5b84 277 }
ojan 3:5358a691a100 278 }
ojan 3:5358a691a100 279
ojan 17:03b45055ca05 280 /** カルマンフィルタの更新を行う関数
ojan 17:03b45055ca05 281 *
ojan 17:03b45055ca05 282 */
ojan 15:d14d385d37e2 283 void KalmanUpdate()
ojan 15:d14d385d37e2 284 {
ojan 13:df1e8a650185 285 // 重力
ojan 15:d14d385d37e2 286
ojan 11:083c8c9a5b84 287 {
ojan 13:df1e8a650185 288 // ヤコビアンの更新
ojan 13:df1e8a650185 289 float f2[25] = {
ojan 15:d14d385d37e2 290 1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, post_x2.GetComp(3)*dt,
ojan 15:d14d385d37e2 291 -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, -post_x2.GetComp(3)*dt, 0.0f,
ojan 15:d14d385d37e2 292 (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, post_x2.GetComp(2)*dt, -post_x2.GetComp(1)*dt,
ojan 15:d14d385d37e2 293 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 13:df1e8a650185 294 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 13:df1e8a650185 295 };
ojan 15:d14d385d37e2 296
ojan 13:df1e8a650185 297 F2.SetComps(f2);
ojan 15:d14d385d37e2 298
ojan 11:083c8c9a5b84 299 // 事前推定値の更新
ojan 13:df1e8a650185 300 //pri_x2 = F2 * post_x2;
ojan 15:d14d385d37e2 301
ojan 13:df1e8a650185 302 float pri_x2_vals[5] = {
ojan 15:d14d385d37e2 303 post_x2.GetComp(1) + post_x2.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x2.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt,
ojan 15:d14d385d37e2 304 post_x2.GetComp(2) + post_x2.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x2.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt,
ojan 15:d14d385d37e2 305 post_x2.GetComp(3) + post_x2.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x2.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt,
ojan 15:d14d385d37e2 306 post_x2.GetComp(4),
ojan 13:df1e8a650185 307 post_x2.GetComp(5)
ojan 13:df1e8a650185 308 };
ojan 15:d14d385d37e2 309
ojan 13:df1e8a650185 310 pri_x2.SetComps(pri_x2_vals);
ojan 15:d14d385d37e2 311
ojan 11:083c8c9a5b84 312 // 事前誤差分散行列の更新
ojan 11:083c8c9a5b84 313 pri_P2 = F2 * post_P2 * F2.Transpose() + R2;
ojan 15:d14d385d37e2 314
ojan 11:083c8c9a5b84 315 // カルマンゲインの計算
ojan 11:083c8c9a5b84 316 S2 = Q2 + H2 * pri_P2 * H2.Transpose();
ojan 13:df1e8a650185 317 static float det;
ojan 11:083c8c9a5b84 318 if((det = S2.Inverse(S_inv2)) >= 0.0f) {
ojan 11:083c8c9a5b84 319 pc.printf("E:%.3f\r\n", det);
ojan 11:083c8c9a5b84 320 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 11:083c8c9a5b84 321 }
ojan 15:d14d385d37e2 322 K2 = pri_P2 * H2.Transpose() * S_inv2;
ojan 15:d14d385d37e2 323
ojan 11:083c8c9a5b84 324 // 事後推定値の更新
ojan 13:df1e8a650185 325 post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2);
ojan 11:083c8c9a5b84 326 // 事後誤差分散行列の更新
ojan 11:083c8c9a5b84 327 post_P2 = (I2 - K2 * H2) * pri_P2;
ojan 11:083c8c9a5b84 328 }
ojan 15:d14d385d37e2 329
ojan 15:d14d385d37e2 330
ojan 11:083c8c9a5b84 331 // 地磁気
ojan 11:083c8c9a5b84 332 {
ojan 11:083c8c9a5b84 333 // ヤコビアンの更新
ojan 13:df1e8a650185 334 float f1[49] = {
ojan 15:d14d385d37e2 335 1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, 0.0f, 0.0f, -post_x1.GetComp(2) * dt,
ojan 15:d14d385d37e2 336 -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 0.0f, 0.0f, 0.0f, post_x1.GetComp(1) * dt,
ojan 15:d14d385d37e2 337 (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
ojan 15:d14d385d37e2 338 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 15:d14d385d37e2 339 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 15:d14d385d37e2 340 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 13:df1e8a650185 341 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 11:083c8c9a5b84 342 };
ojan 15:d14d385d37e2 343
ojan 13:df1e8a650185 344 F1.SetComps(f1);
ojan 15:d14d385d37e2 345
ojan 11:083c8c9a5b84 346 // 事前推定値の更新
ojan 13:df1e8a650185 347 //pri_x1 = F1 * post_x1;
ojan 13:df1e8a650185 348 float pri_x1_vals[7] = {
ojan 15:d14d385d37e2 349 post_x1.GetComp(1) + post_x1.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x1.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt,
ojan 15:d14d385d37e2 350 post_x1.GetComp(2) + post_x1.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x1.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt,
ojan 15:d14d385d37e2 351 post_x1.GetComp(3) + post_x1.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x1.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt,
ojan 15:d14d385d37e2 352 post_x1.GetComp(4),
ojan 15:d14d385d37e2 353 post_x1.GetComp(5),
ojan 15:d14d385d37e2 354 post_x1.GetComp(6),
ojan 13:df1e8a650185 355 post_x1.GetComp(7)
ojan 13:df1e8a650185 356 };
ojan 15:d14d385d37e2 357
ojan 13:df1e8a650185 358 pri_x1.SetComps(pri_x1_vals);
ojan 15:d14d385d37e2 359
ojan 11:083c8c9a5b84 360 // 事前誤差分散行列の更新
ojan 11:083c8c9a5b84 361 pri_P1 = F1 * post_P1 * F1.Transpose() + R1;
ojan 15:d14d385d37e2 362
ojan 11:083c8c9a5b84 363 // カルマンゲインの計算
ojan 11:083c8c9a5b84 364 S1 = Q1 + H1 * pri_P1 * H1.Transpose();
ojan 13:df1e8a650185 365 static float det;
ojan 11:083c8c9a5b84 366 if((det = S1.Inverse(S_inv1)) >= 0.0f) {
ojan 11:083c8c9a5b84 367 pc.printf("E:%.3f\r\n", det);
ojan 11:083c8c9a5b84 368 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 11:083c8c9a5b84 369 }
ojan 15:d14d385d37e2 370 K1 = pri_P1 * H1.Transpose() * S_inv1;
ojan 15:d14d385d37e2 371
ojan 11:083c8c9a5b84 372 // 事後推定値の更新
ojan 11:083c8c9a5b84 373 post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
ojan 11:083c8c9a5b84 374 // 事後誤差分散行列の更新
ojan 11:083c8c9a5b84 375 post_P1 = (I1 - K1 * H1) * pri_P1;
ojan 3:5358a691a100 376 }
ojan 3:5358a691a100 377 }
ojan 3:5358a691a100 378
ojan 23:79cdc1432160 379 /* ------------------------------ 割り込み関数 ------------------------------ */
ojan 23:79cdc1432160 380
ojan 20:00afba164688 381 /** データ取得＆更新関数
ojan 20:00afba164688 382 *
ojan 20:00afba164688 383 */
ojan 20:00afba164688 384 void DataUpdate()
ojan 15:d14d385d37e2 385 {
ojan 4:45dc5590abc0 386 // センサーの値を更新
ojan 4:45dc5590abc0 387 mpu.read();
ojan 4:45dc5590abc0 388 hmc.read();
ojan 15:d14d385d37e2 389
ojan 4:45dc5590abc0 390 for(int i=0; i<3; i++) {
ojan 4:45dc5590abc0 391 raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
ojan 4:45dc5590abc0 392 raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 4:45dc5590abc0 393 raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
ojan 4:45dc5590abc0 394 }
ojan 15:d14d385d37e2 395
ojan 14:f85cb5340cb8 396 Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g
ojan 4:45dc5590abc0 397 raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ
ojan 4:45dc5590abc0 398 raw_g = raw_g.Normalize();
ojan 15:d14d385d37e2 399
ojan 4:45dc5590abc0 400 KalmanUpdate();
ojan 15:d14d385d37e2 401
ojan 24:8838be99cec3 402 if(INT_flag) {
ojan 4:45dc5590abc0 403 g = raw_g;
ojan 4:45dc5590abc0 404 for(int i=0; i<3; i++) {
ojan 11:083c8c9a5b84 405 geomag.SetComp(i+1, post_x1.GetComp(i+1));
ojan 4:45dc5590abc0 406 }
ojan 4:45dc5590abc0 407 acc = raw_acc;
ojan 4:45dc5590abc0 408 gyro = raw_gyro;
ojan 4:45dc5590abc0 409 press = raw_press;
ojan 23:79cdc1432160 410 temp = raw_temp;
ojan 15:d14d385d37e2 411
ojan 13:df1e8a650185 412 vrt_acc = raw_acc * raw_g;
ojan 0:bc6f14fc60c7 413 }
ojan 3:5358a691a100 414 }