정적 자세결정: QUEST

1978년 HEAO-B(High Energy Astronomy Observatory)의 자세 추정을 계산하는 데 Davenport의 q-방법을 사용했다. 그러나 당시의 컴퓨터 성능으로는 1년 후에 발사된 MAGSAT에서 요구된 보다 빈번한 자세 계산을 감당할 수는 없었다고 한다. QUEST(QUaternion Estimator) 알고리즘은 이러한 요구에 부응하기 위해 고안되었으며, Wahba 의 자세결정 문제를 해결하기 위한 알고리즘으로 널리 사용되게 되었다.

 

 

QUEST는 Davenport의 q-방법과 마찬가지로 행렬 \(K\) 의 고유값을 찾아 Wahba의 문제를 푼다(https://pasus.tistory.com/303). 그러나 QUEST는 고유값을 계산할 때 Newton-Raphson 반복법을 사용하여 Davenport의 q-방법과 동일한 정밀도를 가지면서도 계산 속도는 대폭 향상시켰다.

 

 

Wahba의 자세결정 문제는 다음과 같았다.

 

\[ \min_{C_b^a } ⁡J(C_b^a )= \frac{1}{2} \sum_{k=1}^n w_k \vert \mathbf{s}_k^a-C_b^a \mathbf{s}_k^b \vert^2 \tag{1} \]

 

여기서 \(\mathbf{s}_k^a\) 와 \(\mathbf{s}_k^b\) 는 각각 \(n\) 개의 측정 단위벡터 \(\hat{s}_k\) 를 기준좌표계 \(\{a\}\) 와 동체좌표계 \(\{b\}\) 로 표현한 벡터이고, \(C_b^a\) 는 좌표계 \(\{a\}\) 에서 \(\{b\}\) 로의 DCM, \(w_k\) 는 \(k\) 번째 벡터의 중요성을 나타내는 가중값(weight)이다.

식 (1)의 \(J(C_b^a )\) 를 전개하면 다음과 같았다.

 

\[ J(C_b^a )= \sum_{k=1}^n w_k - \sum_{k=1}^n w_k (\mathbf{s}_k^a )^T C_b^a \mathbf{s}_k^b \tag{2} \]

 

위 식 우변의 첫번째 항은 \(C_b^a\) 의 함수가 아니므로 \(J(C_b^a )\) 를 최소화하기 위해서는 두번째 항을 최대화하면 된다. 몇 단계의 연산을 통하여 위 최소화 문제는 다음과 같은 최대화 문제로 바뀐다.

 

\[ \max_{\mathbf{q}} \bar{J}( \mathbf{q})= \mathbf{q}^T K \mathbf{q} \tag{3} \]

 

여기서 \(\mathbf{q}\) 는 좌표계 \(\{a\}\) 에서 \(\{b\}\) 로의 쿼터니언 \(\mathbf{q}_b^a\) 를 의미하며 구성 요소는 다음과 같이 실수부와 벡터부로 표현되고,

 

\[ \mathbf{q}= \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}= \begin{bmatrix} q_0 \\ \mathbf{q}_{1:3} \end{bmatrix} \tag{4} \]

 

행렬 \(K\) 는 다음과 같이 주어진다.

 

\[ \begin{align} K &= \begin{bmatrix} tr(B) & \mathbf{z}^T \\ \mathbf{z} & B+B^T-tr(B) I \end{bmatrix} \tag{5} \\ \\ B &= \sum_{k=1}^n w_k \mathbf{s}_k^a (\mathbf{s}_k^b )^T \tag{6} \end{align} \]

 

식 (3)의 \(\bar{J}(\mathbf{q})\) 를 최대화하는 쿼터니언은 행렬 \(K\) 의 최대 고유값에 해당하는 고유벡터이므로 Davenport의 q-방법에서는 다음과 같이 고유값 문제를 직접 푸는 방법을 택했다.

 

\[ K \mathbf{q}= \lambda \mathbf{q} \tag{7} \]

 

반면에 QUEST에서는 식 (7)의 고유값 \(\lambda\) 를 계산할 때 Newton-Raphson 반복법을 사용한다.

식 (7)에 (4)와 (5)를 대입하면 다음과 같다.

 

