浅谈陀螺仪和加速度计的互补滤波
ref:https://blog.csdn.net/zhaojun1204/article/details/52790697
结合加速度计和陀螺仪的平衡解决方案
首先通过ADC采样，读出陀螺仪和编码器的电压值，并将其转化为可用单位：
1.偏移（offset）：让陀螺仪静止，加速度计保持水平并静止，这时候读到电压值，就是偏移值。如果读数不稳定，取平均值即可。
2.转换系数（scale):传感器输出电压值乘以系数，得到我们想要的单位，这个系数可以在传感器的数据手册（datasheet）中找到。
关于加速度计
根据加速度计Y轴和X轴的值，我们可以用反正切函数求出角度值，没有Y轴，我们可以通过sin或者cos以及X轴值，求得角度值（重力加速度是已知的）。
数字滤波器
实际单片机用的积分，其实就是个累加；
设采样间隔为dt，每次采到的角速度值是gyro，那么，
角度可以这样得到： angle += gyro * dt;
低通滤波器
低通滤波的目的，保留长期变化，滤掉短暂变化。让信号一点一点的起作用而不是一下子猛烈作用，即在时间范围上取平均。
angle = 0.98 * angle + 0.02 * x_acc;//x_acc是加速度计的实时角度值。
高通滤波器
数字高通滤波器，不那么容易解释，它跟低通滤波器是刚好相反的：除去长期变化，保留短暂变化。高通滤波器加于陀螺仪，可以滤掉温漂；
采样周期
程序每次大循环用的时间，下面标记为dt；
时间常数
时间常数用来定义滤波器对信号的作用；对于低通滤波器，信号长度比时间常数持久的，可以通过滤波器；对于高通滤波器，信号长度比时间常数短的，可以通过滤波器；对于一个数字滤波器：
y = a * y + (1-a) * x ;
时间常数tau可以这样计算：
tau = (a*dt) / (1-a) <—> a = tau/(tau + dt);
所以，当你知道了要用的时间常数，就可以通过上述公式得到滤波器的系数a；
互补
互补意味着滤波器的两个部分的和总为1，以确保得到的结果是准确的，也使得线性估算变得可行；这里用到的角度互补滤波，并不是完全“互补”，不过得到的结果还能满足我们需求。
angle = (0.98) * (angle + gyro*dt) + (0.02) * (x_acc);
如果这里滤波器的循环频率时每秒100次(亦即循环周期 = 0.01s)，那么上述公式的时间常数就是：
tau = (a * dt)/(1-a) = ( 0.98*0.01s )/0.02s = 0.49s
这个时间常数确定了陀螺仪和加速度计的信任边界。信号时间周期低于半秒时，陀螺仪占主导地位，加速度计的噪声除掉；信号时间周期大于半秒时，加速度计的角度平均值就占据主导地位，有温漂的陀螺仪可以站一边去。
实验，是检验时间常数的标准之一。首先你要挑一个时间常数，然后算出滤波器的系数a，弄到代码中去。检查角度拟合曲线，如果拟合角度与实际角度总相差那么2°，就说明陀螺仪的温漂影响较大，你可能需要把时间常数设置到1s以下，来保证加速度计能尽快校正陀螺仪的温漂值。不过，时间常数越短，就意味着加速度计的噪声更容易通过。
- 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 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);
}
//二阶互补滤波
float K2 =0.2; // 对加速度计取值的权重
float x1,x2,y1;
float dt=20*0.001;//注意：dt的取值为滤波器采样时间
float angle2;
void Erjielvbo(float angle_m,float gyro_m)//采集后计算的角度和角加速度
{
x1=(angle_m-angle2)(1-K2)(1-K2);
y1=y1+x1*dt;
x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
angle2=angle2+ x2*dt;
}