Drone's DIYer

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

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

한편 의 관측 혹은 측정을 위한 식은 다음과 같이 주어집니다.

위 식에서와 같이 는 현재 상태 가  행렬과 곱하여지고 측정 잡음 와 합하여 집니다.

는 관측 모델로 불려지며 실제 상태 공간을 관측된 상태의 공간으로 매핑하기 위해서 사용되어집니다. 실제 상태는 관측될 수 없는데 이는 측정이 단지 가속도계로부터의 측정에 불과하기 때문입니다.

는 다음과 같습니다.

측정 잡음은 게다가 '0'의 평균과 다음과 같이 의 공분산을 가져야 합니다. 

그러나 은 행렬이 아니기 때문에 측정 잡음은 단지 측정값의 분산과 같습니다. 왜냐하면 같은 변수의 공분산은 분산과 같기 때문입니다. 그러므로 우리는 R을 다음과 같이 쓸 수 있습니다.

우리는 측정 잡음은 동일하고 시간 k에 의존하지 않는다고 가정합니다.

만일 여러분이 측정 잡음 분산 을 너무 높게 설정하면 필터는 너무 늦게 응답하는데 이는 새로운 측정값을 덜 믿기 때문이며, 반면에 너무 낮게 설정한다면 값은 오버슈트(overshoot)가 발생하고 잡음이 많은데 이는 우리가 가속도계 측정값을 너무 많이 믿기 때문임을 참고하세요.

그래서 여러분은 프로세스 잡음 분산 와  그리고 측정 잡음 의 측정 분산을 찾아야만 합니다.

Time Update("Predict")

처음 2개의 방정식에서 시간 k에서 우리는 현재 상태와 오차 공분산 행렬을 예측할 것입니다. 우선 필터는 모든 이전 상태들과 자이로 측정으로부터 근거 된 현재 상태를 추정할 것입니다.

위 식의 우측 두번째 항을 제어 입력이라고 부르는 이유는 우리가 현재 시간 k에서 상태를 추정하기 위해 이를 추가적인 입력으로 사용했기 때문입니다. 다음은 우리가 이전 오차 공부산 행렬 에 근거하여 다음에 정의된 것처럼 오차 공분산 행렬 을 추정할 것입니다.

이 행렬은 추정된 상태의 현재 값을 얼마나 많이 신뢰하는가를 추정하기 위해서 사용되어집니다. 작을수록 우리는 좀더 현재 추정된 상태를 신뢰하게 됩니다. 위 방정식의 원리는 실제로 이해하기에 매우 쉽습니다. 이는 우리가 상태의 추정을 마지막 갱신한 후에 오차 공분산은 증가하는 것이 매우 분명하기 때문입니다. 그러므로 우리는 오차 공분산 행렬을 상태 전이 모델 과 그 행렬의 전치행렬 을 곱하고 시간 k에서 현재 프로세스 잡음 를 더합니다.

오차 공분산 행렬 는 2x2 행렬입니다.

Measurement Update("Correct")

우리가 계산해야 할 첫번째 일은 측정치 와 보정되지 않은 이전 상태로부터 예측된 상태 변수 사이에 차를 계산하는 것입니다.

여기서 관측 모델 는 이전 상태 추정치를 관측된 공간과 매핑하게 위해서 사용되어집니다. 관측 공간이라 함은 가속도계로부터의 측정치로 그러므로 위 식은 행렬이 아닙니다.

다음에 할 것은 를 계산하는 것입니다.

이전 오차 공분산 행렬 와 측정 공분산 행렬 에 근거한 측정을 얼마나 믿을 수 있는지 예측하는 것입니다. 관측 모델 는 이전 오차 공분산 행렬 를 관측 공간으로 매핑하기 위해서 사용되어집니다.

큰 측정 잡음은 값을 크게 합니다. 이는 입력되는 측정값을 그렇게 많이 신뢰할 수 없음을 의미합니다. 이 경우 는 행렬이 아니고 다음과 같이 쓸 수 있습니다.

