【卡尔曼滤波】数据融合Fusion的应用 C语言、Python实现（Kalman Filter）

【卡尔曼滤波】数据融合Fusion的应用 C语言、Python实现（Kalman Filter）

更新以gitee为准：
gitee地址

卡尔曼滤波数据融合

与上一章节类似
当你在测量一个静态目标时 难免会有误差（也有可能是一个偏大 一个偏小）
测量误差通常满足正态分布 且标准差就是sigma
加入有两把尺子 测量出来一个偏大 一个偏小 且两个标准差都不同的情况下 如何计算呢？
那就要进行数据融合
譬如一个测出来85 sigma是1 另一个测出来是90 sigma是10
那么肯定就能想到真实值在85-90之间 且靠近85（因为后者误差更大）
同样 带人到卡尔曼滤波中就可以得到公式：
其中 z1是第一种方式测量结果 z2是第二种方式测量结果
K就是卡尔曼增益

可以看到 当K比较小时 估计值就是z1
当K接近1时 估计值就是z2

对应的 也有 σ1和σ2
同时 融合生成以后 会有新的σ 要保证这个值最小 才是最优解

那么最终的方差与之关系就为：

又因为z1和z2相互独立 所以：

对其求导可得：

当导数为0时 方差最小
求得此时K值为：

然后再求出估计值与估计方差即可

此时：
当σ1特别大时 K接近1 估计值就是z2 选择相信z2的数据
当σ1特别小时 K接近0 估计值就是z1 选择相信z1的数据

Python实现

a=[]
b=[]
c=[]
d=[]

x = [85.6,84.3,84.0,86.5,85.5,85.0,84.8,84.5,84.5,85.1,85.2,84.4,85.0,86.1,85.2,85.5,84.9,84.8,84.5,85.3];

y = [84.6,83.6,83.2,85.1,84.3,84.2,83.7,83.5,83.0,84.5,84.3,83.4,84.2,85.3,84.7,84.1,83.6,83.5,83.6,84.5];

m = [84.6,85.3,83.0,87.5,84.5,86.0,83.8,85.5,83.5,86.1,85.2,85.4,84.0,85.1,84.2,86.5,85.9,83.8,83.5,86.3];

plt.figure(2)
plt.plot(x,color="g")     
plt.plot(y,color="b")  

def Data_Fusion(z1,z2,s1,s2):
    kg = s1/(s1+s2)
    kg_2=kg*kg
    kg_3=(1-kg)*(1-kg)
    z=z1+kg*(z2-z1)
    s=kg_3*s1+kg_2*s2
    return z,s
    
for i in range(len(x)):
    z,s = Data_Fusion(x[i],y[i],2,1.5)
    a.append(z)
plt.plot(a,color="r")  

如果改变标准差大小 那么就会让红线靠近两边的线
比如改成特别大 表示不信任2号数据

反之亦然
那么Python的函数打包可以是：

def Data_Fusion(z1,z2,s1,s2):
    kg = s1/(s1+s2)
    z=z1+kg*(z2-z1)
    s=(1-kg)*(1-kg)*s1+kg*kg*s2
    return z,s

这里的s就是方差
如果要传入标准差 则需要计算后再传入

C语言实现

同样 C语言比较简单：

typedef struct
{
    double Measure_1;
    double Measure_2;
    double Var_1;
    double Var_2;
    double Fusion;
    double Var;
}Kalman_Filter_Fusion_Struct;

Kalman_Filter_Fusion_Struct Kalman_Filter_Fusion(Kalman_Filter_Fusion_Struct Stu);

Kalman_Filter_Fusion_Struct Kalman_Filter_Fusion(Kalman_Filter_Fusion_Struct Stu)
{
    double kg =  Stu.Var_1/(Stu.Var_1+Stu.Var_2);    
    Stu.Var = (1-kg)*(1-kg)*Stu.Var_1+kg*kg*Stu.Var_2;    
    Stu.Fusion = Stu.Measure_1+kg*(Stu.Measure_2-Stu.Measure_1);

    return Stu;
}

调用：

void Fusion(void)
{
    double x[20]= {85.6,84.3,84.0,86.5,85.5,85.0,84.8,84.5,84.5,85.1,85.2,84.4,85.0,86.1,85.2,85.5,84.9,84.8,84.5,85.3};
    double y[20]= {84.6,83.6,83.2,85.1,84.3,84.2,83.7,83.5,83.0,84.5,84.3,83.4,84.2,85.3,84.7,84.1,83.6,83.5,83.6,84.5};
    Kalman_Filter_Fusion_Struct Stu;
    uint8_t i=0;

    Stu.Var_1=2;
    Stu.Var_2=1.5;    
    double buf_2[20];

    for(i=0;i<20;i++)
    {
        Stu.Measure_1=x[i];
        Stu.Measure_2=y[i];
        Stu = Kalman_Filter_Fusion(Stu);
        buf_2[i]=Stu.Fusion;
        printf("%f \n",buf_2[i]);
    }
}

输出结果：

85.028571 
83.900000 
83.542857 
85.700000 
84.814286 
84.542857 
84.171429 
83.928571 
83.642857 
84.757143 
84.685714 
83.828571 
84.542857 
85.642857 
84.914286 
84.700000 
84.157143 
84.057143 
83.985714 
84.842857 

多个数据如何融合

对于三个数据的融合 只需要将前两次融合后的结果算出 然后得到标准差 再以同样的方式融合第三个数据即可
譬如上面的m数组与a数组融合：

for i in range(len(a)):
    z,s = Data_Fusion(a[i],m[i],0.85,3)
    b.append(z)
plt.figure(3)
plt.plot(a,color="g")     
plt.plot(m,color="b")  
plt.plot(b,color="r") 

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

压缩字符串

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