智能车四种常见滤波和 MATLAB 仿真:一阶 RC 低通滤波,二阶 IIR 低通滤波,五阶 FIR 低通滤波,卡尔曼滤波

简介: 智能车四种常见滤波和 MATLAB 仿真:一阶 RC 低通滤波,二阶 IIR 低通滤波,五阶 FIR 低通滤波,卡尔曼滤波

四种常见滤波和 MATLAB 仿真:一阶 RC 低通滤波,二阶 IIR 低通滤波,五阶 FIR 低通滤波,卡尔曼滤波

开源代码地址: https://github.com/ittuann/Enterprise_E

博客目录:
https://blog.csdn.net/sorcererr/article/details/124989905
https://ittuann.github.io/2021/08/30/Car.html

一阶 RC 低通滤波

FilterData = cof * RawData + (1.000f - cof) * FilterData;
AI 代码解读

一阶 RC 低通滤波又叫一阶惯性滤波,因为本质上也是积分滤波器。

不能滤除频率高于采样频率的二分之一的干扰。

cof是一阶RC低通滤波参数,代表新采样值在滤波结果中的权重。

滤波系数越小,滤波结果越平稳,灵敏度越低。

滤波系数可以理解为对采样值的相信程度。

float Cof =  = 1.0 / (1.0 + (1.0 / 2Pi * T(s) * fc));
AI 代码解读

上面是cof的计算公式。但一阶 RC 低通滤波在实际情况也可以不按照公式计算的结果,用上位机比较原始值和滤波后数值波形来试凑出来一版系数也是可行的。

二阶 IIR 低通滤波

IIR 滤波公式:

yn=1b0(nk=0bkx(nk)nk=1akx(nk))

或是

y(n)=b0x(n)+b1x(n1)++bMx(nM)a1y(n1)aNy(nN)

IIR滤波就是这个个方程。yn为输出数据,xn为输入数据,a和b为滤波器的系数。

C语言实现:

#define ORDER 2

typedef struct {
    float A[ORDER + 1];
    float B[ORDER + 1];
    float OriginData[ORDER + 1];
    float FilterData[ORDER + 1];
} LpfIIR2nd_t;

/**
 * @brief            二阶IIR低通滤波器
 * @param[out]        lpf : 滤波结构数据指针
 * @param[in]        rawData : 原始数据
 */
void LowPassFilterIIR2nd(LpfIIR2nd_t* lpf, float rawData)
{
    uint8_t i = 0;
    // 递推旧值
    for (i = ORDER; i > 0; i--) {
        lpf->OriginData[i] = lpf->OriginData[i - 1];
        lpf->FilterData[i] = lpf->FilterData[i - 1];
    }
    // 计算滤波结果
    lpf->OriginData[0] = rawData;
    lpf->FilterData[0] = lpf->B[0] * lpf->OriginData[0];    // NUM 分子
    for (i = 1; i <= ORDER; i++) {
        lpf->FilterData[0] = lpf->FilterData[0] + lpf->B[i] * lpf->OriginData[i] - lpf->A[i] * lpf->FilterData[i];
    }
}
AI 代码解读

最后计算滤波结果也可以写成

lpf->FilterData[0] = (lpf->B[0] * lpf->OriginData[0] + lpf->B[1] * lpf->OriginData[1] + lpf->B[2] * lpf->OriginData[2] - lpf->A[1] * lpf->FilterData[1] - lpf->A[2] * lpf->FilterData[2]) / lpf->A[0];
AI 代码解读

注意在使用滤波前要先初始化结构体的滤波系数。只需要初始化一次即可。

其中分子系数是成对出现的,可以先合并来提高运算效率。

也可以优化成int型或是用移位器和加法器来代替乘法。

滤波系数的计算:

/**
 * @brief    二阶 IIR 低通滤波
 *            APM 和 PX4 内的计算系数方式
 */
