확장 칼만 필터로 만드는 AHRS

AHRS

AHRS(Attitude and Heading Reference System)는 현재 자세와 방위를 측정하는 시스템입니다. 주로 드론, 로봇, 자동차 등에서 사용합니다. AHRS에서 사용하는 센서로는 가속도계, 자이로스코프, 지자기계가 있습니다. 이 글에서는 드론에 적용하는 경우를 위주로, 세 센서값과 칼만 필터를 사용하여 AHRS를 구현해보겠습니다.

좌표계의 변환

관성좌표계와 동체고정좌표계

관성좌표계(inertial frame)는 뉴턴의 운동 법칙을 따르는 좌표계이며, 가속하거나 회전하지 않습니다. 드론은 어딘가에 부딪히지 않는 이상 대부분 가속도를 무시할 수 있으므로, 드론에 대한 관성좌표계는 드론과 같이 평행이동하지만 회전하지는 않는 좌표계로 생각할 수 있습니다. 관성좌표계는 일반적으로 연직 아래 방향을 $z$축으로 정의하며 $x$축과 $y$축은 북쪽과 동쪽으로 두거나 초기 상태에 따라 정의할 수도 있습니다.

동체고정좌표계(body-fixed frame) 또는 간단히 동체좌표계(body frame)은 드론 동체와 같이 움직이는 좌표계입니다. 위에서 언급한 세 센서들은 모두 동체에 고정되어 있으니 이들의 출력값 또한 동체좌표계에 대한 값입니다. 보통 항공기에서는 동체 중심에서 앞쪽을 $x$축으로, 오른쪽을 $y$축으로, 아래쪽을 $z$축으로 정의합니다. 이때 $x$축에 대한 회전을 롤(roll), $y$축에 대한 회전을 피치(pitch), $z$축에 대한 회전을 요(yaw)라고 부릅니다.

AHRS의 목표는 관성좌표계에 대해 동체좌표계가 얼마나 회전했는지를 알아내는 것입니다. 이를 위해서는 관성좌표계와 동체좌표계 사이의 좌표계 변환이 필수적입니다.

오일러각

오일러각(Euler angle)은 세 각도 $\phi$, $\theta$, $\psi$를 이용해 회전을 표현하는 방법입니다. 먼저 관성좌표계 $I$의 $z$축을 중심으로 $\psi$만큼 회전한 좌표계 $I’$을 정의합니다. 다시 $I’$을 그 $y$축에 대해 $\theta$만큼 회전한 좌표계 $I’‘$을 정의하고, 마지막으로 $I’‘$을 그 $x$축에 대해 $\phi$만큼 회전한 좌표계가 바로 동체좌표계 $B$입니다. 정의를 보면 알겠지만 $\phi$, $\theta$, $\psi$가 각각 롤, 피치, 요에 대한 회전각에 해당합니다.

어떤 벡터 $\v$의 관성좌표계에서의 좌표를 $\v_I$, 동체좌표계에서의 좌표를 $\v_B$라고 하면 이 둘 사이의 변환은 어떻게 될까요? 일단 $\v_I$를 $\v_{I’}$으로 바꾸려면 $z$축을 중심으로 $\psi$만큼 회전해야 하므로

