칼만 필터는 어렵지 않아
이 책은 칼만 필터 자체가 어려운 것이 아니라 설명하는 방식이 불친절해 배우기 어려웠다는 문제의식에서 출발합니다. 그래서 수학적인 유도와 증명을 앞세우는 대신 칼만 필터의 핵심 알고리
book.naver.com
기울기 자세 측정하기
가속도계와 자이로스코프로 수평 자세(roll, pitch)를 찾아내는 간단한 항법 문제를 다룬다.
1. 관성항법 센서
2. 자이로를 이용하여 자세 결정하기
3. 가속도계를 이용하여 자세 결정하기
4. 센서 융합을 통해 자세 결정하기
5. 가속도계를 이용하여 정밀 자세 결정하기
4. 센서 융합을 통해 자세 결정하기
4.1 자이로와 가속도계는 상호보완 관계
자이로 구한 자세는 자세 변화를 잘 감지하지만, 시간이 지남에 따라 오차가 누적되어 발산하는 문제가 있다.
가속도계로 구한 자세는 시간이 지나도 그 오차가 커지지 않고 일정 범위로 제한되는 장점이 있다.
즉 단기적으로는 자이로 자세가 더 낫지만, 중장기적으로는 가속도 자세가 더 좋다.
이러한 특성을 바탕으로 두 센서를 융합하여 단점을 보완한다.
가속도계로 결정한 자세가 칼만 필터의 측정값이 되고
칼만 필터는 이 값을 자이로의 각속도를 적분하여 계산한 자세와 비교하여 오차를 보정한다.
4.2 자이로와 가속도계의 센서 융합에 필요한 시스템 모델
칼만 필터를 적용해 센서 융합을 하므로 시스템 모델 변수를 찾아야 한다.
오일러 각을 상태 변수로
우리가 관심 있는 물리량은 자세이다. 따라서 자세를 상태 변수로 잡는다.
$$ x = \begin{Bmatrix} \phi \\ \theta \\ \psi \end{Bmatrix} $$
이제 시스템 모델을 유도해보자.
앞에서 나온 자이로 각속도와 오일러 각도 사이의 관계식이다.
$$ \begin{Bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{Bmatrix} = \begin{bmatrix} 1 & \sin \phi\tan \theta & \cos \phi \tan \theta \\ 0 & \cos \phi & -\sin \phi \\ 0 & \sin \phi / \cos \theta & \cos \phi / \cos \theta \end{bmatrix} \begin{Bmatrix} p \\ q \\ r \end{Bmatrix}$$
시스템 모델이 되려면 다음과 같은 형태를 갖춰야 하는데 위 식은 이렇게 표현할 수가 없다.
$$ x_{k+1} = Ax_k + w_k \iff \begin{Bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{Bmatrix} = \begin{bmatrix} \qquad \qquad \\ \bullet \\ \qquad \qquad \\ \end{bmatrix} \begin{Bmatrix} \phi \\ \theta \\ \psi \end{Bmatrix}$$
행렬 안에 있는 오일러 각도를 밖으로 빼낼 방법이 없기 때문이다.
이런 경우에는 칼만 필터를 쓰지 못한다.
쿼터니언을 상태 변수로
오일러 각으로는 칼만 필터를 적용하지 못하기 때문에 쿼터니언을 상태 변수로 잡는다.
$$ x=\begin{Bmatrix} q1 \\ q2 \\ q3\\ q4 \end{Bmatrix}$$
쿼터니언과 각속도의 관계는 이렇다.
$$ \begin{Bmatrix} \dot{q1} \\ \dot{q2} \\ \dot{q3} \\ \dot{q4} \end{Bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{bmatrix} \begin{Bmatrix} q1 \\ q2 \\ q3 \\ q4 \end{Bmatrix}$$
위 식은 칼만 필터의 시스템 모델에 필요한 요구조건을 만족한다.
이것을 이산 시스템으로 바꾸면, 다음과 같은 시스템 모델을 얻을 수 있다.
$$ \begin{Bmatrix} q1 \\ q2 \\ q3 \\ q4 \end{Bmatrix}_{k+1} = \left( I + \Delta t \cdot \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{bmatrix} \right) \begin{Bmatrix} q1 \\ q2 \\ q3 \\ q4 \end{Bmatrix}_k$$
시스템 모델의 행렬 A. 각속도에 따라 행렬 A가 바뀐다. ($I$는 단위 행렬이다.)
$$ A = I + \Delta t \cdot \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{bmatrix} $$
측정 관계식
이제 측정 관계식을 유도해보자.
앞에서 가속도로 계산한 오일러 각도를 측정값으로 사용한다고 이야기 했는데
$$ \phi = \sin^{-1} (\frac{-f_y}{g\cos \theta}) $$
$$ \theta = \sin^{-1} (\frac{f_x}{g})$$
시스템 모델의 상태 변수가 쿼터니언이기 때문에 측정값을 쿼터니언으로 바꿔줘야 한다.
오일러 각도($\phi, \theta, \psi$)를 쿼터니언($q1, q2, q3, q4$)으로 바꾸는 공식
$$ \begin{Bmatrix} q1 \\ q2 \\ q3 \\ q4 \end{Bmatrix} = \begin{Bmatrix} \cos \frac{\phi}{2} \cos \frac{\theta}{2} \cos \frac{\psi}{2} + \sin \frac{\phi}{2} \sin \frac{\theta}{2} \sin \frac{\psi}{2} \\ \sin \frac{\phi}{2} \cos \frac{\theta}{2} \cos \frac{\psi}{2} - \cos \frac{\phi}{2} \sin \frac{\theta}{2} \sin \frac{\psi}{2} \\ \cos \frac{\phi}{2} \sin \frac{\theta}{2} \cos \frac{\psi}{2} + \sin \frac{\phi}{2} \cos \frac{\theta}{2} \sin \frac{\psi}{2} \\ \cos \frac{\phi}{2} \cos \frac{\theta}{2} \sin \frac{\psi}{2} - \sin \frac{\phi}{2} \sin \frac{\theta}{2} \cos \frac{\psi}{2} \end{Bmatrix} $$
가속도계로 결정한 자세를 위의 식에 대입하면 쿼터니언으로 표현된 측정값을 얻을 수 있다.
이 쿼터니언이 칼만 필터의 측정값($z_k$)이 된다.
모든 상태 변수(쿼터니언)가 측정되므로 출력 행렬 $H$ ($ z_k = Hx_k + v_k $) 는 단위 행렬이 된다.
$$ H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$
4.3 시스템 모델 정리
칼만 필터 알고리즘을 적용하기 위해 알아야 하는 시스템 모델 변수는 $A, H, Q, R$이다.
$$ A = I + \Delta t \cdot \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{bmatrix}\qquad H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$ $ $
$$ Q = \begin{bmatrix} 0.0001 & 0 & 0 & 0 \\ 0 & 0.0001 & 0 & 0 \\ 0 & 0 & 0.0001 & 0 \\ 0 & 0 & 0 & 0.0001 \end{bmatrix} \qquad R = \begin{bmatrix} 10& 0 & 0 & 0 \\ 0 & 10& 0 & 0 \\ 0 & 0 & 10& 0 \\ 0 & 0 & 0 & 10\end{bmatrix}$$
$A, H$는 위에서 구한 값이고
잡음의 공분산 행렬 $Q, R$은 시스템의 신호 특성과 관련 있는 값이라, 이론적으로는 구하기 어렵고 실제 데이터를 분석해봐야한다.
보통은 이 행렬을 설계인자로 보고, 이런 저런 값을 넣어 성능 변화의 추이를 보면서 결정한다. 책에서는 위 값으로 선정했다.
4.4 칼만 필터 적용
상태 변수와 오차 공분산 행렬의 초깃값은 다음과 같이 선정했다.
상태 변수 초깃값의 물리적인 의미는 오일러 각도가 모두 0이라는 뜻이다.
$$ \widehat{x}_0^- = \begin{Bmatrix} q1 \\ q2 \\ q3 \\ q4 \end{Bmatrix} = \begin{Bmatrix} 1 \\ 0 \\ 0 \\ 0 \end{Bmatrix} \qquad P_0^- = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$
1. 시스템 모델 변수 정의 $A, H, Q, R$
$$ A = I + \Delta t \cdot \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{bmatrix}\qquad H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$ $ $
$$ Q = \begin{bmatrix} 0.0001 & 0 & 0 & 0 \\ 0 & 0.0001 & 0 & 0 \\ 0 & 0 & 0.0001 & 0 \\ 0 & 0 & 0 & 0.0001 \end{bmatrix} \qquad R = \begin{bmatrix} 10& 0 & 0 & 0 \\ 0 & 10& 0 & 0 \\ 0 & 0 & 10& 0 \\ 0 & 0 & 0 & 10\end{bmatrix}$$
2. 가속도 값으로 오일러 각도 계산
$$ \phi = \sin^{-1} (\frac{-f_y}{g\cos \theta}) $$
$$ \theta = \sin^{-1} (\frac{f_x}{g})$$
3. 오일러 각도 쿼터니언 각도로 변환
$$ \begin{Bmatrix} q1 \\ q2 \\ q3 \\ q4 \end{Bmatrix} = \begin{Bmatrix} \cos \frac{\phi}{2} \cos \frac{\theta}{2} \cos \frac{\psi}{2} + \sin \frac{\phi}{2} \sin \frac{\theta}{2} \sin \frac{\psi}{2} \\ \sin \frac{\phi}{2} \cos \frac{\theta}{2} \cos \frac{\psi}{2} - \cos \frac{\phi}{2} \sin \frac{\theta}{2} \sin \frac{\psi}{2} \\ \cos \frac{\phi}{2} \sin \frac{\theta}{2} \cos \frac{\psi}{2} + \sin \frac{\phi}{2} \cos \frac{\theta}{2} \sin \frac{\psi}{2} \\ \cos \frac{\phi}{2} \cos \frac{\theta}{2} \sin \frac{\psi}{2} - \sin \frac{\phi}{2} \sin \frac{\theta}{2} \cos \frac{\psi}{2} \end{Bmatrix} $$
4. 칼만 필터 알고리즘 적용
$$ \widehat{x}_k^- = A\widehat{x}_{k-1} $$
$$ P_k^- = AP_{k-1}A^T + Q $$
$$ K_k = P_k^-H^T(HP_k^-H^T + R)^{-1} $$
$$ \widehat{x}_k = \widehat{x}_k^- + K_k(z_k - H\widehat{x}_k^-) $$
$$ P_k = P_k^- - K_kHP_k^- $$
5. 쿼터니언 추정값($\widehat{x}_k$)을 오일러 각도로 변환
4.5 적용 결과
자세오차가 거의 없고 자이로의 누적오차도 사라졌다.
(책에 있는 실습예제의 결과. )