\[ K \mathbf{q}= \begin{bmatrix} tr(B) & \mathbf{z}^T \\ \mathbf{z} & B+B^T-tr(B) I \end{bmatrix} \begin{bmatrix} q_0 \\ \mathbf{q}_{1:3} \end{bmatrix} = \lambda \begin{bmatrix} q_0 \\ \mathbf{q}_{1:3} \end{bmatrix} \tag{8} \]

 

위 식을 전개하면 다음과 같다.

 

\[ \begin{align} & tr(B) q_0+ \mathbf{z}^T \mathbf{q}_{1:3}= \lambda q_0 \tag{9} \\ \\ & \mathbf{z} q_0+(B+B^T-tr(B)I) \mathbf{q}_{1:3}= \lambda \mathbf{q}_{1:3} \tag{10} \end{align} \]

 

식 (10)의 양변을 \(q_0\) 로 나누고 \(\frac{\mathbf{q}_{1:3}}{q_0}\) 을 계산하면 다음과 같다.

 

\[ \frac{ \mathbf{q}_{1:3}}{q_0} = \left( ( \lambda+tr(B)) I-B-B^T \right)^{-1} \mathbf{z} \tag{11} \]

 

참고로 \(\frac{ \mathbf{q}_{1:3} }{q_0}\) 를 로드리게스 파라미터(Rodrigues parameters)라고 한다. 식 (11)로 로드리게스 파라미터를 계산할 수 있다면 쿼터니언의 크기 조건을 이용하여 쿼터니언을 계산할 수 있다. 즉 \(\mathbf{q}^T \mathbf{q}=1\) 이므로 \(q_0^2+ \mathbf{q}_{1:3}^T \mathbf{q}_{1:3}=1\) 이 성립하기 때문에, 쿼터니언의 실수부는 다음과 같이 계산할 수 있고,

 

\[ q_0= \frac{1}{ \sqrt{ 1+ \left( \frac{ \mathbf{q}_{1:3}}{q_0^2 } \right)^T \left( \frac{ \mathbf{q}_{1:3}}{q_0^2} \right) } } \tag{12} \]

 

벡터부는 다음과 같이 계산할 수 있다.

 

\[ \mathbf{q}_{1:3}=q_0 \frac{ \mathbf{q}_{1:3} }{q_0} \tag{13} \]

 

문제는 어떻게 식 (11)을 계산하느냐이다. 식 (11)은 고유값을 포함하고 있으므로 고유값부터 계산해야 한다.

 

방향코사인행렬, 오일러각, 그리고 쿼터니언 | 박성수 | 딥웨이브- 교보ebook

 

우선 표기를 단순하게 하기 위해서 다음과 같은 변수를 도입한다.

 

\[ \begin{align} & \sigma =tr(B) \tag{14} \\ & \rho = \lambda + \sigma \\ & S=B+B^T \end{align} \]

 

그려면 식 (11)은 다음과 같이 표기할 수 있다.

 

\[ \frac{ \mathbf{q}_{1:3} }{q_0} =( \rho I-S)^{-1} \mathbf{z} \tag{15} \]

 

식 (15)에서 역행렬을 계산하기 위해서 우선 행렬식(determinant)부터 계산한다. 일반적인 \(3 \times 3\) 행렬의 특성방정식은 다음과 같이 주어진다.

 

\[ \begin{align} f(\rho) &= \det ⁡(\rho I-S) \tag{16} \\ \\ &= -\rho^3+tr(S) \rho^2- \frac{1}{2} \left( tr(S)^2-tr(S^2 ) \right) \rho+ \det⁡(S) \\ \\ & = 0 \end{align} \]

 

케일리-해밀톤의 정리 (Cayley-Hamilton theorem)에 의하면, 행렬은 자신의 특성방정식을 만족한다. 즉 다음 식이 성립한다.

 

\[ \begin{align} f(S) &= -S^3+tr(S) S^2- \frac{1}{2} \left( tr(S)^2-tr(S^2 ) \right) S+ \det⁡(S) \tag{17} \\ \\ & = 0 \end{align} \]

 

