【卡尔曼滤波】卡尔曼滤波简单实例

参考&引用
[1] 严老师捷联惯导课程视频：https://www.bilibili.com/video/BV177411K7sS/
[2] 【卡尔曼滤波】离散线性系统的卡尔曼滤波：https://blog.csdn.net/Gou_Hailong/article/details/123050594

Part.I 问题&解决方案

ps: 之前想做的 Kalman 滤波的实例今天终于有时间做了！

问题描述：某房间内温度受随机干扰影响，每小时用温度计测量一次温度，试对该房间温度作最佳估计（单位℃） ；假设：干扰 $W\sim N(0,0.4^2)$ ，温度计的量测误差 $V\sim N(0,0.3^2)$ 单位都是℃，假设 $t_1$ 时刻房间的温度是 25±0 ℃，在 $t_2,t_3$ 时刻分别用温度计量测的温度为 25.2, 25.5 ℃，求 $t_2,t_3$ 时刻房间的真实温度。

Chap.I 解决方案

估计流程如下所示：

计算过程如下：

Chap.II 建模与分析

对上面的问题进行建模如下：
$X_k=X_{k-1}+W_{k-1}$
$Z_k=X_k+V_k$

• 上面第一个式子的含义：这一时刻房间真实温度=上一时刻的房间温度+这段时间的干扰项
• 上面第二个式子的含义：这一时刻的房间温度量测值=这一时刻房间的真实温度+量测误差(噪声)
• 模型目的就是根据上一时刻房间的真实温度、干扰、这一时刻的量测值、量测噪声，求这一时刻的房间真实温度。

---

一些思考与讨论

• 如果W=0，换言之干扰项为零，这个房间的温度是恒定不变的，但是房间温度的初值不知道，这就完全取决于量测噪声的影响了。这就类似于拿一个尺子，多次量测一个固定长度的桌子，最优解就是多次量测取均值，并且量测次数越多，最后得到的精度越高。
• 如果V=0，换言之温度计足够精准，那么温度计的量测值就是房间的真实温度！这种情况下就不需要这套估计理论了。实际上，量测误差总会存在的，绝对精准的仪器是不存在的。

Part.II 简单实现

下面对上面的问题用python简单实现了一下：

import math

def perdict(x1_1,W):
    ''' Predict '''
    x=x1_1[0]+W[0]      # I'm not sure about this
    sigma=math.sqrt(x1_1[1]**2+W[1]**2)
    return [x,sigma]

def estimate(x2_1,z):
    ''' Estimate '''
    x=x2_1[0]*z[1]**2/(z[1]**2+x2_1[1]**2)+z[0]*x2_1[1]**2/(z[1]**2+x2_1[1]**2)
    sigma=math.sqrt(1/((1/x2_1[1])**2+(1/z[1])**2))
    return [x,sigma]

def print_result(x2_1,x2_2):
    ''' Print formatting '''
    str1="Predict: "+"%.2f" %x2_1[0]+'±'"%.2f" %x2_1[1]+'; '
    str1+="Estimate: "+"%.2f" %x2_2[0]+'±'"%.2f" %x2_2[1]+' '
    print(str1)

def process(time,x1_1,w,z2):
    ''' process for one epoch '''
    x2_1=perdict(x1_1,w)
    x2_2=estimate(x2_1,z2)
    print('t_{} '.format(time),end="")
    print_result(x2_1,x2_2)
    return x2_2

x1_1=[25,0]
Sw=0.4
Sv=0.3
w=[0,Sw]
z2=[25.2,Sv]
z3=[25.5,Sv]
x2_2=process(2,x1_1,w,z2)
x3_3=process(3,x2_2,w,z3)

输出结果为：

t_2 Predict: 25.00±0.40; Estimate: 25.13±0.24
t_3 Predict: 25.13±0.47; Estimate: 25.39±0.25