다음 단계는 칼만 이득(Kalman gain)을 계산하는 것입니다. 칼만 게인은 측정치 와 이전 상태 변수와의 차이를 얼마나 신뢰할 수 있는지를 나타내는데 사용되어집니다.

만일 우리가 그 차이를 도저히 신뢰하지 못한다면, 차이에 대한 공분산 는 높게 될 것입니다. 그리고 만일 우리가 추정된 상태를 신뢰한다면 오차 공분산 행렬 는 작게 될 것이고 칼만 이득도 작아질 것입니다. 반대로 차이를 신뢰하지만 현재 상태의 추정치를 믿지 못한다면 반대가 될 것입니다.

여기서 관측 모델 H의 전치행렬이 오차 공분산 행렬 P를 관측 공간으로 매핑하는데 사용되어짐을 알 수 있습니다. 그다음 우리는 공분산 S로 나누어 오차 공분산 행렬을 비교합니다.

이는 관측 모델 H를 상태 오차 공분산을 빼기 위해서 그리고 S의 현재 추정치와 비교하기 때문에 의미가 있다는 것입니다. 만일 여러분이 초기에 상태를 모른다면 오차 공분산 행렬을 다음과 같이 설정할 수 있다.

여기서 은 큰 숫자를 대표하며 초기에 상태가 알려진다면 오차 공분산 행렬을 다음과 같습니다.

칼만 이득은 2x1 행렬입니다.

우리는 추정된 현재 상태를 보상합니다.

이는 yk와 칼만 이득을 곱하여 보상되지 않은 현재 상태에 더하는 것입니다.

는 측정된 와 보상되지 않은 추정된 현재 상태의 차이로 양수나 음수가 될 수 있습니다.

가장 간단화된 방정식은 보상되지 않은 현재 추정 상태 를 가속도계 측정값으로 간단하게 바로잡는 것으로 이해할 수 있습니다.

여기서 는 이전 상태와 자이로 측정값으로부터 계산되어진 것입니다.

마지막으로 우리는 보상되지 않은 오차 공분산 행렬을 갱신하는 것입니다.

여기서 I는 항등행렬입니다.

필터가 하는 일은 기본적으로 얼마나 많이 우리가 추정치를 바로잡았는가에 근거하여 오차 공분산 행렬을 스스로 바로잡는 것입니다. 이것은 우리가 이전의 오차 공분산 과 공분산 에 근거된 상태를 바로잡았기 때문에 의미가 있다는 것입니다. 

아두이노(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) 할 수 있습니다. 우리는 칼만 필터의 동작을 위해서 이들 상수들을 알아야 합니다. 만일 여러분이 큰 값을 설정한다면 상태 추정에 잡음이 커진다는 것을 기억해야 합니다. 예를 들어, 추정 각도가 드리프트 하기 시작하면 여러분은 의 값을 증가시켜야 합니다. 반면에 추정이 여러분이 생각하는 것보다 너무 많이 느려진다면 반응이 빠르도록 의 값을 감소시켜 볼 수 있습니다.

3축 가속도와 3축 자이로 센서를 조합한 후 각각의 센서 출력을 내보내는 장치를 관성측정장치(IMU; Inertial Measurement Unit)이라고 부릅니다. 스마트폰에도 탑재되어 있을 정도로 그 용도가 근래에 흔하며, 비행기의 항법 장치에 필수적인 요소입니다. 기체의 자세 제어에 요구되는 롤(Roll), 피치(Pitch), 요(Yaw)의 기울어진 각도를 알기 위함으로, 롤은 좌우로 기울어짐, 피치는 앞뒤로 기울어짐, 요는 z축 방향으로 기울어짐(회전각)을 의미합니다. 여기서 롤과 피치는 중력방향을 기준으로 얼마나 기울어져 있는지를 나타냅니다.

이처럼 롤과 피치 그리고 요와 같은 자세 측정을 위해서는 필요한 장치가 자이로와 가속도 센서입니다.

