更新参数的一阶低通滤波算法 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增加到一定程度时，说明数据真的在连续大幅度增长（或减小）。这时候就应该对新的数据信任度高一点，提高对变化的灵敏度。

好的，我们来总结一下
如果数据连续大幅度同向变化（同向是指一直增长或者一直减小），那么num就会因为多次增加大于某个值；此时我们增大k，使得滤波器对新数据更敏感，即对变化更敏感。当num一直很小时，我们更多的信任旧数据，因为新数据可能带来误差。

所以代码如下：

#include<iostream>
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#define Threshold_1 8    //临界点1，当数据变化值超过这个值时开始改变num_x 
#define Threshold_2 20   //临界点2，当数据变化值超过这个值时开始改变k_x 
using namespace std;
int num_x = 0;      //一个过渡值，对于同方向变化量大的数据，num_x越大 
float k_x = 0.2;    //表示对新读入的数据的信任度，取值范围0-1 
int old_flag = 0;   //表示第n-2个数据到第n-1个数据的变化趋势，加为1，减为0 
int new_flag = 0;   //表示第n-1个数据到第n个数据的变化趋势，加为1，减为0
float old_data = 0;     //第n-1次的答案 
float new_data = 0;     //第n次的输入值 

int main()
{
     freopen("asd0.txt","r",stdin);
     freopen("asd2.txt","w",stdout);
     //for (int i=1;i<=3;i++)
    //  cout<<i<<",";
    //  cout<<endl<<endl;

     for (int i = 1;i <= 300;i++ )      //一个300个数据 
     {
        cin>>new_data;                  
        if (new_data - old_data > 0 )       //计算方向 
            new_flag = 1;
        else new_flag = 0;
        if (new_flag == old_flag)           //变化同向 
        {
            if (abs (new_data - old_data) > Threshold_1)    //变化很大则num增大 
                num_x += 5;
            if (num_x >= Threshold_2)       //变化持续同向且一直变化很大则k_x增大 
                k_x += 0.2; 
        }

        else                //变化反向 
        {
            num_x = 0;      
            k_x = 0.2;
            old_flag = new_flag; 
        } 
        if (k_x > 0.95)  k_x = 0.95;    //系数限幅 
        new_data = (1-k_x) * old_data + k_x * new_data;   //计算答案 

        cout<<new_data<<",";
        //cout<<"old_data "<<old_data<<" new_data  "<<new_data<<" oN flag "<<old_flag<<" "<<new_flag<<" k "<<k_x<<endl;
        old_data = new_data;        //更新old_data 
     } 

}

测试数据用随机数生成，图中红色为原始数据，灰色为K比较小时的图线，橘色为K比较大时的图线。可以看出，K越小，图线对变化不敏感，对高频误差滤波效果好；K越大，图线对变化越敏感，表现为反应快。

这就是更新参数的一阶低通滤波算法。

代码和相关数据我上传到了百度网盘：
https://pan.baidu.com/s/1sl9LEln
新手作品，如有错误请多指教