请教大家关于卡尔曼滤波的问题

wumin0566

一叶落尘 发表于 2012-2-23 18:24
R_angle=0.5这个参数有问题，调到0.001去试试。但你的程序我估计是抄的，参数基本都是错的。

你好，void Kalman_Filter(int Gyro,int Accel)//{
        Angle+=(Gyro - Q_bias) * dt;//X(k|k-1)，先验估计

        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;

        Angle_err =Accel-Angle;//测量值-系统预测值

        PCt_0 = C_0 * PP[0][0];
        PCt_1 = C_0 * PP[1][0];

        E = R_angle + C_0 * PCt_0;

        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;

        t_0 = PCt_0;
        t_1 = C_0 * PP[0][1];

        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;

        Angle        += K_0 * Angle_err;//计算角度最优估计值
        Q_bias        += K_1 * Angle_err;//更新静态偏差最优估计偏差
        Gyro_x   = Gyro - Q_bias;


}卡尔曼三个参数的物理意义是什么啊，看了好长一段时间了，Q_angle，Q_gyro,R_angle，我的理解是Q_angle是角度预测系统噪声，是陀螺仪静态偏差预测系统噪声，R_angle是加速度记测量噪声；Q_angle是陀螺仪的测量噪声吗，Q_gyro是陀螺仪温度引起静态偏差噪声吗，迷糊了有论文上说Q_angle是加速度计测量噪声，Q_gyro是陀螺仪测量噪声？？求解

华仔情调

666666