1-12 자이로 센서 데이터 LOW PASS FILTERING

L3G4200D 나 MCU 6050 자이로 센서는  I2C 방식으로 아두이노에 인터페이스 되어 있으며 Wire.h 라이브러리를 사용한다.

자이로 센서로부터 데이타를  읽기 위해서 setup() 에서  Wire.begin()을 실행하고 set_gyro_resistor() 루틴에 의해 초기화 한다. 이어서 드론을 이륙지점에 놓은 상태에서 배터리 연결과 함께 devo 7 스위치를 넣어 바인딩 후 약 10초간 정지 상태에서 gyro_signalen() 루틴을 불러 2000 번 자이로 센서 교정 작업을 수행한다. 즉 2000 개의 데이터를 센싱하여 합한 후 2000으로 나누어 평균값을 구한다. 정지 상태이므로 여기서 얻어지는 교정 값은 드론 비행을 시작하기 전의 평균 노이즈 값으로 삼으면 될 것이며 loop() 문에서 매번 자이로 센서 값을 센싱하게 되면 이 노이즈 값을 뺄셈하여 사용하게 된다.

loop() 문에서도 매번 gyro_signalen() 루틴을 불러 사용한다. gyro_signale

n() 루틴에서는 I2C 인터페이스를 통해 자이로 센서의 주소에서 ROLL PITCH YAW 각각 2 바이트 씩 6바이트 데이터를 읽어 오게 된다. 각 데이터는 lowbyte와 highbyte로 구성된다. highbyte를 256배로 키워주기 위해서 8비트씩 왼쪽으로 이동시킨다. 이 highbyte와  lowbyte를 비트 단위의 OR 연산 수행을 통해 gyro_axis[1] 즉 ROLL 데이터를 준비한다. 동일한 방법으로 PITCH 데이터와 YAW 데이터를 준비한다.

이 부분에 해당하는 코드는 위의 코드를 참고하면 자이로 센서의 측정 값 gyro_roll을 직접 드론 제어에 사용하는 것이 아니라 이 전 gyro_roll_input 값 85%에 자이로 센서 측정치 gyro_roll 값의 15% 가중된 값을 계산하여 사용함에 유의하자. 이 계산법은 자이로 센서 측정에 포함될 수 있는 높은 주파수 영역의 노이즈를 배제하는 IIR (Infinite Impulse Recursive) LOW PASS FILTERING(LPF)  기법이다.
첫 번째 센싱된 자이로 측정값을 s(1) 이라 두고 LPF 처리된 값을 X(1) 그리고 이 전 값을 X(0)=0 으로 두자.
0<α<1
X(1) = αX(0) + (1-α)s(1) = (1-α)s(1)
X(2) = αX(1) + (1-α)s(2) = α(1-α)s(1) + (1-α)s(2)
X(3) = αX(2) + (1-α)s(3) =  αα(1-α)s(1) + α(1-α)s(2)+ (1-α)s(3)

시간이 흐름에 따라 X(n)은 s(1),s(2),s(3), ∙∙∙,s(n) 의 항들을 사용하여 시계열 형태로 표현됨을 알 수 있다. YMFC-3D_V2 코드에서는 α =0.7 이 사용되고 있다. 하지만 사용자는 드론 시험 비행 실시와 더불어 적절한 0.7≤ α ≤0.85 사이 값을 선택해서 사용하면 된다.
코딩 측면에서 시계열 형태 표현을 사용하려면 많은 양의 메모리가 필요하지만
다음과 같은 X(n) = αX(n-1) +(1-α)s(n) 형태의 recursive  공식을 사용하면 X(n), X(n-1) 및 s(n) 3개의 변수를 위한 적은 메모리가 소요된다.

이 LPF 의 주파수 특성은 MATLAB 소프트웨어를 사용하여 쉽게 계산할 수 있다. 물론 LPF도 드론 비행에 영향을 미치지만 드론 리모델링 작업에서 고유하게 결정되는 PID 와는 달리 자이로 센서의 노이즈를 억제할 수 있는 효과가 있다.