目的

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

姿勢推定手法

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

参考文献

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

オイラー角度の計算

センサーの向き

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

加速度センサの正方向

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

地磁気センサ

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

角度算出

roll

pitch

yaw

カルマンフィルタ

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)