[VIO] Stereo-MSCKF 알고리즘 분석 및 적용

S-MSCKF 는 stereo camera 를 사용하는 MSCKF 알고리즘 이다

MSCKF 는 IMU 와 Camera 센서를 사용하여 고정된 feature 에 대해서 measurement update 를 수행하여 odometry 를 제공하는 알고리즘이다.

 

MSCKF는 기본알고리즘은 아래와 같다

 

 

- State Definition

State 에서 1개의 IMU state 와 N 개의 Cam sate 가 있다.

 

- Propagation (IMU)

위의 그림과 같이 IMU 의 msg 는 Camera msg 보다 많은 양의 msg 가 들어온다. 따라서 Camera msg 와 다음 Camera msg 사이동안 받은 IMU msg 를 IMU buffer에 저장하고 Camera msg 가 들어오면 imu Propagation 을 진행한다

 

imu propagation 수식은 아래와 같다

State propagation 은 실제 코드에서 4th runge-kutta 를 사용한

imu noise 는 아래와 같이 구성한다.

위의 수식을 통해 State propagation 수식을 아래와 같이 적을 수 있다.

 

 

- 새로운 Camera msg 에대해서 state 추가

 

Camera state 에서 orientation 과 position 값은 IMU state 값을 사용한다.

State covariance 값의 크기를 바꾸어여한다. 아래의 수식을 통해서 Covariance matrix 를 바꾼다

실제로 Camera State 에 있는 position 과 orientation 이 measurement update 과정을 거치면서 값이 바뀐다. 그리하여 observability constraint 를 유지하기 위해서 position_null, orientation_null 이라는 변수를 만들어서 처음에 state 가 생성될때 값을 null 에도 저장하여 observability constraint 가 유지 되도록 한다.

 

 

- Measurement

H_f 의 null space를 통해 residual 을 projection 한다. 이를통해 P 의 uncertainty가 영향을 미치지 않도록 한다

measurement jacobian  을 구하는 수식이다

- Kalman update

Kalman gain 을 계산하여 state 의 오차를 구해서  State 에 더해준다 (IMU + Camera)

- measurement update

measurement update 는 Cam state 의 개수가 일정값 이상일때와 측정되던 feature 가 더이상 관측되지 않을때 일어난다

 

아래는 Cam state 의 개수가 일정 값 이상일때의 상황이다

만약 끝에서 3번째&4번째 State 의 distance, angle, feature tracking 이 조건에 만족하지 않으면 아래 그림과 같이 가장 오래된 2개의 state 를 사용하여 measurement update 를 진행한다

이때 사용된 2개의 state 는 meausurement update 이후 삭제된다 (covariance matrix 에서도 2개의 state 부분 삭제)

 

관측되던 feature 가 더이상 관측되지 않는다면, feature 를 보았던 Cam state 에 대해서 measurement update 를 진행한다. 

-reference 

https://github.com/KumarRobotics/msckf_vio

GitHub - KumarRobotics/msckf_vio: Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight

 

https://arxiv.org/pdf/1712.00036.pdf

https://ieeexplore.ieee.org/abstract/document/4209642?casa_token=nQfDD7_Lt3kAAAAA:OOGpDWjqffwibqDpsMV29eplnLqm4hFoaM7Y4DR69PngZDeLwMAiSGqBfJtwN57_pXEbwSLMdQ 

A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation

- code analysis

https://snow-mask-3bb.notion.site/msckf-64d19a46d23441a1bf821e777cb36801

msckf 해석