打印
[开发资料]

ADC采样滤波算法利用卡尔曼滤波算法

[复制链接]
88|0
手机看帖
扫描二维码
随时随地手机跟帖
跳转到指定楼层
楼主
mikewalpole|  楼主 | 2024-11-13 21:00 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
1 ADC采样模型
(本文为笔者早期所写,当时对卡尔曼滤波器理解尚未透彻,如今回顾,该模型还有所缺陷,
假设ADC采样的值已经为稳定状态,设 k + 1 k+1 k+1时刻ADC采样值为 X k + 1 X_{k+1} Xk+1​,则 k k k时刻ADC实际值为 X k X_k Xk​,假设 k + 1 k+1 k+1时刻的采样值为 Z k + 1 Z_{k+1} Zk+1​,则有:  { X k + 1 = X k + δ 1 , δ 1 为系统噪声 Z k + 1 = X k + 1 + δ 2 , δ 2 为测量噪声 \begin{cases} X_{k+1} = X_k+\delta_1, & \text{δ2为测量噪声} \end{cases} {         Xk+1​=Xk​+δ1​,Zk+1​=Xk+1​+δ2​,​δ1​为系统噪声δ2​为测量噪声​
2 卡尔曼滤波算法
我们知道卡尔曼滤波算法的公式如下:



由于相关系数都为1,于是可以得出如下公式:  { P 0 , 0 = 0 P k , k − 1 = P k − 1 , k − 1 + Q G k = P k , k − 1 / ( P k , k − 1 + R ) P k , k = ( 1 − G k ) P k , k − 1 x 0 ∣ 0 = 0 x k ∣ k − 1 = x k − 1 ∣ k − 1 x k ∣ k = x k ∣ k − 1 + G k ( Z k − x k ∣ k − 1 ) \begin{cases} P_{0,0} = 0 \\P_{k,k-1}=P_{k-1,k-1} +Q \\ G_k = P_{k,k-1}/(P_{k,k-1}+R) \\ P_{k,k}=(1-G_k)P_{k,k-1} \\ x_{0|0} = 0 \\ x_{k|k-1} = x_{k-1|k-1} \\ x_{k|k}=x_{k|k-1}+G_k(Z_k-x_{k|k-1}) \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​P0,0​=0Pk,k−1​=Pk−1,k−1​+QGk​=Pk,k−1​/(Pk,k−1​+R)Pk,k​=(1−Gk​)Pk,k−1​x0∣0​=0xk∣k−1​=xk−1∣k−1​xk∣k​=xk∣k−1​+Gk​(Zk​−xk∣k−1​)​
3 C语言代码
用C实现的代码如下:




unsigned long kalman_filter(unsigned long ADC_Value){    float x_k1_k1,x_k_k1;    static float ADC_OLD_Value;    float Z_k;    static float P_k1_k1;    static float Q = 0.0001;    static float R = 0.005;    static float Kg = 0;    static float P_k_k1 = 1;    float kalman_adc;    static float kalman_adc_old=0;    Z_k = ADC_Value;    x_k1_k1 = kalman_adc_old;    x_k_k1 = x_k1_k1;    P_k_k1 = P_k1_k1 + Q;    Kg = P_k_k1/(P_k_k1 + R);    kalman_adc = x_k_k1 + Kg * (Z_k - kalman_adc_old);    P_k1_k1 = (1 - Kg)*P_k_k1;    P_k_k1 = P_k1_k1;    ADC_OLD_Value = ADC_Value;    kalman_adc_old = kalman_adc;    return kalman_adc;}

4 如何优化
用以上的代码实现的滤波算法,通常要么滞后严重,要么滤波效果不明显,在这里给出两种优化方案。 方案一:在采样值与优化值相差大于某值时采用一阶滞后滤波算法,小于该值时采用卡尔曼滤波算法; 方案二:比较一段时间内的ADC采样值与优化值差值,若一直处于某个范围如(6~30),采用一阶滞后滤波算法,反之采用卡尔曼滤波算法。  





unsigned long kalman_filter(unsigned long ADC_Value){float x_k1_k1,x_k_k1;static float ADC_OLD_Value;float Z_k;static float P_k1_k1;static float Q = 0.0001;static float R = 5;static float Kg = 0;static float P_k_k1 = 1;float kalman_adc;static float kalman_adc_old=0;Z_k = ADC_Value;if (abs(kalman_adc_old-ADC_Value)>=10){     x_k1_k1= ADC_Value*0.382 + kalman_adc_old*0.618;}else{    x_k1_k1 = kalman_adc_old;}x_k_k1 = x_k1_k1;P_k_k1 = P_k1_k1 + Q;Kg = P_k_k1/(P_k_k1 + R);kalman_adc = x_k_k1 + Kg * (Z_k - kalman_adc_old);P_k1_k1 = (1 - Kg)*P_k_k1;P_k_k1 = P_k1_k1;ADC_OLD_Value = ADC_Value;kalman_adc_old = kalman_adc;return get_int_num(kalman_adc);}



使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则

35

主题

1424

帖子

0

粉丝