Duda variables y funcionamiento Filtro de Kalman

[kaki]

HOla.. Estoy tratando de entender el filtro de Kalman pero es chino basico. aca dejo el codigo que saque del foro por un compañero que al leer los comentarios funciona muy bien.

Código: [Seleccionar]

float q_bias, angulo, gyro, tiempo;

void state_update( void )
{
float Pdot[4];

Pdot[0]= Q_angle - P[0][1] - P[1][0];
Pdot[1]= -P[1][1];
Pdot[2]= -P[1][1];
Pdot[3]= Q_gyro;

gyro -= q_bias;
angulo += gyro * tiempo;

P[0][0] += Pdot[0] * tiempo;
P[0][1] += Pdot[1] * tiempo;
P[1][0] += Pdot[2] * tiempo;
P[1][1] += Pdot[3] * tiempo;
}


void kalman_update( float angle_m )
{
float K_0;
float K_1;

K_0 = P[0][0] / (R_angle + P[0][0]);
K_1 = P[1][0] / (R_angle + P[0][0]);

P[0][0] -= K_0 * P[0][0];
P[0][1] -= K_0 * P[0][1];
P[1][0] -= K_1 * P[0][0];
P[1][1] -= K_1 * P[0][1];

angulo += K_0 * (angle_m - angulo);
q_bias += K_1 * (angle_m - angulo);
}
Lo que me gustaria es si alguien me puede dar un pantallaso de como funciona y que son cada una de las variable en el codigo.

Una de las tantas dudas que tengo es de donde Saco el Tiempo, q_angle es el acelerometro, Q_gyro el giroscopio, que es q_bias, si angulo es el angulo ya corregido...

Espero que me puedan ayudar les agradeceria mucho.

[rivale]

checa este hilo, ahí esta explicado lo que buscas

http://www.todopic.com.ar/foros/index.php?topic=12748.msg301253#msg301253

[kaki]

Rivale gracias por tu respuesta eh aprendido varias cosas..

Solo me quedan unas pocas dudas!!

q_bias es siempre 0???
por que tiempo es 0.0005, como se que dt poner??
El filtro solo usa el valor en X del acelorometro??
como ubcico el giro para usar el giro en X del mimso??

Saludos!!!

[rivale]

qbias se va actualizando como puedes ver en esta linea

Código: [Seleccionar]

q_bias += K_1 * (angle_m - angulo);
en cuanto al dt, este es el tiempo de muestreo que estés utilizando, tu tiempo debe ser lo mas pequeño posible para que se asemeje al tiempo continuo, sin embargo tu tiempo mínimo estaría determinado por la velocidad de tu cristal, tienes que asegurar que a tu micro lle de tiempo de hacer todas las operaciones antes de que se cumpla el tiempo de muestreo.

en cuanto a los ejes de tus sensores, eso depende de como los acomodes en tu tarjeta, es decir, tienes que ver en que posición ponerlos para que tus ejes coincidan, en la hoja de especificaciones viene como están definidos los ejes para cada sensor

[kaki]

si lo de q_bias lo note despues que escribi el post.

No me queda calor lo del dt. tiene que ver con la frecuencia del giro?? Mi giro es digital se comunica por i2c y cuando inicio el poc llamo a una funcion de inicializicion del giro que lleva un tiempo. Yo lo tengo en 5ms.  o es la velocidad en que actulizo los angulos(que vuelvo a leer sus valores ya sea del conversor para el acelerometro o el bus i2c para el giro).


con respecto a los ejes yo voy a usar el X del acelerometro y por lo tanto el X del  giroscopio.. la ubicacion tiene q ser la siguiente:

                  Aceleromtro                 Giroscopio
                              ^ y+                           ^ y +           
                               |                                 |                           
                               |                                 |   
                 +x<------O z              + x<-------Oz             
                                   

[rivale]

El dt es el tiempo de muestreo, es decir, cada cuando actualizas las lecturas de tus sensores y calculas el filtro

[kaki]

Comprendido!!! Lo de los eje no me estan bien ubicados???

[rivale]

lo de los ejes, si así dice en la hoja de especificaciones, entonces están bien ubicados