정적 자세결정: 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의 자세결정 문제는 다음과 같았다.

 

$$\begin{array}{}\text{(1)}& \underset{C_b^a}{min}J(C_b^a)=\frac{1}{2}\sum _{k=1}^n{w}_k|{\mathbf{s}}_k^a-C_b^a{\mathbf{s}}_k^b{|}^2\end{array}$$

 

여기서 ${\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)$ 를 전개하면 다음과 같았다.

 

$$\begin{array}{}\text{(2)}& 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\end{array}$$

 

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

 

$$\begin{array}{}\text{(3)}& \underset{\mathbf{q}}{max}\overline{J}(\mathbf{q})={\mathbf{q}}^{T}K\mathbf{q}\end{array}$$

 

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

 

$$\begin{array}{}\text{(4)}& \mathbf{q}=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}{q}_0\\ {\mathbf{q}}_{1:3}\end{array}\right]\end{array}$$

 

행렬 $K$ 는 다음과 같이 주어진다.

 

$$\begin{array}{r}\text{(5)}& K& =\left[\begin{array}{cc}tr(B)& {\mathbf{z}}^T\\ \mathbf{z}& B+B^T-tr(B)I\end{array}\right]\\ \text{(6)}& B& =\sum _{k=1}^n{w}_k{\mathbf{s}}_k^a({\mathbf{s}}_k^b{)}^T\end{array}$$

 

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

 

$$\begin{array}{}\text{(7)}& K\mathbf{q}=\lambda \mathbf{q}\end{array}$$

 

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

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

 

$$\begin{array}{}\text{(8)}& K\mathbf{q}=\left[\begin{array}{cc}tr(B)& {\mathbf{z}}^T\\ \mathbf{z}& B+B^T-tr(B)I\end{array}\right]\left[\begin{array}{c}{q}_0\\ {\mathbf{q}}_{1:3}\end{array}\right]=\lambda \left[\begin{array}{c}{q}_0\\ {\mathbf{q}}_{1:3}\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{r}\text{(9)}& & tr(B)q_0+{\mathbf{z}}^T{\mathbf{q}}_{1:3}=\lambda q_0\\ \text{(10)}& & \mathbf{z}{q}_0+(B+B^T-tr(B)I){\mathbf{q}}_{1:3}=\lambda {\mathbf{q}}_{1:3}\end{array}$$

 

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

 

$$\begin{array}{}\text{(11)}& \frac{{\mathbf{q}}_{1:3}}{q_0}={((\lambda +tr(B))I-B-B^T)}^{-1}\mathbf{z}\end{array}$$

 

참고로 $\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$ 이 성립하기 때문에, 쿼터니언의 실수부는 다음과 같이 계산할 수 있고,

 

$$\begin{array}{}\text{(12)}& 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)}}\end{array}$$

 

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

 

$$\begin{array}{}\text{(13)}& {\mathbf{q}}_{1:3}=q_0\frac{{\mathbf{q}}_{1:3}}{q_0}\end{array}$$

 

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

 

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

 

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

 

$$\begin{array}{rl}\text{(14)}& & \sigma =tr(B)& \rho =\lambda +\sigma \\ & S=B+B^T\end{array}$$

 

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

 

$$\begin{array}{}\text{(15)}& \frac{{\mathbf{q}}_{1:3}}{q_0}=(\rho I-S{)}^{-1}\mathbf{z}\end{array}$$

 

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

 

$$\begin{array}{r}\text{(16)}& f(\rho )& =det(\rho I-S)\\ & =-{\rho }^3+tr(S){\rho }^2-\frac{1}{2}(tr(S{)}^2-tr(S^2))\rho +det(S)\\ \\ & =0\end{array}$$

 

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

 

$$\begin{array}{r}\text{(17)}& f(S)& =-S^3+tr(S)S^2-\frac{1}{2}(tr(S{)}^2-tr(S^2))S+det(S)\\ & =0\end{array}$$

 

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

 

$$\begin{array}{}\text{(18)}& (\rho I-S{)}^{-1}={\alpha }_{0}I+{\alpha }_{1}S+{\alpha }_2{S}^2\end{array}$$

 

여기서 ${\alpha }_0,{\alpha }_1,{\alpha }_2$ 는 위 식을 만족하는 어떤 상수다. 계산상의 편의를 위해서 각 상수를 다음과 같이 다른 상수 $\alpha ,\beta ,\gamma$ 로 표현한다.

 

$$\begin{array}{}\text{(19)}& (\rho I-S{)}^{-1}=\frac{\alpha }{\gamma }I+\frac{\beta }{\gamma }S+\frac{1}{\gamma }{S}^2\end{array}$$

 

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

 

