Bueno, parece que despues de tanto sufrir llegamos a buen puerto.
El Kalman anterior que habiamos hecho no tenia una referencia para "resetear" los angulos, el que tenemos ahora si.
Lo que hice fue agregar un Kalman anidado para no aumentar la complejidad del actual (recordar que ya era de 9 estados). Es entonces que arme un kalman de 3 estados el cual resetea los angulos mediante el uso de los accelerometros para theta y phi.
Para el angulo psi (heading) lo que uso es el psi del gps si la velocidad es mayor a 2m/s, de lo contrario uso el estado anterior. Lo de la velocidad es porque si bien el gps me da un angulo de heading, este es valido solamente a ciertas velocidades.
Ya sabemos que lo mejor hubiera sido usar una brujula 3d o algun dispositivo absoluto de angulos. Si bien compramos una brujula (que para la practica resulto ser lenta y un tanto imprecisa), el manejo de la interfaz requeria demasiado tiempo, del cual no disponemos. La otra opcion era comprar un dispositivo, sin embargo los sensores de angulos de 3 ejes andan por los 700 dolares en origen.(mas caro que todo nuestro sistema)
El update de los angulos mediante los accelerometros tiene una contrapartida, y es que durante un giro muy largo o durante un ascenso muy prolongado, las medidas de angulos que obtenemos no son buenas. Sin embargo, se puede hacer que el update por kalman2 (asi le llame a la pequeña funcion) se haga durante ciertos instantes convenientes.
El dispositivo ahora esta andando correctamente en la mesa. Saque el muestreo de 8 veces de los gyros (y el caculo de su media) porque demoraban demasiado los loops. Esto hacia que el paso de tiempo discreto fuera demasiado grande, lo que hacia que el filtro divergiera (creo que es por como calculamos ciertas exponenciales de matrices, precisamos deltaTs chicos, tambien tenemos la integracion por Runge-Kutta). Los deltaTs acutales andan entre 0.01 y 0.02 (ideal 0.01)
Cuando muestreabamos mas, los deltaTs llegaban incluso a 0.9 (era demasiado pedir) lo cual provocaba NAN luego de cierto tiempo.
El sistema como dije esta quieto, Andres Chappe, antes, para mejorar el rendimiento lo movia, lo cual hacia que las medidas variaran mas y su media fuera mas cercana a cero. Esto ya no es necesario; Al momento de escribir esto, acaba de terminar un logueo de 30 minutos. El error en la posicion es menor al metro. El error en los angulos esta en el orden de 0.2 grados.
Ahora lo que falta es subirse al auto y ver que nos da un log del camino.
Hoy es un gran dia :D
El Kalman anterior que habiamos hecho no tenia una referencia para "resetear" los angulos, el que tenemos ahora si.
Lo que hice fue agregar un Kalman anidado para no aumentar la complejidad del actual (recordar que ya era de 9 estados). Es entonces que arme un kalman de 3 estados el cual resetea los angulos mediante el uso de los accelerometros para theta y phi.
Para el angulo psi (heading) lo que uso es el psi del gps si la velocidad es mayor a 2m/s, de lo contrario uso el estado anterior. Lo de la velocidad es porque si bien el gps me da un angulo de heading, este es valido solamente a ciertas velocidades.
Ya sabemos que lo mejor hubiera sido usar una brujula 3d o algun dispositivo absoluto de angulos. Si bien compramos una brujula (que para la practica resulto ser lenta y un tanto imprecisa), el manejo de la interfaz requeria demasiado tiempo, del cual no disponemos. La otra opcion era comprar un dispositivo, sin embargo los sensores de angulos de 3 ejes andan por los 700 dolares en origen.(mas caro que todo nuestro sistema)
El update de los angulos mediante los accelerometros tiene una contrapartida, y es que durante un giro muy largo o durante un ascenso muy prolongado, las medidas de angulos que obtenemos no son buenas. Sin embargo, se puede hacer que el update por kalman2 (asi le llame a la pequeña funcion) se haga durante ciertos instantes convenientes.
El dispositivo ahora esta andando correctamente en la mesa. Saque el muestreo de 8 veces de los gyros (y el caculo de su media) porque demoraban demasiado los loops. Esto hacia que el paso de tiempo discreto fuera demasiado grande, lo que hacia que el filtro divergiera (creo que es por como calculamos ciertas exponenciales de matrices, precisamos deltaTs chicos, tambien tenemos la integracion por Runge-Kutta). Los deltaTs acutales andan entre 0.01 y 0.02 (ideal 0.01)
Cuando muestreabamos mas, los deltaTs llegaban incluso a 0.9 (era demasiado pedir) lo cual provocaba NAN luego de cierto tiempo.
El sistema como dije esta quieto, Andres Chappe, antes, para mejorar el rendimiento lo movia, lo cual hacia que las medidas variaran mas y su media fuera mas cercana a cero. Esto ya no es necesario; Al momento de escribir esto, acaba de terminar un logueo de 30 minutos. El error en la posicion es menor al metro. El error en los angulos esta en el orden de 0.2 grados.
Ahora lo que falta es subirse al auto y ver que nos da un log del camino.
Hoy es un gran dia :D
