[INS] 관성항법시스템 오차 방정식 (INS Error Equations)

관성항법시스템(INS)은 초기 위치, 속도 및 자세 정보와 가속도계 및 자이로스코프에서 얻어지는 측정 정보를 이용하여 현재의 위치, 속도 및 자세 정보를 제공하는 시스템이다. INS는 항법 좌표계에서 가속도를 적분하여 속도와 위치를 결정하는데, 가속도 신호는 동체 좌표계에서 측정되므로 이 값을 동체 좌표계에서 항법 좌표계로 변환해야 한다. 그런데 동체 좌표계와 항법 좌표계간에는 자세 변화가 있으므로 두 좌표계간의 자세각을 알아야 하고, 이를 위해서는 동체 좌표계에서 측정된 자이로스코프 신호를 적분해야 한다.

 

 

따라서 INS는 위치 결정을 위해서는 세 번의 수치적분, 속도 결정을 위해서는 두 번의 수치적분, 자세 결정을 위해서는 한 번의 수치적분이 수행되어야 한다. 이와 같은 수치적분 때문에 INS의 항법 오차는 시간이 지남에 따라 누적된다는 단점이 있다.

INS 오차 방정식은 관성센서 오차, 계산 오차, 환경 및 정렬 오차 등과 같은 다양한 오차가 시간이 따라 어떻게 항법 오차로 전파되는지를 미분방정식으로 표현한 것이다. INS 오차 방정식은 섭동(perturbation) 방법으로 구할 수 있다. 실제로 항법 오차는 항법 좌표계에 대한 섭동으로 정의할 수 있다. INS가 계산한 항법 좌표계는 실제 항법 좌표계와 다르므로 항법 좌표계에 관련된 위치, 속도, 자세각 또는 각속도 등에 대해서는 섭동을 수행해야 한다.

