drone
KalmanFilter
estimation
姿勢推定
Attitude

目的

ドローンの姿勢制御がしたい
だが姿勢が分からない

姿勢推定手法

  • 加速度センサの傾き等からオイラー角計算&カルマンフィルタ
  • マドウィックフィルタ
    • Quaternionで記述するので特異点に陥らない

参考文献

いつも参考にさせて頂いているEnigmaブログです
スマホの姿勢推定

オイラー角度の計算

センサーの向き

全てのセンサの向きをそろえること
image.png

加速度センサの正方向

+1gを読んでいる方向が正方向
x, y, z で繰り返す

地磁気センサ

北半球では北&下方向に1軸を向けて最大値になる方が正,逆に向けて最小値になる方向が負
x, y, z軸周りで繰り返す

角度算出

roll

image.png

pitch

image.png

yaw

image.png

カルマンフィルタ

A = 
\begin{bmatrix}
1 & 0 & 0 & Ts & 0 & 0 \\
0 & 1 & 0 & 0 & Ts & 0  \\
0 & 0 & 1 & 0 & 0 & Ts  \\
0 & 0 & 0 & 1 & 0 & 0  \\
0 & 0 & 0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 & 0 & 1  \\ 
\end{bmatrix}
状態量x=
\begin{bmatrix}
phi \\
theta \\
psi \\
p \\
q \\ 
r \\
\end{bmatrix}

パラメータは
Ts = 1/400
Q = 0.01^2*eye(6)
R = diag([2 2 10 1 1 10])

観測できるのは
角度は先ほど求めた式から,角速度はGyroから得られる.
そのため観測行列は
C = eye(6)