void AccLowpassIIR2Filter_E(void)
{
    const float sample_freq = 500;
    const float _cutoff_freq = 200;
    
    const float fr = sample_freq / _cutoff_freq;
    const float ohm = tanf(M_PI_F / fr);
    const float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
    
    const float _b0 = ohm * ohm / c;
    const float _b1 = 2.0f * _b0;
    const float _b2 = _b0;
    const float _a1 = 2.0f * (ohm * ohm - 1.0f) / c;
    const float _a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm +ohm * ohm) / c;
    
//    lFilter[0] = Origin[0] * _b0 + Origin[1] * _b1 + Origin[2] * _b2 - Filter[1] * _a1 - Filter[2] * _a2;
}
AI 代码解读

滤波系数的计算也可以使用MATLAB仿真的方式计算:

打开 Matlab 在APP中选择 Filter Designer(fdatool)

img

  • 设计方法(Design Method)用于选择IIR滤波器还是FIR滤波器。这里我们选择IIR滤波器,类型选择Butterworth。当然也可以选择其他类型,不同类型的频率响应不同,选择后默认的滤波器结构是直接II型。
  • 响应类型(Response Type)用于选择低通、高通、带通、带阻等类型。这里选择选择低通(Lowpass)。
  • 频率设定(Frequency Specifications)用于设置采样频率以及截止频率,这里填入500以及50,即采样率为500Hz,50Hz以上的频率将被滤除掉。
  • 滤波器阶数(Fiter Order )选择指定滤波器阶数为二阶。
  • 参数设置好后点击设计滤波器(Design filter)按钮,将按要求设计滤波器。
  • 默认生成的IIR滤波器类型是直接Ⅱ型,每个Section是一个二阶滤波器(Direct-Form II, Second-Order Sections)。
  • 在工具栏上点击右数第五个滤波器系数(Filter Coefficients)图标,或菜单栏上选择分析(Analysis)→滤波器系数(Filter Coefficients),即可查看生成的滤波器系数。
  • 最后在菜单栏点击编辑(edit)→转换为二节点(Convert to Single Section),即是我们最终用的滤波器系数。在MATLAB里,IIR公式中的ab又分别叫做分子(Numerator)和分母(Denominator)。
  • 左边实现模型(Realize Model)还可以生成Simulink模型进行仿真。

五阶 FIR 低通滤波

FIR 滤波公式:

y(n)=N1k=0k(k)x(nk)

FIR滤波就是这个个方程。x(n)为输入信号,k(n)为FIR滤波系数,y(n)为经过滤波后的信号;N表示FIR滤波器的抽头数,滤波器阶数为N-1。

C语言实现:

#define ORDER 5

void FIR5Filter(void)
{
    const float H[ORDER + 1] = {...};
    
    Filter[0] = 0;
    for (uint16_t j = 0; j < ORDER + 1; j++) {
         Filter[0] += H[j] * Origin[j];
    }
}
AI 代码解读

滤波系数的计算:

  • FIR滤波器设计方法有多种,如图4所示,最常用的是窗函数设计法(Window)、等波纹设计法(Equiripple)和最小二乘法 (Least-Squares)等。
  • 设置频率响应的参数,包括采样频率Fs、通带频率Fpass和阻带频率Fstop
  • 可以使用ARM官方DSP库实现FIR和IIR滤波,提高运行速度和效率。

卡尔曼滤波

算法的思想是,根据当前的仪器"测量值" 和上一刻的 "预测量" 和 "误差",计算得到当前的最优量,再预测下一刻的量。卡尔曼滤波器就是一个带有预测功能的低通滤波器。

一维卡尔曼滤波C语言实现:

typedef struct {
    float x;        // state
    float A;        // x(n)=A*x(n-1)+u(n),u(n)~N(0,q)
    float H;        // z(n)=H*x(n)+w(n),w(n)~N(0,r)
    float q;        // process(predict) noise convariance
    float r;        // measure noise(error) convariance
    float p;        // estimated error convariance
    float gain;        // kalman gain
} kalman1_filter_t;


/**
 * @brief            一阶卡尔曼滤波初始化
 * @param[out]        state : 滤波结构数据指针
 * @param[in]        q & r
 */