$$\begin{array}{r}\text{(20)}& (\rho I-S)(\rho I-S{)}^{-1}& =(\rho I-S)(\frac{\alpha }{\gamma }I+\frac{\beta }{\gamma }S+\frac{1}{\gamma }{S}^2)\\ & =I\end{array}$$

 

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

 

$$\begin{array}{}\text{(21)}& \rho \alpha I+(\rho \beta -\alpha )S+(\rho -\beta )S^2-S^3=\gamma I\end{array}$$

 

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

 

$$\begin{array}{}\text{(22)}& S^3=tr(S)S^2-\frac{1}{2}(tr(S{)}^2-tr(S^2))S+det(S)\end{array}$$

 

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

 

$$\begin{array}{}\text{(23)}& (\rho \alpha -\gamma -det(S))I+(\rho \beta -\alpha +\kappa )S+(\rho -\beta -tr(S))S^2=0\end{array}$$

 

여기서

 

$$\begin{array}{}\text{(24)}& \kappa =\frac{1}{2}(tr(S{)}^2-tr(S^2))\end{array}$$

 

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

 

$$\begin{array}{r}\text{(25)}& \gamma & =\rho \alpha -det(S)=(\lambda +\sigma )\alpha -det(S)\\ \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{array}$$

 

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

 

$$\begin{array}{}\text{(26)}& tr(B)+{\mathbf{z}}^T{((\lambda +tr(B))I-B-B^T)}^{-1}\mathbf{z}=\lambda \end{array}$$

 

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

 

$$\begin{array}{}\text{(27)}& \gamma \sigma +{\mathbf{z}}^T(\alpha I+\beta S+S^2)\mathbf{z}=\gamma \lambda \end{array}$$

 

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

 

$$\begin{array}{r}\text{(28)}& & ((\lambda +\sigma )({\lambda }^2-{\sigma }^2+\kappa )-det(S))\sigma \\ & +{\mathbf{z}}^T(({\lambda }^2-{\sigma }^2+\kappa )I+(\lambda -\sigma )S+S^2)\mathbf{z}\\ \\ & =((\lambda +\sigma )({\lambda }^2-{\sigma }^2+\kappa )-det(S))\lambda \end{array}$$

 

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

 

$$\begin{array}{}\text{(29)}& {\lambda }^4+a_2{\lambda }^2+a_1\lambda +a_0=0\end{array}$$

 

여기서

 

$$\begin{array}{rl}{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{array}$$

 

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

 

$$\begin{array}{}\text{(30)}& p(\lambda )=det(K-\lambda I)=0\end{array}$$

 

매트랩 함수 '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 반복법의 초기 추정값으로 이 값을 선정한다. 즉,

 

$$\begin{array}{}\text{(31)}& {\lambda }_0=\sum _{k=1}^n{w}_k\end{array}$$

 

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

 

$$\begin{array}{}\text{(32)}& {\lambda }_i={\lambda }_{i-1}-\frac{p({\lambda }_{i-1})}{p^{\prime }({\lambda }_{i-1})},i=1,2,...\end{array}$$

 

최대 고유값을 구했다면, 이제 이 고유값에 대응하는 고유벡터를 식 (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$ 가 다음과 같이 주어졌다고 하자.

 

$$\begin{array}{}\text{(33)}& {\mathbf{s}}_1^a=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],{\mathbf{s}}_2^a=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(34)}& C_b^a=\left[\begin{array}{ccc}0.9254& 0.0180& 0.3785\\ 0.1632& 0.8826& -0.4410\\ -0.3420& 0.4698& 0.8138\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(35)}& {\mathbf{s}}_1^b=\left[\begin{array}{c}0.9254\\ 0.0180\\ 0.3785\end{array}\right],{\mathbf{s}}_2^b=\left[\begin{array}{c}-0.3420\\ 0.4698\\ 0.8138\end{array}\right]\end{array}$$

 

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

 

$$B=\left[\begin{array}{ccc}0.9254& 0.0180& 0.3785\\ 0& 0& 0\\ -0.3420& 0.4698& 0.8138\end{array}\right]$$

 

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

 

$$p(\lambda )=0={\lambda }^4-4{\lambda }^2$$

 

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

 

$$\mathbf{q}=\left[\begin{array}{c}0.9515\\ 0.2393\\ 0.1893\\ 0.0381\end{array}\right]$$

 

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

 

$$C_b^a=\left[\begin{array}{ccc}0.9254& 0.0180& 0.3785\\ 0.1632& 0.8826& -0.4410\\ -0.3420& 0.4698& 0.8138\end{array}\right]$$

 

 

 

다음은 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];