|
发表于 2009-1-17 17:31:01
|
显示全部楼层
#include <math.h>
struct Gyro1DKalman
{
double x_angle;
double x_bias;
double P_00;
double P_01;
double P_10;
double P_11;
double Q_angle;
double Q_gyro;
double R_angle;
};
void init_Gyro1DKalman(struct Gyro1DKalman *filterdata, double Q_angle, double Q_gyro, double R_angle)
{
filterdata->Q_angle = Q_angle;
filterdata->Q_gyro = Q_gyro;
filterdata->R_angle = R_angle;
}
void ars_predict(struct Gyro1DKalman *filterdata, const double dotAngle, const double dt)
{
filterdata->x_angle += dt * (dotAngle - filterdata->x_bias);
filterdata->P_00 += - dt * (filterdata->P_10 + filterdata->P_01) + filterdata->Q_angle * dt;
filterdata->P_01 += - dt * filterdata->P_11;
filterdata->P_10 += - dt * filterdata->P_11;
filterdata->P_11 += + filterdata->Q_gyro * dt;
}
double ars_update(struct Gyro1DKalman *filterdata, const double angle_m)
{
const double y = angle_m - filterdata->x_angle;
const double S = filterdata->P_00 + filterdata->R_angle;
const double K_0 = filterdata->P_00 / S;
const double K_1 = filterdata->P_10 / S;
filterdata->x_angle += K_0 * y;
filterdata->x_bias += K_1 * y;
filterdata->P_00 -= K_0 * filterdata->P_00;
filterdata->P_01 -= K_0 * filterdata->P_01;
filterdata->P_10 -= K_1 * filterdata->P_00;
filterdata->P_11 -= K_1 * filterdata->P_01;
return filterdata->x_angle;
}
double PredictAccG_roll(double a_z, double a_y, double a_x)
{
return -(atan2(-a_z, a_y)-(3.14159/2.0));
}
void main(void)
{
double tmp, roll_angle;
double accelero_x,accelero_y,accelero_z;
double gyro_roll_rad;
double dt=20;
struct Gyro1DKalman filter_roll;
init_Gyro1DKalman(&filter_roll, 0.0001, 0.0003, 0.69);
while(1)
{
accelero_x=rand();
accelero_y=rand();
accelero_z=rand();
gyro_roll_rad=rand();
tmp = PredictAccG_roll(accelero_z, accelero_y, accelero_x);
ars_predict(&filter_roll, gyro_roll_rad, dt);
roll_angle = ars_update(&filter_roll, tmp);
}
} |
|