卡尔曼滤波法C编程

float Angle = 0.0;//卡尔曼滤波器的输出值，最优估计的角度
//float Gyro_x = 0.0;//卡尔曼滤波器的输出值，最优估计的角速度
float Q_angle = 0.001;//陀螺仪噪声的协方差（估计过程的误差协方差）
float Q_gyro = 0.003;//陀螺仪漂移噪声的协方差（估计过程的误差协方差）
float R_angle = 0.5;//加速度计测量噪声的协方差
float dt = 0.005;//积分时间，dt为滤波器采样时间（秒）
char C_0 = 1;//H矩阵的一个数
float Q_bias = 0,Angle_err = 0;//Q_bias为陀螺仪漂移
float PCt_0 = 0,PCt_1 = 0,E = 0;//中间变量
float K_0 = 0,K_1 = 0,t_0 = 0,t_1= 0;//K是卡尔曼增益，t是中间变量
float Pdot[4] = {0,0,0,0};//计算P矩阵的中间变量
float PP[2][2] = {{1,0},{0,1}};//公式中P矩阵，X的协方差

//gx...分别为重力加速度在三个轴向的分力    由加速度计测得
//ax...分别为角速度在三个轴向的角速度    由陀螺仪测得
//最后得到最终滤波完毕的x、y、z方向的角度值(°)
//调用下面的函数应该如下调用，使用atan2更为准确
//Kalman_Filter(gx,atan2(ay,sqrt(ax*ax+az*az))*1800/3.14);//与x轴角度
//Kalman_Filter(gx,atan(ay/sqrt(ax*ax+az*az))*1800/3.14);//与x轴角度
//三个方位具体带入如下
//atan(sqrt(ax*ax+ay*ay)/z)*1800/3.14;
//atan(ay/sqrt(ax*ax+az*az))*1800/3.14;
//atan(ax/sqrt(ay*ay+az*az))*1800/3.14;

void Kalman_Filter(float Gyro,float Accel)//Gyro陀螺仪的测量值，Accel加速度计的角度计算值
{
    Angle+=(Gyro-Q_bias)*dt;
    //角度测量模型方程，角度估计值=上一次的最优角度+（角速度-上一次的最优零漂）*dt为滤波器采样时间（秒）
    //就漂移来说认为每次都是相同的Q_bias=Q_bias

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

    PP[0][0] += Pdot[0] * dt;
    PP[0][1] += Pdot[1] * dt;
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;

    PCt_0 = C_0 * PP[0][0];//矩阵乘法的中间变量
    PCt_1 = C_0 * PP[1][0];//C_0 = 1
    E = R_angle + C_0 * PCt_0;//分母
    K_0 = PCt_0 / E;//卡尔曼增益，两个，一个是Angle的，一个是Q_bias的
    K_1 = PCt_1 / E;

    Angle_err = Accel - Angle;
    Angle += K_0 * Angle_err;//计算最优角度
    Q_bias += K_1 * Angle_err;//计算最优零漂
    //Gyro_x = Gyro - Q_bias;//计算得最优角速度

    t_0 = PCt_0;//矩阵计算中间变量，相当于a
    t_1 = C_0 * PP[0][1];//矩阵计算中间变量，相当于b

    PP[0][0]-=K_0 * t_0;
    PP[0][1]-=K_0 * t_1;
    PP[1][0]-=K_1 * t_0;
    PP[1][1]-=K_1 * t_1;

}