【C语言/Python】嵌入式常用数据滤波处理：卡尔曼滤波器的简易实现方式（Kalman Filter）

卡尔曼滤波

卡尔曼滤波适用于在正态分布的情况下 处理数据抖动的问题 常用于温度传感器、加速度传感器等数据滤波处理
这里简单介绍下：

具体关于卡尔曼滤波网上资料很多 这里就不多做介绍

一点：卡尔曼滤波能有效减小系统方差

卡尔曼滤波公式

卡尔曼滤波数据处理效果

我用ADXL345采集了一组数据
然后用Python进行卡尔曼滤波处理
代码如下：

import matplotlib.pyplot as plt

"""
Q 系统噪声
R 测量噪声
X(k|k-1)   上一次状态预测结果
X(k-1|k-1) 上一时刻的最优预测值
P(k|k-1)   X(k|k-1)对应的convariance协方差
P(k-1|k-1) X(k-1|k-1) 对应的convariance协方差
"""

x_last = 0
y_last = 0
z_last = 0
px_last = 0
py_last = 0
pz_last = 0
Q = 0.1  #系统噪声
R = 0.5  #测量噪声

def kalman(measure,result_last=0,prediction_last=0,Q=0.018,R=0.0542):
    result_mid = result_last
    prediction_mid = prediction_last + Q
    kg = prediction_mid/(prediction_mid + R)
    result_now = result_mid + kg*(measure - result_mid)
    prediction_now = (1-kg)*prediction_mid
    prediction_last = prediction_now
    result_last = result_now
    return result_now,result_last,prediction_last
    

f=open("4.txt","r",encoding="UTF-8")
f_list=f.readlines()
f.close()

x = []
y = []
z = []

px=[]
py=[]
pz=[]

ppx=[]
ppy=[]
ppz=[]

for i in f_list:
    try:        
        s=i.split("x: ")[1]
        s=s.split("	y: ")
        x.append(float(s[0]))
        s=s[1].split("	z: ")
        y.append(float(s[0]))
        s=s[1].split("\n")
        z.append(float(s[0]))
    except:
        pass

x_last = x[0]
px_last = x[0]

y_last = y[0]
py_last = y[0]

z_last = z[0]
pz_last = z[0]

    
for i in range(len(x)):
    pred,x_last,px_last = kalman(x[i],x_last,px_last,Q,0.5)

    px.append(pred)
    
    pred,y_last,py_last = kalman(y[i],y_last,py_last,Q,0.5)
    py.append(pred)
    
    pred,z_last,pz_last = kalman(z[i],z_last,pz_last,Q,0.5)
    pz.append(pred)


x_last = px[0]
px_last = px[0]

y_last = py[0]
py_last = py[0]

z_last = pz[0]
pz_last = pz[0]

for i in range(len(px)):
    pred,x_last,px_last = kalman(px[i],x_last,px_last,Q,0.5)

    ppx.append(pred)
    
    pred,y_last,py_last = kalman(py[i],y_last,py_last,Q,0.5)
    ppy.append(pred)
    
    pred,z_last,pz_last = kalman(pz[i],z_last,pz_last,Q,0.5)
    ppz.append(pred)
    
#plt.plot(real,color="b")  #真实值
plt.figure(1)
plt.plot(x,color="g")     
plt.plot(px,color="r")  
plt.plot(ppx,color="b")  
plt.figure(2)
plt.plot(y,color="g")     
plt.plot(py,color="r")  
plt.plot(ppy,color="b")  
plt.figure(3)
plt.plot(z,color="g")  
plt.plot(pz,color="r")   
plt.plot(ppz,color="b")    
plt.show()

运行效果：



绿色的是原始数据 红色的是一次滤波 蓝色的是二次滤波（将红色的结果再次滤波）

C语言的卡尔曼滤波实现

有了Python代码 照抄就能改到C语言

typedef struct
{
    float Measure_Now;
    float Result_Now;
    float Result_Last;
    float Prediction_Last;
    float Q;
    float R;
}Kalman_Filter_Normal_Struct;

Kalman_Filter_Normal_Struct Kalman_Filter_Normal(Kalman_Filter_Normal_Struct Stu)
{
   float result_mid = Stu.Result_Last;
   float prediction_mid = Stu.Prediction_Last + Stu.Q;
   float kg = prediction_mid/(prediction_mid + Stu.R);
   Stu.Result_Now = result_mid + kg*(Stu.Measure_Now - result_mid);
   float prediction_now = (1-kg)*prediction_mid;
   Stu.Prediction_Last = prediction_now;
   Stu.Result_Last = Stu.Result_Now;

   return Stu;
}

