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

ADC采样滤波算法利用卡尔曼滤波算法详解1ADC采样模型假设ADC采样的值已经为稳定状态,设k+1k+1k+1时刻ADC采样值为Xk+1Xk+1X_{k+1},则kkk时刻ADC采样值为XkXkX_k,假设k+1k+1k+1时刻的采样值为Zk+1Zk+1Z_{k+1},则有:{Xk+1=Xk,Zk+1=Xk+1+δ,δ为噪声{Xk+1=Xk,Zk+1=Xk+1+δ,δ为噪声\begin{cases}X_{k+1}=X…

大家好,又见面了,我是你们的朋友全栈君。

1 ADC采样模型

(本文为笔者早期所写,当时对卡尔曼滤波器理解尚未透彻,如今回顾,该模型还有所缺陷,推荐读者看卡尔曼的推导过程或者B站大佬Dr_CAN的空间

假设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{$\delta_1$为系统噪声}\\Z_{k+1}=X_{k+1} +\delta_2, & \text{$\delta_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,k1=Pk1,k1+QGk=Pk,k1/(Pk,k1+R)Pk,k=(1Gk)Pk,k1x00=0xkk1=xk1k1xkk=xkk1+Gk(Zkxkk1)

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),采用一阶滞后滤波算法,反之采用卡尔曼滤波算法。
以下代码是方案一的优化代码,其优化效果可见我另一篇博文https://blog.csdn.net/moge19/article/details/80915432:

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);
}

用python实现并对其进行仿真,详见博文
https://blog.csdn.net/moge19/article/details/82531119

利用FFT分析卡尔曼滤波与低通滤波详见博文:
https://blog.csdn.net/moge19/article/details/87389728
卡尔曼滤波算法的推导过程详见博文:
https://blog.csdn.net/moge19/article/details/89464151

若有错误望方家指正

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/131263.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)


相关推荐

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号