CIA
MPU6050/9250で姿勢を推定するプログラム ・ジャイロ積算のみ（update()） ・ジャイロ積算後，加速度で補正（update_correction()） の2パターンの関数がある．
Diff: main.cpp
- Revision:
- 0:29dce55dbcfe
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Mar 20 13:46:54 2018 +0000
@@ -0,0 +1,267 @@
+#include "mbed.h"
+#include "MPU9250.h"
+#include "math.h"
+#include "Eigen/Core.h" // 線形代数のライブラリ
+#include "Eigen/Geometry.h" // 同上
+#define PI 3.141592654 // 円周率
+
+Timer timer;
+Ticker ticker;
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+InterruptIn flt_pin(p30);
+Serial pc (USBTX, USBRX);
+MPU9250 mpu(p28,p27);
+
+const float t_calibrate = 25.f;
+const float dt = 0.05;
+
+// 変数用意
+int N = 0; // 較正に用いるサンプル数
+
+float gyro[3]; // ジャイロセンサの出力
+float b_gyro[3] = {}; // ジャイロセンサのバイアス誤差．
+float acc[3]; // 加速度センサの出力
+// float acc_norm; // 加速度ベクトルの2-ノルム（長さ）．
+float acc_normalized[3]; // 正規化した（長さ1の）加速度ベクトル．
+
+
+float pitch = 0.f; // ピッチ角.
+float roll = 0.f; // バンク（ロール）角.
+float yaw; // 方位角（ヨー角）．
+Eigen::Vector4f q; // 姿勢クォータニオン
+Eigen::Matrix4f Omg;
+Eigen::Vector4f q_dot;
+float omg[3];
+float q00_22, q11_33;
+
+void calibrate();
+void set_initial_quaternion(float roll, float pitch);
+void flt_pin_rise();
+void flt_pin_fall();
+void update();
+void update_correction();
+
+int main()
+{
+ mpu.setAcceleroRange(MPU9250_ACCELERO_RANGE_16G);
+ mpu.setGyroRange(MPU9250_GYRO_RANGE_2000);
+
+ pc.printf("=\r\n\n");
+ pc.printf("Calibration (%3.1f s) will start in\r\n 5\r\n", t_calibrate);
+ wait(1.0);
+ pc.printf(" 4\r\n");
+ wait(1.0);
+ pc.printf(" 3\r\n");
+ wait(1.0);
+ pc.printf(" 2\r\n");
+ wait(1.0);
+ pc.printf(" 1\r\n\n");
+ wait(1.0);
+
+ pc.printf("Calibrating... ");
+ ticker.attach(&calibrate, 0.01);
+ timer.start();
+ while (timer.read() < t_calibrate);
+ ticker.detach();
+ pc.printf("Done. (%d samples)\r\n\n", N);
+// timer.stop();
+
+ b_gyro[0] /= N;
+ b_gyro[1] /= N;
+ b_gyro[2] /= N;
+ roll /= N;
+ pitch /= N;
+
+ printf("Initial Attitude\r\n");
+ printf(" bank:%6.1f deg, pitch:%6.1f deg\r\n\n",
+ roll*180/PI , pitch*180/PI);
+
+ set_initial_quaternion(roll, pitch);
+
+ pc.printf("waiting for launch...\r\n");
+ flt_pin.mode(PullUp);
+// flt_pin.fall(&flt_pin_fall);
+ flt_pin.rise(&flt_pin_rise);
+// ticker.attach(&update_correction, dt);
+ while(1);
+}
+
+void calibrate()
+{
+ //////////////////////////////////////
+ // //
+ // ジャイロセンサの較正・初期姿勢角の取得 //
+ // //
+ //////////////////////////////////////
+
+ // b_gyro にはジャイロセンサの出力の平均（バイアス誤差）を格納．
+ mpu.getGyro(gyro);
+ b_gyro[0] += gyro[0];
+ b_gyro[1] += gyro[1];
+ b_gyro[2] += gyro[2];
+
+ // 重力加速度ベクトルを使って，姿勢角を算出
+ mpu.getAccelero(acc);
+// acc_norm = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
+
+// acc_normalized[0] = acc[0] / acc_norm;
+// acc_normalized[1] = acc[1] / acc_norm;
+// acc_normalized[2] = acc[2] / acc_norm;
+ acc_normalized[2] = acc[2] / sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
+
+ // N 回の姿勢角の平均をとって初期姿勢角とする
+ pitch += asin (acc_normalized[2]);
+ roll += atan2(acc[1], -acc[0]);
+
+ // count up
+ N++;
+}
+
+void set_initial_quaternion(float roll, float pitch)
+{
+ /////////////////////////////////////////
+ // //
+ // 初期姿勢角から初期姿勢クォータニオンを計算 //
+ // //
+ /////////////////////////////////////////
+ float c_roll2 = cos(roll/2); // 繰り返しの計算を避けるための一時変数
+ float s_roll2 = sin(roll/2); // 同上
+
+ Eigen::Matrix4f qx0_otimes;
+ qx0_otimes << c_roll2,-s_roll2, 0 , 0 ,
+ s_roll2, c_roll2, 0 , 0 ,
+ 0 , 0 , c_roll2, s_roll2,
+ 0 , 0 ,-s_roll2, c_roll2;
+
+ Eigen::Vector4f q_y0(cos(pitch/2), 0.f, sin(pitch/2), 0.f);
+
+ // 初期姿勢クォータニオン q
+ q = qx0_otimes * q_y0;
+}
+
+void flt_pin_rise()
+{
+ pc.printf("Launch!\r\n");
+ ticker.attach(&update, dt);
+}
+
+void flt_pin_fall()
+{
+ pc.printf("fall.\r\n");
+ ticker.detach();
+}
+
+ /**
+ * 観測値を用いて姿勢角を更新します．
+ * ジャイロセンサの出力を単純に積算します．
+ */
+void update()
+{
+ mpu.getGyro(gyro);
+
+ omg[0] = gyro[2] - b_gyro[2];
+ omg[1] = -gyro[1] + b_gyro[1];
+ omg[2] = gyro[0] - b_gyro[0];
+
+ // 姿勢クォータニオンの時間微分を計算
+ Omg << 0.f ,-omg[0],-omg[1],-omg[2],
+ omg[0], 0.f , omg[2],-omg[1],
+ omg[1],-omg[2], 0.f , omg[0],
+ omg[2], omg[1],-omg[0], 0.f ;
+// q_dot = 0.5f * Omg * q;
+
+ // 現在時刻の姿勢クォータニオン
+ q += Omg * q * 0.5f * dt;
+ q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
+
+ // 姿勢クォータニオンからオイラー角への変換
+ q00_22 = (q[0] + q[2])*(q[0] - q[2]);
+ q11_33 = (q[1] + q[3])*(q[1] - q[3]);
+ roll = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33);
+ pitch = asin (-2*(q[1]*q[3]-q[0]*q[2]));
+// yaw = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33);
+
+ pc.printf("%11.7f ",timer.read());
+// pc.printf("%6.1f %6.1f %6.1f\r\n",
+// roll*180/PI, pitch*180/PI, yaw*180/PI);
+ pc.printf("%6.1f %6.1f\r\n",
+ roll*180/PI, pitch*180/PI);
+}
+
+Eigen::Vector3f bB_tilde;
+Eigen::Vector3f bB_hat;
+Eigen::Vector3f bG(0.f, 0.f, -1.f);
+Eigen::Vector3f rot_vec;
+float rot_q[4] = {1.f, 0.f, 0.f, 0.f};
+Eigen::Matrix3f dcm;
+Eigen::Matrix4f rot_q_otimes;
+const float eps = 0.01;
+float q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
+float a_norm_div;
+
+ /**
+ * 観測値を用いて姿勢角を更新します．
+ * ジャイロセンサの出力を積算した上で，加速度センサの出力を用いて補正します．
+ */
+void update_correction()
+{
+ mpu.getGyro(gyro);
+
+ omg[0] = gyro[2] - b_gyro[2];
+ omg[1] = -gyro[1] + b_gyro[1];
+ omg[2] = gyro[0] - b_gyro[0];
+
+ // 姿勢クォータニオンの時間微分を計算
+ Omg << 0.f ,-omg[0],-omg[1],-omg[2],
+ omg[0], 0.f , omg[2],-omg[1],
+ omg[1],-omg[2], 0.f , omg[0],
+ omg[2], omg[1],-omg[0], 0.f ;
+// q_dot = 0.5f * Omg * q;
+
+ // 現在時刻の姿勢クォータニオン
+ q += Omg * q * 0.5f * dt;
+ q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
+
+ // 姿勢クォータニオンからオイラー角への変換
+ q00 = q[0]*q[0];
+ q01 = q[0]*q[1];
+ q02 = q[0]*q[2];
+ q03 = q[0]*q[3];
+ q11 = q[1]*q[1];
+ q12 = q[1]*q[2];
+ q13 = q[1]*q[3];
+ q22 = q[2]*q[2];
+ q23 = q[2]*q[3];
+ q33 = q[3]*q[3];
+ dcm << q00+q11-q22-q33, 2*(q12+q03) , 2*(q13-q02) ,
+ 2*(q12-q03) , q00-q11+q22-q33, 2*(q23+q01) ,
+ 2*(q13+q02) , 2*(q23-q01) , q00-q11-q22+q33;
+ bB_hat = dcm*bG;
+ mpu.getAccelero(acc);
+ a_norm_div = 1.f / sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
+ bB_tilde << acc[2]*a_norm_div,-acc[1]*a_norm_div, acc[0]*a_norm_div;
+ rot_vec = bB_hat.cross(bB_hat - bB_tilde);
+ rot_q[1] = 0.5f*eps*rot_vec[0];
+ rot_q[2] = 0.5f*eps*rot_vec[1];
+ rot_q[3] = 0.5f*eps*rot_vec[2];
+ rot_q_otimes << 1.f ,-rot_q[1],-rot_q[2],-rot_q[3],
+ rot_q[1], 1.f , rot_q[3],-rot_q[2],
+ rot_q[2],-rot_q[3], 1.f , rot_q[1],
+ rot_q[3], rot_q[2],-rot_q[1], 1.f ;
+ q = rot_q_otimes * q;
+ q /= sqrtf(q00+q11+q22+q33);
+
+ q00_22 = q00 - q22;
+ q11_33 = q11 - q33;
+ roll = atan2( 2*(q23+q01), q00_22 - q11_33);
+ pitch = asin (-2*(q13-q02));
+ yaw = atan2( 2*(q12+q03), q00_22 + q11_33);
+
+// pc.printf("%11.7f ",timer.read());
+ pc.printf("%6.1f %6.1f %6.1f\r\n",
+ roll*180/PI, pitch*180/PI, yaw*180/PI);
+// pc.printf("%6.1f %6.1f\r\n",
+// roll*180/PI, pitch*180/PI);
+}
\ No newline at end of file