//卡尔曼滤波参数与函数 float dt=0.005;//注意：dt的取值为kalman滤波器采样时间 float angle, angle_dot;//角度和角速度 float P[2][2] = {{ 1, 0 }

1. MPU6050陀螺仪 // 陀螺仪 float angleAx,gyroGy; MPU6050 accelgyro; int16_t ax, ay, az, gx, gy, gz; accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//原始数据采集 angleAx=atan2(ax,az)*180/PI;//加速度计算角度 gyroGy=-gy/131.00;//陀螺仪角速度，注意正负号与放置有关 2.滤波参数及函数 //一阶互补滤波 float K1 =0.05; // 对加速度计取值的权重 float dt=20*0.001;//注意：dt的取值为滤波器采样时间 float angle1; void Yijielvbo(float angle_m, float gyro_m)//采集后计算的角度和角加速度 { angle1 = K1 *

在圆点博士小四轴2014版代码里，我们只使用到PD参数。 首先我们来看PID中的比例控制。跟上一节模型提到的一样，比例是针对误差的控制。 首先我们获取