晒一下我写的卡尔曼

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];
}

endlesshai

linping9656 发表于 2012-6-6 22:41
其实自己调一调就能总结出经验了。指导卡尔曼增益吗？

为啥你的多一个R_gyro呢

softwareboy

935631408

楼主可佛留下扣扣，多多请教！

linping9656

chenyu2010550 发表于 2012-6-14 21:42
可以解释下不？？？

呵呵  自己找材料看了 我也不理解

chenyu2010550

可以解释下不？？？

chenyu2010550

真的不理解

linping9656

chenyu2010550 发表于 2012-6-14 01:14
同求啊

不理解啊

chenyu2010550

同求啊

chenyu2010550