第八篇，滤波：二阶低通滤波、卡尔曼滤波

qq_42466012

已于 2023-08-10 10:18:45 修改

阅读量1.4w

点赞数 46

文章标签： matlab 算法

于 2022-06-18 16:47:11 首次发布

本文链接：https://blog.csdn.net/qq_42466012/article/details/125347503

版权

1.引言

相比于上一篇，这篇会写的简单很多，可能因为难度略低吧。

还是工程化，不大篇幅抄理论，直接上可落地的干货。

2.理解与demo

2.1 二阶低通滤波

2.1.1 LP_2Order的个人理解

低通滤波比较常用，原理不甚了了，至于为什么用二阶而不用一阶三阶一百阶，你懂的。

很容易理解也很容易做，照着公式写就行，下一节会给出参考链接。

要实现二阶低通滤波器，有几个很重要的概念，也是必要的参数，一定要掌握并知道怎么赋值，这些也是《自动控制原理》课程里讲到的传递函数里面的概念：

阻尼比epsilon、截止频率 $\omega _{c}$ 、采样时间T。

阻尼比一般就是 $0.707\approx \frac{\sqrt{2}}{2}$ ，截止频率看你想让不超过多大频率的信号通过了，采样时间就是你的功能的执行周期。

要特别注意的是，这里边提到的频率，包括matlab中的实现，都是rad制，而通常我们拿到的都是s制的采样时间T或者Hz制的频率f，这里要注意做转换：

$\omega =2\pi f=2\pi /T$

看下matlab中的sine wave模块，注意Frequency参数那里的单位是rad/sec，所以你如果想要个10Hz或者叫周期0.1s的正弦波，你得按图里我那样写：2*pi*10。

2.1.2 refer

下面几个链接是二阶的理论推导还有人家写的demo代码，借鉴度很高，致谢。

二阶滤波器原理及算法程序_tuxinbang1989的博客-CSDN博客_二阶滤波算法

数字二阶低通滤波器公式推导及代码实现_苏箭的博客-CSDN博客_二阶低通滤波器公式

2.1.3 demo

代码奉上，很简单，对着上边的讲解一看便知。

function y = fcn_filter_Order2(x, omega_c, epsilon, Tsw)
%二阶低通滤波器，采样频率50Hz，截止频率暂定f=50Hz
%ω=2*pi*f

b0 = omega_c*omega_c * Tsw*Tsw;
a0 = b0 + 4*epsilon*omega_c*Tsw + 4;
a1 = 2*b0 - 8;
a2 = b0 - 4*epsilon*omega_c*Tsw + 4;

persistent p_x;
if isempty(p_x)
    p_x = zeros(2,1);
end

persistent p_y;
if isempty(p_y)
    p_y = zeros(2,1);
end

y = (b0*x + 2*b0*p_x(1) + b0*p_x(2) - a1*p_y(1) - a2*p_y(2)) / a0;

p_x(2) = p_x(1);
p_x(1) = x;

p_y(2) = p_y(1);
p_y(1) = y;

2.2 卡尔曼滤波

2.2.1 理解

kalman filter，神一般的存在，究其本质也是一种低通滤波，甚至极端场景下退化成加权平均，所以不用害怕，花时间啃一啃就懂了。

首先，经典五部曲奉上：

第一步，利用上一次的KF结果，根据模型预测/估计当前状态。这个是篇理论的模型计算，要对功能建模的，比如经典的 $s=s_{0}+v_{0}t+\frac{1}{2}at^{2}$ ，一定要有个这样的公式，哪怕近似也要近似个出来，把AB矩阵造出来；
利用上一次的KF协方差计算当前预测/估计协方差；
利用步骤2得到的协方差更新KF增益K，图中公式3、4的顺序颠倒了，怎么可能KF都完了才去计算K；
利用步骤3得到的KF增益K，结合步骤1里的预测/估计值和当前测量值，做滤波，本次卡尔曼滤波状态最优估计结束；这里有个测量的概念，就是对状态量一下子，比如步骤1里根据模型/公式估计出来走了1米，你噶尺子一量走了9.99米，这个9.99米就是测量值，这是现实世界中物理看得见摸得着的东西，不说人话叫“观测量”；
最后一步，为了下一循环能继续，需要更新本轮KF最优估计的协方差，over。

2.2.2 refer

参考1：卡尔曼滤波理论

卡尔曼滤波原理详解及系统模型建立（simulink）_Donald�的博客-CSDN博客_卡尔曼滤波simulink

参考2：C代码实现，一维版、矩阵版都有

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用_Beyonderwei的博客-CSDN博客_kalman算法卡尔曼滤波C语言实现（矩阵版）_Fly_Into_The_Sky的博客-CSDN博客_卡尔曼滤波c语言jjd 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用_Beyonderwei的博客-CSDN博客_kalman算法

矩阵版这个NB大了，还有均匀分布随机数、正态分布随机数 ，可飨！

参考3：一个无敌的讲解，怎么就这么6！

【卡尔曼滤波器】1_递归算法_Recursive Processing_哔哩哔哩_bilibili

2.2.3 卡尔曼滤波的几个tips

状态转移矩阵A、观测矩阵H，一般都是单位阵；
初始建立，没有“上一次的KF协方差”，这时候差不多给个数就行了，别太大太小别是0，据说随着迭代的进行收敛很快，初值影响不大；
对于某些场景，鉴于矩阵乘法的行列特性，B矩阵构造不出来可不予构造，因为B仅在估计的时候使用，可直接用相关公式计算，在其它几个KF算式中用不到B。

2.2.4 demo

为了提高通用性，做了个4维的demo ，不过没考虑控制。

function y = KalmanFilter(measure)
%卡尔曼滤波，通用矩阵形式，未考虑控制u
%measure为当前时刻新的观测值，y为本次KF输出
%维数先固定好

dim = 4;

persistent p_y;
if isempty(p_y)
    p_y = zeros(dim,1);
end

persistent p_p;
if isempty(p_p)
    p_p = eye(dim)*0.01;
end

% A = eye(dim);
% B = zeros(dim,1);
% H = eye(dim);

Q = eye(dim)*0.01;
R = eye(dim)*0.01;

X_predict = p_y; % 公式1，根据上一步最优结果预测当前状态，A为单位阵，不考虑U
p_predict = p_p + Q; % 公式2，根据上一步最优结果的协方差计算当前预测状态协方差，Q为过程噪声协方差，A为单位阵
K = p_predict / (p_predict+R); % 公式3，计算KF增益K，测量矩阵H为单位阵，R为测量噪声协方差
y = X_predict + K*(measure-X_predict); % 公式4，计算最优输出，H为单位阵
p_p = (eye(dim)-K) * p_predict; % 公式5，计算本次最优结果的协方差，供下一步迭代使用
p_y = y; % 保存本次最优结果，供下一步迭代使用

3.其它

滤波的方法很多，经典的还有中值滤波什么的。

滤波的一大问题：实时滤波会带来相移(后处理没这个问题)，时域上看滤波结果会比输入数据有延迟。以一维中值滤波为例，假定滤波窗口大小为5，做实时滤波，假设当前数据是 $x_{5}$ ，那么前边几帧的数据是 $x_{4}$ 、 $x_{3}$ 、 $x_{2}$ 、 $x_{1}$ 、 $x_{0}$ ...，此时做滤波实际滤的是 $x_{3}$ 不是 $x_{5}$ ，因为用到的数据是 $x_{1...5}$ ， $x_{3}$ 才是中位数据，即延迟两帧，滤波窗口再大，比如我测试过31，很明显就能从波形图上看到一段滞后。