ardupilot 中低通滤波器知识学习

ardupilot 中低通滤波器知识学习

目录

文章目录

​​目录​​​​摘要​​​​1.一阶RC低通滤波器​​​​2.连续系统如何转换为离散系统​​

​​1.总共有三种方法可以实现：​​

​​1.前项差分变换 （T为采样间隔）​​​​2.后项差分变换 （T为采样间隔）​​​​3.Tustin (双线性变换)变换 （常用方法） （T为采样间隔）​​

​​2.三种方法推导：​​​​1.总共有三种方法可以实现：​​

​​3.一阶RC低通滤波器的拉普拉斯变换​​

​​1.采用一节后向差分法从S变换到Z变换​​

​​4.ardupilot中低通滤波器代码实现​​​​5.二阶低通滤波器​​

​​1.二阶低通滤波器推导过程​​​​2.二阶低通滤波器拉普拉斯变换​​

摘要

本节主要学习ardupilot中滤波器的知识，欢迎批评指正！

1.一阶RC低通滤波器

一阶RC滤波器如下图所示

电阻R与电容C构成一个串联电路，输入电压为Ui,输出电压为U0,电容的容抗Xc为1/jwc,则其传递函数为

所以可以得到传递函数的幅频特性：

我们画下一阶低通滤波器的幅频特性曲线：

2.连续系统如何转换为离散系统

由于我们的飞控处理的是数字信号，因此需要把上面的时域中的连续函数转换成离散形式。学过信号与系统就知道：信号与系统中时域方程——》傅里叶变换——》拉普拉斯变换——》Z变换——》差分方程。这里我们要实现从时域到差分方程的转换。具体资料视频可以参考：​​视频资料​​

今天跟大家分享连续系统转化为离散系统的方法之-- z变换。z变换主要研究如何将连续系统传递函数G(s) 转化为离散传递函数 G(z)。再简单一点，找到一个s和z的关系，直接将G(s)中的s全部替换为z便大功告成。不足纰漏之处，大家多多批评指正。

线性连续系统的表达方法之一便是传递函数，用 s 表示微分

线性离散系统的表达方法之一也是传递函数，用 Z表示差分：

第一次划重点， 连续系统传递函数（1）中的基本单元是s，离散系统传递函数（2）中基本单元是 z。离散化便是找到一个s和z的对应关系，然后将s替换成z即可。

1.总共有三种方法可以实现：

1.前项差分变换 （T为采样间隔）

2.后项差分变换 （T为采样间隔）

3.Tustin (双线性变换)变换 （常用方法） （T为采样间隔）

2.三种方法推导：

1.总共有三种方法可以实现：

想进一步了解三种变换怎么来的吗？请继续往下看。公式较多，但是很简单，希望能帮助记忆！ 为找到s和z的对应关系，只需研究下边简单系统（4）

3.一阶RC低通滤波器的拉普拉斯变换

可以知道上面电路的传递函数的S变换为：

从前面知道低通滤波器的截至频率fc等于：

所以我们可以得到RC的值为

知道这个值后，等下S变换转Z变换会使用。

1.采用一节后向差分法从S变换到Z变换

这里假设输入是x(t) 则输出为y(t),离散化后就是差分方程，因此我们把上式变成差分方程即可。这里假设2pif等于一个常数tao.

到这里我们推导出了低通滤波器的表达式，这个表达式可以在单片机中实现。其中输入量是x[n]，输出量就是y[n]

4.ardupilot中低通滤波器代码实现

//滤波更正加速度信息------ filter correction acceleration _accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f * ekfNavVelGainScaler)); _accel_target_filter.apply(accel_target, dt);

可以看出我们需要设定低通滤波器的截止频率_accel_xy_filt_hz，只有这样我们才能计算出系数。然后进行低通滤波

计算系数公式：

template void DigitalLPF::compute_alpha(float sample_freq, float cutoff_freq) { if (cutoff_freq <= 0.0f || sample_freq <= 0.0f) { alpha = 1.0; } else { float dt = 1.0/sample_freq; float rc = 1.0f/(M_2PI*cutoff_freq); alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f); }}

对比下是不是一样的 rc=1/2pifc

template T DigitalLPF::apply(const T &sample, float cutoff_freq, float dt) { if (cutoff_freq <= 0.0f || dt <= 0.0f) { _output = sample; return _output; } float rc = 1.0f/(M_2PI*cutoff_freq); alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f); _output += (sample - _output) * alpha; return _output;}

是不是感觉有点不一样，其实一样，我们看_output += (sample - _output) * alpha;可以表示成

_output _当前=_output _+ (sample - _output) * alpha= sample alpha+（1-alpha）_output _

5.二阶低通滤波器

1.二阶低通滤波器推导过程

其中U1是输入电平，U2是输出电平。电容拥有阻抗特性，其等效阻抗 ：

w为角频率，C为电容大小，可得U1、U2间的传输函数

因此我们可以得到：

对应的截至频率为fc，则有：

因此有：

2.二阶低通滤波器拉普拉斯变换

采用双线性变化法计算出Z变换

版权声明：本文内容由网络用户投稿，版权归原作者所有，本站不拥有其著作权，亦不承担相应法律责任。如果您发现本站中有涉嫌抄袭或描述失实的内容，请联系我们jiasou666@gmail.com 处理，核实后本网站将在24小时内删除侵权内容。