Madgwick filter(sensor fusion)のペーパーmadgwick_internal_report.pdfを再読してのメモ. 完全に自己流の理解です. madgwick_internal_reportは

Open source IMU and AHRS algorithms

にあります.

Quaternionについてはわかりやすい解説がたくさんあるので詳しいことはパス.

ただ次のことに注意:

絶対値1のQuaternion q, q'を3次元球面上の点だと思うとq'=-qでない限り

(t q + (1-t) q')/|t q + (1-t) q'|

where 0 <= t <= 1 が球面上q,q'間の最短コース（測地線)になる.　2次元球面上の大円と同じ感じ.

で、ノイズのこととかいったん忘れて

• ジャイロで角速度がわかるので微小時間をかけると微小角変位つまり微小回転がわかる
• それを現在までの回転に続けると微小時間後の回転q'が推定できる
• 一方初期の方向から回転で得られた方向と加速度計やコンパスでわかる現在の方向の差が最小になるような回転qを勾配降下法で探す
• これらの回転の推計（あまり変わらないに違いない)をQuaternionで表しておいてそれらを結ぶ（短い）測地線上に最良の答えがあるはずだと考える
• t q の'divergence'と (1-t) q'の'convergence'が一致する場合がいいはず. 要するにそれぞれのずれていき方がキャンセルする場合がいいということだと理解した 　相補型フィルターという考え方?（Madgwickのinternal-report paperではtじゃなくてγを使って式が書かれている. 式番号 36)
• さらに近似+磁気擾乱+ジャイロのオフセットのドリフトなどを考えて最終的な計算式をゲット