void kalman1_init(kalman1_filter_t *state, float q, float r)
{
    state->x = 0.0f;
    state->p = 0.0f;
    state->A = 1.0f;
    state->H = 1.0f;
    state->q = q;
    state->r = r;
}

/**
 * @brief            一阶卡尔曼滤波
 * @param[out]        state : 滤波结构数据指针
 * @param[in]        z_measure : 原始数据
 */
float kalman1_filter(kalman1_filter_t *state, float z_measure)
{
    /* Predict */
    // 时间更新(预测): X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
    state->x = state->A * state->x;
    // 更新先验协方差: P(k|k-1) = A(k,k-1)*A(k,k-1)^T*P(k-1|k-1)+Q(k)
    state->p = state->A * state->A * state->p + state->q;

    /* Measurement */
    // 计算卡尔曼增益: K(k) = P(k|k-1)*H(k)^T/(P(k|k-1)*H(k)*H(k)^T + R(k))
    state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
    // 测量更新(校正): X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
    state->x = state->x + state->gain * (z_measure - state->H * state->x);
    // 更新后验协方差: P(k|k) =(I-K(k)*H(k))*P(k|k-1)
    state->p = (1 - state->gain * state->H) * state->p;

    return state->x;
}
AI 代码解读
  • 一维卡尔曼滤波全部是标量计算
  • R固定,Q越大,代表越信任侧量值,Q无穷代表只用测量值(Q/R取0无意义)Q/R决定了滤波器的稳态频率响

    应。Q增大和R减小都会导致卡尔曼增益K变大

  • 测量噪声大 R需要设置的较大
  • 模型不准确 Q需要增大
  • 卡尔曼增益的值越大,意味着越相信测量值的输出,从而使得估计值倾向于测量值,收敛速度越快,最优估计值震荡越明显
  • A B H 根据模型来确定
  • P变大也会导致卡尔曼增益变大,但是很快会迭代至稳定值,因此P的初始值只会对前几个周期的估计值有影响。
  • 二维及以上的卡尔曼滤波运算过程涉及到大量的矩阵运算,如果将矩阵运算拆开成基本的四则运算扩展性很差,并且在遇到例如求逆运算的时候比较麻烦。可以使用ARM的DSP库里面的矩阵运算功能
