滤波算法

算法一：一阶滤波算法（低通滤波器）

首先要讲的是一阶滤波算法，也就是低通滤波算法，这个滤波算法对于低频的噪声具有非常好的效果，对于0到一定频率的信号是能够无失真接收的。这个算法通过硬件的电路推导，因其十分的简单，一阶滤波算法为：滤波结果=a*本次采样值+(1-a)上一次滤波的结果。

推导过程：

                                                             

       从电路图开始说起。为方便推导，此时我们将负载拉到最大，进入最理想得的状态。由公式Q=I*T得I=Q\T。因为负载拉到最大，此时负载的阻抗极大，想当于没有电流通过。则此时通过的电流相当于电阻通过的所有电流。

               因为电容和负载相互并联。

                                                    Vout=Vin-(Q\T)*R

               由电容公式得，C=Q\U,代入上式得

Vout=Vin-(C*Vout\T)*R;

               整理得：

            Vout=Vin-(Vout\T)*R*C;

                对其进行离散化得。

                                               Vout(K)=Vin(K)-{[Vout(k)-Vout(k-1)]/T}*R*C     

                 最后进行整理得：

                                               Vout(k)=[Vin(k)+(RC\T)*Vout(k-1)]/1+RC/T

                 如果设1/1+RC/T=a,那么(RC/T)/1+RC/t=1-a;

                                              由此可得，

                                              Vout(K)=[aVin(K)+(1-a)*Vout(k-1)]

                      

                   完结。

那么还有最后一个问题。如何算滤波的频率范围呢？在电子学的定义中，截止频率一般为                           

                                 fL=1/2*Π*R*C

想要滤多频率的波，一般求出R*C的值，代入便可。

历程：

float last;

float wave_filtering(float y1)
{
   float output;
   output=y1*A+(1-A)*last;

   last=output;
   return output;
}

算法二：高通滤波算法

高通滤波算法是低通滤波得好兄弟。这个滤波算法对于高频的噪声具有非常好的效果，到截止频率的信号是能够接收的。这个算法也是通过硬件的电路推导，一阶滤波算法为：滤波结果=(1-a)*本次采样值+a*上一次滤波的结果。

      推导过程。引起推导过程与低通滤波还是非常相识的

                                              

                                                   这里的话就不推导了。

       滤波结果=(1-a)*本次采样值+a*上一次滤波的结果。

历程：

   

float last;

float High_pass_filtering(float y1)
{
   float output;
   output=y1*(1-A)+A*last;

   last=output;
   return output;
}

算法三：互补滤波算法

    互补滤波跟卡尔曼滤波非常的相似。相比于卡尔曼滤波，互补滤波的系数值都是固定的，而卡尔曼滤波是在不断的变化的。

   为了更好得滤波，互补滤波可以跟高通滤波和低通滤波一起来增加滤波得准确性。

                

    具体做法为，滤波数据=（高通滤波的数据*a）+（低通滤波的数据*b）。（a+b=1）

      

算法四：卡尔曼滤波算法

了解了卡尔曼滤波，就可以被成为地表极为强大的男人了。上到神州奔月到电池电量估计都能用上。

因为观测器的观测值不是完全准确的，存在一定的误差。此时我们可以用上一时刻的值来得出一个估计值，并用估计值和实际值进行一个加权算平均。比如说，为了测量电机的速度，我们可以根据上一时刻电机的速度，以及在一段时间内对电机的操作数值得出一个估计值。将估计值乘以一个系数K，同时将观察值乘以另一个系数R，最后将两数求和，以此来弥补观测值的不准确度。

 对于通过电机的上一个状态得出电机的此时的状态。公式为。

                                             

其中Xk-1为上一次电机的状态，A为状态转移矩阵，B为控制矩阵，Uk为控制输入。Wk-1为环境噪声，环境噪声服从高斯分布。

 通过这个我们可以从前一次的状态推导出此时此刻的状态。

开头说到，我们通过将估计值乘以一个系数K，同时将观察值乘以另一个系数R，最后将两数求和得出最优估计值。那么问题来了，系数K和系数R分别是什么呢。那么公式来了。

                                              

由上式我们可以轻易得出系数K即是Kn——增益矩阵。（H为系数矩阵，即由x到观测值的系数矩阵，Zx即为观测矩阵）

那么问题又来了，Kn——增益矩阵怎么得来的呢?

                                                        

增益矩阵由此可得。其中R为测量噪声协方差，一般不用求，属于器件的基本属性。

那么问题又出现了Pt-是个什么东西，其实是估计值的方差。Pt-由公式得

                                    

此处的A与上处的A意义相同，都为Xn-1化为Xn的矩阵。Q为环境噪声（Wk-1）的方差。（此处注意，Pk-是有—号的）

那么问题又双来了，Pk-1又是怎么来的呢？Pk-1来自于上一次，但问题是现在的Pk-是带—号的，那么下一次的Pk+1怎么办呢。

 来了：

                                  

通过以上的过程，可实现卡尔曼滤波过程的。总的来说，卡尔曼滤波可以分为预测步和更新步。

其中，预测步为：

1   ，

2     

然后，更新步为：

1，

2，

3，

算法五：限幅滤波算法

设有采样值y(n),y(n-1).根据自己的经验，设定一个值，若|y(n)-y(n-1)|的值大于限幅值，则可判断y(n)为干扰，此时可将采样值y（n）舍弃。取y(n-1)为y(n).

历程：

#include <stdio.h>

#define xianfuzhi 777777

float xianfu(float y1, float y2)
{
   float output;
   if (((y2 - y1) > xianfuzhi) || ((y1 - y2) > xianfuzhi))
   {

      output = y1;
   }
   else
   {
      output = y2;
   }
   return output;
}

int main()
{
}