求助关于卡尔曼滤波

只看该作者 · 2011-2-25 11:19

刚开始学卡尔曼滤波，一直是一知半解，下面的源码注释是我自己加上的，但不知道对不对，求大虾们，给我注释完整，并在关键的地方能有个说明，先谢谢了，小弟拜上！
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;
}

只看该作者 · 2011-3-6 17:26

标记

只看该作者 · 2011-9-15 15:31

:victory:

只看该作者 · 2011-9-16 21:51

陌生

只看该作者 · 2011-9-18 07:59

看不懂。。。。。。想知道。。。。。。。。。

只看该作者 · 2011-11-22 20:48

用这个程序和参数，角度传感器测到的角度反应很慢，不知道道还要怎么处理

只看该作者 · 2011-11-22 22:05

最讨厌装B的！对同步电机怎么靠转速去判断空载还是负载？10Kw电动机空载也比1Kw电机负载电流大，你能说10Kw电机永远是负载？而无论什么电机的某时刻功率因数都反映这此时该电机的负载装态：如果是负的说明工作在发电机状态；如果趋近于零说明是空载，趋近于1说明是负载状态。如果不知道功率因数怎么检测，就参考一下功率因数表吧。