C++で拡張カルマンフィルタ(EKF)を実装する

Abstract

• C++の勉強として加速度と角速度の時系列データから拡張カルマンフィルタを使って姿勢を計算するプログラムを書いてみる.
• 具体的なプログラムの流れは以下のものを考えている.

Version

• 2016/07/06 21:00時点 : 立式完了
• 2016/07/07 18:00時点 : Eigenのダウンロードと使用方法の勉強
• 2016/07/08 12:00時点 : ひと通りコーディング完了, 値はすぐ発散する. バグ修正開始
• 2016/07/11 21:00時点 : それらしい値を吐くようになった. これで一応verion1.0とする. 今後もっとC++っぽくしていきたい.

Index

• How to Sample Data
• Coordinate System
• Attitude Expression
• Angular Velocity
• Gravity Vector in Object coordinate system
• Discretization of Quaternion
• Equation of State
• Jacobian Matrix
• EKF Algorithm and Initial Values
• Eigen
• References

How to Sample Data

• 加速度と角速度のデータは以下のシステムを組んで取得した. 取得データは「data」に, 取得するのに使ったマイコンのプログラムは「sampling program」に置く.

Coordinate System

• 座標系と方向余弦行列ついては以下のように設定する.

Attitude Expression

• DCMとオイラー角の関係, DCMとクオータニオンの関係は以下のように定義する.

Angular Velocity

• 角速度は物体座標系で定義し, DCM とクオータニオンの時間微分との関係は以下の通りである.

Gravity Vector in Object coordinate system

• カルマンフィルタで加速度センサの値を出力方程式で用いるためにクオータニオン(状態変数にする予定)と重力加速度の関係を導出しておく.

Discretization of Quaternion

• カルマンフィルタで状態方程式を立てるためにクオータニオン(状態変数にする予定)を離散化しておく.

Equation of State

• カルマンフィルタを適用させる状態方程式・観測方程式は以下のようになる. 観測方程式が非線形となっているため今回はEKFを使う.

Jacobian Matrix

• EKFを回すために状態方程式・観測方程式それぞれのヤコビアン行列を求めた.

EKF Algorithm and Initial Values

• EKFのアルゴリズムとその初期値を以下に示す.

Eigen

• プログラム作成にあたって, Eigenという行列のライブラリを用いた. 忘れないように, 使い方をまとめておく.

#include <iostreasm>
#include <Eigen/core> // 基本機能を使うため
#include <Eigen/LU> // 逆行列を使うため
　
int main(){
　
	// Matrix<typename Scalar, int RowsAtCompileTime, int ColsAtCompileTime>
	Eigen::Matrix<double, 3, 3> A; //定義
	Eigen::Matrix<double, 3, 3> B;
	Eigen::Matrix<double, 3, 3> C;
	Eigen::Matrix<double, 3, 3> D;
	Eigen::Matrix<double, 3, 3> E;
　
	A << 1.0, 0.0, 0.0, // 代入
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0;
	B.setZero(); // 0で埋める
　
	C = A * B; // 掛け算
	D = A.inverse(); // 逆行列
	E = A.transpose(); // 転置
　
	std::cout << E << std::endl; // シフト演算子も定義されている.
　
	double val = A(0,0); // 要素へのアクセス
	std::cout << val << std::endl; // 1.0
　
	return 0;
}

References

• 座標系の取り方や方向余弦行列DCM, クオータニオンについて以下の資料を参考にした.
  [1] 航空宇宙技術研究所資料 : クオータニオンとオイラー角によるキネマティックス表現の比較について
• 行列のライブラリEigen
  [2] Eigen, The Matrix class
• Eigenの基本的な使い方が日本語でまとめられているページ
  [3] でらうま倶楽部, Eigen - C++で使える線形代数ライブラリ