* 칼만필터는 노이즈가 섞인 모든 센서 데이터 분석에 응용 가능한 필터이다.

* 상보필터는 자이로+가속도 센서계에서 각도를 출력하는데 사용 가능한 필터이다.

난이도 :: 칼만필터 > 상보필터

처리속도 :: 상보필터 > 칼만필터 (칼만필터는 연산량이 많다)

필터성능 :: 칼만필터 > 상보필터

일반적으로 드론을 띄우기 위해 필터를 찾아다니는 경우 그냥 상보필터 써도 잘 뜬다.

=============================================

쉬운 상보필터 먼저 소개하겠다.

필터 없는 가속도 + 자이로 센서를 통한 각도 측정에서는 다음과 같은 문제가 존재한다.

1. 가속도 센서는 가속운동에 의해 엄청난 노이즈가 발생하지만 평균값은 믿을만 하다.

2. 자이로 센서는 노이즈가 적지만 적분을 통해 각도를 측정하므로 시간이 지나면서 오차가 누적된다.

따라서 이 두개를 서로 보완하도록 만든게 상보필터다.

가속도센서의 출력값에는 Low pass filter를, 자이로 센서의 출력값에는 high pass filter를 적용한다.

* 구글에 검색해보면 아래 코드가 상보필터라고 이름붙힌 채 돌아다니는걸 볼 수 있는데, 이건 일반적으로 말하는 상보필터가 아니다!

사실상 의미 없는 필터니까 이런거 사용하지 말자! -> 대충 잠깐 띄우려면 물론 이런거 써도 되긴 한다만, 평생 대충살건가?

그래도 아두이노 코드 예시는 한번 봐줄만 하니까 스크랩 해뒀다.

======================================================================================

출처 :: http://blog.naver.com/PostView.nhn?blogId=roboholic84&logNo=220401407348

상보필터는 기본적으로 아래와 같은 공식을 따르고 있습니다.

(Blonix 첨언 :: 개소리다! 상보필터는 이거 아니다!!)

angle은 출력할 각도, gyrData는 자이로값, dt는 적분할 시간, accData는 가속도를 이용한 각도 데이터입니다.

angle이 2개가 있는데 오른쪽에 있는 angle은 이전 각도값을 의미합니다. 이 각도값은 매번 업데이트됩니다.

여기서 단위를 보면 이해가 쉽습니다.

위 공식에서 angle은 각도값(deg)이며 gyrData는 각속도(deg / sec), dt는 시간, accData는 각도(deg) 입니다.

서로 단위가 다르면 더할 수 없기에 단위를 통일해 주어야 합니다. 그렇기에 자이로값에 적분할 시간을 곱해주어 각도로 단위를 통일해준 후 더하는 것입니다.

그러면 실제 코드를 보도록 하겠습니다.

#include <Wire.h> #define mpu_add 0x68 //mpu6050 address long ac_x, ac_y, ac_z, gy_x, gy_y, gy_z ; //acc, gyro data  double angle = 0, deg ; // angle, deg data double dgy_x ; //double type acc data void setup() {   // put your setup code here, to run once:   Serial.begin(9600) ;  //set serial baud   Wire.begin() ;  //set I2C   Wire.beginTransmission(mpu_add) ;   Wire.write(0x6B) ;   Wire.write(0) ;   Wire.endTransmission(true) ; } void loop() {   // put your main code here, to run repeatedly:   Wire.beginTransmission(mpu_add) ; //get acc data   Wire.write(0x3B) ;   Wire.endTransmission(false) ;   Wire.requestFrom(mpu_add, 6, true) ;   ac_x = Wire.read() << 8 | Wire.read() ;   ac_y = Wire.read() << 8 | Wire.read() ;   ac_z = Wire.read() << 8 | Wire.read() ;   Wire.beginTransmission(mpu_add) ; //get gyro data   Wire.write(0x43) ;   Wire.endTransmission(false) ;   Wire.requestFrom(mpu_add, 6, true) ;   gy_x = Wire.read() << 8 | Wire.read() ;   gy_y = Wire.read() << 8 | Wire.read() ;   gy_z = Wire.read() << 8 | Wire.read() ;   deg = atan2(ac_x, ac_z) * 180 / PI ;  //rad to deg   dgy_x = gy_y / 131. ;  //16-bit data to 250 deg/sec   angle = (0.95 * (angle + (dgy_x * 0.001))) + (0.05 * deg) ; //complementary filter   Serial.println(angle) ; }

쭉 내려가다가 보면 위에선 설명되지 않은 알 수 없는 두 연산이 있습니다.

deg = atan2(ac_x, ac_z) * 180 / PI ;  //rad to deg

dgy_x = gy_y / 131. ;  //16-bit data to 250 deg/sec

위 두줄입니다.

atan2는 값을 라디안값으로 출력하기에 이를 deg값으로 변환해주어야 합니다.

그렇기에 * 180 / PI를 이용하여 변환해 주었습니다.

자이로센서 값의 출력 범위는 ±250°/sec 인데 출력 값은 ±16383입니다. 

이 말이 무슨말이냐? 만약 mpu-6050이 측정한 각속도가 250°/sec라면 16383이라는 값을 출력합니다.

125°/sec일 경우는 약 8191을 출력할 것이구요. 이 출력된 값을 각속도로 변환해주는 과정이 /131.입니다.

변환이 다 끝났으므로 위에서 봤던 상보필터 공식에 각각 값을 대입합니다.

그렇게 해서 

angle = (0.95 * (angle + (dgy_x * 0.001))) + (0.05 * deg) ; //complementary filter 

라는 공식이 나왔습니다.

위에 나왔던 공식과 바뀐게 있습니다. 바로 0.98이 0.95로, 0.02가 0.05로 바뀌었습니다.

이 값은 사용자가 임의대로 결정하는 파라미터값입니다.

가속도쪽에 곱해지는 값이 상대적으로 작고 자이로 값 + 이전값 쪽에 더해지는 값이 상대적으로 큽니다.

이는 가속도 값을 상대적으로 약화시킵니다. 

실시간으로 변하는 값에 약한 가속도 값의 중요성을 줄이고 이전값 + 자이로 값을 더 신뢰함으로써 어느정도 순간적으로 변하는 값을 막아준 것입니다. 가속도 값의 영향력이 작다고 해도 지속적으로 영향을 끼치기에 이 값으로 인해 변하게 되는 것입니다.

======================================================================================

이제 제대로 된 상보필터에 대해 알아보자.

이거다.

이걸 C 코드로 구현하자면 다음과 같다.

Kp, Ki 값은 상수로 실험을 통해 결정해야 하는 값이다. 저 블로그에서 실험을 통해 최소자승법으로 구해둔 값은..

Kp = 0.1973

Ki = 0.00546

이다.

물론 사람마다 환경에 따라 값은 다 다르겠지만 어쨌든 저정도 되는 값이라는 거다.

참고자료

https://sites.google.com/a/kookmin.ac.kr/mr-h/kudos/robot-study/complementary-filter-sangbo-pilteo

http://pinkwink.kr/254

http://pinkwink.kr/242