匿名飞控TI版_滤波函数和互补融合滤波

匿名飞控代码的互补滤波代码和滤波函数Ano_Filter.c的一些函数。

---

---

一、互补滤波

参考链接

1.陀螺仪和加速计（特性分析）

1）陀螺仪

灵敏度
        \space\space\space\space\space\space\space        测量角速度，具有高动态特性，它是一个间接测量角度的器件其中一个关键参数就是gyro sensitivity（其单位是millivolts per degree persecond，把转速转换到电压值），测量范围越小气灵敏度越好。也就是说测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。陀螺仪就是内部有一个陀螺，它的轴由于陀螺效应始终与初始方向平行，这样就可以通过与初始方向的偏差计算出旋转方向和角度。

偏移
        \space\space\space\space\space\space\space        偏移就是在陀螺没有转动的时候却又输出，这个输出量的大小和供电电压以及温度有关，该偏移可以在陀螺仪上电时通过一小段时间的测量来修正。

漂移
        \space\space\space\space\space\space\space        它是由于在时间的积累下偏移和噪声相互影响的结果，例如有一个偏置（offset）0.1dps加在上面，于是测量出来是0.1dps，积分一秒之后，得到的角度是0.1度，1分钟之后是6度，还能忍受，一小时之后是360度，转了一圈，也就是说，陀螺仪在短时间内有很大的参考价值。

白噪声
        \space\space\space\space\space\space\space        电信号的测量中，一定会带有白噪声，陀螺仪数据的测量也不例外。所以获得的陀螺仪数据中也会带有白噪声，而且这种白噪声会随着积分而累加。

积分误差
        \space\space\space\space\space\space\space        对陀螺仪角速度的积分是离散的，长时间的积分会出现漂移的情况。所以要考虑积分误差的问题。

溢出
        \space\space\space\space\space\space\space        就是转速超过了其测量的最大转速范围。

捷联式惯性导航系统传送门

2）加速度计

        \space\space\space\space\space\space\space        加速度计的低频特性好，可以测量低速的静态加速度。在无人机上就是对重力加速度g的测量和分析。当把加速度计拿在手上随意转动时，看的是重力加速度在三个轴上的分量值。
        \space\space\space\space\space\space\space        加速度计在自由落体时，其输出为0。为什么会这样呢？这里涉及到加速度计的设计原理：加速度计测量加速度是通过比力来测量(比力方程，秦永元的书中有介绍)，而不是通过加速度。加速度计仅仅测量的是重力加速度,而重力加速度与刚才所说的R坐标系(地理坐标系)（EarthFrame）是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系。
        \space\space\space\space\space\space\space        加速度计仅仅测量的是重力加速度，3轴加速度计输出重力加速度在加速度计所在机体坐标系3个轴上的分量大小。重力加速度的方向和大小是固定的。通过这种关系，可以得到加速度计所在平面与地面的角度关系。
        \space\space\space\space\space\space\space        加速度计若是绕着重力加速度的轴转动，则测量值不会改变，也就是说加速度计无法感知这种水平旋转。
比力

3）磁力计

        \space\space\space\space\space\space\space        可以测量磁场，在没有其他磁场的情况下，仅仅测量的是地球的磁场，而地磁也是和R坐标系固连的，通过这种关系，可以得到平面A和地平面的关系。(平面A：和磁场方向垂直的平面)，同样的，若是沿着磁场方向的轴旋转，测量值不会改变，无法感知这种旋转。
        \space\space\space\space\space\space\space        综合考虑，加速度计和磁传感器都是极易受外部干扰的传感器，都只能得到2维的角度关系，但是测量值随时间的变化相对较小，结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系，动态性能好，受外部干扰小，但测量值随时间变化比较大。可以看出，它们优缺点互补，结合起来才能有好的效果。

比喻

有关陀螺仪和加速度计和关系，姿态解算融合的原理，再把下面这个比喻放到这里一遍。
        \space\space\space\space\space\space\space        机体好似一条船，姿态就是航向（船头的方位），重力是灯塔，陀螺（角速度积分）是舵手，加速度计是瞭望手。舵手负责估计和把稳航向，他相信自己，本来船向北开的，就一定会一直往北开，觉得转了90度弯，那就会往东开。当然如果舵手很牛逼，也许能估计很准确，维持很长时间。不过只信任舵手，肯定会迷路，所以一般都有瞭望手来观察误差。
        \space\space\space\space\space\space\space         瞭望手根据地图灯塔方位和船的当前航向，算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位，那肯定船的当前航向有偏差了，偏差就是ERR=X-Y。舵手收到瞭望手给的ERR报告，觉得可靠，那就听个90%ERR，觉得天气不好、地图误差大，那就听个10%ERR，根据这个来纠正估算航向。
陀螺仪加速度计MPU6050

			
#ifdef USE_MAG//（用的这个，在普通互补融合滤波基础上增加了磁力计补偿）
			d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ;
#else	//（这个没用，普通的互补融合滤波，用加速度计pi控制器输出补偿陀螺仪）
			d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use ) * dT / 2 ;
#endif
		}
    // 计算姿态。 姿态更新（一阶龙格库塔法，wxyz初值为1000）

    imu->w = imu->w            - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z];
    imu->x = imu->w*d_angle[X] + imu->x            + imu->y*d_angle[Z] - imu->z*d_angle[Y];
    imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y            + imu->z*d_angle[X];
    imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z;
		
		q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);
    imu->w *= q_norm_l;
    imu->x *= q_norm_l;
    imu->y *= q_norm_l;
    imu->z *= q_norm_l;
		

这里的d_angle是陀螺仪得到的3轴角加速度且经过PI器进行修正，修正参考方向为磁力计得到的标准方向。
代码看不懂可以参考匿名姿态解算

二、低通滤波

一阶RC低通滤波

1.一阶低通滤波

void LPF_1(float hz,float time,float in,float *out)  
{
	*out += ( 1 / ( 1 + 1 / ( hz *6.28f *time ) ) ) *( in - *out );

}

void LPF_1_db(float hz,float time,double in,double *out)
{
	*out += ( 1 / ( 1 + 1 / ( hz *6.28f *time ) ) ) *( in - *out );

}

Y(n)-Y(n-1) = A*(X(n)-Y(n-1))

这里 A是$\frac{1}{1+\frac{1}{hz\ast 6.28\ast time}}=\frac{1}{1+\frac{RC}{T}}=\frac{T}{T+RC}$

X为输入 Y为输出