【卡尔曼滤波】递归算法Recursive的应用 C语言、Python实现（Kalman Filter）

更新以gitee为准：
gitee地址

递归算法

卡尔曼滤波一共有好几种算法 而常用的也就几种
递归算法就是其中一种
所谓递归算法 其实就是每一步都求其算术平均根
但是不是直接将所有的数据加起来进行除法
而是每一步平均数都与上一步有关
下文将进行数学推导

递归算法可以应用在测量静态目标时 实际数据没有较大变化的数据处理
这里的误差仅仅是测量的误差 也就是测量噪声
而数据不会因为某个数据导致变化

而测量误差通常满足正态分布 系数即为数据的标准差
测量误差无论在哪个卡尔曼滤波中都是一个重要的参数
但不能滥用准差作为测量误差 因为数据是有限的 最终的标准差实际上不一定就是真实的测量误差

算术平均的递归算法例子

如在进行某个物体长度测量时 由于尺子不准 需要估读 不同人来测多次 导致且每次测量的大小都不一样
譬如：

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];

一眼就可以看到 实际长度就是85左右
算出来算术平均数为85.035
那么我们就可以认为是85.035长度

如果对每一次都进行算术平均数求解 可以写一个简单的代码：

import matplotlib.pyplot as pltx = [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];
a=0
b=[]
for i in range(len(x)):a=x[i]+ab.append(a/(i+1))
print(a/20)plt.figure(1)
plt.plot(x,color="g")
plt.plot(b,color="r")

其中 算术平均数的结果为：

85.6
84.94999999999999
84.63333333333333
85.1
85.17999999999999
85.14999999999999
85.1
85.02499999999999
84.96666666666665
84.97999999999999
85.0
84.95
84.95384615384616
85.03571428571429
85.04666666666667
85.075
85.06470588235295
85.05000000000001
85.02105263157895
85.035

可以看到 随着时间的增加 算术平均数的结果越发稳定 逐渐接近平均值
但缺点就是每一次计算非常耗时 数据量大了以后 甚至会将每一次都加上来

我们将第k次的测量结果定义为Zk
第k次的估计测量结果定义为Xk的顶上再加一个帽子（Hat 就是一个向上的^）
那么Zk与Xk的关系为：

又有：

可得

那么 当k无穷大时
即每次估计值都相近 这也符合算术平均的道理
当k比较小时 估计值与测量值相近

根据最后的递归公式 可以用以下函数实现：

def Recursive(now_z,last_x,k):now_x = last_x + (now_z - last_x)/kreturn now_x

调用方式：

last_x = x[0]
for i in range(len(x)):now_x = Recursive(x[i],last_x,i+1)print(now_x)c.append(now_x)last_x = now_x

每次的结果打印出来即为：

85.6
84.94999999999999
84.63333333333333
85.1
85.17999999999999
85.14999999999999
85.1
85.02499999999999
84.96666666666665
84.97999999999999
84.99999999999999
84.94999999999999
84.95384615384614
85.03571428571428
85.04666666666665
85.07499999999999
85.06470588235292
85.04999999999998
85.02105263157893
85.03499999999998

与算术平均的每次结果保持一致（之所以不一样是因为浮点数运算）
图像画出来即为：

蓝线与先前的红线完全重合

卡尔曼滤波递归

在上面的式子中 可以看到 每次测量结果与测量次数k相关 并且引入了一个系数1/k
在卡尔曼中 把1/k叫做卡尔曼增益系数 计为Kk
那么式子可以写作：

根据卡尔曼的思想 引入两个变量
估计误差：估计值与真实值的误差 计为e下标EST
测量误差：不可避免的在测量中的误差 （多次测量时 通常符合正态分布 标准差即为测量误差） 计为e下标MEA

这两个值与卡尔曼增益系数的关系为：

也就是在k-1时的估计误差 除以 （k-1时的估计误差 和 k时的测量误差）

当k-1时的估计误差 远大于 k时测量误差时 卡尔曼增益趋近于1 估计值趋近于测量值
当k-1时的估计误差 远小于 k时测量误差时 卡尔曼增益趋近于0 估计值与上一次估计值相近

