四轴飞行器和自平衡小车的卡尔曼滤波器程序,用于姿态解算, Kalman filter, 经本人在四轴上试验效果很好

float Q_angle = 0.001;
float Q_gyro = 0.003;
float R_angle = 0.03;

float angle_x = 0,angle_y = 0;
float bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float yy, S;
float K_0, K_1;

float kalmanCalculate_x(float newAngle, float newRate,float ddt)
{
angle_x += ddt * (newRate - bias);
P_00 += - ddt * (P_10 + P_01) + Q_angle * ddt;
P_01 += - ddt * P_11;
P_10 += - ddt * P_11;
P_11 += + Q_gyro * ddt;

yy = newAngle - angle_x;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

angle_x += K_0 * yy;
bias += K_1 * yy;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return angle_x;
}

float kalmanCalculate_y(float newAngle, float newRate,float ddt)
{
angle_y += ddt * (newRate - bias);
P_00 += - ddt * (P_10 + P_01) + Q_angle * ddt;
P_01 += - ddt * P_11;
P_10 += - ddt * P_11;
P_11 += + Q_gyro * ddt;

yy = newAngle - angle_y;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

angle_y += K_0 * yy;
bias += K_1 * yy;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return angle_y;
}