3軸リアクションホイール倒立振子を作るアドカレの20日目です。
前回（Day 19）の実験では、1軸のカルマンフィルタを用いてノイズとドリフトを除去することに成功しました。しかし、我々が開発を進めている「Cubli（3軸倒立振子）」は、辺や頂点でのバランス制御を行うため、3次元空間内で複雑な回転運動を行います。
従来のオイラー角（ロール・ピッチ・ヨー）による姿勢表現では、特定の角度（例：ピッチ角90度）で回転軸が重なり、自由度が喪失する 「ジンバルロック（Gimbal Lock）」 の問題が避けられません。この特異点は、制御不能や計算の発散を引き起こす致命的な要因となります。
そこで今回は、航空宇宙分野やロボティクスにおいて標準的に用いられる 「クォータニオン（四元数）」 を導入し、それを 「拡張カルマンフィルタ（Extended Kalman Filter: EKF）」 で推定するアルゴリズムをSTM32G474に実装します。
1. なぜ「拡張」カルマンフィルタなのか？
線形と非線形の壁
通常のカルマンフィルタ（線形カルマンフィルタ）は、対象とするシステムのダイナミクスが「線形」であることを前提としています。
すなわち、状態 $x$ の遷移や観測が以下の線形方程式で記述できる場合です。
$$x_{k} = A x_{k-1} + B u_{k-1}$$
しかし、3次元空間における回転運動は、クォータニオンの乗算や座標変換において三角関数（$\sin, \cos$）や非線形な積を含みます。これはシステム方程式が非線形関数 $f(x)$ となることを意味します。
$$x_{k} = f(x_{k-1}, u_{k-1})$$
このような非線形システムに対して、現在の推定値付近でテーラー展開を行い、1次の項まで近似（線形化）して扱う手法が 拡張カルマンフィルタ（EKF） です。
今回は、STM32G474が搭載するFPU（浮動小数点演算ユニット）と170MHzの演算能力を活かし、ヤコビ行列（偏微分行列）の計算を含むこの高度なアルゴリズムをリアルタイムで実行させます。
2. 数理モデル：クォータニオンによる姿勢推定
2.1 状態ベクトルの定義
姿勢を表す4つの要素を持つクォータニオン $q$ を状態ベクトル $x$ と定義します。
$$x = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T$$
ここで、$q_0$ はスカラー部、$q_1, q_2, q_3$ はベクトル部を表し、ノルム制約 $||q|| = 1$ を満たすものとします。
2.2 ステップ1：予測（Time Update）
ジャイロセンサから得られる角速度ベクトル $\omega = [\omega_x, \omega_y, \omega_z]^T$ を用いて、次の時間ステップのクォータニオンを予測します。
クォータニオンの時間微分方程式は、角速度ベクトルを用いて以下のように記述されます。
$$\dot{q} = \frac{1}{2} q \otimes \omega_{quat}$$
ここで $\omega_{quat} = [0, \omega_x, \omega_y, \omega_z]^T$ です。これを行列形式で書き下すと以下の線形微分方程式になります。
\begin{bmatrix}
\dot{q}_0 \\ \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3
\end{bmatrix}
=
\frac{1}{2}
\begin{bmatrix}
0 & -\omega_x & -\omega_y & -\omega_z \\
\omega_x & 0 & \omega_z & -\omega_y \\
\omega_y & -\omega_z & 0 & \omega_x \\
\omega_z & \omega_y & -\omega_x & 0
\end{bmatrix}
\begin{bmatrix}
q_0 \\ q_1 \\ q_2 \\ q_3
\end{bmatrix}
この右辺の角速度行列を $\Omega(\omega)$ と置きます。この行列は対角成分が0であり、反対称行列（歪対称行列：skew-symmetric matrix）に近い構造を持ちます。
離散時間（サンプリング周期 $\Delta t$）における状態遷移行列 $F_k$ は、以下のように近似できます。
$$F_k = I + \frac{\Delta t}{2} \Omega(\omega_k)$$
これを用いて、先見状態推定値 $\hat{x}{k|k-1}$ と先見誤差共分散行列 $P{k|k-1}$ を計算します。
\hat{x}_{k|k-1} = F_k \hat{x}_{k-1}
$$P_{k|k-1} = F_k P_{k-1} F_k^T + Q_k$$
2.3 ステップ2：更新（Measurement Update）
予測した姿勢の確からしさを、加速度センサから得られる重力ベクトルを用いて補正します。
加速度センサは、静止状態において重力加速度ベクトル $g = [0, 0, 1]^T$（正規化済み）を観測します。
現在の推定姿勢（クォータニオン）において、センサ座標系で観測されるはずの重力ベクトル $h(x)$ は、回転行列 $R(q)$ を用いて計算できます。
クォータニオンから回転行列への変換式より、推定重力ベクトル $h(x)$ は以下の非線形関数となります。
h(x) = \begin{bmatrix} 2(q_1 q_3 - q_0 q_2) \\ 2(q_0 q_1 + q_2 q_3) \\ q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix}
ここで、観測行列 $H_k$（ヤコビ行列）を求めるために、$h(x)$ を状態ベクトル $x$ で偏微分します。
H_k = \frac{\partial h}{\partial x} \bigg|_{\hat{x}_{k|k-1}}
具体的に計算すると、以下のような $3 \times 4$ 行列が得られます。
H_k = 2 \begin{bmatrix} -q_2 & q_3 & -q_0 & q_1 \\ q_1 & q_0 & q_3 & q_2 \\ q_0 & -q_1 & -q_2 & q_3 \end{bmatrix}
このヤコビ行列 $H_k$ を用いて、カルマンゲイン $K_k$ と状態の更新を行います。実際の加速度センサの観測値を $z_k = [a_x, a_y, a_z]^T$ とすると、
\begin{array}{@{}l@{}}
\text{(1) 観測残差（イノベーション)}
\quad y_k = z_k - h(\hat{x}_{k\mid k-1}) \\[4pt]
\text{(2) イノベーション共分散}
\quad S_k = H_k P_{k\mid k-1} H_k^\top + R \\[4pt]
\text{(3) カルマンゲイン}
\quad K_k = P_{k\mid k-1} H_k^\top S_k^{-1} \\[4pt]
\text{(4) 状態更新}
\quad \hat{x}_{k} = \hat{x}_{k\mid k-1} + K_k y_k \\[4pt]
\text{(5) 共分散更新}
\quad P_k = (I - K_k H_k) P_{k\mid k-1}
\end{array}
最後に、更新されたクォータニオン $\hat{x}_k$ のノルムが1になるように正規化を行います。
3. 実装：STM32へのC言語実装
外部の線形代数ライブラリ（Eigen等）に依存せず、C言語標準ライブラリのみで動作する軽量なEKFコードを実装しました。
STM32G474のFPUを有効化することで、これらの浮動小数点行列演算を高速に処理します。
以下は、更新ステップ（Measurement Update）の主要部分の抜粋です。
// EKF更新関数（一部抜粋）
void EKF_Update(EKF_t *filter, float gx, float gy, float gz, float ax, float ay, float az) {
// ローカル変数定義
float q0 = filter->q[0], q1 = filter->q[1], q2 = filter->q[2], q3 = filter->q[3];
float P[4][4];
// コピー
for(int i=0; i<4; i++) for(int j=0; j<4; j++) P[i][j] = filter->P[i][j];
// ===========================
// 1. 予測ステップ (Time Update)
// ===========================
// クォータニオンの時間発展: q_new = q + 0.5 * q * omega * dt
float hdt = 0.5f * EKF_DT;
float q0_dot = -q1*gx - q2*gy - q3*gz;
float q1_dot = q0*gx - q3*gy + q2*gz;
float q2_dot = q3*gx + q0*gy - q1*gz;
float q3_dot = -q2*gx + q1*gy + q0*gz;
float q0_pred = q0 + q0_dot * hdt;
float q1_pred = q1 + q1_dot * hdt;
float q2_pred = q2 + q2_dot * hdt;
float q3_pred = q3 + q3_dot * hdt;
// ヤコビアン F (状態遷移行列の線形化)
// F = I + 0.5 * Omega * dt
float F[4][4] = {
{1.0f, -hdt*gx, -hdt*gy, -hdt*gz},
{hdt*gx, 1.0f, hdt*gz, -hdt*gy},
{hdt*gy, -hdt*gz, 1.0f, hdt*gx},
{hdt*gz, hdt*gy, -hdt*gx, 1.0f}
};
// 誤差共分散の予測: P = F*P*F^T + Q
float FP[4][4] = {0}; // F*P
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
for(int k=0; k<4; k++) FP[i][j] += F[i][k] * P[k][j];
}
}
float P_pred[4][4] = {0}; // FP*F^T + Q
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
for(int k=0; k<4; k++) P_pred[i][j] += FP[i][k] * F[j][k]; // F^Tは添字入れ替え
}
P_pred[i][i] += filter->Q[i]; // Add Process Noise
}
// クォータニオン正規化 (予測値)
float norm_q = sqrtf(q0_pred*q0_pred + q1_pred*q1_pred + q2_pred*q2_pred + q3_pred*q3_pred);
if(norm_q < 1e-6f) return; // エラー回避
float inv_q = 1.0f / norm_q;
q0_pred *= inv_q; q1_pred *= inv_q; q2_pred *= inv_q; q3_pred *= inv_q;
// ===========================
// 2. 更新ステップ (Measurement Update)
// ===========================
// 加速度ベクトルの正規化
float norm_a = sqrtf(ax*ax + ay*ay + az*az);
if(norm_a < 1e-6f) return; // 自由落下などは無視
ax /= norm_a; ay /= norm_a; az /= norm_a;
// 推定された重力ベクトル (Rotation Matrixの第3列)
// h(x) = R(q)^T * [0 0 1]^T
float vx = 2.0f*(q1_pred*q3_pred - q0_pred*q2_pred);
float vy = 2.0f*(q0_pred*q1_pred + q2_pred*q3_pred);
float vz = q0_pred*q0_pred - q1_pred*q1_pred - q2_pred*q2_pred + q3_pred*q3_pred;
// 観測残差 y = z - h(x)
float y[3];
y[0] = ax - vx;
y[1] = ay - vy;
y[2] = az - vz;
// 観測行列 H (ヤコビアン)
// dh(x)/dq
float H[3][4] = {
{-2.0f*q2_pred, 2.0f*q3_pred, -2.0f*q0_pred, 2.0f*q1_pred},
{ 2.0f*q1_pred, 2.0f*q0_pred, 2.0f*q3_pred, 2.0f*q2_pred},
{ 2.0f*q0_pred, -2.0f*q1_pred, -2.0f*q2_pred, 2.0f*q3_pred}
};
// S = H*P*H^T + R (観測残差共分散)
float HP[3][4] = {0}; // H*P
for(int i=0; i<3; i++) {
for(int j=0; j<4; j++) {
for(int k=0; k<4; k++) HP[i][j] += H[i][k] * P_pred[k][j];
}
}
float S[3][3] = {0}; // HP*H^T + R
for(int i=0; i<3; i++) {
for(int j=0; j<3; j++) {
for(int k=0; k<4; k++) S[i][j] += HP[i][k] * H[j][k];
}
S[i][i] += filter->R[i]; // Add Measurement Noise
}
// カルマンゲイン K = P*H^T*S^-1
// Sは3x3なので逆行列計算が必要
// 簡易的に行列式から逆行列を求める
float det = S[0][0]*(S[1][1]*S[2][2] - S[1][2]*S[2][1]) -
S[0][1]*(S[1][0]*S[2][2] - S[1][2]*S[2][0]) +
S[0][2]*(S[1][0]*S[2][1] - S[1][1]*S[2][0]);
if (fabsf(det) < 1e-6f) return; // 逆行列なし
float invDet = 1.0f / det;
float S_inv[3][3];
S_inv[0][0] = (S[1][1]*S[2][2] - S[1][2]*S[2][1]) * invDet;
S_inv[0][1] = (S[0][2]*S[2][1] - S[0][1]*S[2][2]) * invDet;
S_inv[0][2] = (S[0][1]*S[1][2] - S[0][2]*S[1][1]) * invDet;
S_inv[1][0] = (S[1][2]*S[2][0] - S[1][0]*S[2][2]) * invDet;
S_inv[1][1] = (S[0][0]*S[2][2] - S[0][2]*S[2][0]) * invDet;
S_inv[1][2] = (S[0][2]*S[1][0] - S[0][0]*S[1][2]) * invDet;
S_inv[2][0] = (S[1][0]*S[2][1] - S[1][1]*S[2][0]) * invDet;
S_inv[2][1] = (S[0][1]*S[2][0] - S[0][0]*S[2][1]) * invDet;
S_inv[2][2] = (S[0][0]*S[1][1] - S[0][1]*S[1][0]) * invDet;
float K[4][3] = {0}; // P*H^T * S_inv
float PHT[4][3] = {0}; // P*H^T
for(int i=0; i<4; i++) {
for(int j=0; j<3; j++) {
for(int k=0; k<4; k++) PHT[i][j] += P_pred[i][k] * H[j][k];
}
}
for(int i=0; i<4; i++) {
for(int j=0; j<3; j++) {
for(int k=0; k<3; k++) K[i][j] += PHT[i][k] * S_inv[k][j];
}
}
// 状態更新: q = q_pred + K*y
float dq[4] = {0};
for(int i=0; i<4; i++) {
for(int j=0; j<3; j++) dq[i] += K[i][j] * y[j];
}
filter->q[0] = q0_pred + dq[0];
filter->q[1] = q1_pred + dq[1];
filter->q[2] = q2_pred + dq[2];
filter->q[3] = q3_pred + dq[3];
// 正規化
EKF_Normalize_Q(filter->q);
// 誤差共分散更新: P = (I - K*H)*P_pred
// ※数値安定性のため P = P - K*H*P を使用
float KHP[4][4] = {0};
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
for(int k=0; k<3; k++) KHP[i][j] += K[i][k] * HP[k][j]; // HPはH*P_pred済み
}
}
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) filter->P[i][j] = P_pred[i][j] - KHP[i][j];
}
// ===========================
// 3. オイラー角変換 (Roll, Pitch, Yaw)
// ===========================
q0 = filter->q[0]; q1 = filter->q[1]; q2 = filter->q[2]; q3 = filter->q[3];
// Roll (X軸)
filter->Roll = atan2f(2.0f*(q0*q1 + q2*q3), 1.0f - 2.0f*(q1*q1 + q2*q2)) * 57.29578f;
// Pitch (Y軸)
float sinp = 2.0f*(q0*q2 - q1*q3);
if(sinp > 1.0f) sinp = 1.0f;
if(sinp < -1.0f) sinp = -1.0f;
filter->Pitch = asinf(sinp) * 57.29578f;
// Yaw (Z軸)
filter->Yaw = atan2f(2.0f*(q0*q3 + q1*q2), 1.0f - 2.0f*(q2*q2 + q3*q3)) * 57.29578f;
}
この実装において、逆行列 の計算は 行列に対して解析的に（クラメルの公式等を用いて）行うことで、計算コストを最小限に抑えています。
4. 検証結果：静特性と動特性
実装したEKFアルゴリズムの性能を評価するため、STM32からシリアル通信で出力された推定角度（クォータニオンからオイラー角へ変換後のピッチ角）をグラフ化しました。比較対象として、加速度センサからの算出値（Accel Only）、ジャイロセンサの積分値（Gyro Only）を併記しています。
① 静止時のドリフト抑制
センサを水平面に静置した状態でのデータです。
- Accel Only (青): 真値付近（約-0.2度）を示していますが、センサノイズにより常に振動しています。
- Gyro Only (橙): 時間経過とともに、バイアス誤差の影響で一直線に値がズレていく「ドリフト」が発生しています。
- EKF (緑): 極めて安定的です。加速度センサの情報を参照することでドリフトを完全に抑制しつつ、カルマンフィルタの効果でノイズも除去されています。
② 動的な追従性能
センサを手で持ち、 程度の範囲で激しく振った際の応答波形です。EKFの真価はこの動的特性に現れます。
- Gyro Only (橙): 実際の傾き（約60度）に対し、40度付近までしか値が出ていません。これは積分誤差に加え、感度誤差（スケールファクタ誤差）の影響で角度を過小評価しているためです。
- EKF (緑): 加速度センサ（青）が示す物理的な真値（60度）までしっかりと追従しています。さらに特筆すべきは、位相遅れ（Time Delay）がほぼゼロである点です。通常のローパスフィルタでは避けられない遅延がなく、急激な切り返し動作にも瞬時に反応しています。
5. まとめ
本日は、Cubliのような3次元空間での姿勢制御に不可欠な 「拡張カルマンフィルタ（EKF）」 を実装しました。
- クォータニオンの採用: ジンバルロックの問題を数学的に回避し、全方位で安定した姿勢計算を実現しました。
- 非線形フィルタリング: 拡張カルマンフィルタによる線形化プロセスを通じて、ジャイロの「高速応答性」と加速度の「絶対安定性」を最適に統合（センサフュージョン）しました。
- 実機検証: 実験データより、「ノイズ除去」「ドリフト抑制」「遅延なし」 という、制御系にとって理想的な角度データが得られていることを確認しました。
STM32G474のようなFPU搭載マイコンであれば、今回のような複雑な行列演算も十分な更新レート（1kHz以上）で実行可能です。
次回（Day 21）は、もう一つの主要な姿勢推定アルゴリズムであり、より計算負荷の低い 「Madgwickフィルタ」 を実装します。今回作成したEKFと徹底比較（ベンチマーク対決）を行い、「処理速度」と「精度」のトレードオフを検証した上で、本プロジェクトにおける最終的な制御アルゴリズムを決定します。
アドベントカレンダー参加中
STM32×AIで「3軸倒立振子」を作る25日間（ひとりアドカレ）Advent Calendar 2025