|
|
发表于 2012-12-26 15:45:40
|
显示全部楼层
我也动手做了一个大的，想骑着走，电路，电机，车架，控制板什么都搞好，就是卡尔曼没研究透，虽说有效果，但我觉的在网上找的这些卡尔曼程序不是真正的卡尔曼，我把程序贴上来吧是图片，我从网上截的图。看来看去里面只有一句是真实有用的简单的描述就是这个公式（ＧＹＲＯ＝ ＧＹＲＯ+ＡＣＣ*Ｋ），公式中ＧＹＲＯ代表由陀螺积分算出的角度，不是ＡＤ的值，感觉其它没什么用处，也没见到测量结果的方差在哪计算的，那些方差计算与测量值完全没有关系，如果只有这一句的话有用，就和互补差不多的意思了，程序在别的网站下载不了，也不能复制，我发个网址可以吧，我很期待能和人交流交流想法
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;
}
|
|
楼主: rantingting
[复制链接]
楼主
，还是独轮的，厉害！