또한 케일리-해밀톤의 정리에서 파생된 정리에 의하면 정사각형 행렬(square matrix)의 함수는 행렬의 차원보다 \(1\) 이 작은 차수를 갖는 다항식으로 표현할 수 있다. 즉 식 (15)에서 \(3 \times 3\) 역행렬은 다음과 같이 표현할 수 있다.

 

\[ (\rho I-S)^{-1}= \alpha_0 I+ \alpha_1 S+ \alpha_2 S^2 \tag{18} \]

 

여기서 \(\alpha_0, \ \alpha_1, \ \alpha_2\) 는 위 식을 만족하는 어떤 상수다. 계산상의 편의를 위해서 각 상수를 다음과 같이 다른 상수 \(\alpha, \ \beta, \ \gamma\) 로 표현한다.

 

\[ (\rho I-S)^{-1} = \frac{\alpha}{\gamma} I+ \frac{\beta}{\gamma} S+ \frac{1}{\gamma} S^2 \tag{19} \]

 

이 상수를 계산하기 위해서 역행렬의 정의를 이용한다.

 

\[ \begin{align} (\rho I-S) (\rho I-S)^{-1} &= (\rho I-S) \left( \frac{\alpha}{\gamma} I+ \frac{\beta}{\gamma} S+ \frac{1}{\gamma} S^2 \right) \tag{20} \\ \\ &= I \end{align} \]

 

위 식을 전개하면 다음과 같다.

 

\[ \rho \alpha I+ (\rho \beta -\alpha) S+(\rho - \beta) S^2-S^3=\gamma I \tag{21} \]

 

식 (17)에 의하면 \(S^3\) 는 다음과 같이 주어진다.

 

\[ S^3= tr(S) S^2- \frac{1}{2} \left( tr(S)^2-tr(S^2 ) \right) S+ \det ⁡(S) \tag{22} \]

 

식 (22)를 (21)에 대입하면 다음과 같다.

 

\[ (\rho \alpha - \gamma- \det ⁡(S) ) I+(\rho \beta -\alpha+ \kappa )S+(\rho - \beta-tr(S)) S^2=0 \tag{23} \]

 

여기서

 

\[ \kappa = \frac{1}{2} \left( tr(S)^2-tr(S^2 ) \right) \tag{24} \]

 

이다. 식 (23)에 의하면 다음 식이 성립한다.

 

\[ \begin{align} \gamma &= \rho \alpha -\det ⁡(S)= (\lambda + \sigma) \alpha- \det ⁡(S) \tag{25} \\ \\ \beta &= \rho -tr(S)= \lambda + \sigma -2 \sigma =\lambda - \sigma \\ \\ \alpha &= \rho \beta + \kappa = (\lambda + \sigma)(\lambda - \sigma)+ \kappa = \lambda^2- \sigma^2+ \kappa \end{align} \]

 

이제 식 (11)을 (9)에 대입하면 다음과 같이 된다.

 

\[ tr(B)+ \mathbf{z}^T \left( (\lambda +tr(B)) I-B-B^T \right)^{-1} \mathbf{z}= \lambda \tag{26} \]

 

식 (19)를 식 (26)에 대입하면,

 

\[ \gamma \sigma+ \mathbf{z}^T (\alpha I+\beta S+S^2 ) \mathbf{z}= \gamma \lambda \tag{27} \]

 

이 되는데, 위 식에 식 (25)를 대입하면

 

\[ \begin{align} & \left( (\lambda + \sigma )( \lambda^2-\sigma^2+\kappa )-\det⁡(S) \right) \sigma \tag{28} \\ \\ & \ \ \ \ \ \ \ \ \ \ + \mathbf{z}^T \left( (\lambda^2-\sigma^2+\kappa)I+(\lambda-\sigma)S+S^2 \right) \mathbf{z} \\ \\ & \ \ \ \ \ \ = \left( (\lambda+ \sigma )( \lambda^2-\sigma^2+\kappa)-\det⁡(S) \right) \lambda \end{align} \]

 

이 되므로, 다음과 같이 고유값에 대한 4차 방정식을 얻을 수 있다.

 

\[ \lambda^4+a_2 \lambda^2+a_1 \lambda +a_0=0 \tag{29} \]

 

여기서

 

