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

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

1 ADC采样模型

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

假设ADC采样的值已经为稳定状态，设$k+1$时刻ADC采样值为$X_{k+1}$,则$k$时刻ADC实际值为$X_k$，假设$k+1$时刻的采样值为$Z_{k+1}$，则有：
$$\{\begin{array}{ll}{X}_{k+1}=X_k+{\delta }_1,& \text{δ1为系统噪声}\\ Z_{k+1}=X_{k+1}+{\delta }_2,& \text{δ2为测量噪声}\end{array}$$

2 卡尔曼滤波算法

我们知道卡尔曼滤波算法的公式如下：

由于相关系数都为1,于是可以得出如下公式：
$$\{\begin{array}{l}{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{array}$$

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

若有错误望方家指正