UKFによる加速度・角速度を使った姿勢推定

[:ja]IMUの姿勢推定がちょっと必要になりそうな気配があったので，試しにスマートフォンの姿勢を加速度・角速度からUnscented Kalman Filterを使って姿勢を推定するプログラムを作成しました．スマートフォンからPCに加速度・角速度データを送信し，Unscented Kalman Filterを使い，PC側で姿勢を推定・表示しています．

実装には以下のサイト・文献を参考にさせて頂きました．
UKFって何？，Cross Validated Unscented Kalman filter-negative covariance matrix
クォータニオン計算便利ノート，kuroiwa公開用　拡張カルマンフィルタ

推定手順を残しておこうと思いますが，部分部分で間違っている可能性はあるので参考にする場合は注意してください．
まず，姿勢を表すクォータニオンを推定する状態ベクトルとし，センサから得られた角速度を元に現時刻の状態を推定します．状態変数はクォータニオンの各成分を持つ次の様なベクトルです．

${\bold x} = \left[ q_w, q_x, q_y, q_z \right]^T$

センサから得られる角速度 $\omega_*$ をシステムへの入力とし，kuroiwa公開用に従うと，カルマンフィルタの予測ステップに使うシステム方程式は次のようになります．ここで右辺はクォータニオン積であることに注意です．

${\bold x}_{t+1}= {\bold x}_t \left( 1, \omega_x \frac{dt}{2}, \omega_y \frac{dt}{2}, \omega_z \frac{dt}{2} \right)$

次に，修正ステップに用いる観測はセンサの加速度とします．（追記：観測を3次元の加速度ベクトルとしていたのですが観測方程式との整合性をとるためにクォータニオンに変更しました．）

${\bold z} = \left[1, a_x, a_y, a_z \right]^T$

センサの移動を考慮しなければ，センサには重力加速度のみがかかっていると考えられるので，観測方程式は重力加速度ベクトルを現在のクォータニオンで回転させて次のようになります．これもクォータニオン積です．

${\bold g} = \left[ 1, 0, 0, -9.80665 \right] \\{\bold z}_t = {\bold x}_t^* {\bold g} {\bold x}_t$

システム方程式と観測方程式が決まったので，あとはカルマンフィルタで状態推定を行うだけです．システムのヤコビ行列を求めたくなかったので，ここでは Unscented Kalman Filterを用いました．
推定中に計算誤差が蓄積して誤差共分散行列が正定値行列でなくなってしまい，Unscented Transformのシグマ点を求める際にコレスキー分解に失敗することがあったのでこのページを参考に次のような処理で，共分散行列を固有値分解した後に0以下の固有値を微小な正の値に変更して再構築することで正定値行列にしています．

const double eps = 0.0001;
Eigen::MatrixXf cov;    // covariance matrix
Eigen::EigenSolver<Eigen::MatrixXf> solver(cov);
MatrixXt D = solver.pseudoEigenvalueMatrix();
MatrixXt V = solver.pseudoEigenvectors();
for (int i = 0; i < D.rows(); i++) {
	if (D(i, i) < eps) {
		D(i, i) = eps;
	}
}
cov = V * D * V.inverse();

試した感じでは，まあまあ正確に姿勢推定できている感じですね．もちろん重力方向の回転誤差は修正できないのでそこだけは推定ドリフトが起きますがそこまで酷くはないようでした．地磁気ベクトルなんかも使ってやればそれも修正できそうですが，地磁気センサは結構あてにならないことがあるのでなるべくなら使いたくないですね．[:en]IMUの姿勢推定がちょっと必要になりそうな気配があったので，試しにスマートフォンの姿勢を加速度・角速度からUnscented Kalman Filterを使って姿勢を推定するプログラムを作成しました．スマートフォンからPCに加速度・角速度データを送信し，Unscented Kalman Filterを使い，PC側で姿勢を推定・表示しています．

${\bold x} = \left[ q_w, q_x, q_y, q_z \right]^T$

${\bold x}_{t+1}= {\bold x}_t \left( 1, \omega_x \frac{dt}{2}, \omega_y \frac{dt}{2}, \omega_z \frac{dt}{2} \right)$

${\bold z} = \left[1, a_x, a_y, a_z \right]^T$

${\bold g} = \left[ 1, 0, 0, -9.80665 \right] \\{\bold z}_t = {\bold x}_t^* {\bold g} {\bold x}_t$

const double eps = 0.0001;
Eigen::MatrixXf cov;    // covariance matrix
Eigen::EigenSolver<Eigen::MatrixXf> solver(cov);
MatrixXt D = solver.pseudoEigenvalueMatrix();
MatrixXt V = solver.pseudoEigenvectors();
for (int i = 0; i < D.rows(); i++) {
	if (D(i, i) < eps) {
		D(i, i) = eps;
	}
}
cov = V * D * V.inverse();

tetro

UKFによる加速度・角速度を使った姿勢推定

関連

コメントを残すコメントをキャンセル

共有:

関連

コメントを残す コメントをキャンセル

コメントを残すコメントをキャンセル