目录
打赏
0
0
0
0
3
分享
相关文章
基于SC-FDE单载波频域均衡MQAM通信链路matlab仿真,包括帧同步,定时同步,载波同步,MMSE信道估计等
本内容主要介绍基于MATLAB的SC-FDE单载波频域均衡通信链路设计与实现,包括UW序列设计、QAM调制、帧同步、定时同步、载波同步、SNR估计和MMSE信道估计等关键环节。通过仿真(MATLAB 2022a),验证了系统的可行性和性能。核心程序展示了不同QAM调制方式(如256QAM)及同步算法的具体实现,并通过绘图展示帧同步、定时同步和频偏补偿效果。此研究为优化通信系统性能提供了理论与实践基础。
20 0
开关磁阻电机(SRM)系统的matlab性能仿真与分析
本课题基于MATLAB 2022a对开关磁阻电机(SRM)系统进行性能仿真与分析,涵盖平均转矩、转矩脉动、自感与互感、功率及效率等关键参数的对比研究。通过程序仿真,生成了相电流、转子角度、机械转速等多维度数据关系图。SRM以其无刷、无永磁体的特点,具备高可靠性和低成本优势,其工作原理基于磁阻最小原则,通过控制定子绕组电流实现连续旋转运动。核心程序实现了不同电流下平均转矩的计算与可视化,为SRM优化设计提供了理论依据。
基于Qlearning强化学习的机器人路线规划matlab仿真
本内容展示了基于Q-learning强化学习算法的路径规划研究,包括MATLAB仿真效果、理论知识及核心代码。通过训练与测试,智能体在离散化网格环境中学习最优策略以规避障碍并到达目标。代码实现中采用epsilon-贪婪策略平衡探索与利用,并针对紧急情况设计特殊动作逻辑(如后退)。最终,Q-table收敛后可生成从起点到终点的最优路径,为机器人导航提供有效解决方案。
70 20
基于神经网络逆同步控制方法的两变频调速电机控制系统matlab仿真
本课题针对两电机变频调速系统,提出基于神经网络a阶逆系统的控制方法。通过构造原系统的逆模型,结合线性闭环调节器实现张力与速度的精确解耦控制,并在MATLAB2022a中完成仿真。该方法利用神经网络克服非线性系统的不确定性,适用于参数变化和负载扰动场景,提升同步控制精度与系统稳定性。核心内容涵盖系统原理、数学建模及神经网络逆同步控制策略,为工业自动化提供了一种高效解决方案。
基于GA遗传优化TCN-LSTM时间卷积神经网络时间序列预测算法matlab仿真
本项目基于MATLAB 2022a实现了一种结合遗传算法(GA)优化的时间卷积神经网络(TCN)时间序列预测算法。通过GA全局搜索能力优化TCN超参数(如卷积核大小、层数等),显著提升模型性能,优于传统GA遗传优化TCN方法。项目提供完整代码(含详细中文注释)及操作视频,运行后无水印效果预览。 核心内容包括:1) 时间序列预测理论概述;2) TCN结构(因果卷积层与残差连接);3) GA优化流程(染色体编码、适应度评估等)。最终模型在金融、气象等领域具备广泛应用价值,可实现更精准可靠的预测结果。
基于风险的完整性和检查建模(RBIIM)MATLAB仿真
本程序为基于风险的完整性和检查建模(RBIIM)的MATLAB仿真,适用于评估和优化资产完整性管理计划,特别针对石油化工等领域的管道、储罐等设备。程序在MATLAB 2022A版本下运行,对比了先验密度(Prior Density)、后验完美检测(Posterior Perfect Inspection)、后验不完美检测(Posterior Imperfect Inspection)及累积后验不完美检测四个关键指标。算法采用贝叶斯统计框架,通过更新资产健康状况估计,制定最佳维护与检查策略。示例展示了核心原理与运行效果,完整程序无水印。
基于WOA鲸鱼优化的CNN-LSTM-SAM网络时间序列回归预测算法matlab仿真
本内容介绍了一种基于CNN-LSTM-SAM网络与鲸鱼优化算法(WOA)的时间序列预测方法。算法运行于Matlab2022a,完整程序无水印并附带中文注释及操作视频。核心流程包括数据归一化、种群初始化、适应度计算及参数更新,最终输出最优网络参数完成预测。CNN层提取局部特征,LSTM层捕捉长期依赖关系,自注意力机制聚焦全局特性,全连接层整合特征输出结果,适用于复杂非线性时间序列预测任务。
|
7月前
|
【2023高教社杯】D题 圈养湖羊的空间利用率 问题分析、数学模型及MATLAB代码
本文介绍了2023年高教社杯数学建模竞赛D题的圈养湖羊空间利用率问题,包括问题分析、数学模型建立和MATLAB代码实现,旨在优化养殖场的生产计划和空间利用效率。
304 6
【2023高教社杯】D题 圈养湖羊的空间利用率 问题分析、数学模型及MATLAB代码
【2022年华为杯数学建模】B题 方形件组批优化问题 方案及MATLAB代码实现
本文提供了2022年华为杯数学建模竞赛B题的详细方案和MATLAB代码实现,包括方形件组批优化问题和排样优化问题,以及相关数学模型的建立和求解方法。
181 3
【2022年华为杯数学建模】B题 方形件组批优化问题 方案及MATLAB代码实现
【2023五一杯数学建模】 B题 快递需求分析问题 建模方案及MATLAB实现代码
本文介绍了2023年五一杯数学建模竞赛B题的解题方法,详细阐述了如何通过数学建模和MATLAB编程来分析快递需求、预测运输数量、优化运输成本,并估计固定和非固定需求,提供了完整的建模方案和代码实现。
211 0
【2023五一杯数学建模】 B题 快递需求分析问题 建模方案及MATLAB实现代码

热门文章

最新文章