1. An efficient orientation filter for inertial and
inertial/magnetic sensor
Jinyong Jeong
jjy0923@kaist.ac.kr
https://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
4. /34
IMU는 어떤 데이터를 갖고 있는가?
• Gyroscope (자이로센서)
• Angular velocity (각속도)
• 누적을 통해서 회전량을 계산!
• Accelerometer (가속도센서)
• Linear acceleration (가속도)
• 움직이지 않고 있을 때 중력 가속도의 방향으로 Roll, Pitch
계산가능!
• 움직이고 있을 때는 움직임에 따른 가속도가 포함됨
• Magnetometer (지자기센서)
• Magnetic filed (지자기)
• 지구의 자기장 방향으로 데이터 획득
• 노이즈가 큰편 (주변 환경에 영향을 받음)
4
5. /34
AHRS는 무엇인가?
• AHRS: Attitude Heading Reference System
• Gyroscope: local한 회전을 빠르게 알수 있음!
• Accelerometer: 중력 방향을 이용해서 global한 roll, pitch 계산가능!
• Magnetometer: 자기장 방향을 이용해서 global yaw 계산가능!
• 세가지 데이터 융합으로 global rotation을 계산하는것이 AHRS
5
8. /34
Quaternion representation
• 3차원 벡터의 rotation
• 같은 vector 의 표현 (각각 A frame, B frame 에서)
• Quaternion의 rotation matrix의 변환
• Quaternion의 Euler angle 변환
8
9. /34
Orientation from angular rate
• Angular rate to derivative of quaternion (미분 과정은 아래 링크 참고)
• 실제 구현
• Update: 이전 step에서 추정된
quaternion으로 변환
9https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/
참고
Angular rate
이전 step의 quaternion
Quaternion Update
10. /34
Orientation from vector observation
• 실제 Accelerometer & magnetometer
• Accelerometer = gravity + 실제 모션에 따른 가속
• Magnetometer = earth’s magnetic filed + local magnetic flux + distortion
• 우선 먼저 가정!
• accelerometer 는 gravity만 포함
• Magnetometer 는 earth’s magnetic filed 만 포함
10
11. /34
Orientation from vector observation
• Optimization Problem!
• Sensor의 magnetometer 측정값이 global하게 북쪽을 향하도록!
(실제로는 북쪽 + 아랫방향)
• Sensor의 accelerometer 측정값이 global하게 z방향으로 되도록!
11
Optimization
Objective Function
구하고자 하는 Quaternion
Sensor 측정값
알고있는 global 값 (중력, 자기)
알고있는 값을 센서 frame으로 변환
12. /34
Orientation from vector observation
• Optimization Problem
• Gradient descent algorithm
• 1. 현재 값에서 선형화
• 2. 작아지는 방향으로 optimization
12
Objective Function
참고
Step
size
Gradient
에 대한 Jacobian q1으로 미분 q2로 미분
q3로 미분
q4로 미분
13. /34
Orientation from vector observation
• Accelerometer 데이터를 이용한 Optimization
• Gravity는 z 방향!
13
참고
센서 데이터
Gravity (z방향)
Object function
Jacobian
14. /34
Orientation from vector observation
• Magnetometer 데이터를 이용한 Optimization
• Global의 magnetic filed는 지구 좌표계에서 Z방향과 X방향
(이 값은 어떤 좌표계를 쓰냐에 따라 다름)
14
참고
15. /34
Orientation from vector observation
• 어떤 좌표계가 있을까?
• NED
• NWU
• ENU
15
현재의 quaternion Global 값
측정값
J*F
전체적으로 직관적인 coordinate와 비교했을 때 방향이 반대로 되어 있는데 처음부터 정의를 반대로 한건지 확인 필.
16. /34
Orientation from vector observation
• Gradient decent Code!
16
참고
현재 rotation
Global target (gravity or magnetic)
Sensor measurement
result
Global target data를 sensor coordinate으로 변형
17. /34
Orientation from vector observation
• 두 식을 합치면
17
magnetometer
Accelerometer
Gradient step size (angular velocity의 크기에 비례)
18. /34
Filter fusion algorithm
• Measurement 1
• angular velocity 를 누적해서 얻은 quaternion
• Local measurement (엔코더와 같이 누적 에러가 있음)
• Measurement 2
• magnetometer, accelerometer 데이터를 이용하여 gradient decent 방법으로 계산한
quaternion
• Global measurement (GPS와 같이 누적에러는 없으나 오차가 있음)
• 두개를 잘~ 섞어야 한다! => Fusion
18
19. /34
Filter fusion algorithm
19
최종 결과는 Weighted sum
Measurement 2 Measurement 1
convergence rate
Step size
최적의 감마값은?
Divergence rate (measurement error)
최적의 감마값
𝛼가 매우 크다고 가정 = 𝜇도 매우 큼
최종 Fusion!
21. /34
Magnetic Distortion Compensation
• Magnetic Distortion
• Magnetometer로 측정되는 데이터는 주변 환경에 의해 외곡이 될 수 있음
• Global frame에서 Z방향은 그대로 사용
• X, Y 방향의 값은 북쪽 방향으로 수정해서 사용!
21
센서 값
현재 추정된 orientation
Global coordinate
보정된 값
22. /34
Gyroscope bias drift compensation
• Gyroscope bias
• Angular rate에 포함되어 있는 bias
• Kalman filter 의 경우에는 bias를 state로 넣어서 추정 가능
• Mahony[35]가 integral feedback 방법으로도 단순하게 bias를 보정할 수 있음을 증명
22
현재 추정된 quaternion의 미분Angular rate
누적
보정
24. /34
Filter fusion algorithm
• Gain은?
24
Estimated mean zero gyroscope me
asurement error of each axis
Estimated rate of gyroscope bias dr
ift in each axis
Good morning. Thank you for attend my presentation.
My name is JinyongJeong from KAIST. I’d like to talk about extrinsic calibration for stereo camera of non-overlapping configuration.
This work was collaborated with Lucas Cho, Ayoung kim of KAIST.