int main(void)
{
   float buf[10]={85.6,84.3,84.0,86.5,85.5,85.0,84.8,84.5,84.5,85.1};
   uint8_t i=0;
   Kalman_Filter_Normal_Struct Stu;
   Stu.Measure_Now=buf[0];
   Stu.Result_Now=buf[0];
   Stu.Result_Last=buf[0];
   Stu.Prediction_Last=buf[0];
   Stu.Q=0.1;
   Stu.R=0.5;
   for(i=0;i<10;i++)
   {  
      Stu.Measure_Now=buf[i];
      Stu=Kalman_Filter_Normal(Stu);
      printf("%f\n",Stu.Result_Now);
   }
   return 0;
}

最终结果：

85.599998
84.892471
84.511665
85.277679
85.359756
85.229263
85.074692
84.868370
84.736282
84.866631

附录：压缩字符串、大小端格式转换

压缩字符串

首先HART数据格式如下：


重点就是浮点数和字符串类型
Latin-1就不说了 基本用不到

浮点数

浮点数里面 如 0x40 80 00 00表示4.0f

在HART协议里面 浮点数是按大端格式发送的 就是高位先发送 低位后发送

发送出来的数组为：40,80,00,00

但在C语言对浮点数的存储中 是按小端格式来存储的 也就是40在高位 00在低位
浮点数：4.0f
地址0x1000对应00
地址0x1001对应00
地址0x1002对应80
地址0x1003对应40

若直接使用memcpy函数 则需要进行大小端转换 否则会存储为：
地址0x1000对应40
地址0x1001对应80
地址0x1002对应00
地址0x1003对应00

大小端转换：

void swap32(void * p)
{
   uint32_t *ptr=p;
   uint32_t x = *ptr;
   x = (x << 16) | (x >> 16);
   x = ((x & 0x00FF00FF) << 8) | ((x >> 8) & 0x00FF00FF);

   *ptr=x;
}

压缩Packed-ASCII字符串

本质上是将原本的ASCII的最高2位去掉 然后拼接起来 比如空格(0x20)
四个空格拼接后就成了
1000 0010 0000 1000 0010 0000
十六进制：82 08 20
对了一下表 0x20之前的识别不了
也就是只能识别0x20-0x5F的ASCII表

压缩/解压函数后面再写：

//传入的字符串和数字必须提前声明 且字符串大小至少为str_len 数组大小至少为str_len%4*3 str_len必须为4的倍数
uint8_t Trans_ASCII_to_Pack(uint8_t * str,uint8_t * buf,const uint8_t str_len)
{
   if(str_len%4)
   {
      return 0;
   }
	 
   uint8_t i=0;
   memset(buf,0,str_len/4*3);	  
   for(i=0;i<str_len;i++)
   {
      if(str[i]==0x00)
      {
         str[i]=0x20;
      }
   }

   for(i=0;i<str_len/4;i++)
   {
      buf[3*i]=(str[4*i]<<2)|((str[4*i+1]>>4)&0x03);
      buf[3*i+1]=(str[4*i+1]<<4)|((str[4*i+2]>>2)&0x0F);
      buf[3*i+2]=(str[4*i+2]<<6)|(str[4*i+3]&0x3F);
   }

   return 1;
}

//传入的字符串和数字必须提前声明 且字符串大小至少为str_len 数组大小至少为str_len%4*3 str_len必须为4的倍数
uint8_t Trans_Pack_to_ASCII(uint8_t * str,uint8_t * buf,const uint8_t str_len)
{
   if(str_len%4)
   {
      return 0;
   }

   uint8_t i=0;

   memset(str,0,str_len);

   for(i=0;i<str_len/4;i++)
   {
      str[4*i]=(buf[3*i]>>2)&0x3F;
      str[4*i+1]=((buf[3*i]<<4)&0x30)|(buf[3*i+1]>>4);
      str[4*i+2]=((buf[3*i+1]<<2)&0x3C)|(buf[3*i+2]>>6);
      str[4*i+3]=buf[3*i+2]&0x3F;
   }

   return 1;
}