ADC采样-卡尔曼滤波器设计
1. 文档简介
记录机器人项目在ADC采样数据后对数据进行滤波处理的数字滤波器设计和方法。
2. 滤波器选用
2.1 常见滤波算法
2.1.1 中值滤波
将连续多个采样值排序后取中间值作为滤波结果。优点是能有效抑制脉冲噪声等异常值的影响，对于突然出现的噪声具有较好的抑制效果。缺点是对于信号的快速变化或连续出现的噪声不敏感，无法完全恢复信号的快速变化。
2.1.2 限幅滤波
限幅滤波通过设定上下限阈值，将超过阈值的信号值限制在范围内。限幅滤波适用于剔除异常值和脉冲噪声等干扰，优点是简单易实现，缺点是对于信号的快速变化和连续出现的噪声响应较慢。
2.1.3 加权平均滤波
将连续多个采样值按照一定的权重进行加权平均。通过合理设置权重可以在平滑信号的同时保留一定的动态响应能力。缺点是需要确定合适的权重分配方式。
2.1.4 移动均值滤波
将连续多个采样值取平均值作为滤波结果。优点是简单易实现，适用于信号变化较为平稳的情况。缺点是对于快速变化的信号响应较慢，且对于突然出现的异常值较为敏感。
2.1.5 无限脉冲响应滤波(IIR)
基于滤波器的差分方程，对当前采样值与过去采样值进行加权和。IIR 滤波器具有较好的频域特性和动态响应能力，但设计和调试较为复杂。
2.1.5 卡尔曼滤波器
卡尔曼滤波是一种递归滤波算法，能够有效估计系统状态，并结合系统模型和测量值进行状态预测和更新。卡尔曼滤波适用于线性动态系统，能够对系统状态进行较为准确的估计。优点是具有较好的估计精度和较快的动态响应能力，缺点是对于非线性系统需要进行扩展卡尔曼滤波等改进。
2.2 滤波器选用
结合以上滤波器的优缺点,ADC采样本身具有高频噪声,且对于特定的采集量其数值可能会快速变化,这样前四种滤波器就不太能满足要求了.又因为IIR滤波器设计较为复杂,ADC采样过程可以用一个一阶状态方程表示,是一个一阶线性系统,所以对于卡尔曼滤波器来说,设计较为简单,且卡尔曼的最优估计更接近真实值,精度较高的同时还能保留较快的动态响应能力,综上所述,最终选择卡尔曼滤波器.
3. 卡尔曼滤波器
3.1 简介
卡尔曼滤波算法是一种最优化递归数字处理算法,在实际应用过程中基本不存在完美的数学模型,且对于一个系统来说扰动是不可控的,很难建模,对于传感器来说,每次测量的数据也会存在一定的误差.卡尔曼就是用一个不太准确的状态模型和不太准确的测量模型来估计出对应的实际值.
卡尔曼的公式详细推导过程及实际应用举例可以参考以下两位博主的视频进行学习
【【卡尔曼滤波器】1_递归算法_Recursive Processing】【卡尔曼滤波器】1_递归算法_Recursive Processing_哔哩哔哩_bilibili
【从放弃到精通！卡尔曼滤波从理论到实践~】从放弃到精通！卡尔曼滤波从理论到实践~_哔哩哔哩_bilibili
公式推导及相关讲义可以参照这个资料
卡尔曼滤波算法.pdf
3.2 具体原理
3.2.1 基本原理
卡尔曼滤波算法是用上一时刻的估计值以及当前时刻的测量值去估计当前时刻的真实值,对于一阶系统来说计算公式如下图所示
Kk为卡尔曼增益,其由测量误差和估计误差决定.
3.2.2 卡尔曼增益计算
对于一个线性系统,通常可以用状态空间方程的形式进行表示,测量结果也可以用测量方程进行表示.以一个常见的弹簧阻尼系统为例:
蓝色字体为连续系统状态方程和观测方程(测量方程)的一般表达形式,而对于计算机控制系统来说一般为离散控制系统,表达形式如棕色字体所示,在上一节已经提过在实际应用中存在一定的不确定性,所以引入过程噪声和测量噪声来表示这部分不确定性.
过程噪声和测量噪声可近似认为其服从正态分布,其期望和方差如下图所示(系统可能为高阶系统,所以概率分布表示由一维的方差变为多维的协方差矩阵(对于这个不了解的可以去开始部分的第一个视频链接里的第二节进行学习,其期望及协方差矩阵如下图右侧所示:
对于一个系统来说,用卡尔曼滤波器的基本原理进行递归估计,在k时刻的估计值=在k时刻的先验估计值(算出来的) + 系数G * (在k时刻测量值 - 在k时刻的先验估计值),经整理化简可以表示为上图左侧红框的形式.
如何寻找一个合适的Kk(卡尔曼滤波系数),使得估计值更接近真实值这里引入一个误差,这个误差也符合期望为0,协方差矩阵为P的正态分布,只要使得误差的方差最小(协方差矩阵的迹),则能得出最优的估计值,具体公式如下所示:
对Pk的迹求导可得到其极值点,即为最小值点,详细计算过程可以参考第一个视频链接,这里直接给出计算结果,如图所示:
对于上述公式,仅有Pk^(-)一个是未知量,其他都是已知,Pk^(-)和Pk的关系如下图所示:
最后求解Pk^(-)及Pk:
至此卡尔曼滤波器的Kk可以完整的求解出来了.
3.2.3 卡尔曼滤波过程
由上一节推导可以得出卡尔曼滤波器的五大公式,即:
卡尔曼滤波过程由以上五个步骤组成:
1.求先验估计
2.求先验误差的协方差矩阵
3.求卡尔曼增益
4.求后验估计
5.更新误差协方差矩阵
4. 卡尔曼滤波器应用
4.1 ADC采样系统建模
ADC采样系统可看作一个一阶离散系统,其系统状态空间方程和观测方程如下所示
Xk+1为k+1时刻的采样值,Xk为k时刻的采样值,Zk+1为k+1时刻的测量值(采样值),这个系统很简单,对比上一节的状态空间方程,此时A为1(一阶单位阵),u为0,H为1(一阶单位阵),系统噪声的方差为Q,测量噪声的方差为R.
4.2 ADC采样系统卡尔曼滤波过程
结合上一节的推导过程,由五个步骤组成(后续以Xk和Xk-1进行推导):
1.求先验估计
hatXk^(-) = hatXk-1
2.求先验误差的协方差矩阵
Pk^(-) = Pk-1 + Q
3.求卡尔曼增益
Kk = Pk^(-)/(Pk^(-) + R)
4.求后验估计
hatXk = hatXk^(-) + Kk(Zk - hatXk^(-))
5.更新误差协方差矩阵
Pk = (1- Kk)Pk^(-)
这里会设计到K0和P0两个参数,工程上一般设K0 = 0,P0 = 1;
4.3 ADC采样系统卡尔曼滤波程序设计
//卡尔曼滤波
rt_int16_t KalmanFilter(rt_int16_t inData)
{
static float prevData = 0; //先前数值
static float p = 1, q = 0.001, r = 0.1, kGain = 0; // q为过程噪声 r为测量噪声
p = p + q;
kGain = p / ( p + r ); //计算卡尔曼增益
inData = prevData + ( kGain * ( inData - prevData ) ); //计算本次滤波估计值
p = ( 1 - kGain ) * p; //更新测量方差
prevData = inData;
return inData; //返回滤波值
}
其各个变量和4.2公式的对应关系可以自行研究一下,最终输出值为当前时刻的估计值.
4.4 ADC采样系统滤波前后对比
while (1)
{
adcx = get_adc(ADC_CHANNEL_3);
adcx_k = KalmanFilter(adcx);
adc_i = (float)adcx*(5.0/65536.0);
adc_i_k = (float)adcx_k*(5.0/65536.0);
printf("ADC_I :%f,%f,%d,%d,%d\n",adc_i,adc_i_k,adcx,adcx_k,adcx-adcx_k);
rt_thread_mdelay(10);
}
adc_i为未经过滤波的采样数据,adc_i_k为经过卡尔曼滤波器的采样数据,下图是其数据波形对比:
adc_i为红色曲线,adc_i_k为绿色曲线,对比发现经过卡尔曼滤波器后,可以有效过滤掉采集噪声,且数据较为平滑.