MPU6050

//******卡尔曼参数************

float code Q_angle=0.001; float code Q_gyro=0.003; float code R_angle=0.5;

float code dt=0.01; //dt为kalman滤波器采样时间; char code C_0 = 1;

float xdata Q_bias_x, Angle_err_x; float xdata PCxt_0, PCxt_1, EX; float xdata KX_0, KX_1, xt_0, xt_1; float xdata Pdotx[4] ={0,0,0,0};

float xdata PX[2][2] = { { 1, 0 },{ 0, 1 } };

float Kalman_Filter(float Accelx,float Gyrox) {

Angle_x+=(Gyrox - Q_bias_x) * dt; //先验估计

Pdotx[0]=Q_angle - PX[0][1] - PX[1][0]; // Pk-先验估计误差协方差的微分 Pdotx[1]=- PX[1][1]; Pdotx[2]=- PX[1][1]; Pdotx[3]=Q_gyro;

PX[0][0] += Pdotx[0] * dt; // Pk-先验估计误差协方差微分的积分 PX[0][1] += Pdotx[1] * dt; // =先验估计误差协方差

PX[1][0] += Pdotx[2] * dt; PX[1][1] += Pdotx[3] * dt; Angle_err_x = Accelx - Angle_x; PCxt_0 = C_0 * PX[0][0]; PCxt_1 = C_0 * PX[1][0]; EX = R_angle + C_0 * PCxt_0; KX_0 = PCxt_0 / EX; KX_1 = PCxt_1 / EX; xt_0 = PCxt_0; xt_1 = C_0 * PX[0][1]; PX[0][0] -= KX_0 * xt_0; PX[0][1] -= KX_0 * xt_1; PX[1][0] -= KX_1 * xt_0; PX[1][1] -= KX_1 * xt_1; Angle_x += KX_0 * Angle_err_x;

Q_bias_x += KX_1 * Angle_err_x; //后验估计 Gyro_x = Gyrox - Q_bias_x; return Gyro_x; }

void Angle_Calcu(void) {

Accel_y = GetData(ACCEL_XOUT_H);

Angle_ay = (Accel_y +28) /16384; //去除零点偏移,计算得到角度（弧度） Angle_ay = Angle_ay*1.4*180/3.14; //弧度转换为度, Gyro_x = GetData(GYRO_YOUT_H);

Gyro_x = -(Gyro_x -28)/16.4; //去除零点偏移，计算角速度值,负号为方向处理 Angle_x = Angle_x + (((Angle_ay-Angle_x)*0.5 + Gyro_x)*0.01);}