albatross
MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
hiroya taura
Diff: main.cpp
- Revision:
- 13:df1e8a650185
- Parent:
- 12:0d978eb4d639
- Child:
- 14:f85cb5340cb8
--- a/main.cpp Tue Jun 16 17:04:58 2015 +0000
+++ b/main.cpp Fri Jun 19 01:08:35 2015 +0000
@@ -16,11 +16,7 @@
#define TRUE 1
#define FALSE 0
-#define x 1
-#define y 2
-#define z 3
-
-const float dt = 0.1f; // 割り込み周期(s)
+const float dt = 0.01f; // 割り込み周期(s)
const float ServoMax = 0.0023f; // サーボの最大パルス長(s)
const float ServoMin = 0.0006f; // サーボの最小パルス長(s)
const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm)
@@ -76,9 +72,11 @@
int pull_L = 0; // 左サーボの引っ張り量(mm：0~PullMax)
int pull_R = 0; // 右サーボの引っ張り量(mm：0~PullMax)
-float yaw = 0.0f; // 本体のヨー角（deg）
-float pitch = 0.0f; // 本体のピッチ角（deg）
-float roll = 0.0f; // 本体のロール角（deg）
+float yaw = 0.0f; // 本体のヨー角（deg）z軸周り
+float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り
+float roll = 0.0f; // 本体のロール角（deg）x軸周り
+
+float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用）
/** config.txt **
* #から始めるのはコメント行
@@ -90,27 +88,27 @@
/* ----- Kalman Filter ----- */
// 地磁気ベクトル用
-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);
+Vector pri_x1(7);
+Matrix pri_P1(7, 7);
+Vector post_x1(7);
+Matrix post_P1(7, 7);
+Matrix F1(7, 7), H1(3, 7);
+Matrix R1(7, 7), Q1(3, 3);
+Matrix I1(7, 7);
+Matrix K1(7, 3);
Matrix S1(3, 3), S_inv1(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);
+Vector pri_x2(5);
+Matrix pri_P2(5, 5);
+Vector post_x2(5);
+Matrix post_P2(5, 5);
+Matrix F2(5, 5), H2(3, 5);
+Matrix R2(5, 5), Q2(3, 3);
+Matrix I2(5, 5);
+Matrix K2(5, 3);
+Matrix S2(3, 3), S_inv2(3, 3);
/* ----- ------------- ----- */
Timer timer;
@@ -166,12 +164,12 @@
raw_g.SetComp(3, 1.0f);
// 機体に固定されたベクトルの初期化
- b_f.SetComp(1, 1.0f);
+ b_f.SetComp(1, 0.0f);
b_f.SetComp(2, 0.0f);
- b_f.SetComp(3, 0.0f);
- b_u.SetComp(1, 0.0f);
+ b_f.SetComp(3, -1.0f);
+ b_u.SetComp(1, 1.0f);
b_u.SetComp(2, 0.0f);
- b_u.SetComp(3, 1.0f);
+ b_u.SetComp(3, 0.0f);
b_l = Cross(b_u, b_f);
/* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
@@ -182,10 +180,10 @@
// 0.1秒おきにセンサーの出力をpcへ出力
myled = 1; // LED is ON
- INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
// データ処理
{
+ INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
gms.read(); // GPSデータ取得
UTC_t = (int)gms.time;
@@ -205,18 +203,16 @@
float R_23 = r_l * b_u; // 回転行列の(2,3)成分
float R_33 = r_u * b_u; // 回転行列の(3,3)成分
- yaw = atan2(-R_12, R_11) * RAD_TO_DEG;
+ yaw = atan2(-R_12, R_11) * RAD_TO_DEG - MAG_DECLINATION;
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\r\n",
- yaw, roll, pitch);
- */
+ pc.printf("%.3f, %.3f, %.3f\r\n",
+ //geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3));
+ //yaw, pitch, roll);
+ post_x2.GetComp(4),
+ post_x2.GetComp(5),
+ post_x1.GetComp(7));
if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら
p.SetComp(1, gms.longitude * DEG_TO_RAD);
@@ -227,28 +223,53 @@
pre_p = p;
pre_UTC_t = UTC_t;
+
+ float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
+ float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
+
+ // データをmicroSDに保存し、XBeeでPCへ送信する
+ sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n",
+ g.GetComp(1), g.GetComp(2), g.GetComp(3),
+ geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3),
+ press, gms.longitude, gms.latitude,
+ sv, lv, optSensor.read_u16());
+ //fprintf(fp, data);
+ //fflush(fp);
+ //xbee.printf(data);
+
+ INT_flag = TRUE; // 割り込み許可
}
- float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
- float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
-
- sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n",
- g.GetComp(1), g.GetComp(2), g.GetComp(3),
- geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3),
- press, gms.longitude, gms.latitude,
- sv, lv, optSensor.read_u16());
- //fprintf(fp, data);
- //fflush(fp);
- //xbee.printf(data);
-
- INT_flag = TRUE; // 割り込み許可
// 制御ルーチン
{
- pull_L = (pull_L+5)%30;
- pull_R = (pull_R+5)%30;
+
+ if(fabs(roll) > 40.0f) {
+ // スパイラル回避行動
+ if(roll > 0) {
+ pull_L = PullMax;
+ pull_R = 0;
+ } else {
+ pull_L = 0;
+ pull_R = PullMax;
+ }
+ } else {
+
+ }
+
+ //pull_L = (pull_L+5)%PullMax;
+ //pull_R = (pull_R+5)%PullMax;
//pull_L = 0;
//pull_R = 30;
+
+
+
+ // 指定された引っ張り量だけ紐を引っ張る
+ if(pull_L < 0) pull_L = 0;
+ else if(pull_L > PullMax) pull_L = PullMax;
+ if(pull_R < 0) pull_R = 0;
+ else if(pull_R > PullMax) pull_R = PullMax;
+
servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
@@ -311,119 +332,122 @@
// 重力
{
// 誤差共分散行列の値を決める（対角成分のみ）
- float alpha_R2 = 0.001f;
+ float alpha_R2 = 0.002f;
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[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
+ // 観測方程式のヤコビアンの値を設定（時間変化無し）
+ float h2[15] = {
+ 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f, 0.0f, 0.0f
};
- F2.SetComps(f);
-
- // 観測方程式のヤコビアンの値を設定（時間変化無し）
- float h[8] = {
- 1.0f, 0.0f, 0.0f, 0.0f,
- 0.0f, 1.0f, 0.0f, 0.0f
- };
-
- H2.SetComps(h);
+ H2.SetComps(h2);
}
// 地磁気
{
// 誤差共分散行列の値を決める（対角成分のみ）
- float alpha_R1 = 500.0f;
- float alpha_Q1 = 1000.0f;
+ float alpha_R1 = 0.002f;
+ float alpha_Q1 = 0.5f;
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
+ // 観測方程式のヤコビアンの値を設定（時間変化無し）
+ float h1[21] = {
+ 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.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);
+ H1.SetComps(h1);
}
}
void KalmanUpdate() {
+ // 重力
+
{
- // 入力ベクトル（角速度）
- 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);
+ // ヤコビアンの更新
+ float f2[25] = {
+ 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,
+ -(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,
+ (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,
+ 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
+ };
+
+ F2.SetComps(f2);
// 事前推定値の更新
- pri_x2 = F2 * post_x2;
+ //pri_x2 = F2 * post_x2;
+
+ float pri_x2_vals[5] = {
+ 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,
+ 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,
+ 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,
+ post_x2.GetComp(4),
+ post_x2.GetComp(5)
+ };
+
+ pri_x2.SetComps(pri_x2_vals);
+
// 事前誤差分散行列の更新
pri_P2 = F2 * post_P2 * F2.Transpose() + R2;
// カルマンゲインの計算
S2 = Q2 + H2 * pri_P2 * H2.Transpose();
- float det;
+ static 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;
- // 観測ベクトル（重力加速度ベクトルから算出した角度）
- 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 * (alpha - H2 * pri_x2);
+ post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2);
// 事後誤差分散行列の更新
post_P2 = (I2 - K2 * H2) * pri_P2;
}
+
// 地磁気
{
// ヤコビアンの更新
- 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
+ float f1[49] = {
+ 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,
+ -(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,
+ (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,
+ 0.0f, 0.0f, 0.0f, 1.0f, 0.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, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
};
- F1.SetComps(f);
+ F1.SetComps(f1);
// 事前推定値の更新
- pri_x1 = F1 * post_x1;
+ //pri_x1 = F1 * post_x1;
+ float pri_x1_vals[7] = {
+ 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,
+ 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,
+ 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,
+ post_x1.GetComp(4),
+ post_x1.GetComp(5),
+ post_x1.GetComp(6),
+ post_x1.GetComp(7)
+ };
+
+ pri_x1.SetComps(pri_x1_vals);
+
// 事前誤差分散行列の更新
pri_P1 = F1 * post_P1 * F1.Transpose() + R1;
// カルマンゲインの計算
S1 = Q1 + H1 * pri_P1 * H1.Transpose();
- float det;
+ static float det;
if((det = S1.Inverse(S_inv1)) >= 0.0f) {
pc.printf("E:%.3f\r\n", det);
return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
@@ -432,6 +456,19 @@
// 事後推定値の更新
post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
+ // 地磁気ベクトルの大きさに拘束条件を与える
+ /*{
+ Vector gm(3);
+ gm.SetComp(1, post_x1.GetComp(1));
+ gm.SetComp(2, post_x1.GetComp(2));
+ gm.SetComp(3, post_x1.GetComp(3));
+
+ gm = MAG_MAGNITUDE * gm.Normalize();
+
+ post_x1.SetComp(1, gm.GetComp(1));
+ post_x1.SetComp(2, gm.GetComp(2));
+ post_x1.SetComp(3, gm.GetComp(3));
+ }*/
// 事後誤差分散行列の更新
post_P1 = (I1 - K1 * H1) * pri_P1;
}
@@ -440,13 +477,13 @@
float distance(Vector p1, Vector p2) {
if(p1.GetDim() != p2.GetDim()) return 0.0f;
- float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
- float s_mu_y = sin(mu_y);
- float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
- float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
- float n = GPS_A / w;
- float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
- float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
+ static float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
+ static float s_mu_y = sin(mu_y);
+ static float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
+ static float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
+ static float n = GPS_A / w;
+ static float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
+ static float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
return sqrt(d1 * d1 + d2 * d2);
}
@@ -486,6 +523,8 @@
gyro = raw_gyro;
press = raw_press;
+ vrt_acc = raw_acc * raw_g;
+
}
}