IMU Preintergration on Maniforld for Efficient Visual-Inertial Maximum-a-Posteriori Estimation(2)

IMU Preintergration on Maniforld for Efficient Visual-Inertial Maximum-a-Posteriori Estimation

 

이전 포스팅엔 페이퍼를 읽기 위해 약간의 백그라운드를 적었습니다. 쓰다보니 논문 내용은 하나도 못담고 Lie Group과 Lie Algebra에 대한 내용만 적었네요.

 

IMU Model

Angular velocity and Acceleration

다시 돌아가서 IMU의 측정값에서 우리가 관심있는 두가지는 angular velocity(각속도)와 acceleration(가속도)입니다.

 

In general, the angular velocity in an n-dimensional space is the time derivative of the angular displacement tensor, which is a second rank skew-symmetric tensor. 

 

바로 각속도가 \(so(3)\) 에 해당합니다. 위 수식을 간단히 설명하면 Prefix \(B\) 는 IMU의 Body, \(W\)는 World를, \(R_{WB}, \prescript{W}p\)는 B 에서 W의 transformation을 의미합니다. 하지만 센서는 완벽히 정확한 값을 주지 않고 bias \(b\) 와 noise \(\eta\)를 포함한 측정값을 줍니다.

 

IMU Motion Integration

우리가 하고싶은 것은 제목에서도 보여지듯이 Maximum-a-posteriori, 즉 최적화입니다. 이를 위해서 State 정의를 하는데 \([R_{i}, p_{i}, v_{i}, b_{i}]\)입니다. Bias를 제외하면 회전, 위치, 속도에 해당하고 이는 우리에게 꽤 친숙하니 바로 들어가겠습니다. 

Rotation, Velocity, Translation

위 3개의 수식은 전부 자명합니다. 다만 \(Exp(\prescript{B}\omega_{WB}(t)\delta t)\) 만 보고 넘어가면 Lie Group 과 Lie Algebra 사이에는 아래와 같은 Mapping이 존재합니다.

Correspondence between SO(3), SE(3), so(3), se(3)

따라서 위 회전 역시 단순히 회전행렬 곱이 됩니다.

Rotation, Velocity, Translation with IMU Model

수식에 우리의 IMU Model(측정값 아님)을 대입해주면 위와 같은 형태가 됩니다. 완벽하진 않지만 이제 얼추 우리가 원하는 State을 표현할 수 있게 되었습니다.