불변 확장 칼만 필터

**불변 확장 칼만 필터**(Invariant extended Kalman filter, **IEKF**)반복 확장 칼만 필터(iterated extended Kalman filter)와 혼동하지 말 것.는 대칭성(또는 불변성)을 가진 비선형 시스템을 위한 확장 칼만 필터(EKF)의 한 버전으로 처음 도입되었고,Bonnabel, S. (2007, December). Left-invariant extended Kalman filter and attitude estimation. In 2007 46th IEEE Conference on Decision and Control (pp. 1027-1032). IEEE. 이후 리 군에 대한 선형 칼만 필터링 이론의 적용으로 일반화되고 재구성되었다.Barrau, A., & Bonnabel, S. (2016). The invariant extended Kalman filter as a stable observer. IEEE Transactions on Automatic Control, 62(4), 1797-1812.issn=1066-033X 선형 출력 오차에 기반한 선형 보정 항을 사용하는 대신, IEKF는 불변 출력 오차에 기반한 기하학적으로 적응된 보정 항을 사용한다. 이와 동일하게 이득 행렬은 선형 상태 오차가 아닌 불변 상태 오차로부터 업데이트된다. 주요 장점은 이득 및 공분산 방정식이 상태의 추정 값에 대한 의존도가 줄어든다는 것이다. 어떤 경우에는 EKF의 경우보다 훨씬 더 큰 궤적 집합에서 일정한 값으로 수렴하여 추정의 더 나은 수렴을 가져온다.
# 필터 유도

