Con la amplificación por 6 y con la nueva calibración de los acelerómetros, tomando en cuenta los rate reales de cada uno, hemos mejorado el funcionamiento del filtro de Kalman. Los datos ahora no divergen como antes. Notamos que el movimiento aleatorio en torno de las posiciones de equilibro ayuda a mantener el valor.
Tenemos un problema de que en determinado momento los valores pasaron a NaN sin previamente percibir un crecimiento sostenido de los mismos, por tanto no parece ser que los datos divergan por un tema de acumulación de errores sino que puede deberse a que en algún cociente se esté produciendo una división por cero.
Vamos a implementar una corrección de los gyros con estimaciones obtenidas a partir de los valores de los acelerómetros. Cuando el vuelo es recto y nivelado a una velocidad constante tenemos que las aceleraciones ax, ay , az son despreciables y por tanto las aceleraciones que sensan los acelerómetros solo tienen las proyecciones de la gravedad.
ax = -g.sin(theta)
ay = g.cos(theta).sin(phi)
az = g.cos(theta).cos(phi)
por tanto:
theta = atan[ -ax/(ay.sin(phi) + az.cons(phi)) ]
phi = atan(ay/az)
Para el caso de psi debemos utilizar el GPS y determinar la dirección de vuelo a partir de dos valores consecutivos.
