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

news2024/11/15 13:57:34

文章目录

卡尔曼滤波
卡尔曼滤波公式
卡尔曼滤波数据处理效果
C语言的卡尔曼滤波实现
附录：压缩字符串、大小端格式转换
- 压缩字符串
- - 浮点数
  - 压缩Packed-ASCII字符串

卡尔曼滤波

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

具体关于卡尔曼滤波网上资料很多这里就不多做介绍
在这里插入图片描述
一点：卡尔曼滤波能有效减小系统方差

卡尔曼滤波公式

在这里插入图片描述

卡尔曼滤波数据处理效果

我用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;
}

最终结果：

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

压缩字符串

首先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;
}