这也跟上面的递归原理一致

那么在进行应用时 要分三步进行计算：
前提：已知当前测量值 上一次估计值 上一次估计误差 测量误差（测量误差通常为定值）
第一步：计算卡尔曼增益
第二步：计算当前估计值
第三步：更新当前的估计误差

而当前估计误差的公式为：

下文将从Python和C语言两个方向进行讨论

Python实现

还是以上面的数据为例子
我们估计真实值为85
可以看到数据波动在84-86.5之间
差不多标准差就是1.5
那么我们的测量误差就是1.5
（如果测量没发生改变 那么这个测量误差就是一个定值 否则就不适用该递归滤波算法
当然 随着测量的增加 也可能发生小幅度的变动
比如一开始头几次 测量比较精准 但是后面可能就出现了新的测量误差值 导致标准差变大）
假设估计误差是2（这个会在后面的计算中不断更新的 第一次使用也是一个估计值）
第一次估计值为85
函数和调用为：

def Kalman_Recursive(now_z,last_x,now_e_MEA,last_e_EST):kg =  last_e_EST/(last_e_EST+e_MEA)now_x = last_x + kg*(now_z-last_x)now_e_EST = (1-kg)*last_e_ESTprint(now_e_EST)return now_x,now_e_ESTe_MEA = 1.5
last_e_EST = 1
last_x = 85for i in range(len(x)):now_x,now_e_EST= Kalman_Recursive(x[i],last_x,e_MEA,last_e_EST)b.append(now_x)last_x = now_xlast_e_EST = now_e_EST

在每次循环中进行打印输出e_EST
可以看到：

0.6
0.42857142857142855
0.3333333333333333
0.2727272727272727
0.23076923076923075
0.19999999999999998
0.1764705882352941
0.15789473684210525
0.14285714285714285
0.13043478260869565
0.12
0.1111111111111111
0.10344827586206895
0.0967741935483871
0.09090909090909091
0.08571428571428572
0.08108108108108109
0.07692307692307693
0.07317073170731707
0.06976744186046512

也就是说 估计误差再逐步减小
估计值的输出为：

85.24
84.97142857142856
84.75555555555555
85.07272727272726
85.13846153846153
85.11999999999999
85.08235294117647
85.02105263157894
84.97142857142856
84.98260869565216
84.99999999999999
84.95555555555555
84.95862068965516
85.03225806451611
85.04242424242423
85.06857142857142
85.05945945945945
85.04615384615383
85.01951219512193
85.03255813953487

也与递归保持一致

在进行若干次计算后
与递归法基本完全重合
此时当前估计值与前一次的估计值基本一样
也就相当于是求算术平均了
如果我们将第一次的估计值 直接变成第一次的测量值会出现：

可以看到变化也是相同的 但是整体会较为偏高一些
因为第一次的测量值是85.5 肯定是要比85大的 但随着时间的增长 往后会趋于稳定 也就差不多了

如果我们直接将算术平均法求出的85.035代入到第一次结果中时

可以说是后半段完全吻合 并且最后一次卡尔曼滤波的结果就是85.035

85.261
84.98642857142856
84.76722222222222
85.08227272727272
85.14653846153846
85.127
85.0885294117647
85.0265789473684
84.97642857142856
84.98717391304346
85.00419999999998
84.95944444444443
84.96224137931033
85.0356451612903
85.04560606060605
85.07157142857142
85.06229729729729
85.04884615384614
85.0220731707317
85.035

如果第一次估值太大 那么可能就需要很多次才能校正回来 如第一次给80：

当然 如果第一次是80 那么估计值的偏差就很大了
所以就得更改小测量误差e_MEA的值才能快速收敛（表示更加信任测量值 而非估计值）
比如改成0.5：

反之收敛更慢 表示更相信估计值
比如改为5：

相似的 如果第一次估计值为80 标准差还是1.5的情况下 第一次估计值明显与实际值有差距
所以可以将e_EST改大 表示更不相信估计值 更相信测量值
比如改成5：