\[ \begin{align} a_2 & = \kappa -2 \sigma^2- \mathbf{z}^T \mathbf{z} \\ \\ a_1 &= -\det⁡(S)-\mathbf{z}^T S \mathbf{z} \\ \\ a_0 &= (\sigma^2-\kappa)(\sigma^2+ \mathbf{z}^T \mathbf{z})+ \sigma \det⁡(S)+ \sigma \mathbf{z}^T S \mathbf{z}-\mathbf{z}^T S^2 \mathbf{z} \end{align} \]

 

이다. 사실 고유값 정의에 의하면 행렬 \(K\) 의 고유값은 다음 특성방정식의 해이다.

 

\[ p(\lambda)=\det⁡(K- \lambda I)=0 \tag{30} \]

 

매트랩 함수 'poly(K)' 를 이용하면 식 (29)와 동일한 방정식을 얻을 수 있을 것이다.

식 (30) 또는 (29)로 주어지는 특성방정식은 4차 다항식이므로 4개의 해가 존재한다. 하지만 식 (3)을 최대로 또는 식 (1)을 최소로 하는 고유값은 최대 고유값이므로 식 (29)에서 최대 고유값만 계산하면 된다.

식 (29)에 Newton-Raphson 반복법을 적용하여 최대 고유값을 구하는 방법은 다음과 같다. 먼저 최대 고유값의 초기값을 선정해야 하는데, 식 (1)에 의하면 측정값에 오류가 없이 완벽하다면 \(J(C_b^a )\) 의 최소값은 \(0\) 이 될 것이기 때문에 식 (3)에 의해서 행렬 \(K\) 의 최대 고유값은 \(\sum_{k=1}^n w_k\) 이 될 것이다. 측정 오류를 감안하더라도 최대 고유값은 이 값과 근사한 값이 될 것이므로 Newton-Raphson 반복법의 초기 추정값으로 이 값을 선정한다. 즉,

 

\[ \lambda_0= \sum_{k=1}^n w_k \tag{31} \]

 

그런 다음 고유값이 수렴할 때까지 다음을 반복한다.

 

