目的
ドローンの姿勢制御がしたい
だが姿勢が分からない
姿勢推定手法
- 加速度センサの傾き等からオイラー角計算＆カルマンフィルタ
- マドウィックフィルタ
- 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)