質問があったのでメモ

ヨー軸方向へのカルマンフィルタ実装方法です. ピッチ・ロール方向は加速度センサで重力加速度の方向を検出すれば 絶対角の計算が可能ですが, ヨー軸は重力加速度が検出できませんので 絶対角を地磁気(デジタルコンパス)から計算します.

しかしデジタルコンパスの追従性能が遅いので, 早いダイナミクスはジャイロセンサによる角速度の情報でカバーします.

毎度のことですが,間違いがあるかもしれません,あしからず

状態空間表現

まずはモデルを立てましょう. 角度を,角速度をとします. 単位はdegでもradでも統一していればOKです.

角速度と角度の関係はオイラー微分で

ですね.ここで, はサンプリング周期[sec],はステップ数です.

状態空間表現だと

となりますが, ここでとおけますね.

カルマンフィルタに適用するモデル

カルマンフィルタで考えるモデルは,

です. は,平均値が0,分散をそれぞれと仮定します.

実際ここでのは観測ノイズを表すので,センサの精度・分解能の2乗した値を設定します. 今回はデジタルコンパスの精度or分解能の大きい方の値を入れるといいかと.

は更新ノイズとか言われ,運動方程式に関する誤差の2乗値が入ります. 今回はジャイロセンサの精度or分解能の2乗値ですが, 厳密にはオイラー微分の誤差も含むので,もう少し大きい値になりますね. オイラー微分の定式化は誤差の大きい離散化なので, 本当に使う場合はもう少し制度の良い離散化を使う必要があります.

カルマンフィルタの手順

コンパスの角度データをcompass_yaw, ジャイロセンサの角速度データをjyro_yawとします.

void kf(double xhat,double px,double stp,double compass_yaw,double jyro_yaw){
  double q = 1, r = 1; //Q,R
  double a = 1, b = stp, c = 1; //A,b.C

  double x_ = a * xhat + b * jyro_yaw; // prior estimation of value
  double p1 = a * px * a + q ; // prior coveriance
  double p2 = c * p1 * c + r ; // posterior coveriance

  double kg = p1 * c / p2 ; // Kalman gain
  double e = compass_yaw - c * x_ ; //error between model & compass

  xhat = xhat + kg * e ; // estimating value for next step
  px =   ( 1 - kg * e ) * px ; // estimating coveriance for next step
}

xhat,pxはそれぞれ推定される状態変数とその分散になります. 宣言時には,xhatには絶対角の初期値を入れ,pxと同値を入れるか, 何回か実験してpxの収束先の値を得て,それを使うと良いと思います.

以上駆け足でしたがまとめてみました. てかwikipediaのまとめがよくまとまってるのでそれでいいです.