Paso 15: IMU parte 2: filtro complementario
La imagen muestra el un filtro complementario en forma de diagrama de bloque. Las señales del sensor, convertidas a grados y grados por segundo, son las entradas. El ángulo del acelerómetro es low-pass filtrado, reduciendo la influencia de las señales de corta duración pero manteniendo un promedio a largo plazo (de gravedad). Primero se integra en la velocidad angular de giro, para obtener una estimación del ángulo y entonces high-pass filtraron, para eliminar cualquier deriva a largo plazo. Las dos señales filtradas se suman juntos para crear una estimación de ángulo único que combina las mejores partes de dos señales de los sensores.
Es uno de los pocos casos donde el código es más conciso que el diagrama de bloques o la explicación por escrito:
Extracto de pd_digitalIMU.pde en 4pcb_ARD:
// -------------------------------------------------------------------
rate_pitch = (float) g_pitch * G_GAIN;
angle_pitch = AA * (angle_pitch + rate_pitch * DT);
angle_pitch += (1,0 - AA) * (float) a_pitch * A_GAIN;
// -------------------------------------------------------------------
La primera línea convierte la señal de giro crudo, g_pitch, en º / s, como se explica en el paso anterior. Al final de la segunda línea es la integración numérica. DT es el bucle del tiempo, 0.01s. Un pequeño cambio en el ángulo, rate_pitch * DT, se añade a la anterior estimación del ángulo.
El resto de la segunda línea y tercera línea forman los filtros paso alto y paso bajo, en forma discreta. El paso alto actúa sobre la estimación del ángulo integrado y el low-pass actúa sobre la señal del acelerómetro, a_pitch. La constante AA determina la escala de tiempo de los filtros de paso alto y bajo y es un valor que se puede modificar dependiendo de tu cuadrotor. Mayor AA confía en la señal de giro integrado por más tiempo. Más corto AA combina la señal del acelerómetro en más rápido.
El valor de AA en 4pcb corresponde con una constante de tiempo de 1s. Esta es la longitud aproximada del tiempo para que la estimación del ángulo de giro es de confianza y la longitud aproximada del tiempo que es un promedio de la señal de accerometer. Puesto que éstos son infinitos filtros de respuesta del impulso (IIR), la transición es suave y continuo y la constante de tiempo representa sólo donde la ponderación de los dos filtros es igual.
Escribió una explicación más profunda del filtro complementario aplicado a uno mismo-equilibrio sistema en este documento. Cubre algunos de los detalles de cómo AA se relaciona con la constante de tiempo de los filtros paso bajo y paso alto, como tipo afecta bies sensor, el filtro y algunos detalles más.
