칼만필터와 자이로센서
칼만 필터 (Kalman Filter)
칼만 필터는 LQE (Linear Quadratic Estimation)라고도 한다. 노이즈가 있는 신호를 측정할 때 칼만 필터를 사용하면 보다 정확한 값을 얻을 수 있다.
칼만 필터는 1950~60년대에 처음 개발되었다.
칼만 필터는 아폴로 계획의 로켓과 대류간 탄도 미사일과 순항 미사일 등의 항법 컴퓨터에 사용되었다.
칼만 필터 방정식
시스템의 상태 방정식은 다음 식과 같다.
여기서,
A = State transition model , B = Input model
u = 입력, w = 시스템 노이즈
H = 측정 모델 , v = 측정 노이즈
위의 상태 방정식에서 시스템 노이즈 w와 측정 노이즈 v의 확률 분포는 다음과 같다. 평균은 모두 0이고 편차는 각각 Q과 R이다.
위에서 Q과 R은 다음과 같다.
w가 2x1 행렬이라면 Q는 2x2 행렬이 된다. v가 1x2 행렬이라면 R은 1x1 행렬이 된다.
칼만 필터는 다음과 같이 구현된다. Predict에서는
을 계산하고 이 값은 Update의 계산에 사용된다. Update의 계산 결과
는 Predict에서 사용된다.
위 그림의 오른쪽의 Update에서 1번째 식은 칼만 게인을 나타내고 2번짹 식은 Update Estimate이고 3번째 식은 Update Covariance이다.
위에서 시스템 모델에서 A, B, H가 결정되고, Q와 R은 노이즈 특성에 따라 결정해야 한다.
칼만 필터를 구현하기 위해서는 시스템 모델과 노이즈 편차를 알기만 하면 된다.
자이로 센서와 가속도 센서의 칼만 필터
가속도 센서는 가속도의 방향 (각도)을 측정한다. 자이로 센서는 회전율 (각속도)을 측정한다. 가속도 센서는 값이 불안정한 문제가 있고 자이로 센서는 값은 정확하지만 서서히 변하는 드리프트가 있다. 이 두 센서의 값을 결합하여 정확한 가속도를 구할 수 있다. 단순하게는 Complementary 필터를 사용할 수 있지만, 가장 많이 사용하는 방법은 칼만 필터를 사용하는 것이다.
상태 방정식은 다음과 같다.
측정 방정식은 다음과 같다. 가속도 센서의 각도는 zk의 값으로 사용된다.
R은 측정 노이즈 vk의 분산으로 스칼라이다.
위 칼만 필터를 구현한 코드는 다음과 같다. 가속도 센서의 각도 Angle과 자이로 센서의 각속도 Rate을 입력하여 최종 각도 x_angle을 출력한다.
| float dt = 0.001; // 1ms float Q_angle = 0.01; float Q_gyro = 0.0003; float R_angle = 0.01; float x_bias = 0; float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; float y, S; float K_0, K_1; // Angle : 가속도 센서의 각도 // Rate : 자이로 센서의 각속도 float Kalman(float Angle, float Rate) { // Predict x_angle += dt * (Rate - x_bias); P_00 += - dt * (P_10 + P_01) + Q_angle * dt; P_01 += - dt * P_11; P_10 += - dt * P_11; P_11 += + Q_gyro * dt; // Update // Kalman gain y = Angle - x_angle; S = P_00 + R_angle; K_0 = P_00 / S; K_1 = P_10 / S; // Update estimate x_angle += K_0 * y; x_bias += K_1 * y; // Update covariance P_00 -= K_0 * P_00; P_01 -= K_0 * P_01; P_10 -= K_1 * P_00; P_11 -= K_1 * P_01; return x_angle; } |
참고
http://web.mit.edu/kirtley/kirtley/binlustuff/literature/control/Kalman%20filter.pdf
https://statweb.stanford.edu/~candes/acm116/Handouts/Kalman.pdf
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
http://www.mate.tue.nl/mate/pdfs/6868.pdf