;
float dt; //微分时间
int16_t ax, ay, az, gx, gy, gz; //加速度计陀螺仪原始数据...; //加速度计比例系数
float GyroRatio = 131.0; //陀螺仪比例系数
uint8_t n_sample...//x,y轴采样队列
long aax_sum, aay_sum,aaz_sum; //x,y轴采样和
float a_x[10]={0}, a_y[10]=...accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
axo += ax; ayo += ay; azo += az; //采样和...Kx = Px / (Px + Rx); //计算卡尔曼增益
agx = agx + Kx * (aax - agx); //陀螺仪角度与加速度计速度叠加