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

Guopinglu

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

阅读量644

点赞数 1

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

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

本文链接：https://blog.csdn.net/Guopinglu/article/details/115081375

版权

机器学习专栏收录该内容

1 篇文章 0 订阅

订阅专栏

1 充分理解贝叶斯滤波

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

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

https://blog.csdn.net/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()