需求:在c++中实现对N个信道的数据实时滤波,每次输入1个数据进入滤波器,循环N次,实现实时滤波。
原始信号:20Hz
滤波器种类:巴特沃斯低通滤波器
滤波器特性:4阶,直接I型,Fs=20Hz,Fc=0.5
直接I型IIR滤波器介绍
直接I型IIR滤波器是基于Biquad级联的方式来实现的,Biquad本身是一个二阶滤波器,其差分方程为:
y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
对应N阶差分方程:
直接I型算法每个阶段需要5个系数和4个状态变量
比较详细的原理介绍参考博客:
https://www.cnblogs.com/21207-iHome/p/7059144.html
使用MATLABr2023a中的滤波器设计工具确定滤波器系数
在命令行窗口输入filterDesigner,选择需求中的响应类型、滤波器阶数、频率设定,如图所示
可以看到此时还是直接II型,需要为其转换结构,转换成直接I型。
Ctrl+E选择导出选项,选择导出到Coefficient File(ASCII),十进制。
可以在Matlab中看到SOS矩阵和定标值,即为差分方程系数。
C++如何使用这些滤波器系数
!注意!这里从matlab中得到的系数需要对a1和a2取反才能用在我的c++函数中,滤波器的输出值必须乘以定标值(缩放系数)才行。
因为我这里是总阶数是4阶,需要2个Biquad级联,因此系数排布为{b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
因此输入的第一个Biquad系数{b10, b11, b12, a11, a12}得到该Biquad的差分方程:
y[n] = (1.0)* x[n] + (2.0) * x[n-1] + (1.0) * x[n-2] + (1.863800492075235348821138359198812395334) * y[n-1] +
(-0.8870329996526947757828907015209551900639) * y[n-2]
此时输出应该y[n]应该作为第二个滤波器的输入,y[n-1]、y[n-2]以此类推。再通过第二个Biquad,系数为{ b20, b21, b22, a21, a22},其差分方程:
y[n] = (1.0)* x[n] + (2.0) * x[n-1] + (1.0) * x[n-2] + (1.725933395036940654065915623505134135485) * y[n-1] +
(-0.747447371907790980571917316410690546036) * y[n-2]
多信道实时滤波还需要什么?
从差分方程中可以看到,每次的迭代都跟上一次,上上次的输入和输出有关,因此滤波器本身必定是要记录下上一次,上上次的迭代结果,并在每次使用滤波器后进行更新。那在我c++程序中的体现就是一个不断更新的数组,我称之为状态变量。
另外在使用时N个信道需要N个滤波器吗,初始化滤波器的时候需要用一个数组存储这些滤波器,因为每个信道的状态量的计算结果都不尽相同。
详解请看附上的程序。
滤波器类
lowpassfilter.h
#ifndef LOWPASSFILTER_H
#define LOWPASSFILTER_H
#include
#include
#include
class LowPassFilter
{
public:
LowPassFilter();
//构造IIR低通滤波器
LowPassFilter( int id,
const unsigned int stagenum,
double* state,
const double* coeffs);
double BiquadCascadeFiltering( double& input, double& output );
inline double* GetState() { return state_;};
inline int GetId() {return id_;};
private:
//二阶滤波器的个数,总阶数是 2*numStages
unsigned int stagenum_;
//指向状态变量数组,这个数组用于函数内部计算数据的缓存,总大小 4*numStages
double* state_;
/*
* 指向滤波因数,滤波因数数组长度为 5*numStages。
* 但要注意滤波因数应该按照如下的逆序进行排列:
* {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
*/
const double* coeffs_;
//创建多个对象用到的身份识别码
int id_;
};
#endif // LOWPASSFILTER_H
lowpassfilter.cpp
#include "lowpassfilter.h"
LowPassFilter::LowPassFilter()
{
}
LowPassFilter::LowPassFilter( int id,
const unsigned int stagenum,
double* state,
const double* coeffs)
{
//分配内存
state_ = new double(4*stagenum);
coeffs_ = new double(5*stagenum);
//初始化
id_ = id;
stagenum_ = stagenum;
state_ = state;
coeffs_ = coeffs;
}
double LowPassFilter::BiquadCascadeFiltering( double& input,
double& output )
{
double* pState = state_;
const double* pCoeffs = coeffs_;
unsigned int stage = stagenum_;
double acc; /* 计算中转 */
double b0, b1, b2, a1, a2; /* 滤波器系数 */
double Xn1, Xn2, Yn1, Yn2;
double Xn;
do {
/* 读取滤波器系数 */
b0 = *pCoeffs++;
b1 = *pCoeffs++;
b2 = *pCoeffs++;
a1 = *pCoeffs++;
a2 = *pCoeffs++;
/* 读取状态变量值 */
Xn1 = pState[0];
Xn2 = pState[1];
Yn1 = pState[2];
Yn2 = pState[3];
//滤波开始 这里限制只处理一个数据
Xn = input;
acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
output = acc;
Xn2 = Xn1;
Xn1 = Xn;
Yn2 = Yn1;
Yn1 = acc;
//如果是级联,将输出置为输入
*pState++ = Xn1;
*pState++ = Xn2;
*pState++ = Yn1;
*pState++ = Yn2;
input = output;
/* 阶数迭代 */
stage--;
} while (stage > 0);
return output;
}
其他类使用
mythread.h
#ifndef MYTHREAD_H
#define MYTHREAD_H
#include "lowpassfilter.h"
#define numStages 2 /* 2阶IIR滤波的个数 */
class MyThread
{
public:
explicit MyThread(QObject *parent = 0);
//接收主线程采集的数据
void receiveData(QByteArray array);
private:
QVector
QVector
//滤波因数 20 0.5
const double IIRCoeffs_[5*numStages] = {
1.0, 2.0, 1.0, 1.863800492075235348821138359198812395334,//b0 b1 b2 a1 a2 a1和a2取反
-0.8870329996526947757828907015209551900639,
1.0, 2.0, 1.0, 1.725933395036940654065915623505134135485,
-0.747447371907790980571917316410690546036
};
/*放缩系数 */
const double scale_value_ = 0.005808126894364891434907605116677586921 *
0.005378494217712603310543872936477782787;
}
mythread.cpp
其他类使用这块是部分程序,因此不能直接复制使用,但是核心代码均已贴出,使用比较简单。
为方便理解解释下几个参数:
NUM_S是上文所说的信道N
test0和test1是存储原始数据和滤波后数据的矩阵
MyThread::MyThread(QObject *parent) : QObject(parent)
{
//构造208个滤波器
for (int i = 0; i < NUM_S; ++i) {
double* state = new double[4*numStages];
memset(state, 0, 4*numStages*sizeof(double));
stategroup.push_back(state);
LowPassFilter* filter = new LowPassFilter(i, numStages, state,
(double*)&IIRCoeffs_[0]);
filtergroup.push_back(filter);
}
}
void MyThread::receiveData(QByteArray array)
{
memcpy(myBuff, array,sizeof(float)*NUM_S);
for(int i = 0; i < NUM_S; i++) {
test0(i+1, frame) = myBuff[i-1];
test(i+1, frame) = myBuff[i-1];
}
///低通滤波//
for (int i = 0; i < NUM_S; i++) {
double output = filtergroup[i]->BiquadCascadeFiltering(test(i+1, frame),
testnew(i+1, frame));
testnew(i+1, frame) = output*scale_value_;
}
}
本人代码小白,写的不好的地方还请大家多多留言批评指正,共同进步!
实时采集数据并记录下来,通过matlab对原始数据和滤波后数据进行曲线绘制
可以清除看到IIR滤波器会引入一定的相位延迟,但是滤波效果良好。为了避免延迟,考虑使用零相位滤波方法,这在离线处理非常方便实现,但是实时零相位滤波方法还没有想到一个合适的解决方法,还望大佬不吝赐教。
另外由于状态变量初值赋值为0,开始会有一个从零开始的阶跃,这个同样在离线处理可以避免,但是实时处理还没有解决,还望大佬不吝赐教。
好文链接
发表评论