MEMS 기반의 3축 가속도 센서(Accelerometer)는 x축, y축, z축 방향의 가속도를 측정할 수 있으며 단위는 [g]입니다. 가속도 센서는 정지한 상태에서 중력 가속도를 감지하기 때문에 z축 방향으로 -g 만큼의 값을 출력합니다. 센서의 초기 출력은 모두 '0'이라고 가정하고 센서를 y축 기준으로 45도 기울여 보면, 기울어진 상태에서 z축 방향과 x축 방향으로 동일한 값의 가속도가 측정되며 중력방향으로 g가 측정되어야 하므로 0.707g 만큼 z축과 x축 방향으로 값이 출력됩니다. 결과적으로는 z축과 x축 값의 비율을 arctan으로 계산하여 기울어진 값을 구할 수 있습니다.

그러나 정지 상태가 아닌 움직이는 가속 상태의 경우, 또다른 힘의 영향으로 중력 방향이 변한 것처럼 올바른 값을 얻을 수가 없습니다. 즉, 정지하지 않은 움직임 상태에서는 가속도 센서만으로 기울기 값을 측정할 수는 없습니다. 뿐만 아니라 z축상에 회전각인 요는 중력 방향(중력가속도)이 전혀 변하지 않으므로 측정이 불가능합니다.

3축 자이로 센서(Gyroscope)는 가속도를 측정하는 가속도 센서와 달리 각속도를 측정하므로 단위는 [degree/sec]입니다. 자이로는 각속도를 재는 장치이기에 이를 이용해서 각도를 알려면 전체 시간에 대하여 적분을 하여 얻게 됩니다. 그러나 센서에서 측정되는 각속도는 노이즈나 어떠한 이유에 의해 측정값에 에러가 계속 생기는데, 이 오차가 적분시에는 누적이 되어서 최종 값이 드리프트 되는 현상이 발생합니다. 게다가 시간이 지날수록 이 오차는 커져 각도가 변하게 된다는 것입이다.

결과적으로 정지상태의 긴 시간의 관점에서 보면 가속도 센서에 의해 계산된 각도는 올바른 값을 보여주지만, 자이로 센서에서는 시간이 지날 수록 틀린 값을 내보내게 됩니다. 반대로 움직이는 짧은 시간의 관점에서 보면 자이로 센서는 올바른 값을 보여주지만 가속도 센서는 다른 값을 내보내게 됩니다.

그러므로 가속도 센서와 자이로 센서를 모두 사용해서 각각의 단점을 보상할 수 있는 알고리즘을 적용하여 롤 또는 피치 값을 계산하여야 한다는 것입니다. 많이 사용하는 보상 방법 및 필터링으로는 칼만 필터(Kalman filter)의 적용입니다.

요의 회전축은 z축방향, 즉 중력방향과 같으므로 가속도 센서가 아닌 자이로 센서의 z축 값을 측정해서 이 값을 이용해 요값을 계산하여야 하고 드리프트되는 오차를 보상하는 다른 센서를 추가적으로 사용하는데, 이것이 지자기 센서(magnetometer or compass)입니다. 자이로는 온도가 변하면 그 값이 같이 변하는 특성이 있어 정교한 측정을 위해서는 온도 센서도 함께 사용해서 오차를 보상하기도 합니다. 이를 모두 고려하면 3축 가속도 센서, 3축 자이로 센서, 3축 지자기 센서를 내장한 IMU 센서를 9축 센서라 부르는 이유에서 입니다.

위 그림의 칩은 Invensense사의 9DOF IMU인 MPU-9150입니다. 이는 3축 자이로 센서와 3축 가속도 센서인 MPU-6050과 3축 지자기 센서(digital compass)를 One Pakage(SiP)하였으며, I2C(TWI)를 지원합니다. 여기서 DOF는 'Degree Of Freedom'의 약자로 '축(Axis)'을 의미합니다.