更新参数的一阶低通滤波算法 C++

本文属于个人原创作品，未经作者允许请勿转载

最近自己想做一个四轴，可是卡尔曼滤波看不懂，emmm，大一狗。但是因为MPU6050的加速度计传出的数据大部分误差是高频误差，那就可以用低通滤波器来进行滤波啊

所以低通滤波器到底是什么呢
就是一个能够过滤掉高频率信号使得数据图线变得平滑的算法。
核心公式是：
new_data = (1-K) old_data + K* new_data;*
其中new_data是新读取的数据，old_data是上次滤波结果，K是系数
我们先来定义几个量，并解释他们的用处
系数K
用来衡量我们对新读取的数据的信任程度。K越大表示我们对新读取的数据越信任，K越大，滤波结果对输入数据的变化月灵敏。K取值范围0-1。在算法中我们会不断的更新这个系数。

所以什么时候我们对新数据的信任程度大呢
答案是：当数据连续大幅度增长或减小的时候。试想，如果数据连续大幅度增长或减小的时候我们对新的输入数据信任程度依旧很小，那么新的滤波结果就会更加接近上次滤波结果，导致滤波器对变化的敏感度小，滤波器比较迟钝。

所以如何判断数据连续大幅度增长或减小呢
我们定义一个数num。通过新数据与上次滤波结果作差即可算出增长（减小）幅度，但是如何判断连续呢？num就是用来干这个的。如果某次数据增长（减小），我们让num增加，num增加到一定程度时，说明数据真的在连续大幅度增长（或减小ÿ