CIA / Mbed 2 deprecated AttitudeEstimation_usingTicker

MPU6050/9250で姿勢を推定するプログラム ・ジャイロ積算のみ（update()） ・ジャイロ積算後，加速度で補正（update_correction()） の2パターンの関数がある．

Dependencies:   Eigen mbed

main.cpp@0:29dce55dbcfe, 2018-03-20 (annotated)

Committer:
daqn
Date:
Tue Mar 20 13:46:54 2018 +0000
Revision:
0:29dce55dbcfe
??????????????; ??????????????; ?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daqn 0:29dce55dbcfe 1 #include "mbed.h"
daqn 0:29dce55dbcfe 2 #include "MPU9250.h"
daqn 0:29dce55dbcfe 3 #include "math.h"
daqn 0:29dce55dbcfe 4 #include "Eigen/Core.h" // 線形代数のライブラリ
daqn 0:29dce55dbcfe 5 #include "Eigen/Geometry.h" // 同上
daqn 0:29dce55dbcfe 6 #define PI 3.141592654 // 円周率
daqn 0:29dce55dbcfe 7
daqn 0:29dce55dbcfe 8 Timer timer;
daqn 0:29dce55dbcfe 9 Ticker ticker;
daqn 0:29dce55dbcfe 10 DigitalOut led1(LED1);
daqn 0:29dce55dbcfe 11 DigitalOut led2(LED2);
daqn 0:29dce55dbcfe 12 DigitalOut led3(LED3);
daqn 0:29dce55dbcfe 13 InterruptIn flt_pin(p30);
daqn 0:29dce55dbcfe 14 Serial pc (USBTX, USBRX);
daqn 0:29dce55dbcfe 15 MPU9250 mpu(p28,p27);
daqn 0:29dce55dbcfe 16
daqn 0:29dce55dbcfe 17 const float t_calibrate = 25.f;
daqn 0:29dce55dbcfe 18 const float dt = 0.05;
daqn 0:29dce55dbcfe 19
daqn 0:29dce55dbcfe 20 // 変数用意
daqn 0:29dce55dbcfe 21 int N = 0; // 較正に用いるサンプル数
daqn 0:29dce55dbcfe 22
daqn 0:29dce55dbcfe 23 float gyro[3]; // ジャイロセンサの出力
daqn 0:29dce55dbcfe 24 float b_gyro[3] = {}; // ジャイロセンサのバイアス誤差．
daqn 0:29dce55dbcfe 25 float acc[3]; // 加速度センサの出力
daqn 0:29dce55dbcfe 26 // float acc_norm; // 加速度ベクトルの2-ノルム（長さ）．
daqn 0:29dce55dbcfe 27 float acc_normalized[3]; // 正規化した（長さ1の）加速度ベクトル．
daqn 0:29dce55dbcfe 28
daqn 0:29dce55dbcfe 29
daqn 0:29dce55dbcfe 30 float pitch = 0.f; // ピッチ角.
daqn 0:29dce55dbcfe 31 float roll = 0.f; // バンク（ロール）角.
daqn 0:29dce55dbcfe 32 float yaw; // 方位角（ヨー角）．
daqn 0:29dce55dbcfe 33 Eigen::Vector4f q; // 姿勢クォータニオン
daqn 0:29dce55dbcfe 34 Eigen::Matrix4f Omg;
daqn 0:29dce55dbcfe 35 Eigen::Vector4f q_dot;
daqn 0:29dce55dbcfe 36 float omg[3];
daqn 0:29dce55dbcfe 37 float q00_22, q11_33;
daqn 0:29dce55dbcfe 38
daqn 0:29dce55dbcfe 39 void calibrate();
daqn 0:29dce55dbcfe 40 void set_initial_quaternion(float roll, float pitch);
daqn 0:29dce55dbcfe 41 void flt_pin_rise();
daqn 0:29dce55dbcfe 42 void flt_pin_fall();
daqn 0:29dce55dbcfe 43 void update();
daqn 0:29dce55dbcfe 44 void update_correction();
daqn 0:29dce55dbcfe 45
daqn 0:29dce55dbcfe 46 int main()
daqn 0:29dce55dbcfe 47 {
daqn 0:29dce55dbcfe 48 mpu.setAcceleroRange(MPU9250_ACCELERO_RANGE_16G);
daqn 0:29dce55dbcfe 49 mpu.setGyroRange(MPU9250_GYRO_RANGE_2000);
daqn 0:29dce55dbcfe 50
daqn 0:29dce55dbcfe 51 pc.printf("=\r\n\n");
daqn 0:29dce55dbcfe 52 pc.printf("Calibration (%3.1f s) will start in\r\n 5\r\n", t_calibrate);
daqn 0:29dce55dbcfe 53 wait(1.0);
daqn 0:29dce55dbcfe 54 pc.printf(" 4\r\n");
daqn 0:29dce55dbcfe 55 wait(1.0);
daqn 0:29dce55dbcfe 56 pc.printf(" 3\r\n");
daqn 0:29dce55dbcfe 57 wait(1.0);
daqn 0:29dce55dbcfe 58 pc.printf(" 2\r\n");
daqn 0:29dce55dbcfe 59 wait(1.0);
daqn 0:29dce55dbcfe 60 pc.printf(" 1\r\n\n");
daqn 0:29dce55dbcfe 61 wait(1.0);
daqn 0:29dce55dbcfe 62
daqn 0:29dce55dbcfe 63 pc.printf("Calibrating... ");
daqn 0:29dce55dbcfe 64 ticker.attach(&calibrate, 0.01);
daqn 0:29dce55dbcfe 65 timer.start();
daqn 0:29dce55dbcfe 66 while (timer.read() < t_calibrate);
daqn 0:29dce55dbcfe 67 ticker.detach();
daqn 0:29dce55dbcfe 68 pc.printf("Done. (%d samples)\r\n\n", N);
daqn 0:29dce55dbcfe 69 // timer.stop();
daqn 0:29dce55dbcfe 70
daqn 0:29dce55dbcfe 71 b_gyro[0] /= N;
daqn 0:29dce55dbcfe 72 b_gyro[1] /= N;
daqn 0:29dce55dbcfe 73 b_gyro[2] /= N;
daqn 0:29dce55dbcfe 74 roll /= N;
daqn 0:29dce55dbcfe 75 pitch /= N;
daqn 0:29dce55dbcfe 76
daqn 0:29dce55dbcfe 77 printf("Initial Attitude\r\n");
daqn 0:29dce55dbcfe 78 printf(" bank:%6.1f deg, pitch:%6.1f deg\r\n\n",
daqn 0:29dce55dbcfe 79 roll*180/PI , pitch*180/PI);
daqn 0:29dce55dbcfe 80
daqn 0:29dce55dbcfe 81 set_initial_quaternion(roll, pitch);
daqn 0:29dce55dbcfe 82
daqn 0:29dce55dbcfe 83 pc.printf("waiting for launch...\r\n");
daqn 0:29dce55dbcfe 84 flt_pin.mode(PullUp);
daqn 0:29dce55dbcfe 85 // flt_pin.fall(&flt_pin_fall);
daqn 0:29dce55dbcfe 86 flt_pin.rise(&flt_pin_rise);
daqn 0:29dce55dbcfe 87 // ticker.attach(&update_correction, dt);
daqn 0:29dce55dbcfe 88 while(1);
daqn 0:29dce55dbcfe 89 }
daqn 0:29dce55dbcfe 90
daqn 0:29dce55dbcfe 91 void calibrate()
daqn 0:29dce55dbcfe 92 {
daqn 0:29dce55dbcfe 93 //////////////////////////////////////
daqn 0:29dce55dbcfe 94 // //
daqn 0:29dce55dbcfe 95 // ジャイロセンサの較正・初期姿勢角の取得 //
daqn 0:29dce55dbcfe 96 // //
daqn 0:29dce55dbcfe 97 //////////////////////////////////////
daqn 0:29dce55dbcfe 98
daqn 0:29dce55dbcfe 99 // b_gyro にはジャイロセンサの出力の平均（バイアス誤差）を格納．
daqn 0:29dce55dbcfe 100 mpu.getGyro(gyro);
daqn 0:29dce55dbcfe 101 b_gyro[0] += gyro[0];
daqn 0:29dce55dbcfe 102 b_gyro[1] += gyro[1];
daqn 0:29dce55dbcfe 103 b_gyro[2] += gyro[2];
daqn 0:29dce55dbcfe 104
daqn 0:29dce55dbcfe 105 // 重力加速度ベクトルを使って，姿勢角を算出
daqn 0:29dce55dbcfe 106 mpu.getAccelero(acc);
daqn 0:29dce55dbcfe 107 // acc_norm = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
daqn 0:29dce55dbcfe 108
daqn 0:29dce55dbcfe 109 // acc_normalized[0] = acc[0] / acc_norm;
daqn 0:29dce55dbcfe 110 // acc_normalized[1] = acc[1] / acc_norm;
daqn 0:29dce55dbcfe 111 // acc_normalized[2] = acc[2] / acc_norm;
daqn 0:29dce55dbcfe 112 acc_normalized[2] = acc[2] / sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
daqn 0:29dce55dbcfe 113
daqn 0:29dce55dbcfe 114 // N 回の姿勢角の平均をとって初期姿勢角とする
daqn 0:29dce55dbcfe 115 pitch += asin (acc_normalized[2]);
daqn 0:29dce55dbcfe 116 roll += atan2(acc[1], -acc[0]);
daqn 0:29dce55dbcfe 117
daqn 0:29dce55dbcfe 118 // count up
daqn 0:29dce55dbcfe 119 N++;
daqn 0:29dce55dbcfe 120 }
daqn 0:29dce55dbcfe 121
daqn 0:29dce55dbcfe 122 void set_initial_quaternion(float roll, float pitch)
daqn 0:29dce55dbcfe 123 {
daqn 0:29dce55dbcfe 124 /////////////////////////////////////////
daqn 0:29dce55dbcfe 125 // //
daqn 0:29dce55dbcfe 126 // 初期姿勢角から初期姿勢クォータニオンを計算 //
daqn 0:29dce55dbcfe 127 // //
daqn 0:29dce55dbcfe 128 /////////////////////////////////////////
daqn 0:29dce55dbcfe 129 float c_roll2 = cos(roll/2); // 繰り返しの計算を避けるための一時変数
daqn 0:29dce55dbcfe 130 float s_roll2 = sin(roll/2); // 同上
daqn 0:29dce55dbcfe 131
daqn 0:29dce55dbcfe 132 Eigen::Matrix4f qx0_otimes;
daqn 0:29dce55dbcfe 133 qx0_otimes << c_roll2,-s_roll2, 0 , 0 ,
daqn 0:29dce55dbcfe 134 s_roll2, c_roll2, 0 , 0 ,
daqn 0:29dce55dbcfe 135 0 , 0 , c_roll2, s_roll2,
daqn 0:29dce55dbcfe 136 0 , 0 ,-s_roll2, c_roll2;
daqn 0:29dce55dbcfe 137
daqn 0:29dce55dbcfe 138 Eigen::Vector4f q_y0(cos(pitch/2), 0.f, sin(pitch/2), 0.f);
daqn 0:29dce55dbcfe 139
daqn 0:29dce55dbcfe 140 // 初期姿勢クォータニオン q
daqn 0:29dce55dbcfe 141 q = qx0_otimes * q_y0;
daqn 0:29dce55dbcfe 142 }
daqn 0:29dce55dbcfe 143
daqn 0:29dce55dbcfe 144 void flt_pin_rise()
daqn 0:29dce55dbcfe 145 {
daqn 0:29dce55dbcfe 146 pc.printf("Launch!\r\n");
daqn 0:29dce55dbcfe 147 ticker.attach(&update, dt);
daqn 0:29dce55dbcfe 148 }
daqn 0:29dce55dbcfe 149
daqn 0:29dce55dbcfe 150 void flt_pin_fall()
daqn 0:29dce55dbcfe 151 {
daqn 0:29dce55dbcfe 152 pc.printf("fall.\r\n");
daqn 0:29dce55dbcfe 153 ticker.detach();
daqn 0:29dce55dbcfe 154 }
daqn 0:29dce55dbcfe 155
daqn 0:29dce55dbcfe 156 /**
daqn 0:29dce55dbcfe 157 * 観測値を用いて姿勢角を更新します．
daqn 0:29dce55dbcfe 158 * ジャイロセンサの出力を単純に積算します．
daqn 0:29dce55dbcfe 159 */
daqn 0:29dce55dbcfe 160 void update()
daqn 0:29dce55dbcfe 161 {
daqn 0:29dce55dbcfe 162 mpu.getGyro(gyro);
daqn 0:29dce55dbcfe 163
daqn 0:29dce55dbcfe 164 omg[0] = gyro[2] - b_gyro[2];
daqn 0:29dce55dbcfe 165 omg[1] = -gyro[1] + b_gyro[1];
daqn 0:29dce55dbcfe 166 omg[2] = gyro[0] - b_gyro[0];
daqn 0:29dce55dbcfe 167
daqn 0:29dce55dbcfe 168 // 姿勢クォータニオンの時間微分を計算
daqn 0:29dce55dbcfe 169 Omg << 0.f ,-omg[0],-omg[1],-omg[2],
daqn 0:29dce55dbcfe 170 omg[0], 0.f , omg[2],-omg[1],
daqn 0:29dce55dbcfe 171 omg[1],-omg[2], 0.f , omg[0],
daqn 0:29dce55dbcfe 172 omg[2], omg[1],-omg[0], 0.f ;
daqn 0:29dce55dbcfe 173 // q_dot = 0.5f * Omg * q;
daqn 0:29dce55dbcfe 174
daqn 0:29dce55dbcfe 175 // 現在時刻の姿勢クォータニオン
daqn 0:29dce55dbcfe 176 q += Omg * q * 0.5f * dt;
daqn 0:29dce55dbcfe 177 q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
daqn 0:29dce55dbcfe 178
daqn 0:29dce55dbcfe 179 // 姿勢クォータニオンからオイラー角への変換
daqn 0:29dce55dbcfe 180 q00_22 = (q[0] + q[2])*(q[0] - q[2]);
daqn 0:29dce55dbcfe 181 q11_33 = (q[1] + q[3])*(q[1] - q[3]);
daqn 0:29dce55dbcfe 182 roll = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33);
daqn 0:29dce55dbcfe 183 pitch = asin (-2*(q[1]*q[3]-q[0]*q[2]));
daqn 0:29dce55dbcfe 184 // yaw = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33);
daqn 0:29dce55dbcfe 185
daqn 0:29dce55dbcfe 186 pc.printf("%11.7f ",timer.read());
daqn 0:29dce55dbcfe 187 // pc.printf("%6.1f %6.1f %6.1f\r\n",
daqn 0:29dce55dbcfe 188 // roll*180/PI, pitch*180/PI, yaw*180/PI);
daqn 0:29dce55dbcfe 189 pc.printf("%6.1f %6.1f\r\n",
daqn 0:29dce55dbcfe 190 roll*180/PI, pitch*180/PI);
daqn 0:29dce55dbcfe 191 }
daqn 0:29dce55dbcfe 192
daqn 0:29dce55dbcfe 193 Eigen::Vector3f bB_tilde;
daqn 0:29dce55dbcfe 194 Eigen::Vector3f bB_hat;
daqn 0:29dce55dbcfe 195 Eigen::Vector3f bG(0.f, 0.f, -1.f);
daqn 0:29dce55dbcfe 196 Eigen::Vector3f rot_vec;
daqn 0:29dce55dbcfe 197 float rot_q[4] = {1.f, 0.f, 0.f, 0.f};
daqn 0:29dce55dbcfe 198 Eigen::Matrix3f dcm;
daqn 0:29dce55dbcfe 199 Eigen::Matrix4f rot_q_otimes;
daqn 0:29dce55dbcfe 200 const float eps = 0.01;
daqn 0:29dce55dbcfe 201 float q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
daqn 0:29dce55dbcfe 202 float a_norm_div;
daqn 0:29dce55dbcfe 203
daqn 0:29dce55dbcfe 204 /**
daqn 0:29dce55dbcfe 205 * 観測値を用いて姿勢角を更新します．
daqn 0:29dce55dbcfe 206 * ジャイロセンサの出力を積算した上で，加速度センサの出力を用いて補正します．
daqn 0:29dce55dbcfe 207 */
daqn 0:29dce55dbcfe 208 void update_correction()
daqn 0:29dce55dbcfe 209 {
daqn 0:29dce55dbcfe 210 mpu.getGyro(gyro);
daqn 0:29dce55dbcfe 211
daqn 0:29dce55dbcfe 212 omg[0] = gyro[2] - b_gyro[2];
daqn 0:29dce55dbcfe 213 omg[1] = -gyro[1] + b_gyro[1];
daqn 0:29dce55dbcfe 214 omg[2] = gyro[0] - b_gyro[0];
daqn 0:29dce55dbcfe 215
daqn 0:29dce55dbcfe 216 // 姿勢クォータニオンの時間微分を計算
daqn 0:29dce55dbcfe 217 Omg << 0.f ,-omg[0],-omg[1],-omg[2],
daqn 0:29dce55dbcfe 218 omg[0], 0.f , omg[2],-omg[1],
daqn 0:29dce55dbcfe 219 omg[1],-omg[2], 0.f , omg[0],
daqn 0:29dce55dbcfe 220 omg[2], omg[1],-omg[0], 0.f ;
daqn 0:29dce55dbcfe 221 // q_dot = 0.5f * Omg * q;
daqn 0:29dce55dbcfe 222
daqn 0:29dce55dbcfe 223 // 現在時刻の姿勢クォータニオン
daqn 0:29dce55dbcfe 224 q += Omg * q * 0.5f * dt;
daqn 0:29dce55dbcfe 225 q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
daqn 0:29dce55dbcfe 226
daqn 0:29dce55dbcfe 227 // 姿勢クォータニオンからオイラー角への変換
daqn 0:29dce55dbcfe 228 q00 = q[0]*q[0];
daqn 0:29dce55dbcfe 229 q01 = q[0]*q[1];
daqn 0:29dce55dbcfe 230 q02 = q[0]*q[2];
daqn 0:29dce55dbcfe 231 q03 = q[0]*q[3];
daqn 0:29dce55dbcfe 232 q11 = q[1]*q[1];
daqn 0:29dce55dbcfe 233 q12 = q[1]*q[2];
daqn 0:29dce55dbcfe 234 q13 = q[1]*q[3];
daqn 0:29dce55dbcfe 235 q22 = q[2]*q[2];
daqn 0:29dce55dbcfe 236 q23 = q[2]*q[3];
daqn 0:29dce55dbcfe 237 q33 = q[3]*q[3];
daqn 0:29dce55dbcfe 238 dcm << q00+q11-q22-q33, 2*(q12+q03) , 2*(q13-q02) ,
daqn 0:29dce55dbcfe 239 2*(q12-q03) , q00-q11+q22-q33, 2*(q23+q01) ,
daqn 0:29dce55dbcfe 240 2*(q13+q02) , 2*(q23-q01) , q00-q11-q22+q33;
daqn 0:29dce55dbcfe 241 bB_hat = dcm*bG;
daqn 0:29dce55dbcfe 242 mpu.getAccelero(acc);
daqn 0:29dce55dbcfe 243 a_norm_div = 1.f / sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
daqn 0:29dce55dbcfe 244 bB_tilde << acc[2]*a_norm_div,-acc[1]*a_norm_div, acc[0]*a_norm_div;
daqn 0:29dce55dbcfe 245 rot_vec = bB_hat.cross(bB_hat - bB_tilde);
daqn 0:29dce55dbcfe 246 rot_q[1] = 0.5f*eps*rot_vec[0];
daqn 0:29dce55dbcfe 247 rot_q[2] = 0.5f*eps*rot_vec[1];
daqn 0:29dce55dbcfe 248 rot_q[3] = 0.5f*eps*rot_vec[2];
daqn 0:29dce55dbcfe 249 rot_q_otimes << 1.f ,-rot_q[1],-rot_q[2],-rot_q[3],
daqn 0:29dce55dbcfe 250 rot_q[1], 1.f , rot_q[3],-rot_q[2],
daqn 0:29dce55dbcfe 251 rot_q[2],-rot_q[3], 1.f , rot_q[1],
daqn 0:29dce55dbcfe 252 rot_q[3], rot_q[2],-rot_q[1], 1.f ;
daqn 0:29dce55dbcfe 253 q = rot_q_otimes * q;
daqn 0:29dce55dbcfe 254 q /= sqrtf(q00+q11+q22+q33);
daqn 0:29dce55dbcfe 255
daqn 0:29dce55dbcfe 256 q00_22 = q00 - q22;
daqn 0:29dce55dbcfe 257 q11_33 = q11 - q33;
daqn 0:29dce55dbcfe 258 roll = atan2( 2*(q23+q01), q00_22 - q11_33);
daqn 0:29dce55dbcfe 259 pitch = asin (-2*(q13-q02));
daqn 0:29dce55dbcfe 260 yaw = atan2( 2*(q12+q03), q00_22 + q11_33);
daqn 0:29dce55dbcfe 261
daqn 0:29dce55dbcfe 262 // pc.printf("%11.7f ",timer.read());
daqn 0:29dce55dbcfe 263 pc.printf("%6.1f %6.1f %6.1f\r\n",
daqn 0:29dce55dbcfe 264 roll*180/PI, pitch*180/PI, yaw*180/PI);
daqn 0:29dce55dbcfe 265 // pc.printf("%6.1f %6.1f\r\n",
daqn 0:29dce55dbcfe 266 // roll*180/PI, pitch*180/PI);
daqn 0:29dce55dbcfe 267 }