## 이산 시간 프레임워크
시간 단계 n 에서 상태가 리 군 G 의 요소 x_n 에 의해 인코딩되고 다음과 같은 동역학을 갖는 시스템을 고려하자.Barrau, A., & Bonnabel, S. (2019). Linear observed systems on groups. Systems & Control Letters, 129, 36-42. : x_n = \phi_n(x_{n-1}) \cdot u_n 여기서 \phi_n 은 G 의 군 자기동형사상이고, \cdot 은 군 연산이며, u_n 은 G 의 요소이다. 시스템은 다음과 같은 형태의 측정 y_n 을 통해 관측된다고 가정한다. : y_n = x_n * b_n 여기서 b_n 은 G 의 요소들의 왼쪽 작용이 부여된 벡터 공간 \mathcal{Y} 에 속하며, 이 작용은 다시 * 로 표시된다 (연산의 두 번째 항이 G 가 아닌 \mathcal{Y} 의 요소이므로 군 연산과 혼동되지 않는다). 대안적으로, 동일한 이론은 오른쪽 작용으로 정의된 측정에도 적용된다. : y_n = b_n * x_n
## 필터 방정식
불변 확장 칼만 필터는 측정 함수가 왼쪽 작용인 경우 다음 방정식으로 정의되는 관측기 \hat{x}_n 이다. : \hat{x}_{n|n-1} = \phi_n(\hat{x}_{n-1|n-1}) \cdot u_n : \hat{x}_{n|n} = \hat{x}_{n|n-1} \cdot \exp(K_n (\hat{x}_{n|n-1}^{-1} * y_n-b_n)) 여기서 \exp() 는 G 의 리 지수 사상이며 K_n 은 리카티 방정식을 통해 조정되는 이득 행렬이다. 측정 함수가 오른쪽 작용인 경우 업데이트 상태는 다음과 같이 정의된다. : \hat{x}_{n|n} = \exp(K_n (y_n * \hat{x}_{n|n-1}^{-1}-b_n)) \cdot \hat{x}_{n|n-1}
## 연속 시간 프레임워크
위의 이산 시간 프레임워크는 원래 다음과 같은 형태의 연속 시간 동역학에 대해 도입되었다. : \frac{d}{dt} x_t = f_t(x_t), 여기서 벡터 필드 f_t 는 임의의 시간 t 에서 다음 관계를 만족한다. : \forall a,b \in G, f_t(a \cdot b) = f_t(a) b + a f_t(b) - a f(Id)b 여기서 군의 항등원은 Id 로 표시되고, g u = L_g(u) (각각 u g = R_g(u) )는 왼쪽 변환 L_g: TG_x \rightarrow TG_{gx} (각각 오른쪽 변환 R_g: TG_x \rightarrow TG_{xg} )의 축약 표기법으로 사용되며, 여기서 TG_x 는 x 에서의 G 에 대한 접공간을 나타낸다. 이는 이산 시간 프레임워크보다 더 복잡한 계산을 필요로 하지만, 속성은 유사하다.
# 주요 속성
불변 확장 칼만 필터링의 주요 장점은 불변 오차 변수의 동작이며, 이 변수의 정의는 측정 유형에 따라 달라진다. 왼쪽 작용의 경우 왼쪽 불변 오차 변수를 다음과 같이 정의한다. : e^L_{n|n-1} = \hat{x}_{n|n-1}^{-1} x_n , : e^L_{n|n} = \hat{x}_{n|n}^{-1} x_n , 반면 오른쪽 작용의 경우 오른쪽 불변 오차 변수를 다음과 같이 정의한다. : e^R_{n|n-1} = x_n \hat{x}_{n|n-1}^{-1} , : e^R_{n|n} = x_n \hat{x}_{n|n}^{-1} , 실제로 x_n , \hat{x}_{n|n-1} , \hat{x}_{n|n} 을 그들의 값으로 대체하면, 몇몇 대수학적 계산 후에 왼쪽 작용에 대해 다음을 얻는다. : e^L_{n|n-1} = u_n^{-1} \cdot \phi_n(e^L_{n-1|n-1}) \cdot u_n , : e^L_{n|n} = \exp(-K_n(e^L_{n|n-1} * b_n-b_n)) \cdot e^L_{n|n-1} , 그리고 오른쪽 작용에 대해서는 다음과 같다. : e^R_{n|n-1} = \phi_n(e^R_{n-1|n-1}) , : e^R_{n|n} = e^R_{n|n-1} \exp(-K_n(b_n * e^R_{n|n-1}-b_n)) , 상태의 추정 값이 오차 변수가 따르는 방정식에 포함되지 않는다는 것을 알 수 있는데, 이는 고전적인 확장 칼만 필터와 달리 선형 칼만 필터링의 속성이다. 그러나 선형 경우와의 유사성은 훨씬 더 깊다. 항등식으로 정의된 오차 변수의 선형 버전 \xi_{n|n-1}, \xi_{n|n} 을 다음과 같이 두자. : e_{n|n-1} = \exp(\xi_{n|n-1}), : e_{n|n} = \exp(\xi_{n|n}). 그러면 테일러 전개 \xi_{n|n} = F_n \xi_{n|n-1} + \circ(||\xi_{n|n-1}||) 에 의해 정의된 F_n 에 대해 실제로 다음이 성립한다. : \xi_{n|n} = F_n\xi_{n|n-1}. 다른 말로 하면, 고차 항이 없다. 즉, 오차 변수 \xi_{n|n}, \xi_{n|n-1} 에 대한 동역학은 선형이다. 이 결과와 오차 동역학의 독립성은 IEKF의 이론적 속성과 실질적인 성능의 핵심이다.
# 대칭 보존 관측기와의 관계
대부분의 물리 시스템은 자연스러운 대칭성(또는 불변성)을 가지고 있다. 즉, 시스템을 변경되지 않게 하는 변환(예: 회전, 병진, 스케일링)이 존재한다. 수학적 및 공학적 관점에서, 고려 중인 시스템에 잘 설계된 필터는 동일한 불변성 속성을 유지해야 한다는 것은 합리적이다. IEKF의 아이디어는 시스템의 대칭성을 활용하기 위해 EKF 방정식을 수정하는 것이다.
## 정의
다음 시스템을 고려하자. : \dot{x}{=}f(x,u)+M(x) w : y {=}h(x,u) +N(x)v 여기서 w,v 는 독립적인 백색 가우스 잡음이다. 항등원 e를 가진 리 군 G와 (국소) 변환군 \varphi_g, \psi_g, \rho_g (g \in G)가 (X,U,Y)=(\varphi_g(x),\psi_g(u),\rho_g(y))를 만족한다고 가정하자. 잡음이 있는 이전 시스템은 변환군 \varphi_g, \psi_g, \rho_g의 작용에 의해 변경되지 않으면 불변이라고 한다. 즉, 다음이 성립한다. : \dot X{=}f(X,U)+M(X) w . : Y{=}h(X,U) +N(X)v
## 필터 방정식 및 주요 결과
대칭 보존 필터이므로, IEKF의 일반적인 형태는 다음과 같다.S. Bonnabel, Ph. Martin, and P. Rouchon, “Symmetry-preserving observers,” IEEE Transactions on Automatic and Control, vol. 53, no. 11, pp. 2514–2526, 2008. :\dot{\hat x}=f(\hat x,u) +W(\hat x)L\Bigl(I(\hat x,u),E(\hat x,u,y)\Bigr)E(\hat x,u,y) 여기서 * E(\hat x,u,y)는 불변 출력 오차이며, 이는 일반적인 출력 오차 \hat y-y와 다르다. * W(\hat x)=\bigl(w_1(\hat x),..,w_n(\hat x)\bigr)는 불변 프레임이다. * I(\hat x,u)는 불변 벡터이다. * L(I,E)는 자유롭게 선택된 이득 행렬이다. 오차 수렴을 분석하기 위해 불변 상태 오차 \eta(\hat x,x)가 정의되는데, 이는 표준 출력 오차 \hat x-x 와 다르다. 표준 출력 오차는 일반적으로 시스템의 대칭성을 보존하지 않기 때문이다. 고려된 시스템과 관련된 변환군이 주어지면, 이동 프레임 방법을 기반으로 E(\hat x,u,y),W(\hat x),I(\hat x,u),\eta(\hat x,x)를 결정하는 구성적 방법이 존재한다. EKF와 유사하게, 이득 행렬 L(I,E)은 다음 방정식으로부터 결정된다. : L{=}PC^T\bigl(N(e)N^T(e)\bigr)^{-1}, : \dot P{=}AP+PA^T+M(e)M^T(e)-PC^T\bigl(N(e)N^T(e)\bigr)^{-1}CP, 여기서 행렬 A,C는 표준 EKF의 경우와 같이 (\hat x,u)에 의존하는 대신, 알려진 불변 벡터 I(\hat x,u)에만 의존한다. 이러한 훨씬 단순한 의존성과 그 결과가 IEKF의 주요 관심사이다. 실제로 행렬 A,C는 EKF의 경우와 같은 평형점보다 훨씬 더 큰 궤적 집합(영구 궤적이라고 불림)에서 일정하다. 이러한 궤적 근처에서는 수렴이 보장되는 "진정한", 즉 선형 칼만 필터로 돌아간다. 비공식적으로, 이는 IEKF가 일반적으로 EKF의 경우와 같이 느리게 변하는 평형점 주변뿐만 아니라 모든 느리게 변하는 영구 궤적 주변에서도 수렴함을 의미한다.
# 적용 사례

