晒一下我写的卡尔曼

linping9656

static  float Q_angle=0.01 ,Q_gyro=0.0001 ,R_angle=0.25,R_gyro=0.01,dt=0.1;
                        //注意：dt的取值为kalman滤波器采样时间;
static float Pk[2][2] = { {1, 0 }, {0, 1 }};
       
static float q_bias ,angle_err,E_0,E_1,K_0 ,K_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m)                       
{
        angle+=(gyro_m-q_bias) * dt;//先估计倾角 陀螺仪
  Pk[0][0]+=Q_angle;
  Pk[1][1]+=Q_gyro;
  E_0=Pk[0][0]+R_angle;
  E_1=Pk[1][1]+=R_gyro;
  angle_err=angle_m-angle;
  K_0=Pk[0][0]/E_0;
  K_1=Pk[1][1]/E_1;
  angle+=K_0*angle_err;
  q_bias=K_1*angle_err;
  angle_dot=gyro_m-q_bias;
  Pk[0][0]=(1-K_0)*Pk[0][0];
  Pk[1][1]=(1-K_1)*Pk[1][1];
}

xingyan

顶一下……

linping9656

这个波形

zhangwenqiang

楼主这几个值是怎么调的啊 Q_angle=0.01 ,Q_gyro=0.0001 ,R_angle=0.25,R_gyro=0.01

linping9656

zhangwenqiang 发表于 2012-6-6 22:18
楼主这几个值是怎么调的啊 Q_angle=0.01 ,Q_gyro=0.0001 ,R_angle=0.25,R_gyro=0.01

其实自己调一调就能总结出经验了。指导卡尔曼增益吗？

zhangwenqiang

就这几个参数Q_angle=0.01 ,Q_gyro=0.0001 ,R_angle=0.25,R_gyro=0.01

linping9656

zhangwenqiang 发表于 2012-6-6 22:44
就这几个参数Q_angle=0.01 ,Q_gyro=0.0001 ,R_angle=0.25,R_gyro=0.01

自己好好调调啊。我的可能理解错了 会误导你的

zhangwenqiang

呵呵 楼主太谦虚了  就算错了我以为心甘情愿

chenyu2010550

chenyu2010550

同求啊