滤波器设计实验一
一. 无人机滤波器简介
无人机在飞行时会使用滤波器来处理传感器数据、控制飞行和稳定飞行，以及实现导航和定位等功能。卡尔曼滤波器是无人机领域中常见滤波器类型之一，也称为线性二次型估计，能够从一系列不完全且包含噪声不确定性的观测量中，估计系统的未知状态其估计精度往往比单纯地基于单一观测量的方法更高。这种滤波器以它的发明者鲁道夫·卡尔曼(Rudolf E. Kalman)命名。
卡尔曼滤波器要求噪声误差满足高斯分布假设，在这个特定情况下能够产生精确的条件概率估计,卡尔曼滤波器算法主要分为两步处理。在预测步骤中，卡尔曼滤波器产生当前状态变量的预测估计，这些估计量包含不确定性,一旦出现下一个观测量（伴随着一定的误差以及随机噪声），之前的估计量会以加权平均的方式更新，其权重值会随着估计的确定性而变化，确定性越大，其权重值越大。卡尔曼滤波器是一种实时的递归滤波器，仅仅利用当前观测量和先验估计量及不确定性矩阵，而不需要增加多余的历史信息。
二. 测量原理
三轴加速度计固连在多旋翼机体，其三轴与机体坐标系一致。因此，低频的俯仰角和滚转角观测量可以由加速度计测量值近似得到:
其中
表示加速度计测量值。
同时还有两点需要注意：
1）为了得到更加精确的角度信息，需要消除加速度计的慢时变漂移。
2）如果机体振动很大，则 αxbm，αybm将被噪声严重污染，这样将进一步影响角度θm，φm 的估计。因此机体的减振很重要。另外姿态变化率 θ·,φ·,ψ·和角速度bω有如下关系：
由此可知，俯仰角和横滚角可以由加速度计测量得到，漂移小，但噪声大。另一方面，姿态角也可以通过角速度积分得到，噪声小，但是漂移大。
三、线性互补滤波器设计
下面我们着重以俯仰角为例，详细推导下线性互补滤波，俯仰角的拉氏变换可以表示为：
为了估计俯仰角，以上式子的 需要用传感器信息替代。
同时，加速度计测量的俯仰角无漂移但噪声大，我们可以将测量到的俯仰角建模为θm =θ+nθ，其中nθ 表示高频噪声，θ表示俯仰角真值。
陀螺仪的角速度测量会有漂移但噪声小，我们可以建模为：
线性互补滤波的标准形式以传递函数方式表示为：
为了计算机算法实现，需要对其进行离散化
通过一阶向后差分法，将s表示为：
进一步表示为：
再把上式化为差分方程可以得到：
具体推导过程请见文献[1]第8章和文献[2]第8章内容。
互补滤波器的MATLAB程序可见如下。
其中，“theta_am”和“phi_am”分别代表由加速度计计算出的俯仰角和滚转角；theta_cf”和“phi_cf”分别代表由互补滤波计算出来的俯仰角和滚转角。
function [ phi_cf, theta_cf ] = Attitude_cf(dt, z, phi_cf_k, theta_cf_k, tao)
%函数描述:
% 互补滤波姿态结算。
%输入：
% dt: 时间间隔,单位：s
% z: 三轴角陀螺仪和三轴加速度计测量值，[gx, gy, gz, ax, ay, az]'，
% 单位：rad/s, m/s2
% phi_cf_k, theta_cf_k: 上一时刻的角度值，单位：rad
% tao: 滤波器系数
%输出:
% phi_cf, theta_cf: 解算的姿态角，单位：rad
gx = z(1); gy = z(2);
ax = z(4); ay = z(5); az = z(6);
%使用加速度计测量值计算姿态角
g = sqrt(ax*ax + ay*ay + az*az);
theta_am = asin(ax/g);
phi_am = -asin(ay/(g*cos(theta_am)));
%互补滤波
theta_cf = tao/(tao + dt)*(theta_cf_k + gy*dt) + dt/(tao + dt)*theta_am;
phi_cf = tao/(tao + dt)*(phi_cf_k + gx*dt) + dt/(tao + dt)*phi_am;
end
> 可以得到结论：
1）直接对陀螺仪测量的角速度进行积分得到的姿态角有很大的累积误差，并且还有可能发散；2）根据来自加速度计的原始数据，计算得到的姿态角不会发散，但噪声最大且有明显的尖峰，尤其是使用实际飞行中的数据时；3）使用互补滤波器估计的姿态角是平滑的并且没有累积误差。
滤波器设计实验2
四、线性互补滤波器参数分析
基于上一讲中得出的滤波器公式：
对其参数т值进行改变，对所给的数据进行滤波，即可分析该参数对滤波效果的影响。可在MATLAB中编写如下程序：
%参数tao对滤波效果的影响
clear;
load logdata
n = length(ax); %采集数据个数
Ts = zeros(1,n); %时间间隔
Ts(1) = 0.004;
for k =1:n-1
Ts(k+1) = (timestamp(k + 1) - timestamp(k))*0.000001;
end
theta_cf = zeros(1, n); %互补滤波得到的滚转角（对应theta）
phi_cf = zeros(1, n); %互补滤波得到的俯仰角（对应phi）
tao = 0.001;
for i = 1 : 3
tao = tao*10;
for k = 2 : n
g = sqrt(ax(k)*ax(k) + ay(k)*ay(k) + az(k)*az(k));
theta_am = asin(ax(k)/g);
phi_am = -asin(ay(k)/(g*cos(theta_am)));
theta_cf(i, k) = tao/(tao + Ts(k))*(theta_cf(i, k - 1) + gy(k)*Ts(k)) + Ts(k)/(tao + Ts(k))*theta_am;
phi_cf(i,k) = tao/(tao + Ts(k))*(phi_cf(i, k- 1) + gx(k)*Ts(k)) + Ts(k)/(tao + Ts(k))*phi_am;
end
end
改变该程序中的 т值分别为0.01，0.1，1时的滤波效果。
可以看到参数т越大，对高频噪声的滤波作用越明显。当т很大时：
互补滤波器变为：
相当于加速度计不起作用，只使用陀螺仪的积分值。因此，要合理选择参数т的值。
注：本实验对应demo文件对于RflySim v3.0以下版本地址为：*\PX4PSP\RflySimAPIs\Exp02_FlightControl\e4-FilterDesign\e4.1；
对于RflySim v3.0及以上版本地址为：*\PX4PSP\RflySimAPIs\5.RflySimFlyCtrl\1.BasicExps\e4-FilterDesign\e4.1
参考文献：
[1] 全权,杜光勋,赵峙尧,戴训华,任锦瑞,邓恒译.多旋翼飞行器设计与控制[M],电子工业出版社,2018.
[2] 全权,戴训华,王帅.多旋翼飞行器设计与控制实践[M],电子工业出版社,2020.