卡尔曼滤波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的协方差

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;
}