Hola,
jfh900, supongo que te refieres a las dos funciones que pegué unas páginas mas atrás.
// Constantes
#define R_angle = 0.3;
#define Q_angle = 0.001;
#define Q_gyro = 0.003;
#define tiempo = 0.01;
// Variables globales
float P[2][2] = { { 1, 0 }, { 0, 1 }, };
float q_bias, angulo, gyro;
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);
}
A ver si se explicarme. Así lo entiendo yo.
Para empezar hay tres variables globales que son: "angulo", "gyro" y "q_bias" donde:
"angulo" es la varialble que contendrá el resultado del algoritmo y te da el ángulo de inclinación en radianes.
"gyro" es la variable que contendrá la velocidad angular a la que el robot se está inclinando. En radianes/segundo.
"q_bias" está variable contiene el valor de error (desviación) que se supone tiene el giroscopio y que el algoritmo va calculando y eliminando de la lectura que obtienes del giroscopio.
Luego tienes las dos funciones, "state_update(void)" y "kalman_update(float angle_m)" cada una de ellas tiene una misión. Las vemos mas al detalle:
Función "state_update(void)"
Esta función calcula una ángulo estimado "angulo" y la velocidad angular de inclinación "gyro" una vez se le ha quitado la desviación. "gyro" es la lectura del giroscopio en radianes por segundo (rd/s).
Otra característica de esta función es que hay que llamarla en espacios delimitados de tiempo, por ejemplo con una frecuencia de 100 veces por segundo, lo que significa que la constante "tiempo" deberá valer en este caso 0.01 segundos. La explicación es sencilla, la variable "angulo" se actualiza por medio de esta operación "angulo += gyro * tiempo" o lo que es lo mismo, angulo actual es igual al angulo calculado en el anterior ciclo, mas la distancia en radianes que nos hemos inclinado un tiempo de 0,01seg y que se calcula haciendo "gyro * tiempo" o lo que es lo mismo "velocidad angular * tiempo de ciclo"
Función "kalman_update(float angle_m)"
Esta función refresca el algoritmo y las variables globales "angulo" y "q_bias" Básicamente arregla el error de estimación de la variable "angulo" que se va calculando con la anterior función y calcula un valor de error "q_bias". Se le pasa un valor "angle_m" que es el ángulo calculado con el acelerómetro en radianes. Este ángulo se puede calcular con solo un eje del acelerómetro o con los dos. Si es con un solo eje sería así: "angle_m = asin(lectura_eje_en_fuerza_g)", si es con los dos ejes sería así: "angle_m = atan2(lectura_ejeY_en_fuerza_g, lectura_ejeX_en_fuerza_g)"
De cualquiera de las dos formas la variable "angle_m" contendrá el ángulo de inclinación en radianes según el acelerómetro.
A diferencia de la función "state_update()" a esta función "kalman_update(float angle_m)" no es necesario llamarla en cada ciclo de programa o cada 0.01 segundos. Como he dicho antes esta función "refresca" de alguna manera las desviaciones de ángulo calculadas y lo hace utilizando el acelerómetro como referencia, por lo que podríamos llamarla solo cuando el robot estubiera sometido a pocas aceleraciones, vibraciones, etc.
x gera
Lo que puedes hacer es centrarte en el algoritmo pid que hace que se mantenga el equilibrio, una vez conseguido quizás veas mas claro como introducir los datos de los encoders.
Un saludo...