보상 필터(Complementary filter)

가속도계(Accelerometer)는 움직임이 있을 때마다 값이 튈 수 밖에 없는데 이는 움직임에 따른 가속도와 중력가속도가 합해져 마치 중력가속도 방향이 변한 것처럼 보이기 때문입니다. 이러한 고주파 잡음을 제거하기 위해서는 저역 통과필터(Low pass filter; LPF)를 사용해야 합니다. 반면에 자이로(Gyroscope)는 잡음이 긴 시간에 대해서 누적(적분)되어 드리프트(혹은 바이어스)되므로 짧은 시간인 고역 통과필터(High pass filter; HPF)를 사용해야 합니다. 이러한 아이디어를 구체화한 것이 아래의 블록선도와 같으며 이를 IMU(Inertia Measurement Unit) 데이터를 융합하는데 가장 간단한 방법인 보상필터(Complementary filter) 혹은 상보필러라 부릅니다.

여기서 과 는 각각 1차 저역 통과필터, 1차 고역 통과필터로 그 합은 안정성의 문제로 '1'이 성립되어야만 합니다. 2차 필터도 구현할 수 있지만 편의상 1차 필터로만 제한합니다. 위 그림으로부터 수식을 정리하면 다음과 같습니다.

여기서 는 차단주파수(cut off frequency)를 결정합니다. 위 식을 정리하면 다음과 같습니다.

위 수식으로 블록선도를 다시 그리면 다음과 같습니다.

위 수식을 MCU로 처리하기 위해서는 연속시간에 대한 이산화가 필요하고 하드웨어 상에 아날로그 혹은 디지털 필터(예를 들어 IIR, FIR 등)를 사용하지 않는 이상 코드로 구현해야 합니다. MCU 성능 개선으로 외장 디지털 필터나 디지털 필터 알고리즘을 사용하지 않고 수식을 이산화하여 구현하였습니다.

동일한 연속 시간 전달 함수(continous time transfer function) G(s)를 이산화하는 방법에는 여러가지가 있으며 안정도와 고주파 잡음 등과 같은 성능이 동일하지는 않습니다. 전형적으로 연속 시간 전달 함수(continous time transfer function) G(s)를 이산(불연속 시간 전달 함수(discrete time transfer function) H(z)로 변환하는 방법은 전형적으로 다음과 같은 방법이 있습니다.

1) 후방 차분법(backward difference) : 

2) Bilinear transformation, expansion of ln(z) : 

3) Impulse invariance transformation : 

가장 기본적인 후방 차분법(backward difference)을 적용하면 이므로 이를 대입하여 다시 정리하면 추정 각도 는 다음과 같이 나타낼 수 있습니다.

여기서 편의상 가속계인 accelerometer의 첫글자 , 자이로 측정치로부터의 각속도(rotation rate)라 놓았습니다.

위 식에 z 변환인 은 이산 시스템에서 이므로 위 식을 이산 신호 시스템으로 변경하면 다음과 같습니다. 다른 말로 는 단위 시간 지연(unit time delay)과도 같기 때문입니다.

위 식에서 이면 다음과 같습니다.

정성적으로 위 수식에서 만일 가속도계에 측정치가 튄다면 즉, 고주파 잡음이 들어갔다면, 0.01이 곱하여져 새로운 각도가 계산됩니다. 그러나 0.01이 곱하여지므로 기여도는 작아 각도는 프로그램 루프(loop)가 상당히 반복되어도 매우 천천히 증가하게 됩니다. 이는 가속도계 입력이 추정 각도 계산에 실시간으로 반영되기 어렵고 이는 출력이 반응하지 않는다는 의미로 고주파 잡음은 제거되고 서서히 증감되는 저주파 측정치만 반영되게 된다는 의미입니다.

는 weight factor라 부르며 통상 0.95~0.98 범위의 값을 사용합니다.

Appendix A.

Reference:

http://d1.amobbs.com/bbs_upload782111/files_44/ourdev_665531S2JZG6.pdf

http://www.olliw.eu/2013/imu-data-fusing/#chapter21

http://www.dummies.com/education/science/signals-and-systems-working-with-transform-theorems-and-pairs/(z-transform)