항법 좌표계에서 지구 자전 각속도 벡터는 다음 식으로 계산한다 (https://pasus.tistory.com/323).

 

\[ \begin{align} \hat{\omega}_{ie}^n = \begin{bmatrix} \omega_{ie} \cos \hat{\lambda}_{lat} \\ 0 \\ -\omega_{ie} \sin \hat{\lambda}_{lat} \end{bmatrix} \tag{1} \end{align} \]

 

여기서 hat(\(\hat{ \ \ \ }\)) 표시는 항법 컴퓨터가 계산한 값을 나타낸다. 항법 변수의 오차는 계산된 값과 실제값(참값)의 차이로 정의한다. 즉,

 

\[ \begin{align} & \delta \omega_{ie}^n= \hat{\omega}_{ie}^n- \omega_{ie}^n \tag{2} \\ \\ & \delta \lambda_{lat}= \hat{\lambda}_{lat}- \lambda_{lat} \end{align} \]

 

여기서 \(\delta ( \ \ )\) 는 오차를 뜻한다. 오차가 매우 작다는 가정하에 식 (2)를 (1)에 대입하고 근사화시키면 다음과 같이 지구 자전 각속도 벡터의 선형화된 오차 방정식을 얻을 수 있다.

 

\[ \begin{align} \omega_{ie}^n+ \delta \omega_{ie}^n &= \begin{bmatrix} \omega_{ie} \cos⁡ (\lambda_{lat}+ \delta \lambda_{lat} ) \\ 0 \\ -\omega_{ie} \sin (\lambda_{lat}+ \delta \lambda_{lat} ) \end{bmatrix} \tag{3} \\ \\ & \approx \begin{bmatrix} \omega_{ie} \cos \lambda_{lat} - \omega_{ie} \sin \lambda_{lat} \delta \lambda_{lat} \\ 0 \\ -\omega_{ie} \sin \lambda_{lat} - \omega_{ie} \cos \lambda_{lat} \delta \lambda_{lat} \end{bmatrix} \\ \\ \to \ \delta \omega_{ie}^n \approx & \begin{bmatrix} -\omega_{ie} \sin \lambda_{lat} \delta \lambda_{lat} \\ 0 \\ -\omega_{ie} \cos \lambda_{lat} \delta \lambda_{lat} \end{bmatrix} = \begin{bmatrix} -\omega_{ie} \sin \lambda_{lat} & 0 & 0 \\ 0 & 0 & 0 \\ -\omega_{ie} \cos \lambda_{lat} & 0 & 0 \end{bmatrix} \begin{bmatrix} \delta \lambda_{lat} \\ \delta \lambda_{lon} \\ \delta h \end{bmatrix} \end{align} \]

 

ECEF 좌표계에 대한 항법 좌표계의 각속도 벡터는 다음 식으로 계산한다.

 

\[ \begin{align} \omega_{en}^n= \begin{bmatrix} \dot{\hat{\lambda}}_{lon} \cos \hat{\lambda}_{lat} \\ -\dot{\hat{\lambda}}_{lat} \\ -\dot{\hat{\lambda}}_{lon} \sin \hat{\lambda}_{lat} \end{bmatrix} = \begin{bmatrix} \frac{ \hat{v}_E }{ ( \hat{R}_{tr}+ \hat{h} ) } \\ -\frac{ \hat{v}_N}{(\hat{R}_{mr}+ \hat{h}) } \\ - \frac{ \hat{v}_E \tan \hat{\lambda}_{lat}}{( \hat{R}_{tr}+ \hat{h} ) } \end{bmatrix} \tag{4} \end{align} \]

 

식 (3)과 마찬가지로 식 (4)의 각 항법 변수의 섭동을 고려하면 다음과 같이 선형화된 오차 방정식을 구할 수 있다.

 

\[ \begin{align} \delta \omega_{en}^n & \approx \begin{bmatrix} -\frac{v_E}{(R_{tr}+h)^2} \delta h+ \frac{1}{(R_{tr}+h)} \delta v_E \\ \frac{v_N}{(R_{mr}+h)^2} \delta h- \frac{1}{(R_{mr}+h) } \delta v_N \\ \frac{v_E \tan \lambda_{lat} }{(R_{tr}+h)^2} \delta h- \frac{ \tan \lambda_{lat} }{(R_{tr}+h) } \delta v_E- \frac{v_E}{(R_{tr}+h) \cos^2 \lambda_{lat} } \delta \lambda_{lat} \end{bmatrix} \tag{5} \\ \\ &= \begin{bmatrix} 0 & 0 & -\frac{v_E}{(R_{tr}+h)^2} \\ 0 & 0 & \frac{v_N}{(R_{mr}+h)^2} \\ -\frac{v_E}{(R_{tr}+h) \cos^2 \lambda_{lat} } & 0 & \frac{v_E \tan \lambda_{lat} }{(R_{tr}+h)^2 } \end{bmatrix} \begin{bmatrix} \delta \lambda_{lat} \\ \delta \lambda_{lon} \\ \delta h \end{bmatrix} \\ \\ & \ \ \ \ \ + \begin{bmatrix} 0 & \frac{1}{(R_{tr}+h) } & 0 \\ - \frac{1}{(R_{mr}+h) } & 0 & 0 \\ 0 & -\frac{\tan \lambda_{lat} }{(R_{tr}+h) } & 0 \end{bmatrix} \begin{bmatrix} \delta v_N \\ \delta v_E \\ \delta v_D \end{bmatrix} \end{align} \]

 

여기서 접선 반경(tangential radius)의 섭동 \(\delta R_{tr}\) 과 자오선 반경(meridian radius)의 섭동 \(\delta R_{mr}\) 은 무시하였다.

 

 

항법 좌표계에서 동체 좌표계로의 DCM은 다음 미분방정식으로 계산한다.

 

\[ \begin{align} \dot{\hat{C}}_b^n= \hat{C}_b^n [ \hat{\omega}_{nb}^b \times ] \tag{6} \end{align} \]

 

여기서 DCM의 오차를 다음과 같이 덧셈형과 곱셈형으로 정의해보자.

 

\[ \begin{align} \hat{C}_b^n &= C_b^n+ \delta C_b^n \tag{7} \\ \\ &= C_n^c C_b^n \end{align} \]

 

여기서 \(\delta C_b^n\) 은 덧셈형 오차라고 하고 \(C_n^c\) 은 곱셈형 오차라고 한다. '계산된 항법 좌표계' \(\{c\}\) 는 \(\hat{C}_b^n=C_b^c\) 가 되도록 정의한다. 그러면 덧셈형 오차와 곱셈형 오차는 다음 관계식을 갖는다.

 

\[ \begin{align} \delta C_b^n &= C_n^c C_b^n-C_b^n \tag{8} \\ \\ &=(C_n^c-I) C_b^n \end{align} \]

 

식 (7)을 (6)에 대입하면 다음과 같다.

 

\[ \begin{align} & \dot{C}_n^c C_b^n+C_n^c \dot{C}_b^n=C_n^c C_b^n \left( [\omega_{nb}^b \times ]+[\delta \omega_{nb}^b \times] \right) \tag{9} \\ \\ & \to \ \dot{C}_n^c C_b^n+C_n^c \left( C_b^n [\omega_{nb}^b \times] \right) = C_n^c C_b^n \left( [\omega_{nb}^b \times ]+[ \delta \omega_{nb}^b \times] \right) \\ \\ & \to \ \dot{C}_n^c=C_n^c C_b^n [\delta \omega_{nb}^b \times ] (C_b^n )^T \end{align} \]

 

\(C_n^c\) 는 계산된 항법 좌표계와 실제 항법 좌표계간의 자세 오차를 의미하므로 자세 오차각이 매우 작다는 가정을 하면 다음과 같이 표현할 수 있다.

 

\[ \begin{align} C_c^n &= C(z, \delta \Phi_D ) C(y, \delta \Phi_E ) C(x, \delta \Phi_N ) \tag{10} \\ \\ & \approx \begin{bmatrix} 1 & - \delta \Phi_D & 0 \\ \delta \Phi_D & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & \delta \Phi_E \\ 0 & 1 & 0 \\ -\delta \Phi_E & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & -\delta \Phi_N \\ 0 & \delta \Phi_N & 1 \end{bmatrix} \\ \\ & \approx \begin{bmatrix} 1 & -\delta \Phi_D & \delta \Phi_E \\ \delta \Phi_D & 1 & -\delta \Phi_N \\ -\delta \Phi_E & \delta \Phi_N & 1 \end{bmatrix} \\ \\ &= I+[ \delta \Phi \times ] \end{align} \]

 

여기서

 

\[ \begin{align} \delta \Phi = \begin{bmatrix} \delta \Phi_N \\ \delta \Phi_E \\ \delta \Phi_D \end{bmatrix} \end{align} \]

 

이며, \(\delta \Phi_N \) 과 \(\delta \Phi_E\) 는 수직축에 대한 자세각 오차로서 틸트(tilt) 오차라고 하며 \(\delta \Phi_D\) 는 방향각 오차라고 한다. 식 (10)에서 주의할 점은 자세각 오차 \(\delta \Phi\) 는 실제 항법 좌표계를 기준으로 정의되었으므로 \(C_n^c\) 가 아니라 \(C_c^n\) 로 주어졌다는 것이다. \(C_c^n\) 는 실제 항법 좌표계에서 '계산된 항법 좌표계'로의 DCM이다.

 

 

\(C_n^c\) 는 \(C_c^n\) 의 전치(transpose) 행렬이므로 다음과 같다.

 

\[ \begin{align} C_n^c=(C_c^n )^T=I-[ \delta \Phi \times ] \tag{11} \end{align} \]

 

식 (11)을 이용하면 식 (9)는 다음과 같이 된다.

 

\[ \begin{align} [\delta \dot{\Phi} \times ] &= -(I-[\delta \Phi \times ]) C_b^n [\delta \omega_{nb}^b \times ] (C_b^n )^T \tag{12} \\ \\ & \approx -C_b^n [\delta \omega_{nb}^b \times ] (C_b^n )^T \end{align} \]

 

또는 자세각 오차 미분방정식은 다음과 같이 된다.

 

\[ \begin{align} \delta \dot{\Phi} =-C_b^n \delta \omega_{nb}^b \tag{13} \end{align} \]

 

항법 좌표계에 대한 동체 좌표계의 각속도 벡터는 다음과 같이 계산한다.

 

\[ \begin{align} \hat{\omega}_{nb}^b= \hat{\omega}_{ib}^b-(\hat{C}_b^n )^T \hat{\omega}_{in}^n \tag{14} \end{align} \]

 

\(\hat{\omega}_{nb}^b=\omega_{nb}^b+\delta \omega_{nb}^b\) 및 \(\hat{\omega}_{in}^n= \omega_{in}^n+\delta \omega_{in}^n\) 와 식 (7)을 대입하면,

 

\[ \begin{align} \omega_{nb}^b+\delta \omega_{nb}^b= \omega_{ib}^b+ \delta \omega_{ib}^b-(C_n^c C_b^n )^T (\omega_{in}^n+ \delta \omega_{in}^n ) \tag{15} \end{align} \]

 

이 된다. 여기서 \(\delta \omega_{nb}^b\) 을 풀면 다음과 같다.

 

\[ \begin{align} & -(C_b^n )^T \omega_{in}^n+ \delta \omega_{nb}^b= \delta \omega_{ib}^b-(C_b^n )^T (C_n^c )^T (\omega_{in}^n+ \delta \omega_{in}^n ) \tag{16} \\ \\ & \to \ \delta \omega_{nb}^b= \delta \omega_{ib}^b+(C_b^n )^T \left( I-(C_n^c )^T \right) \omega_{in}^n-(C_b^n )^T (C_n^c )^T \delta \omega_{in}^n \end{align} \]

 

식 (11)과 (12)를 이용하면 식 (16)은 다음과 같이 된다.

 

\[ \begin{align} \delta \dot{\Phi} &= -C_b^n \delta \omega_{ib}^b-(I-(C_n^c )^T ) \omega_{in}^n+(C_n^c )^T \delta \omega_{in}^n \tag{17} \\ \\ &=-C_b^n \delta \omega_{ib}^b- \left( I-(I+[\delta \Phi \times]) \right) \omega_{in}^n+(I+[\delta \Phi \times] ) \delta \omega_{in}^n \\ \\ & \approx -C_b^n \delta \omega_{ib}^b+[\delta \Phi \times ] \omega_{in}^n + \delta \omega_{in}^n \\ \\ &=-C_b^n \delta \omega_{ib}^b-[\omega_{in}^n \times ] \delta \Phi + \delta \omega_{in}^n \end{align} \]

 

여기서 \(\delta \omega_{in}^n = \delta \omega_{ie}^n+ \delta \omega_{en}^n\) 이며 \(\delta \omega_{ib}^b\) 는 동체 좌표계에서의 자이로스코프 측정 오차다. 식 (17)을 자세각 오차 미분방정식이라고 한다.

 

 

항법 좌표계에서 속도는 다음 미분방정식으로 계산한다.

 

\[ \begin{align} \dot{\hat{ \mathbf{v} }}^n =-([\hat{\omega}_{en}^n \times ]+2[ \hat{\omega}_{ie}^n \times]) \hat{\mathbf{v}}^n+\hat{C}_b^n \hat{\mathbf{f}}^b+ \hat{\mathbf{g}}_{ER}^n \tag{18} \end{align} \]

 

계산되는 값과 실제값의 차이를 오차로 정의하고,

 

\[ \begin{align} \hat{\mathbf{v}}^n-\mathbf{v}^n= \delta \mathbf{v}^n, \ \ \ \hat{\mathbf{f}}^b-\mathbf{f}^b=\delta \mathbf{f}^b, \ \ \ \hat{\mathbf{g}}_{ER}^n- \mathbf{g}_{ER}^n= \delta \mathbf{g}_{ER}^n \tag{19} \end{align} \]

 

식 (18)에 대입하면 다음과 같이 된다.

 

\[ \begin{align} \dot{\mathbf{v}}^n+ \delta \dot{\mathbf{v}}^n &= -([\omega_{en}^n \times]+[\delta \omega_{en}^n \times]+2[ \omega_{ie}^n \times]+2[ \delta \omega_{ie}^n \times ])( \mathbf{v}^n+ \delta \mathbf{v}^n ) \tag{20} \\ \\ & \ \ \ +(C_b^n+\delta C_b^n )( \mathbf{f}^b+ \delta \mathbf{f}^b )+ \mathbf{g}_{ER}^n+ \delta \mathbf{g}_{ER}^n \end{align} \]

 

위 식에서 \(\delta \dot{\mathbf{v}}^n\) 을 풀면 다음과 같다.

 

\[ \begin{align} \delta \dot{\mathbf{v}}^n & \approx -([\delta \omega_{en}^n \times ]+2[ \delta \omega_{ie}^n \times]) \mathbf{v}^n-([\omega_{en}^n \times ]+2[ \omega_{ie}^n \times]) \delta \mathbf{v}^n \tag{20} \\ \\ & \ \ \ + \delta C_b^n \mathbf{f}^b+C_b^n \delta \mathbf{f}^b+ \delta \mathbf{g}_{ER}^n \end{align} \]

 

여기서 식 (8)과 식 (11)에 의하면, \(\delta C_b^n=-[\delta \Phi \times] C_b^n\) 이므로 위 식에 대입하면 다음과 같이 된다.

 

\[ \begin{align} \delta \dot{\mathbf{v}}^n & \approx [\mathbf{v}^n \times ]( \delta \omega_{en}^n+2 \delta \omega_{ie}^n )-([\omega_{en}^n \times]+2[ \omega_{ie}^n \times]) \delta \mathbf{v}^n \tag{22} \\ \\ & \ \ \ +[\mathbf{f}^n \times] \delta \Phi+C_b^n \delta \mathbf{f}^b+ \delta \mathbf{g}_{ER}^n \end{align} \]

 

여기서 \(\delta \mathbf{f}^b\) 는 동체 좌표계에서의 가속도계 측정 오차다. \(\delta \mathbf{g}_{ER}^n\) 는 중력 가속도 계산 오차로서 다음과 같이 근사화 한다.

 

\[ \begin{align} \delta \mathbf{g}_{ER}^n \approx \begin{bmatrix} 0 \\ 0 \\ g_{eq} g_2 \delta h \end{bmatrix} \approx \begin{bmatrix} 0 \\ 0 \\ - \frac{2g_{eq}}{R_{eq}} \delta h \end{bmatrix} \tag{23} \end{align} \]

 

\(\hat{n}_1\) (N)과 \(\hat{n}_2\) (E) 방향의 중력 가속도 섭동은 무시했고 수직 방향만 고려하였다. 이에 대해서는 나중에 설명하도록 하겠다. 식 (22)를 속도 오차 미분방정식이라고 한다.

위도, 경도, 높이는 다음 미분방정식으로 계산한다.

 

\[ \begin{align} & \dot{\hat{\lambda}}_{lat}= \frac{ \hat{v}_N}{(\hat{R}_{mr}+\hat{h})} \tag{24} \\ \\ & \dot{\hat{\lambda}}_{lon}= \frac{\hat{v}_E}{(\hat{R}_{tr}+\hat{h}) \cos \hat{\lambda}_{lat}} \\ \\ & \dot{\hat{h}} ̇=- \hat{v}_D \end{align} \]

 

식 (24)에서 각 항법 변수의 섭동을 고려하면 다음과 같이 선형화된 오차 방정식을 구할 수 있다.

 

\[ \begin{align} \begin{bmatrix} \delta \dot{\lambda}_{lat} \\ \delta \dot{\lambda}_{lon} \\ \delta \dot{h} \end{bmatrix} &= \begin{bmatrix} -\frac{v_N}{(R_{mr}+h)^2} \delta h+ \frac{1}{(R_{mr}+h) } \delta v_N \\ -\frac{v_E}{(R_{tr}+h)^2 \cos \lambda_{lat} } \delta h+ \frac{ v_E \sin \lambda_{lat} }{ (R_{tr}+h) \cos^2 \lambda_{lat} } \delta \lambda_{lat}+ \frac{1}{(R_{tr}+h) \cos \lambda_{lat} } \delta v_E \\ -\delta v_D \end{bmatrix} \tag{25} \\ \\ &= \begin{bmatrix} 0 & 0 & -\frac{v_N}{(R_{mr}+h)^2} \\ \frac{v_E \sin \lambda_{lat}}{(R_{tr}+h) \cos^2 \lambda_{lat} } & 0 & - \frac{v_E}{(R_{tr}+h)^2 \cos \lambda_{lat} } \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \delta \lambda_{lat} \\ \delta \lambda_{lon} \\ \delta h \end{bmatrix} \\ \\ & \ \ \ + \begin{bmatrix} \frac{1}{(R_{mr}+h) } & 0 & 0 \\ 0 & \frac{1}{(R_{tr}+h) \cos \lambda_{lat} } & 0 \\ 0 & 0 & -1 \end{bmatrix} \begin{bmatrix} \delta v_N \\ \delta v_E \\ \delta v_D \end{bmatrix} \end{align} \]

 

여기서 접선 반경의 섭동 \(\delta R_{tr}\) 과 자오선 반경의 섭동 \(\delta R_{mr}\) 은 무시하였다. 식 (25)를 위치 오차 미분방정식이라고 한다.