ImuKalman

Introducción

Para estimar la posición del quadcopter contamos con un IMU que posee un acelerómetro de tres ejes (ax, ay, az) y tres giróscopos (gp,gr,gy (pitch, roll y yaw)).

La información proveniente de los gyros es menos ruidosa que la que se obtiene de los acc, pero hay un drift que no se puede evitar, aunque se puede reducir con una calibración adecuada. El punto es que utilizando solamente los gyros la posición estimada se irá alejando de la posición real.

La idea es partir de una cierta posición e ir integrando las medidas de los gyros (velocidad angular), y utilizar como referencia absoluta la medida de los acc. Esto permite evitar el drift, gracias a los acc, y evitar la varianza de los acc, gracias a los gyros.

La información de los gyros se mezcla con la de los acc en un filtro de Kalman.

Kalman

Las tres componentes (roll,pitch,yaw) de tratan de manera independiente.

Se llevan dos variables por componente: estado (st) y covarianza (cov)

El filtro tiene una etapa de predicción, donde se utiliza la medida del gyro como entrada:

Predicción

st_ = st + gyro*T

cov_ = cov + proc_noise

Variables en juego:

• gyro es la lectura del gyro.
• T es el período de muestreo.
• proc_noise es una medida del ruido que se espera en el dato proveniente del gyro.

Actualización

En esta etapa se compara la predicción contra la lectura del acc, suponiendo que el acc da la posición real.

y = acc_angle - st_

s = cov_ + measure_noise

k = cov/_s

st = st_ + k*y

cov = (1-k)*cov_

Variables en juego:

• acc_angle es la lectura del acc.
• measure_noise es la varianza que se espera en el dato proveniente del acc.
• k es la ganancia de Kalman. Cabe destacar que si se espera mucha varianza en el acc, es decir, se confia mmuy poco en la información proveniente del acc, entonces s será grande, k será chico, y por lo tanto el estado será determinado principalmente por la predicción basada en la lectura del gyro.

Yaw

No es posible obtener una medida del yaw (heading) del IMU a partir de los accelerómetros, por lo tanto se utilizan solamente los gyros para determinar el yaw.

Una forma intuitiva de ver esto es que los accelerómetros nos dicen hacia donde está el centro de la tierra, solamente nos dan el vector g. Puedo rotar en torno a ese vector, y la información proveniente del IMU no va a cambiar, ya que los accelerómetros son solidarios al IMU.

Yo opté por estar muchas horas para darme cuenta de que no era posible. Incluso llegué a una ecuación, que claramente no resolvía el problema, por más que me esforzara.