返回列表

求助关于卡尔曼滤波

[复制链接]

2364|0

手机看帖

扫描二维码
随时随地手机跟帖

电梯直达

楼主

wang1987|

楼主 | 2011-2-26 10:56 | 只看该作者回帖奖励

|倒序浏览 |阅读模式

卡尔曼滤波, AN, pd, ST, TE

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

使用特权