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

文章目录

  • Part.I 问题&解决方案
    • Chap.I 解决方案
    • Chap.II 建模与分析
  • Part.II 简单实现

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

Part.I 问题&解决方案

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

问题描述:某房间内温度受随机干扰影响,每小时用温度计测量一次温度,试对该房间温度作最佳估计(单位℃) ;假设:干扰 W ∼ N ( 0 , 0. 4 2 ) W∼N(0,0.4^2) WN(0,0.42) ,温度计的量测误差 V ∼ N ( 0 , 0. 3 2 ) V∼N(0,0.3^2) VN(0,0.32) 单位都是℃,假设 t 1 t_1 t1 时刻房间的温度是 25±0 ℃,在 t 2 , t 3 t_2,t_3 t2,t3 时刻分别用温度计量测的温度为 25.2, 25.5 ℃,求 t 2 , t 3 t_2,t_3 t2,t3 时刻房间的真实温度。

Chap.I 解决方案

估计流程如下所示:
【卡尔曼滤波】卡尔曼滤波简单实例_第1张图片
计算过程如下:
【卡尔曼滤波】卡尔曼滤波简单实例_第2张图片

Chap.II 建模与分析

对上面的问题进行建模如下:
X k = X k − 1 + W k − 1 X_k=X_{k-1}+W_{k-1} Xk=Xk1+Wk1
Z k = X k + V k Z_k=X_k+V_k Zk=Xk+Vk

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

一些思考与讨论

  • 如果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

你可能感兴趣的:(Surveying,And,Mapping,1024程序员节,卡尔曼滤波)