아두이노(Arduino) 환경에서 전형적인 MPU6050 센서 입력에 사용되는 칼만 필터(Kalman filter)의 예제입니다.
Step 1)
위 식에서 각도(angle)의 추정치 
게다가 우리는 bias를 직접 측정할 수 없기 때문에 bias의 추정치는 이전 것을 사용합니다.
이는 다음과 같이 C 언어로 쓸 수 있습니다.
rate = newRate - bias;
angle += dt * rate;
Step 2)
위 식은 다음과 같이 C 언어로 쓸 수 있습니다.
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_gyroBias * dt;
Step 3)
참고로 우측에 현재 상태변수는 보정되지 않았기 때문에 angle 변수를 그대로 사용합니다.
위 식은 다음과 같이 C 언어로 쓸 수 있습니다.
y = newAngle - angle;
Step 4)
위 식은 다음과 같이 C 언어로 쓸 수 있습니다.
S = P[0][0] + R_measure;
Step 5)
다른 경우에 S는 행렬이 될 수 있습니다. 그 경우에는 여러분은 간단히 S로 P를 나눌 수 없으며 역행렬을 구해서 곱해야 합니다.
위 식은 다음과 같이 C 언어로 쓸 수 있습니다.
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
Step 6)
위 식은 다음과 같이 C 언어로 쓸 수 있습니다.
angle += K[0] * y;
bias += K[1] * y;
Step 7)
상태 추정 오차가 감소되었기 때문에 오차 공분산 행렬을 다시 감소시킴을 기억하세요.
C 코드는 다음과 같습니다.
float P00_temp = P[0][0];
float P01_temp = P[0][1];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
참고로 대부분의 IMU에 대해서 다음의 변수들이 완벽하게 동작합니다.
float Q_angle = 0.001;
float Q_gyroBias = 0.003;
float R_measure = 0.03;
초기치로서 각도를 설정하는 것을 기억하세요 왜냐하면 필터가 안정화되는데 시간이 걸리기 때문입니다. 반대로 칼만 필터가 안정화되기 전까지는 상태의 추정치를 믿을 수 없습니다.
'Flight Controller 이해 > 비행제어기(FC)' 카테고리의 다른 글
| 보상 필터(Complementary filter) (0) | 2017.12.03 |
|---|---|
| MPU6050의 칼만 필터(Kalman filter)의 구현 예제(4) (0) | 2017.11.29 |
| MPU6050의 칼만 필터(Kalman filter)의 구현 예제(2) (1) | 2017.11.25 |
| MPU6050의 칼만 필터(Kalman filter)의 구현 예제(1) (1) | 2017.11.20 |
| 칼만 필터(Kalman filter) (0) | 2017.03.15 |
