相補フィルタ

6軸センサ(ジャイロ3軸＋加速度3軸)で姿勢を推定する場合、ジャイロのみでも加速度のみでも姿勢角の推定はできますが、それぞれ一長一短があります。

ジャイロによる推定

• 角速度の積分で姿勢角を推定する。
• 長所：自身の並進運動の影響を受けない。
• 短所：積分によって誤差が蓄積される。

加速度による推定

• 重力加速度の方向で姿勢角を推定する。
• 長所：現在の値のみで計算できる。（誤差の蓄積が無い。）
• 短所：自身の運動による加速度が重畳され外乱となる。
  　　　ヨー軸の推定はできない。

すなわち、短期的にはジャイロによる推定が、長期的には加速度による推定が有利。
そこで、両者の特性を補い合う相補フィルタを用います。

相補フィルタの計算

ここでは、ピッチ軸の姿勢角推定について考えます。

ジャイロ G = (Gx, Gy, Gz), 加速度 A = (Ax, Ay, Az)
ジャイロによるピッチ角変化推定 Δθg = Gy × Δt
加速度によるピッチ角推定 θa = atan2(Ax, Az)
相補フィルタによるピッチ角推定 θ(n+1) = k×(θ(n)+Δθg) + (1-k)×θa
係数 k = 0.996くらい？

すなわち、短期的にはジャイロによる推定が支配的ですが、長期的には加速度による推定によって誤差の蓄積が補償されます。