의 목적

드론 자세를 컨트롤하고 싶어요.
자세를 몰라요.

자세 추정 수법

가속도 센서의 사율 등에 따라 오라각 & 칼만 필터

계산

마비크 필터

Quaternion으로 기술하기 때문에 특이점에 빠지지 않는다

참고 문헌

자꾸 참고하라고블로그 버스
핸드폰 자세 추측

오라 각도 계산하기

센서 방향

모든 센서의 방향 정렬하기

가속도 센서의 정방향

+1g 읽기 방향이 양의 방향
x, y,z로 반복

지자기 센서

북반구는 1축을 북쪽 & 아래 방향으로 최대치가 되는 쪽을 플러스로 하고 반대로 최소치가 되는 쪽을 마이너스로 한다
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)