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,k−1=Pk−1,k−1+QGk=Pk,k−1/(Pk,k−1+R)Pk,k=(1−Gk)Pk,k−1x0∣0=0xk∣k−1=xk−1∣k−1xk∣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),采用一阶滞后滤波算法,反之采用卡尔曼滤波算法。
以下代码是方案一的优化代码,其优化效果可见我另一篇博文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