从贝叶斯滤波到卡尔曼滤波详细推导

Guopinglu

已于 2022-07-10 20:25:45 修改

阅读量768

点赞数 1

文章标签： python 人工智能大数据

于 2021-03-22 17:02:58 首次发布

本文链接：https://blog.youkuaiyun.com/Guopinglu/article/details/115081375

版权

机器学习专栏收录该内容

1 篇文章

订阅专栏

本文快速介绍了贝叶斯滤波的基本概念，通过画图阐述其核心思想，并深入讲解了卡尔曼滤波在导弹定位中的应用，涉及速度预测、传感器融合及卡尔曼滤波算法的实现。同时，讨论了正态分布和多维卡尔曼滤波在模型建立中的作用。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

1 充分理解贝叶斯滤波

我是如何十分钟理解与推导贝叶斯滤波(Bayes Filter)算法？

理解贝叶斯滤波的核心，可以用画图表示出来。

https://blog.youkuaiyun.com/varyshare/article/details/97642209

其实卡尔曼滤波思想非常简单。就是现有我们既知道上个时刻的状态，又知道当前时刻的状态设备测量的观测值。也就是说我既可以根据上个时刻的状态预测现在这个时刻的状态。也可以根据设备测量出的观测值确定当前时刻的状态。为了更加准确，所以我需要融合预测值和测量值来估计出当前的状态值。

在这里插入图片描述

2 卡尔曼滤波

如何理解那个把嫦娥送上天的卡尔曼滤波算法Kalman filter

"""
这个实践的已知量是导弹做水平运动，
已知导弹每个时刻的速度dv,和速度测量仪器的标准差是v_std，
还已知每个时刻用GPS测量出导弹位置position_noise以及GPS的方差是predict_var。
（注意标准差的平方是方差，不用觉得奇怪）
我们需要用卡尔曼滤波根据这些信息获得融合两种传感器后的位置信息position_predict
"""
import numpy as np
import matplotlib.pyplot as plt

t = np.linspace(1,100,100)
a=0.5#a相当于加速度
print(t)
position = (a * t**2)/2
#相当于用GPS传感器测出来的值，包含误差
position_noise = position+np.random.normal(0,120,size=(t.shape[0]))
plt.plot(t,position,'g',label='truth position')
plt.plot(t,position_noise,'r',label='only use measured position')
#plt.show()

# 初试的估计导弹的位置就直接用GPS测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]
predict_var = 0
odo_var = 120 ** 2  # 这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低，方差是标准差的平方
v_std = 50  # 测量仪器的方差
for i in range(1, t.shape[0]):
    dv = (position[i] - position[i - 1]) + np.random.normal(0, 50)  # 模拟从IMU读取出的速度，这里其实位置差除以了1
    position_predict = position_predict + dv  # 利用上个时刻的位置和速度预测当前位置
    predict_var += v_std ** 2  # 更新预测数据的方差
    # 下面是Kalman滤波，注意方差和偏差的区别
    position_predict = position_predict * odo_var / (predict_var + odo_var) + position_noise[i] * predict_var / (
                predict_var + odo_var)
    predict_var = (predict_var * odo_var) / (predict_var + odo_var)**2
    predicts.append(position_predict)

plt.plot(t, predicts, label='kalman filtered position')
plt.legend()
plt.show()