본문 바로가기

Sensor fusion

Extended Kalman Filter & Error state Kalman Filter (IMU + Motion Capture)

IMU 와 motion capture 를 사용하여 Sensor Fusion 을 할 것이다. 

(IMU+ GPS 와 비슷하게 설정하기 위해)

아래와 같이 State를 정의 한다.

Measurement 에는 Motion Capture 에서 받은 position 와 Velocity를 사용한다.

q_measured 는 Accelometer 와 Magnetometer 에서 받은 값을 통해 quaternion을 생성한다.

또한 Error state Kalman Filter 를 통해 Bias 를 update 할 수 있다.

 

 

아래는 Kalman Filter 와 Extended Kalman Filter 의 증명 과정이다.

 

- probablistic Robotics

- https://arxiv.org/abs/1711.02508

 

Quaternion kinematics for the error-state Kalman filter

This article is an exhaustive revision of concepts and formulas related to quaternions and rotations in 3D space, and their proper use in estimation engines such as the error-state Kalman filter. The paper includes an in-depth study of the rotation group a

arxiv.org

를 참조 하여 만들었습니다.

 

코드는 아래에 있습니다.

github.com/rladntjd/extended_kalmanfilter.git