卡尔曼滤波(Kalman Filter)原理及Python实现

这篇具有很好参考价值的文章主要介绍了卡尔曼滤波(Kalman Filter)原理及Python实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

Kalman-Filter-Example

项目地址 https://github.com/zhengjie9510/kalman-filter-example

理论公式

详细理论可参考DR_CAN关于卡尔曼滤波器的视频讲解。https://www.bilibili.com/video/BV1dV411B7ME
卡尔曼滤波公式分为预测更新两部分。

预测公式为:

x_hat_minus[k]= A * x_hat[k-1] + B * u[k-1]

P_minus[k] = A * P[k-1] * A.T + Q

更新公式为:

K[k] = P_minus[k] * H.T * (H * P_minus[k] * H.T + R).I

x_hat[k] = x_hat_minus[k] + K[k] * (z[k] - H * x_hat_minus[k])

P[k] = (np.eye(1) - K[k] * H) * P_minus[k]

其中:

x_hat_minus--先验估计状态  
P_minus--先验估计协方差矩阵  
x_hat--后验估计状态  
P--后验估计协方差矩阵  
A--状态转移矩阵  
B--控制矩阵  
u--控制量  
Q--过程噪声协方差矩阵  
K--卡尔曼增益  
H--观测矩阵  
R--观测噪声协方差矩阵  
z--观测量。

应用

背景

假定有一个匀速运动的人,状态变量x1和x2,Xx1表示人的位置,x2表示人的速度,观测变量z1和z2,z1表示人的位置,z2表示人的速度。
状态转移方程如下:

速度:x2[k] = x2[k-1] + w2[k-1]  
位置:x1[k] = x1[k-1] + x2[k-1] * dt + w1[k-1]

测量方程为:

速度:z2[k] = x2[k] + v2[k]  
位置:z1[k] = x1[k] + v1[k]

读取数据

df = pd.read_csv('data.csv', index_col=0)
x = df[['X1', 'X2']].values
z = df[['Z1', 'Z2']].values

设置参数并调用卡尔曼滤波函数

u = np.zeros(z.shape)  # 控制量,当前场景为0
x_hat_first = np.array([[0, 1]])  # 初始估计
P_first = np.array([[1, 0], [0, 1]])  # 初始估计协方差

A = np.array([[1, 1], [0, 1]])  # 状态转移矩阵
B = np.array([[0, 0], [0, 0]])  # 控制矩阵
H = np.array([[1, 0], [0, 1]])  # 观测矩阵
Q = np.array([[0.1, 0], [0, 0.1]])   # 系统噪声协方差矩阵
R = np.array([[1, 0], [0, 1]])  # 测量噪声协方差矩阵

x_hat = kalman_filter(x, z, u, A, B, H, Q, R, x_hat_first, P_first)

结果展示

plt.figure(figsize=(16, 9))
plt.plot(x[:, 0], 'g--', label='position--noisy system estimate')
plt.plot(z[:, 0], 'k--', label='position--noisy measurements')
plt.plot(x_hat[:, 0], 'b--', label='position--a posteri estimate')
plt.legend()
plt.xlabel('Step')
plt.ylabel('Position')
plt.title('Position')
plt.show()

plt.figure(figsize=(16, 9))
plt.plot(x[:, 1], 'g-', label='speed--noisy system estimate')
plt.plot(z[:, 1], 'k-', label='speed--noisy measurements')
plt.plot(x_hat[:, 1], 'b-', label='speed--a posteri estimate')
plt.legend()
plt.xlabel('Step')
plt.ylabel('Speed')
plt.title('Speed')
plt.show()

卡尔曼滤波(Kalman Filter)原理及Python实现,python,开发语言
卡尔曼滤波(Kalman Filter)原理及Python实现,python,开发语言文章来源地址https://www.toymoban.com/news/detail-719666.html

到了这里,关于卡尔曼滤波(Kalman Filter)原理及Python实现的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 卡尔曼滤波原理及c++代码实现

    最近重新看到了卡尔曼滤波,想把这部分的原理整理写下来。供需要的人理解和学习。 一键三连,点赞收藏关注! 卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法,“optimal recursivedata processing algorithm(

    2024年02月07日
    浏览(38)
  • 卡尔曼滤波器原理讲解及其matlab实现

    目录 一:卡尔曼滤波器的信号模型[1-2] 二:其他方程及变量介绍 三:卡尔曼滤波器递推公式 四:matlab仿真[3] 参考文献: 引言:在进行一些信号处理的过程中,我们通常会采集到一些数据,但是实际测量到的数据是受到噪声干扰了之后的,故与真实的数据有一些偏差。因此

    2023年04月08日
    浏览(43)
  • 卡尔曼滤波的Python实现

    为了在Python编程环境下实现卡尔曼滤波算法,特编写此程序 主要用到了以下3个模块 numpy(数学计算) pandas(读取数据) matplotlib(画图展示) 代码的核心是实现了一个Kf_Params类,该类定义了卡尔曼滤波算法的相关参数 然后是实现了一个kf_init()函数,用来初始化卡尔曼滤波算

    2024年02月12日
    浏览(42)
  • 卡尔曼滤波算法原理及示例

      例程:物体做匀速运动每秒运动1m,观测器观测方差为1m

    2024年02月15日
    浏览(39)
  • 基于卡尔曼滤波的信号处理以及图像追踪Python实现

    这篇文章将由浅入深,从最简单的信号处理开始,到计算机视觉图像跟踪的应用。该文章使用Python语言,在进行视觉处理时,并未使用opencv自带的api,而是从矩阵运算进行逐步处理,更易于理解。 网上现在能看到的原理解释都很详细,这里就不多介绍。本人理解卡尔曼滤波的

    2024年02月05日
    浏览(53)
  • 【图像处理 】卡尔曼滤波器原理

    目录 一、说明 二、它是什么? 2.1 我们可以用卡尔曼滤波器做什么? 2.2 卡尔曼滤波器如何看待您的问题

    2024年02月06日
    浏览(49)
  • 【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码实现 本文包括

    2024年02月08日
    浏览(43)
  • 集合卡尔曼滤波(EnKF)原理及样例应用

    EnKF 是一种基于蒙特卡罗方法预测误差统计信息的卡尔曼滤波。它与 PF 的相同点是都采用了采样粒子的集合来表示状态概率空间,但 EnKF 在更新步使用卡尔曼更新,该方法以集合的形式进行模拟预报和 分析更新这两个过程,通过模式状态的集合来表征 误差协方差的信息,以最小

    2024年02月05日
    浏览(35)
  • 算法介绍及实现——卡尔曼滤波

            本文主要介绍卡尔曼滤波的推导过程及建模步骤,是网站的学习笔记。本文主要是通过例子来引出卡尔曼滤波的建模思想及算法步骤。 参考网站:(这个网站讲得真的很详细很清楚,层层递进,逻辑清晰) Kalman Filter Tutorial https://www.kalmanfilter.net/ 目录 一、引言

    2024年02月14日
    浏览(41)
  • 在Matlab/Simulink搭建卡尔曼kalman模块化模型

           Kalman滤波是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。算法优点在于计算量小,能够利用前一时刻的状态或可能的测量值来得到当前时刻下状态的最优估计。观测数据中包括系统中的噪声和干扰的影响,所以最优估计

    2024年02月08日
    浏览(46)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包