卡尔曼滤波的问题 求教 相信对大神们应该很简单了

遨游天际

以下网上的卡尔曼滤波的程序

static const float Q_angle=0.001, Q_gyro=0.003, R_angle=300, dt=0.005;

                        //注意：dt的取值为kalman滤波器采样时间;

static float P[2][2] = {{ 1, 0 },{ 0, 1 }};

static float Pdot[4] ={0,0,0,0};

static const char C_0 = 1;

static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

//-------------------------------------------------------

        CarAngle+=(gyro_m-q_bias) * dt;

        Pdot[0] = Q_angle - P[0][1] - P[1][0];

        Pdot[1] = -P[1][1];

        Pdot[2] = -P[1][1];

        Pdot[3] = Q_gyro;

        P[0][0] += Pdot[0] * dt;

        P[0][1] += Pdot[1] * dt;

        P[1][0] += Pdot[2] * dt;

        P[1][1] += Pdot[3] * dt;

        angle_err = angle_m - CarAngle;

        PCt_0 = C_0 * P[0][0];

        PCt_1 = C_0 * P[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 * P[0][1]；

        P[0][0] -= K_0 * t_0;

        P[0][1] -= K_0 * t_1;

        P[1][0] -= K_1 * t_0;

        P[1][1] -= K_1 * t_1;

        CarAngle += K_0 * angle_err;

        q_bias        += K_1 * angle_err;

        CarGyro = gyro_m-q_bias;

}

我不知道P[2][2]数组4个数是什么意思，P[0][0]应该存的是最优卡尔曼估计Xk|k时的协方差，那另外3个是什么啊。还有q_bias是什么，最初的先验估计是CarAngle+=(gyro_m-q_bias) * dt，我觉得应该是CarAngle+=gyro_m * dt，那个q_bias没什么必要吧，直接对陀螺仪角速度积分，加上上一时刻的最优卡尔曼估计的角度值就行了吧。为什么要减去q_bias呢？

xiaohai0825

没看懂...

冰冻119

不会啊

抓住指間の落寞.

lz知不知道这个卡尔曼的抑制温漂效果的语句是那几句？抑制温漂的参数是Q_angle=0.001, Q_gyro=0.003, R_angle=300, dt=0.005;的哪一个？
发现LZ的R-angle的值好大，我们的R-angle用的是0.5，小车能直立但是角度会变化，难道R_angle的值大，抑制温漂的效果越好？
表示不懂，希望lz或者是论坛上的大神能帮助我

二百五

q_bias是kalman估计出来的陀螺仪温漂  P[2][2]都是协方差  这是个二阶的系统 先验估计就是CarAngle+=(gyro_m-q_bias) * dt

pangfumin

二百五 发表于 2013-5-8 17:11
q_bias是kalman估计出来的陀螺仪温漂  P[2][2]都是协方差  这是个二阶的系统 先验估计就是CarAngle+=(gyro_ ...

说的很有道理

请教一个问题，为什么对p的估计要这么写呢，从卡拉曼的5个公式的第二个推算额、了一下，应该不是这样的啊

   Pdot[0] = Q_angle - P[0][1] - P[1][0];

        Pdot[1] = -P[1][1];

        Pdot[2] = -P[1][1];

        Pdot[3] = Q_gyro;

        P[0][0] += Pdot[0] * dt;

        P[0][1] += Pdot[1] * dt;

        P[1][0] += Pdot[2] * dt;

        P[1][1] += Pdot[3] * dt;

二百五

pangfumin 发表于 2013-11-18 18:28
说的很有道理

请教一个问题，为什么对p的估计要这么写呢，从卡拉曼的5个公式的第二个推算额、了一下， ...

基本一样，我推过，你仔细看看吧。有一点点不一样。

pangfumin

二百五 发表于 2013-11-18 21:13
基本一样，我推过，你仔细看看吧。有一点点不一样。

嗯，基本一样，化简的基础在哪呢，为什么可以这么化简

pangfumin

pangfumin 发表于 2013-11-18 22:27
嗯，基本一样，化简的基础在哪呢，为什么可以这么化简

哦，算过了，基本一样