一阶惯性滤波算法起源于一阶低通滤波电路。在电路设计中，用于吸收和消除叠加在低频信号源上的高频干扰成份十分有效。如图1所示，激励源 ，通过一个由电阻R和电容器C组成充、放电回路，并以电容两端的电压作为输出，构成了基本的一阶低通滤波系统。由于电容器具有通交流阻直流的特性，因而当信号以较低频率通过该系统时，输出端没有（或几乎没有）削减，从而能够很好地通过；当信号频率较高时，输出端将会受到很大的衰减.当输入信号频率高到一定程序时，电容器相当于短路，高频会完全被滤除掉，在软件方面通过编制数字滤波程序来剔除和削弱信号中存在的高频干扰分量，这是一种重要和有效的常用手段。
根据基尔霍夫定律可得出一阶滤波电路动态输入输出模型为：
(1-1)
式中，T=RC。
阶跃电压Ui下的时域表达式：
(1-2)
在频域中的分析，频域中的表达式：
令（1-3）式中s=jw，用一阶后向差分法中从S域到Z域变换， 其中T是采样周期，表达式如下：
z反变换求差分方程后可得：
A为滤波系数，滤波系数的大小与采样周期 和滤波器的截止频率有关。(1-5)
式可以进一步简写成(1-6)式：
有公式可知一阶滤波器与当前的输入值和上一次的输出值有关；滤波系数越小，滤波结果越平稳，但是灵敏度越低；滤波系数越大，灵敏度越高，但是滤波结果越不稳定。
采用双线性变换法推导公式如下：
令（1-3）式中s=jw，将(1-7)代入得：
一.利用python实现的代码如下：
import numpy as np
import matplotlib.pyplot as plt
Ts=0.003 #采样时间
Fl=5 #截止频率
a=1/(1+1/(2*np.pi*Ts*Fl)) #滤波系数
print(a) #打印滤波计算出的值
x=np.linspace(-np.pi,np.pi,2000) #在[-pi,pi]区间上分割正2000个点
# 可以理解为信号采样时间为 2*pi/2000s
data1=np.zeros_like(x) #输入信号
data=np.zeros_like(x) #输入信号
y1=np.zeros_like(x) #一阶滤波输出
for i in range(len(x)):
#幅值为1 频率为 1Hz的信号 + 幅值为0.5 频率为30Hz的干扰信号+ 幅值为0.2 频率为6Hz的干扰信号
data =np.sin( 2 * np.pi * x)+0.2*np.sin(6* 2 * np.pi * x)\
+0.5*np.sin(100* 2 * np.pi * x)
data1[i]=np.sin( 2 * np.pi * x[i])
if(i>0):
y1[i] = a * data[i] + (1 - a) * y1[i - 1]
else:
y1[0] = data[0]
#绘制被干扰的信号 + 一阶滤波后的信号
plt.subplot(2, 1, 1)
plt.plot(x,data ,label='sig+noise')
plt.plot(x,y1, 'r',label='first order')
plt.title("Lowpass Filter Frequency Response")
plt.xlabel('Time [sec]')
plt.grid()
plt.legend()
#原信号 + 一阶滤波后的信号
plt.subplot(2, 1, 2)
plt.plot(x,data1 ,label='sig')
plt.plot(x,y1, 'r',label='first order')
plt.xlabel('Time [sec]')
plt.grid()
plt.legend()
plt.show()
二.python仿真波形图：
输入信号为幅值为1 频率为 1Hz的信号 + 幅值为0.5 频率为30Hz的干扰信号+ 幅值为0.2 频率为6Hz的干扰信号，不同频率下的仿真图如下所示：
从几次的仿真结果可以看出，在采样频率一定下，滤波效果与期望滤除的干扰的频率有关，即截止频率fc；fc频率越小，滤波系数也越小。滤波也越明显，滞后就更大，也即惯性就更大。所以在在选取滤波系数时，应该合理的选取fc的值。
一.利用python实现的代码如下（双线性变换）：
import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import butter, lfilter, freqz
Ts=0.003 #采样时间
Fl=12 #截止频率
a=1/(1+1/(2*np.pi*Ts*Fl)) #滤波系数
b0=1/(1+(1/(np.pi*Fl*Ts)))
b1=b0
a0=-(1-(1/(np.pi*Fl*Ts)))/(1+(1/(np.pi*Fl*Ts)))
print(b0) #打印滤波计算出的值
print(a0)
print(2*b0+a0)
x=np.linspace(-np.pi,np.pi,2000) #在[-pi,pi]区间上分割正2000个点
# 可以理解为信号采样时间为 2*pi/2000s
data1=np.zeros_like(x) #输入信号
data=np.zeros_like(x) #输入信号
y1=np.zeros_like(x) #一阶滤波输出
for i in range(len(x)):
#幅值为1 频率为 1Hz的信号 + 幅值为0.5 频率为150Hz的干扰信号+ 幅值为0.2 频率为6Hz的干扰信号
data =np.sin( 2 * np.pi * x)\
+0.5*np.sin(150* 2 * np.pi * x)
data1[i]=np.sin( 2 * np.pi * x[i])
if(i>0):
y1[i] = b0 * data[i] +b1 * data[i-1] +a0 * y1[i - 1]
else:
y1[0] = data[0]
#绘制被干扰的信号 + 一阶滤波后的信号
plt.subplot(2, 1, 1)
plt.plot(x,data ,label='sig+noise')
plt.plot(x,y1, 'r',label='first order')
plt.xlabel('Time [sec]')
plt.grid()
plt.legend()
#原信号 + 一阶滤波后的信号
plt.subplot(2, 1, 2)
plt.plot(x,data1 ,label='sig')
plt.plot(x,y1, 'r',label='first order')
plt.xlabel('Time [sec]')
plt.grid()
plt.legend()
plt.show()
#ifndef _ORDER_1_LP_FILTER_H_
#define _ORDER_1_LP_FILTER_H_
#include <stdint.h>
struct Order1LpFilter
{
//初始化
struct
{
void (*Set)(struct Order1LpFilter *self, float cutFreq, float runFreq); //设置截止和采样频率
void (*VaryCutFreq)(struct Order1LpFilter *self, float cutFreq); //改变截止频率
float cutFreq; //截止频率
float samFreq; //采样频率
} Init;
//采样计算
struct
{
float (*In)(struct Order1LpFilter *self, float x);
float out_y; //输出值
} Prd;
//变量 中间变量 系数等，由初始参数 初始化计算得出
struct
{
float Ts; //采样周期
float a0, b0, b1; //差分系数
float Xn_1, Yn_1;
} pri;
};
void Order1LpFilter_Create(struct Order1LpFilter *self);
#endif
//创建方式
// struct Order1LpFilter mlp;
// Order1LpFilter_Create(&mlp);
// mlp.Init.Set(&mlp,12, 1000);#include "Order1LpFilter.h"
#include <string.h>
#include "math.h"
#define MID(a, min, max) (a = (a < min) ? min : (a < max) ? a : max)
static const float PI = 3.1415926535897932384626f;
#define my self->pri
//一阶级惯性滤波
/*******************************************************************************
* 函 数 名 : _Update
* 函数功能 : 各系数计算
* 输入参数 : 无
* 返 回 值 : 无
*******************************************************************************/
static void _Update(struct Order1LpFilter *self)
{
float temp;
float halfdigiW, tgAnaWT;
halfdigiW = PI * self->Init.cutFreq * self->pri.Ts;
tgAnaWT = tan(halfdigiW); //ignore the 1/T
self->pri.b0 = 1.0f/(1.0f +1/tgAnaWT);
self->pri.b1 = self->pri.b0;
self->pri.a0 = -(1.0f - 1/tgAnaWT)/(1.0f + 1/tgAnaWT);
}
/*******************************************************************************
* 函 数 名 : InitSet
* 函数功能 : 初始化
* 输入参数 : 无
* 返 回 值 : 无
*******************************************************************************/
static void InitSet(struct Order1LpFilter *self, float cutFreq, float runFreq)
{
self->Init.cutFreq = MID(cutFreq, 0, runFreq*0.5f); //截止频率
self->Init.samFreq = runFreq; //采样频率
self->pri.Ts = 1.0f / self->Init.samFreq; //采样周期
_Update(self);
}
/*******************************************************************************
* 函 数 名 : InitVaryCutF
* 函数功能 : 改变截止频率
* 输入参数 : 无
* 返 回 值 : 无
*******************************************************************************/
static void InitVaryCutF(struct Order1LpFilter *self, float cutFreq)
{
self->Init.cutFreq = cutFreq;
_Update(self);
}
/*******************************************************************************
* 函 数 名 :
* 函数功能 : 本次输出结果计算
* 输入参数 : 无
* 返 回 值 : 无
* 计算公式 ： Y(n)=b0*X(n)+b1*X(n-1)+a0*Y(n-1)
*******************************************************************************/
static float PrdIn(struct Order1LpFilter *self, float Xn)
{
self->Prd.out_y = self->pri.b0 * Xn + self->pri.b1 * self->pri.Xn_1 + self->pri.a0 * self->pri.Yn_1;
self->pri.Yn_1 = self->Prd.out_y;
self->pri.Xn_1 = Xn;
return self->Prd.out_y;
}
/*******************************************************************************
* 函 数 名 : Order1LpFilter_Create
* 函数功能 : 创建对象 初始化
* 输入参数 : self对象
* 返 回 值 : 无
*******************************************************************************/
void Order1LpFilter_Create(struct Order1LpFilter *self)
{
memset(self, 0, sizeof(struct Order1LpFilter));
self->Init.Set = InitSet;
self->Init.VaryCutFreq = InitVaryCutF;
self->Prd.In = PrdIn;
}