智能车制作

标题: 请教关于卡尔曼滤波的几根问题 [打印本页]

作者: 仰望星空1 时间: 2012-3-9 20:00
标题: 请教关于卡尔曼滤波的几根问题
float kalmanUpdate(const float gyro_m,const float incAngle)//只读变量不可以修改
{ // 陀螺仪角度
float K_0;//含有卡尔曼增益的另外一个函数，用于计算最优估计值
float K_1;//含有卡尔曼增益的函数，用于计算最优估计值的偏差
float Y_0;
float Y_1;

float Rate;
float Pdot[4];
float angle_err;//角度偏量
float E;

static float angle = 0; //下时刻最优估计值角度
static float q_bias = 0; //最优估计值的偏差
static float P[2][2] = {{ 1, 0 }, { 0, 1 }};

Rate = gyro_m - q_bias;
//#define dt 0.0015//滞后
//#define R_angle 0.69
//#define Q_angle 0.0001
//#define Q_gyro 0.0003 //卡尔曼滤波参数

Pdot[0] = Q_angle - P[0][1] - P[1][0]; //卡尔曼增益矩阵
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;
angle += Rate * dt;
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 = incAngle - angle;
E = R_angle + P[0][0];
K_0 = P[0][0] / E;
K_1 = P[1][0] / E;
Y_0 = P[0][0];
Y_1 = P[0][1];

P[0][0] -= K_0 * Y_0; //跟新协方差矩阵
P[0][1] -= K_0 * Y_1;
P[1][0] -= K_1 * Y_0;
P[1][1] -= K_1 * Y_1;

angle += K_0 * angle_err; //给出最优估计值
q_bias += K_1 * angle_err;//跟新最优估计值偏差
return angle;
}

请问函数中协方差矩阵和卡尔曼增益矩阵有什么作用呢？
可调的变量是哪几个呀？

作者: bu1193510501 时间: 2012-4-10 12:57
楼主这程序自己写的，效果如何

作者: 仰望星空1 时间: 2012-4-10 15:49
不是我写的，要是我写的我就不在这提问了

作者: 没文化 时间: 2012-4-27 08:43
楼主的这个程序送进去的是陀螺仪的角速度值呢还是积分后的角度呢？

作者: 抓住指間の落寞. 时间: 2013-5-8 16:26
lz你这个滤波出来的效果如何

作者: 仰望星空1 时间: 2013-5-14 23:23
没仔细调就放弃了，改用的官方的算法

欢迎光临智能车制作 (http://111.231.132.190/)