\[ \lambda_i= \lambda_{i-1} - \frac{ p(\lambda_{i-1} ) }{ p' (\lambda_{i-1} )}, \ \ \ \ \ \ i= 1, 2, ... \tag{32} \]

 

최대 고유값을 구했다면, 이제 이 고유값에 대응하는 고유벡터를 식 (11), (12), (13)을 이용해서 계산하면 된다. 이 고유벡터가 최적 쿼터니언 \(\mathbf{q}_{max}\) 가 된다. 비록 식 (11)에서 역행렬을 계산하는 부분이 있지만 고유값 문제에서 고유값/고유벡터를 계산하는 알고리즘 보다는 속도가 더 빠르다. 마지막으로 \(\mathbf{q}_{max}\) 에 해당하는 DCM \(C_b^a\) 를 DCM과 쿼터니언 변환식을 이용해서 계산하면 된다.

QUEST 알고리즘을 요약하면 다음과 같다. 우선 식 (6)으로 행렬 \(B\) 를 계산한다. 그리고 식 (29)의 특성방정식을 만든다. 식 (32)의 Newton-Raphson 반복법으로 최대 고유값을 구한다. 이어서 식 (11), (12), (13)을 이용해서 이에 해당하는 고유벡터를 계산한다. 이 고유벡터가 좌표계 \(\{a\}\) 에서 \(\{b\}\) 로의 최적 쿼터니언이다.

Davenport의 q-방법에서 풀었던 동일한 예제(https://pasus.tistory.com/303)를 QUEST 알고리즘으로 풀어보겠다.

기준좌표계 \(\{a\}\) 에서 단위벡터 \(\hat{s}_1, \ \hat{s}_2\) 가 다음과 같이 주어졌다고 하자.

 

\[ \mathbf{s}_1^a= \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}, \ \ \ \ \ \mathbf{s}_2^a= \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} \tag{33} \]

 

좌표계 \(\{a\}\) 에서 \(\{b\}\) 로의 참값 DCM은 3-2-1 오일러각 30도, 20도, 10도 회전을 통해서 다음과 같다고 하자.

 

\[ C_b^a= \begin{bmatrix} 0.9254 & 0.0180 & 0.3785 \\ 0.1632 & 0.8826 & -0.4410 \\ -0.3420 & 0.4698 & 0.8138 \end{bmatrix} \tag{34} \]

 

그러면 동체좌표계 \(\{b\}\) 에서 단위벡터 \(\hat{s}_1, \ \hat{s}_2\) 는 다음과 같다.

 

\[ \mathbf{s}_1^b= \begin{bmatrix} 0.9254 \\ 0.0180 \\ 0.3785 \end{bmatrix}, \ \ \ \ \ \mathbf{s}_2^b= \begin{bmatrix} -0.3420 \\ 0.4698 \\ 0.8138 \end{bmatrix} \tag{35} \]

 

이제 식 (21)과 (23)을 측정 단위벡터로 간주하고 QUESR 알고리즘을 적용한다. 가중값을 모두 \(1\) 로 하면 식 (6)에 의해서 행렬 \(B\) 는 다음과 같다.

 

\[ B= \begin{bmatrix} 0.9254 & 0.0180 & 0.3785 \\ 0 & 0 & 0 \\ -0.3420 & 0.4698 & 0.8138 \end{bmatrix} \]

 

식 (29)에 의하면 특성방정식은 다음과 같다.

 

\[ p(\lambda)=0= \lambda^4-4 \lambda^2 \]

 

식 (31)에 의하면 Newton-Raphson 반복법의 초기 추정값은 \(2.0\) 이다. 식 (32)에 의하면 최대 고유값은 \(2.0\) 이다. 식 (11), (12), (13)에 의하면 고유벡터 또는 최적의 쿼터니언은 다음과 같다.

 

\[ \mathbf{q}= \begin{bmatrix} 0.9515 \\ 0.2393 \\ 0.1893 \\ 0.0381 \end{bmatrix} \]

 

이를 DCM으로 변환하면 다음과 같아서 식 (34)로 주어지는 참값 DCM과 일치함을 알 수 있다.

 

\[ C_b^a= \begin{bmatrix} 0.9254 & 0.0180 & 0.3785 \\ 0.1632 & 0.8826 & -0.4410 \\ -0.3420 & 0.4698 & 0.8138 \end{bmatrix} \]

 

 

 

다음은 QUEST 알고리즘을 매트랩 코드로 작성한 것이다.

 

function q = quest(weight, S_a, S_b)

% QUEST: static attitide determination
% input:
%       weight: weights of each vector
%       S_a = [s_a_1, ..., s_a_n] of s_hat_k, k=1,...,n in frame {a}
%       S_b = [s_b_1, ..., s_b_n] of s_hat_k, k=1,...,n in frame {b}
% output:
%       quaternion q^a_b
%
% coded by st.watermelon

% size
[m,n] = size(S_a);


% B
B = zeros(3,3);
for k=1:n
    B = B + weight(k)*S_a(:,k)*S_b(:,k)';
end

% parameters
sigma = trace(B);
Z = [B(3,2)-B(2,3);
      -B(3,1)+B(1,3);
       B(2,1)-B(1,2)];
S = B + B';
kappa = 0.5*(trace(S)*trace(S) - trace(S*S));

% characteristic equation coefficients
a2 = kappa - 2*sigma^2 - Z'*Z;
a1 = -det(S) - Z'*S*Z;
a0 = (sigma^2-kappa)*(sigma^2+Z'*Z) + sigma*det(S) + sigma*Z'*S*Z - Z'*S*S*Z;

% Newton-Rhapson
lambda0 = sum(weight);

for ii = 1:10
    p_lam0 = lambda0^4 + a2*lambda0^2 + a1*lambda0 + a0;
    p_lam0_prime = 4*lambda0^3 + 2*a2*lambda0 + a1;
    lambda = lambda0 - (p_lam0)/(p_lam0_prime);
    if abs(lambda-lambda0)<=1e-5
        break;
    end
    lambda0 = lambda;
end

% max quarternion
rod = inv((lambda+sigma)*eye(3)-S)*Z;
q0 = 1/(sqrt(1+rod'*rod));
q_vec = q0*rod;
q = [q0; q_vec];