智能车制作

标题: 卡尔曼滤波的问题求教相信对大神们应该很简单了 [打印本页]

作者: 遨游天际 时间: 2013-5-1 20:42
标题: 卡尔曼滤波的问题求教相信对大神们应该很简单了

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

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 时间: 2013-5-7 13:03
没看懂...

作者: 冰冻119 时间: 2013-5-7 20:10
不会啊

作者: 抓住指間の落寞. 时间: 2013-5-8 16:45
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或者是论坛上的大神能帮助我

作者: 二百五 时间: 2013-5-8 17:11
q_bias是kalman估计出来的陀螺仪温漂 P[2][2]都是协方差这是个二阶的系统先验估计就是CarAngle+=(gyro_m-q_bias) * dt

作者: pangfumin 时间: 2013-11-18 18:28

二百五发表于 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;

作者: 二百五 时间: 2013-11-18 21:13

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

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

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

作者: pangfumin 时间: 2013-11-18 22:27

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

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

作者: pangfumin 时间: 2013-11-18 23:40

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

哦，算过了，基本一样

欢迎光临智能车制作 (http://dns.znczz.com/)

Powered by Discuz! X3.2