卡爾曼濾波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;
}