当e_EST非常大时 比如100 第一次的Kk值就是1 更新的e_EST就是 1
第二次的Kk就是1/2 第三次就是1/3 那就跟算术平均的递归系数一样了
那么就会出现红线与蓝线重合的情况：

那么Python函数重新定义一下就可以就是：

def Kalman_Recursive(measure,evaluation_last,e_MEA_now,e_EST_last):kg =  e_EST_last/(e_EST_last+e_MEA_now)evaluation_now = evaluation_last + kg*(measure-evaluation_last)e_EST_now = (1-kg)*e_EST_lastreturn evaluation_now,e_EST_now

在第一次调用时 需要将last_e_EST、last_x确定好 now_e_MEA通常是标准差
测量误差是一个定值 而不能因为估计误差太小而去更改测量误差
如果出现上文中 估计误差太小的情况 那么应该将估计误差改大 而不是将测量误差变小

C语言实现

C语言算法函数：

typedef struct
{double Measure_Now;double Evaluation_Last;double e_EST_Last;double e_MEA_Now;
}Kalman_Filter_Recursive_Struct;Kalman_Filter_Recursive_Struct Kalman_Filter_Recursive(Kalman_Filter_Recursive_Struct Stu);Kalman_Filter_Recursive_Struct Kalman_Filter_Recursive(Kalman_Filter_Recursive_Struct Stu)
{double kg =  Stu.e_EST_Last/(Stu.e_EST_Last+Stu.e_MEA_Now);Stu.Evaluation_Last = Stu.Evaluation_Last + kg*(Stu.Measure_Now-Stu.Evaluation_Last);Stu.e_EST_Last = (1-kg)*Stu.e_EST_Last;return Stu;
}

这里写成double 类型更为精确

调用方式：

int main(void)
{Kalman_Filter_Recursive_Struct Stu;uint8_t i=0;Stu.e_MEA_Now=1.5;Stu.Evaluation_Last=85;Stu.e_EST_Last=1;double buf[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 buf_2[20];for(i=0;i<20;i++){Stu.Measure_Now=buf[i];Stu = Kalman_Filter_Recursive(Stu);buf_2[i]=Stu.Evaluation_Last;printf("%f \n",buf_2[i]);}
}

输出结果：

85.240000 
84.971429 
84.755556 
85.072727 
85.138462 
85.120000 
85.082353 
85.021053 
84.971429 
84.982609 
85.000000 
84.955556 
84.958621 
85.032258 
85.042424 
85.068571 
85.059459 
85.046154 
85.019512 
85.032558 

与我们上面的Python结果一致
但如果都改成float类型则为：

85.239998 
84.971428 
84.755554 
85.072723 
85.138458 
85.119995 
85.082352 
85.021049 
84.971428 
84.982605 
85.000000 
84.955559 
84.958626 
85.032265 
85.042427 
85.068573 
85.059464 
85.046158 
85.019516 
85.032562 

中间有些值就会被改变

与普通卡尔曼滤波的比较

之前有做过一种嵌入式传感器相关常用的卡尔曼滤波：
【C语言/Python】嵌入式常用数据滤波处理：卡尔曼滤波器的简易实现方式（Kalman Filter）
适用于有变化的场景
但如果是静态场景 也可以用到 可以显著降低方差
但其特点是会随着数据的变化而变化 并不会经过无穷次测量后 估算值与上一次相近 所以不是算术平均的那种算法
调用：

last_x = 85
pre_last = 85for i in range(len(x)):now_x,last_x,pre_last= kalman(x[i],last_x,pre_last,1,1.5)d.append(now_x)
plt.plot(d,color="y")

黄线即为
可以看到 在后面的时候 数据发生变化时
黄线也会跟着变动 但红线基本不发生改变
这就是两者的区别

而普通卡尔曼滤波函数为：

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

这里有很多累赘 我会在本专栏第三章进行讨论
但可以看到
每次的kg都与上一次的预测值有关
而且每次的预测值 又与上一次的预测值有关
输出的结果与上一次的结果和当前测量值 以及kg有关
所以 kg就不会随着次数增加而逐渐趋于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;
}