MPU6050의 칼만 필터(Kalman filter)의 구현 예제(1)

아두이노(Arduino) 환경에서 전형적인 MPU6050 센서 입력에 사용되는 칼만 필터(Kalman filter)의 예제입니다.

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#comment-57783

MPU6050은 가속도와 자이로센서가 1개의 센서에 모두 포함하고 있는 6DOF(Degrees of Freedom) 센서로, I2C(Inter Integrated Circuit) 통신 프로토콜을 통해서 데이터를 추출 할 수 있습니다. 위 모듈은 MPU6050을 포함하며 사용하기 쉽게 만든 모듈입니다. 

MPU6050의 간단한 스펙입니다.

가속도계(accelerometer)는 움직임이 있을때 일반적으로 많은 잡음(noise)을 포함합니다. 자이로(gyroscope)의 문제는 시간이 누적되면 DC 성분(혹은 이를 bias라 표현)이 누적된다는 것입니다. 결론적으로 자이로는 짧은 시간에 믿을 수 있지만, 반면 가속도계는 긴 시간에 더 신뢰성이 있다는 것입니다.

그렇기 때문에 상보 필터(complimentary filter, 혹은 보상 필터)의 적용이 쉽습니다. 기본적으로 가속도계에 대해서 디지털 low pass filter(LPF), 자이로에 대해서는 디지털 high pass filter(HPF)를 적용하는 것입니다. 그러나 칼만 필터(Kalman filter) 만큼 정확하지는 않다는 것입니다. 근래에 아두이노를 이용하여 처리가 가능하지만 칼만 필터는 아두이노가 주로 제공하는 MCU에서는 무겁다는 단점이 있습니다.

k 시간에 시스템의 상태는 다음과 같습니다.

여기서 는 상태 행렬로 다음과 같습니다.

는 각도 의 시간에 따라 누적된 양으로 bias라고 하며 자이로에서 측정된 값에서 이 bias를 빼는 것에 의하여 현재의 각도를 얻을 수 있습니다.

그러므로 이전 상태 로부터의 전이 행렬 는 다음과 같이 쓸 수 있습니다.

는 제어 입력으로 k 시점에서 자이로의 측정값으로 [˚/s]의 단위를 가지며 이 로 상태방정식을 다음과 같이 다시 쓸 수 있습니다.

여기서 는 제어 입력 행렬로 다음과 같이 나타낼 수 있습니다.

는 프로세스 잡음으로, 주어진 k 시간에서 '0'의 평균과 의 공분산(covariance)을 갖는 가우시안 분포(Gaussian distribution)(혹은 정규분포)를 갖습니다.

는 프로세스 잡음 공분산 행렬이고, 이 경우에 가속도계와 bias의 상태 추정치 공분산 행렬이 됩니다. 우리는 bias와 가속도계의 추정이 서로 독립적임을 가정합니다.

위 식에서 처럼  공분산 행렬은 현재 시간 k에 의존합니다. 따라서 가속도계 분산 와 bias의 분산 은 미소 시간 에 곱하여집니다.

프로세스 잡음은 시간이 길어질수록 커지게 됨을 의미합니다. 이는 상태의 마지막 업데이트이기 때문입니다. 예를 들어 자이로는 드리프트(drift) 할 수 있습니다. 우리는 칼만 필터의 동작을 위해서 이들 상수들을 알아야 합니다. 만일 여러분이 큰 값을 설정한다면 상태 추정에 잡음이 커진다는 것을 기억해야 합니다. 예를 들어, 추정 각도가 드리프트 하기 시작하면 여러분은 의 값을 증가시켜야 합니다. 반면에 추정이 여러분이 생각하는 것보다 너무 많이 느려진다면 반응이 빠르도록 의 값을 감소시켜 볼 수 있습니다.