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

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

 

코드는 아래에 있습니다.

github.com/rladntjd/extended_kalmanfilter.git