## 자세 및 방향 참조 시스템
불변 확장 칼만 필터는 예를 들어 자세 및 방향 참조 시스템에 사용된다. 이러한 시스템에서는 항공기 등 움직이는 강체의 방향, 속도 및 위치가 관성 센서, 자력계, GPS 또는 소나와 같은 다양한 내장 센서로부터 추정된다. IEKF의 사용은 자연스럽게 사원수 오차 \hat qq^{-1}를 고려하게 하며,S. Bonnabel, Ph. Martin and E. Salaün, "Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem", 48th IEEE Conference on Decision and Control, pp. 1297–1304, 2009. 이는 사원수군의 제약을 보존하기 위한 임시 방편으로 종종 사용된다. EKF와 비교한 IEKF의 이점은 광범위한 궤적에 대해 실험적으로 입증되었다.Ph. Martin and E. Salaün, "Generalized Multiplicative Extended Kalman Filter for Aided Attitude and Heading Reference System", AIAA Guidance, Navigation and Control Conference, 2010
## 관성 항법
불변 확장 칼만 필터의 주요 응용 분야는 관성 항법이며, 이는 상태(자세 행렬 R , 속도 벡터 v , 위치 벡터 x 로 구성)를 리 군 SE_2(3) 에 포함시킨 후 프레임워크에 적합하다. SE_2(3) 은 다음 군 연산으로 정의된다. :(R_1, v_1, x_1) \cdot (R_2, v_2, x_2) = (R_1 R_2, x_1 + R_1 x_2, v_1 + R_1 v_2)
## 위치 측정 및 동시 지도화
위치 측정 및 동시 지도화 문제도 상태(자세 행렬 R , 위치 벡터 x , 그리고 정적 특징점들의 시퀀스 p^1, \dots, p^K 로 구성)를 리 군 SE_{K+1}(3) (또는 평면 시스템의 경우 SE_{K+1}(2) )Barrau, A., & Bonnabel, S. (2015). An EKF-SLAM algorithm with consistency properties. arXiv preprint 1510.06263.에 포함시킨 후 불변 확장 칼만 필터링 프레임워크에 적합하다. 이 군은 다음 군 연산으로 정의된다. :(R_1, x_1, p_1^1, \dots, p_1^K) \cdot (R_2, x_2, p_2^2, \dots, p_2^K) = (R_1 R_2, x_1 + R_1 x_2, p_1^1 + R_1 p_2^1, \dots, p_1^K + R_1 p_2^K) 이 경우 불변 확장 칼만 필터의 주요 장점은 잘못된 관측 가능성 문제를 해결하는 것이다.
# 내용주