\[ \v_{I'} = \begin{bmatrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \v_I \]

입니다. 좌표계를 회전하는 것은 정확히 반대 방향으로 같은 각도만큼 벡터를 회전하는 것과 동일하다는 사실을 기억합시다. 마찬가지로 $\v_{I’}$을 $\v_{I’’}$으로, $\v_{I’’}$을 $\v_B$로 변환하면

\begin{gather*} \v_{I''} = \begin{bmatrix} \cos\theta & 0 & -\sin\theta \\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta \end{bmatrix} \v_{I'} \\ \v_B = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & \sin\phi \\ 0 & -\sin\phi & \cos\phi \end{bmatrix} \v_{I''} \end{gather*}

따라서

\begin{equation} \begin{aligned} \v_B &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & \sin\phi \\ 0 & -\sin\phi & \cos\phi \end{bmatrix} \begin{bmatrix} \cos\theta & 0 & -\sin\theta \\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta \end{bmatrix} \begin{bmatrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \v_I \\ &= \begin{bmatrix} \cos\theta\cos\psi & \cos\theta\sin\psi & -\sin\theta \\ \sin\phi\sin\theta\cos\psi-\cos\phi\sin\psi & \sin\phi\sin\theta\sin\psi+\cos\phi\cos\psi & \sin\phi\cos\theta \\ \cos\phi\sin\theta\cos\psi+\sin\phi\sin\psi & \cos\phi\sin\theta\sin\psi-\sin\phi\cos\psi & \cos\phi\cos\theta \end{bmatrix} \v_I \end{aligned} \label{eq:euler} \end{equation}

오일러각은 롤, 피치, 요 각도를 직접 다룬다는 점에서 직관적이지만 $\theta$가 $\pm$90$^\circ$인 경우에 $\phi$와 $\psi$가 한 값으로 정의되지 않는 짐벌락(gimbal lock) 현상이 발생한다는 문제점이 있습니다.

사원수

사원수(quaternion, 쿼터니언)은 복소수의 확장으로, 행렬 연산 없이 3차원 회전을 표현할 수 있습니다. 사원수는 실수부와 서로 다른 세 허수 단위 $i$, $j$, $k$를 갖는 허수부로 정의됩니다.

\[ q = q_w + q_x i + q_y j + q_z k \]

세 허수 단위는 다음을 만족합니다.

\begin{gather*} i^2 = j^2 = k^2 = ijk = -1 \\ ij = -ji = k \\ jk = -kj = i \\ ki = -ik = j \end{gather*}

회전을 나타내는 수답게 곱셈의 교환법칙이 성립하지 않습니다. 이 때문에 곱셈이 상당히 골치아파집니다. 사실 곱셈의 연상량만 본다면 행렬보다도 더 복잡합니다.

\begin{align*} pq &= (p_w + p_x i + p_y j + p_z k)(q_w + q_x i + q_y j + q_z k) \\ &= (p_w q_w - p_x q_x - p_y q_y - p_z q_z) + (p_w q_x + p_x q_w + p_y q_z - p_z q_y)i \\ &\qquad+ (p_w q_y + p_y q_w + p_z q_x - p_x q_z)j + (p_w q_z + p_z q_w + p_x q_y - p_y q_x)k \\ \end{align*}

사원수 $q$의 켤레사원수 $q^*$는 허수부의 부호를 바꾼 것입니다.

\[ q^* = q_w - q_x i - q_y j - q_z k \]

켤레사원수는 다음 성질을 만족합니다.

\begin{gather*} (q^*)^* = q \\ (p+q)^* = p^* + q^* \\ (pq)^* = q^*p^* \end{gather*}

사원수의 크기는 다음과 같이 정의합니다.

\[ |q| = \sqrt{q_w^2 + q_x^2 + q_y^2 + q_z^2} = \sqrt{qq^*} = \sqrt{q^*q} \]

사원수의 곱의 크기는 크기의 곱과 같습니다.

\begin{equation*} |pq| = |p||q| \end{equation*}

사원수의 역수는 분모를 실수화하여 다음과 같이 나타낼 수 있습니다.

\[ q^{-1} = \frac{1}{q} = \frac{q^*}{qq^*} = \frac{q^*}{|q|^2} \]

이제 본격적으로 회전을 사원수로 표현해봅시다. 3차원 벡터에 해당하는 사원수는 실수부가 0이고 허수부가 $xyz$ 좌표인 사원수입니다.

\[ v = v_x i + v_y j + v_z k \]

단위 방향 벡터 $\mathbf{e}=(e_x, e_y, e_z)$를 갖는 축을 중심으로 각도 $\theta$만큼의 회전을 나타내는 사원수는

\begin{equation} q = \cos\htheta + (e_xi + e_yj + e_zk)\sin\htheta \label{eq:quaternion-rot} \end{equation}

$v$에 $q$를 적용하여 회전한 결과는 $v$의 왼쪽에 $q$를, 오른쪽에 $q^{-1}$을 곱한 것입니다. 이 결과는 실수부가 0이므로 기대했던대로 3차원 벡터라고 해석할 수 있습니다.

\[ v_\mathrm{rot} = qvq^{-1} \]

위 식에 따르면 $q$에 적당한 실수배를 해도 회전 결과가 같지만, 특정 회전을 나타내는 사원수를 유일하게 만들기 위해 회전에서는 크기가 1인 사원수만 취급합니다. 실제로 식 \eqref{eq:quaternion-rot}에서 $q$의 크기는 1이고, 따라서 $q^{-1}$ 대신 $q^*$를 사용할 수 있습니다.

비록 사원수는 다뤄야 하는 항이 4개이고 직관적이지 않지만 짐벌락 같은 문제가 없고 임의의 축에 대한 회전을 간단하게 만든다는 장점이 있습니다. 또 각도를 직접 다루는 게 아니고 그 사인과 코사인 값을 사원수의 항으로서 간접적으로 다루기 때문에 삼각함수 계산에서 해방된다는 점도 있고요. 이는 선형성의 측면에서 강력한 장점이 되고, 실제로 칼만 필터를 적용할 때 중요하게 작용합니다.

오일러각과 사원수의 변환

오일러각 $\phi$, $\theta$, $\psi$로부터 동일한 회전을 표현하는 사원수를 구해봅시다. 먼저 좌표계를 $z$축에 대해 $\psi$만큼 회전하는 것은 벡터를 $z$축에 대해 $-\psi$만큼 회전하는 것과 동일하므로 이에 대응하는 사원수는

\[ q_\psi = \cos\hpsi - k\sin\hpsi \]

따라서 $\v_I$를 $\v_{I’}$으로 변환하는 사원수 식은저자에 따라서 사원수는 그대로 두고 좌표변환을 $q^*vq$로 정의하기도 합니다. 부호에 주의하세요.

\[ v_{I'} = q_\psi v_{I} q_\psi^* \]

마찬가지 방법으로 다음과 같이 쓸 수 있습니다.

\begin{align*} q_\theta &= \cos\htheta - j\sin\htheta & v_{I''} &= q_\theta v_{I'} q_\theta^* \\ q_\phi &= \cos\hphi - i\sin\hphi & v_B &= q_\phi v_{I''} q_\phi^* \end{align*}

결론적으로 식 \eqref{eq:euler}에 대응하는 사원수 식은

\begin{gather} v_B = \underbrace{q_\phi q_\theta q_\psi}_{q_I^B} v_I \underbrace{q_\psi^* q_\theta^* q_\phi^*}_{(q_I^B)^*} = q_I^B v_I \left(q_I^B\right)^* \nonumber \\ \begin{aligned} q_I^B &= \left(\cos\hphi - i\sin\hphi\right) \left(\cos\htheta - j\sin\htheta\right) \left(\cos\hpsi - k\sin\hpsi\right) \\ &= \left(\cos\hphi\cos\htheta\cos\hpsi+\sin\hphi\sin\htheta\sin\hpsi\right) + \left(\cos\hphi\sin\htheta\sin\hpsi-\sin\hphi\cos\htheta\cos\hpsi\right)i \\ &\qquad- \left(\cos\hphi\sin\htheta\cos\hpsi+\sin\hphi\cos\htheta\sin\hpsi\right)j + \left(\sin\hphi\sin\htheta\cos\hpsi-\cos\hphi\cos\htheta\sin\hpsi\right)k \end{aligned} \label{eq:quaternion} \end{gather}

역으로 사원수 $q_I^B=q_w+q_xi+q_yj+q_zk$에서 오일러각을 구하려면

\begin{align*} \phi &= \mathrm{atan2}\left[2(q_y q_z - q_w q_x), 1-2(q_x^2 + q_y^2)\right] \\ \theta &= \sin^{-1}\left[-2(q_w q_y + q_x q_z)\right] \\ \psi &= \mathrm{atan2}\left[2(q_x q_y - q_w q_z), 1-2(q_y^2 + q_z^2)\right] \end{align*}

센서로 사원수 계산하기

자이로스코프

자이로스코프(gyroscope) 또는 간단히 자이로(gyro)는 각가속도 또는 각속도 벡터를 측정하는 센서입니다. 일반적으로 주변에서 흔히 볼 수 있는 IC 칩 형태의 MEMS 자이로는 각속도 벡터를 측정합니다. 위에서 언급했듯이 동체에 고정된 자이로 센서가 측정한 각속도 벡터의 좌표는 동체좌표계에서의 좌표입니다. 이 값을 $\omega_x$, $\omega_y$, $\omega_z$로 표기하겠습니다.

\[ \boldsymbol{\omega}_B = (\omega_x, \omega_y, \omega_z) \]

각속도 벡터는 그 이릅답게 $q_I^B$의 시간 변화율을 구하는 데 사용됩니다. 시각 $t$일 때 동체좌표계 $B(t)$를 각속도 벡터 $\boldsymbol{\omega}$의 방향을 따라 각도 $\omega\ud t$만큼 회전시키면 시각 $t+\ud t$일 때의 동체좌표계 $B(t+\ud t)$가 됩니다. 즉, 이 회전 변환에 해당하는 사원수는

\[ q_{B(t)}^{B(t+\ud t)} = \cos\frac{\omega\ud t}{2} - e_xi\sin\frac{\omega\ud t}{2} - e_yj\sin\frac{\omega\ud t}{2} - e_zk\sin\frac{\omega\ud t}{2} \]

여기서 $\boldsymbol{\omega}=\omega(e_x, e_y, e_z)$입니다. 따라서 $q_I^B$의 시간 변화율은

\begin{align*} \dot{q}_I^B &= \lim_{\ud t\rightarrow0}\frac{q_I^{B(t+\ud t)} - q_I^{B(t)}}{\ud t} \\ &= \lim_{\ud t\rightarrow0}\frac{q_{B(t)}^{B(t+\ud t)} - 1}{\ud t} q_I^{B(t)} \\ &= \lim_{\ud t\rightarrow0}\frac{\left(\cos\frac{\omega\ud t}{2}-1\right) - e_xi\sin\frac{\omega\ud t}{2} - e_yj\sin\frac{\omega\ud t}{2} - e_zk\sin\frac{\omega\ud t}{2}}{\ud t} q_I^{B(t)} \\ &= \left(-\frac12\omega e_xi - \frac12\omega e_yj - \frac12\omega e_zk \right) q_I^{B(t)} \\ &= -\frac12(\omega_x i + \omega_y j + \omega_z k) q_I^{B(t)} \end{align*}

$q_I^B$는 이걸 적분하면 되겠죠. 갱신 주기 $\Delta t$가 짧으면 1차 정확도로도 충분합니다.

\begin{equation} (q_I^B)_{k+1} = (q_I^B)_k + (\dot{q}_I^B)_k \Delta t = \left[ 1 - \frac12(\omega_x i + \omega_y j + \omega_z k)\Delta t \right] (q_I^B)_k \label{eq:quaternion-integral} \end{equation}

가속도계와 지자기계

위에서 드론의 가속도는 무시할 수 있다고 했으므로 가속도계(accelerometer)가 측정하는 것은 오직 중력 뿐입니다. 이를 통해 연직 방향에 대한 자세를 계산할 수 있습니다. 일반적인 방법대로 관성좌표계의 $z$축을 연직 아래 방향으로 잡으면 관성좌표계에서의 가속도는 $x$축과 $y$축 성분이 0이고 $z$축 성분은 $-g$입니다.연직 아래 방향으로 중력이 작용하는 것은 연직 위 방향으로 가속도 $+g$가 작용하는 것과 동일하기 때문입니다.

\begin{equation} a_I = -gk \label{eq:acc-inertial} \end{equation}

식 \eqref{eq:quaternion}에 대입하여 좌표계를 변환하면 실제 가속도계가 측정한 가속도 $a_B=a_xi+a_yj+a_zk$가 됩니다.

\begin{equation} a_B = q_I^B a_I \left(q_I^B\right)^* = g(i\sin\theta - j\sin\phi\cos\theta - k\cos\phi\cos\theta) \label{eq:acc-body} \end{equation}

따라서 $\phi$와 $\theta$는

\begin{align} \phi &= \mathrm{atan2}[-a_y, -a_z] \label{eq:acc-phi} \\ \theta &= \mathrm{atan2}\left[a_x, \sqrt{a_y^2 + a_z^2}\right] \label{eq:acc-theta} \end{align}

그런데 위 식에서 $\psi$가 없기 때문에 가속도계만으로는 $\psi$를 구할 수 없습니다. 이는 지자기계의 역할입니다. 생각해보면 동체가 정확히 수평을 이룬 상태에서 요 방향으로 회전하더라도 가속도계의 측정값은 변화가 없으므로 가속도계는 요 각도를 측정할 수 없는 게 당연합니다.

지자기계(magnetometer)는 지구 자기장을 측정하는 센서입니다. 지구 자기장은 북반구 기준 북쪽을 향하면서 지면 아래로 들어가므로, 관성좌표계의 $x$와 $y$ 방향이 각각 북쪽과 동쪽을 가리킨다고 하면 관성좌표계에서 측정한 지구 자기장은 다음과 같이 쓸 수 있습니다.

\begin{equation} m_I = B(i\cos\alpha + k\sin\alpha) \label{eq:mag-inertial} \end{equation}

여기서 $B$는 지구 자기장의 크기, $\alpha$는 지구 자기장이 지면과 이루는 각도입니다. 이를 동체좌표계로 변환하면

\begin{equation} m_B = q_I^B B(i\cos\alpha + k\sin\alpha) \left(q_I^B\right)^* \label{eq:mag1} \end{equation}

식 \eqref{eq:acc-phi} 및 \eqref{eq:acc-theta}에서 $\phi$와 $\theta$를 구했으므로 현재로서는 $q_I^B$가 $\psi$만의 함수입니다. 따라서 식 \eqref{eq:quaternion}에서 $\phi$와 $\theta$ 부분을 상수 $q_\acc$로 분리하여 $\psi$만 남깁시다.

\begin{align*} q_I^B &= \underbrace{\left(\cos\hphi - i\sin\hphi\right) \left(\cos\htheta - j\sin\htheta\right)}_{q_\acc} \left(\cos\hpsi - k\sin\hpsi\right) \\ &= q_\acc \left(\cos\hpsi - k\sin\hpsi\right) \end{align*}

다시 식 \eqref{eq:mag1}에 대입하면

\begin{equation} \begin{aligned} &\left(\cos\hpsi - k\sin\hpsi\right) B(i\cos\alpha + k\sin\alpha) \left(\cos\hpsi + k\sin\hpsi\right) \\ &\quad= B(i\cos\alpha\cos\psi - j\cos\alpha\sin\psi + k\sin\alpha) \\ &\quad= q_\acc^* m_B q_\acc \\ &\quad\equiv m_\acc \end{aligned} \label{eq:macc} \end{equation}

$B\cos\alpha$가 양수이므로 $\psi$는 다음과 같습니다.

\begin{equation} \psi = \mathrm{atan2}\left[ m_{\acc x}, -m_{\acc y} \right] \label{eq:acc-psi} \end{equation}

초기값

초기 자세는 가속도계와 지자기계의 초기 측정값을 식 \eqref{eq:acc-phi}, \eqref{eq:acc-theta}, \eqref{eq:acc-psi}에 대입하여 구할 수 있습니다. 물론 측정 오차가 있으니 드론을 움직이지 않는 곳에 가만히 둔 상태에서 여러 번 측정하여 평균을 취하는 것이 좋습니다.

확장 칼만 필터

센서 융합

위에서 세 가지 선세를 사용해 현재 자세를 구하는 데 사용되는 회전변환 사원수를 구하는 두 가지 방법을 알아보았습니다. 그런데 왜 두 가지 다른 방법으로 사원수를 계산했을까요? 가속도계와 지자기계만으로 자세를 구해도 되지 않을까요?

실제로 센서 측정값을 보면 알 수 있는 사실이지만, 가속도계와 지자기계는 외부 잡음에 매우 취약합니다. 동체가 조금만 흔들려도 가속도계와 지자기계는 즉각적으로 반응하여 측정값을 내놓고, 이를 바탕으로 자세를 계산하면 엔진이나 모터가 만드는 진동을 그대로 반영해버립니다. 즉, 동체가 거시적으로는 가만히 있는데도 불구하고 진동이 구조를 따라 센서에 전파되면서 센서값을 크게 왜곡해 동체가 아주 빠른 속도로 움직이는 것으로 계산하는 것입니다. 게다가 가속도계로 롤과 피치 각도를 계산할 때 동체의 가속도가 없다고 가정했는데 이것도 현실적으로는 맞기가 힘든 가정이고, 이 때문에도 상당한 오차가 발생합니다.

이와는 다르게 자이로로 계산하는 사원수는 적분 과정을 거치기 때문에, 자이로 측정값에 고주파 잡음이 있어도 적분 결과엔 영향을 크게 미치지 않습니다. 따라서 사원수가 부드럽게 변하게 됩니다. 더군다나 계산 과정에서 가속도가 없다는 가정 같은 것도 사용하지 않았으므로 단기적으로는 굉장히 정확한 자세를 구할 수 있습니다. 하지만 장기적으로는 잡음이 누적되면서 적분 결과에 상당한 크기의 저주파 오차가 발생합니다.

이렇듯 두 방법 모두 부정확하기 때문에 양쪽의 단점을 서로 보완해서 가능한 한 정확한 측정값을 얻고자 하는 방법들이 개발되었는데, 이렇게 여러 센서값을 참고해 오차를 줄이는 것을 센서 융합(sensor fusion)이라고 합니다. AHRS에 사용되는 센서 융합 알고리즘은 대표적으로 상보 필터(complementary filter), Madgwick 필터, 칼만 필터(Kalman filter)가 있습니다. 모두 공통점으로 단기적으로는 자이로를 사용하되, 자이로의 장기적 오차를 가속도계와 지자기계로 보정합니다. 이 글에서는 제목대로 확장 칼만 필터를 사용하여 센서 융합을 해보겠습니다.

시스템 모델

(확장) 칼만 필터의 목적은 시스템 모델이 알려져 있을 때 센서의 측정값으로부터 최적의 상태를 추정하는 것입니다. 확장 칼만 필터는 다음과 같은 시스템 모델을 가정합니다.

\begin{align*} \x_k &= \f_k(\x_{k-1}) + \w_k \\ \z_k &= \h_k(\x_k) + \v_k \\ \w_k &\sim \mathcal{N}(0, Q_k) \\ \v_k &\sim \mathcal{N}(0, R_k) \end{align*}

$\x$는 시스템의 상태를 나타내는 상태 변수(state variable)입니다. $\f$는 상태가 다음 단계에서 어떻게 바뀌는지를 나타내는 함수이고, $\w$는 과정 잡음(process noise)이며 평균이 0이고 공분산 행렬이 $Q$인 다변량 정규분포를 따릅니다. 즉, 칼만 필터는 각 단계에서 상태 변수가 선형적으로 바뀌되 정규분포를 따르는 잡음이 발생할 수 있다고 가정합니다.

나머지 두 식은 측정에 관련된 것입니다. $\z$는 센서로 측정하는 값들입니다. $\h$는 상태 변수와 측정값 사이의 관계를 나타내는 함수이고, $\v$는 측정 잡음(measurement noise)입니다. $\w$처럼 $\v$는 평균이 0이고 공분산 행렬이 $R$인 다변량 정규분포를 따른다고 가정합니다.

여기서 다루는 AHRS의 상태 변수는 사원수의 네 성분으로 잡는 것이 직관적입니다.

\[ \x = \begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix} \]

이때 상태 전이 함수 $\f$는 식 \eqref{eq:quaternion-integral}를 풀어 써서 구할 수 있습니다.

\begin{equation} \begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix}_k = \f_k\left( \begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix}_{k-1} \right) = \left( I - \frac12 \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & -\omega_z & \omega_y \\ \omega_y & \omega_z & 0 & -\omega_x \\ \omega_z & -\omega_y & \omega_x & 0 \end{bmatrix}_k \Delta t \right) \begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix}_{k-1} \label{eq:system-model} \end{equation}

상태 전이 함수 $\f$가 각속도 성분들을 포함하므로 결국 $\f$를 알기 위해선 자이로 측정값이 필요합니다.

그리고 공분산 행렬 $Q$를 정해야 합니다. 과정 잡음은 식 \eqref{eq:system-model}에서 각속도 성분에 측정 오차가 있기 때문에 발생합니다. 따라서 $\f$를 각속도 $\boldsymbol\omega$의 함수로 보고 $\boldsymbol\omega$의 공분산으로부터 $\f$의 공분산을 구하면 됩니다.

일반적으로 변수 $\x$의 공분산 행렬 $\Sigma_\x$이 주어질 때 함수 $\mathbf{F}(\x)$의 공분산 행렬 $\Sigma_{\mathbf{F}}$은 야코비 행렬을 이용해 계산할 수 있습니다.

\begin{equation} \Sigma_{\mathbf{F}} = \left( \frac{\partial\mathbf{F}}{\partial\x} \right) \Sigma_\x \left( \frac{\partial\mathbf{F}}{\partial\x} \right)^T \label{eq:cov-propagation} \end{equation}

$\f_k$를 $\boldsymbol\omega_k$로 미분하면

\[ \frac{\partial\f_k}{\partial\boldsymbol\omega_k} = \frac12 \begin{bmatrix} q_x & q_y & q_z \\ -q_w & -q_z & q_y \\ q_z & -q_w & -q_x \\ -q_y & q_x & -q_w \end{bmatrix}_{k-1} \Delta t \]

그리고 자이로의 각속도 측정 오차가 방향별로 독립적이고 동일한 표준편차 $\sigma_{\boldsymbol{\omega}}$을 가진다고 가정합시다. 이때 $\Sigma_{\boldsymbol\omega}=\sigma_{\boldsymbol{\omega}}^2I$로 쓸 수 있으므로 행렬 $Q$는

\[ Q_k = \left( \frac{\partial\f_k}{\partial\boldsymbol\omega_k} \right) \left( \sigma_{\boldsymbol{\omega}}^2I \right) \left( \frac{\partial\f_k}{\partial\boldsymbol\omega_k} \right)^T = \sigma_{\boldsymbol\omega}^2 \left( \frac{\partial\f_k}{\partial\boldsymbol\omega_k} \right) \left( \frac{\partial\f_k}{\partial\boldsymbol\omega_k} \right)^T \]

한편 상태 전이 함수에 각속도를 이용한 사원수 적분식을 사용했으므로 자연스럽게 나머지 가속도계와 지자기계의 측정값이 시스템 모델의 측정값 $\z$가 됩니다. 편의상 측정값을 정규화하여 가속도와 지자기 벡터의 크기를 1로 만들겠습니다. 즉, 식 \eqref{eq:acc-inertial}과 \eqref{eq:mag-inertial}에서 $g$와 $B$를 제거한다고 생각하면 됩니다.

\[ \z = \begin{bmatrix} \hat{a}_x \\ \hat{a}_y \\ \hat{a}_z \\ \hat{m}_x \\ \hat{m}_y \\ \hat{m}_z \end{bmatrix} \] \begin{equation*} \hat{\mathbf{a}} = \mathbf{a} / g, \qquad \hat{\mathbf{m}} = \mathbf{m} / B \end{equation*}

이제 측정 함수 $\h$를 구하려면 사원수의 네 성분으로 $\z$를 나타내야 합니다. 식 \eqref{eq:acc-inertial}을 동체좌표계로 변환하면

\[ \begin{bmatrix} \hat{a}_x \\ \hat{a}_y \\ \hat{a}_z \end{bmatrix} = \begin{bmatrix} -2(q_wq_y+q_xq_z) \\ 2(q_wq_x-q_yq_z) \\ (2q_x^2+2q_y^2-1) \end{bmatrix} \]

마찬가지로 식 \eqref{eq:mag-inertial}으로부터

\[ \begin{bmatrix} \hat{m}_x \\ \hat{m}_y \\ \hat{m}_z \end{bmatrix} = \begin{bmatrix} (2q_w^2+2q_x^2-1)\cos\alpha + 2(q_wq_y+q_xq_z)\sin\alpha \\ 2(q_wq_z+q_xq_y)\cos\alpha + 2(q_yq_z-q_wq_x)\sin\alpha \\ 2(q_xq_z-q_wq_y)\cos\alpha + (2q_w^2+2q_z^2-1)\sin\alpha \end{bmatrix} \]

따라서 측정 함수 $\h$는

\begin{equation} \z = \h\left( \begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix} \right) = \begin{bmatrix} -2(q_wq_y+q_xq_z) \\ 2(q_wq_x-q_yq_z) \\ (2q_x^2+2q_y^2-1) \\ (2q_w^2+2q_x^2-1)\cos\alpha + 2(q_wq_y+q_xq_z)\sin\alpha \\ 2(q_wq_z+q_xq_y)\cos\alpha + 2(q_yq_z-q_wq_x)\sin\alpha \\ 2(q_xq_z-q_wq_y)\cos\alpha + (2q_w^2+2q_z^2-1)\sin\alpha \end{bmatrix} \label{eq:meas-model} \end{equation}

각 $\alpha$는 식 \eqref{eq:macc}에 따라 아래와 같이 계산할 수 있습니다.

\[ \alpha = \mathrm{atan2}\left[ m_{\acc z}, \sqrt{m_{\acc x}^2 + m_{\acc y}^2} \right] \]

드론이 비행하는 시간적, 공간적 범위 내에선 지자기가 변하지 않는다고 가정할 수 있으므로 $\alpha$를 처음에 한 번 초기 측정값으로 계산해도 됩니다.

마지막으로 측정 오차의 공분산 행렬 $R$를 구해야 하는데, 자이로와 동일하게 가속도계와 지자기계의 각 성분 사이에는 상관관계가 없고, 각 축별로 동일한 분산을 갖는다고 가정합니다. 정규화된 가속도의 표준편차를 $\sigma_{\hat{\mathbf{a}}}$, 지자기의 표준편차를 $\sigma_{\hat{\mathbf{m}}}$라고 하면

\[ R = \begin{bmatrix} \sigma_{\hat{\mathbf{a}}}^2 I & \mathbf{0} \\ \mathbf{0} & \sigma_{\hat{\mathbf{m}}}^2 I \end{bmatrix} \]

확장 칼만 필터 알고리즘

칼만 필터는 두 단계로 구성됩니다. 첫 번째인 예측 단계는 직전에 추정한 상태 변수만을 이용하여 다음 상태 변수를 추정합니다.

\begin{align*} \x_k^- &= \f_k(\x_{k-1}) \\ P_k^- &= A_k P_{k-1} A_k^T + Q_k \end{align*}

행렬 $P$는 $\x$의 추정값의 공분산 행렬입니다. 칼만 필터의 목표는 바로 이 $P$를 최소화해서 추정값의 정확도를 높이는 데 있죠. 위첨자로 붙은 $-$는 예측 단계에서 구한 값(사전 추정값)이란 뜻으로, 아직 측정값 $\z$를 반영하지 않은 상황입니다. 그리고 $A$는 함수 $\f$의 야코비 행렬입니다.

\[ A_k = \frac{\partial\f_k}{\partial\x}(\x_{k-1}) \]

여기선 식 \eqref{eq:system-model}이 선형이기 때문에 야코비 행렬 $A$는 단순히 다음과 같습니다.

\[ A_k = I - \frac12 \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & -\omega_z & \omega_y \\ \omega_y & \omega_z & 0 & -\omega_x \\ \omega_z & -\omega_y & \omega_x & 0 \end{bmatrix}_k \Delta t \]

두 번째인 보정 단계에서는 측정값으로 예측 단계의 결과를 보정합니다.

\begin{align*} K_k &= P_k^- H_k^T (H_k P_k^- H_k^T + R_k)^{-1} \\ \x_k &= \x_k^- + K_k [\z_k - \h_k(\x_k^-)] \\ P_k &= (I - K_k H_k) P_k^- \end{align*}

$K$는 칼만 이득(Kalman gain)이라는 것으로, 예측 단계의 결과 $\x^-$를 예측 단계와 실제 측정 사이의 차이 $\z - H\x^-$로 보정할 때 사용하는 계수입니다. 이 칼만 이득은 측정의 공분산 행렬 $R$이 클수록 작아져서 보정이 줄어듭니다. 한마디로 신뢰하기 힘든 측정값이라면 그만큼 보정에 반영하지 않겠다는 뜻입니다. 마지막으로 $H$는 함수 $\h$의 야코비 행렬입니다.

\[ H_k = \frac{\partial\h_k}{\partial\x}(\x_k^-) \]

식 \eqref{eq:meas-model}을 대입하면

\[ H_k = 2 \begin{bmatrix} -q_y & -q_z & -q_w & -q_x \\ q_x & q_w & -q_z & -q_y \\ 0 & 2q_x & 2q_y & 0 \\ 2q_w\cos\alpha + q_y\sin\alpha & 2q_x\cos\alpha + q_z\sin\alpha & q_w\sin\alpha & q_x\sin\alpha \\ q_z\cos\alpha - q_x\sin\alpha & q_y\cos\alpha - q_w\sin\alpha & q_x\cos\alpha + q_z\sin\alpha & q_w\cos\alpha + q_y\sin\alpha \\ -q_y\cos\alpha + 2q_w\sin\alpha & q_z\cos\alpha & -q_w\cos\alpha & q_x\cos\alpha + 2q_z\sin\alpha \end{bmatrix}_k^- \]

참고문헌

김성필. 『칼만 필터는 어렵지 않아』 (한빛아카데미, 2019)
“Extended Kalman Filter,” 「AHRS」, 2023년 1월 11일, https://ahrs.readthedocs.io/en/latest/filters/ekf.html

GNC