python：基于Kalman滤波器的移动物体位置估计

已于 2023-08-06 02:55:27 修改
阅读量514 收藏
点赞数 1
分类专栏： python Python机器学习专栏 文章标签： python 开发语言
于 2023-08-06 02:54:13 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_35591253/article/details/132126858
版权
本文介绍了Kalman滤波器的基本原理和应用，通过Python代码示例展示了如何使用Kalman滤波器估计移动物体的位置和速度。代码运行结果以图像形式显示了估计效果，证实了Kalman滤波器在考虑噪声影响下能提供稳定准确的估计。

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

CSDN@_养乐多_

Kalman滤波器是一种经典的估计方法，广泛应用于估计系统状态的问题。本篇博客将介绍Kalman滤波器的基本原理，并通过一个简单的Python代码示例，演示如何使用Kalman滤波器来估计移动物体的位置。

通过运行代码，我们将得到一个包含两个子图的图像，分别展示了估计的位置和速度随时间的变化情况。从图像中可以看出，Kalman滤波器能够很好地估计移动物体的位置和速度，同时考虑了测量噪声和过程噪声的影响，使得估计结果更加稳定和准确。

在这里插入图片描述

文章目录

一、Kalman滤波器简介

Kalman滤波器是一种递归估计滤波算法，用于估计线性动态系统的状态。它结合了测量值和状态模型，并通过递归的方式进行状态估计。Kalman滤波器的核心思想是使用先验估计和测量更新，以最优的方式估计系统的状态，并考虑到过程噪声和测量噪声。

二、Kalman滤波器原理

Kalman滤波器可以分为两个主要步骤：预测步骤和更新步骤。预测步骤用于预测下一个时刻的状态和协方差，更新步骤用于根据测量值来更新状态和协方差。预测步骤使用系统的状态转移矩阵和过程噪声协方差来估计下一时刻的状态和协方差。更新步骤使用测量模型和测量噪声协方差来根据实际测量值更新状态和协方差。

三、代码示例

本文将通过一个简单的Python代码示例来演示Kalman滤波器的应用。代码使用NumPy库来进行矩阵运算，并使用Matplotlib库来绘制状态随时间的变化曲线。代码模拟了一个移动物体的位置估计过程，通过不断更新测量值，最终得到估计的位置和速度信息。

from numpy import dot, sum, tile, linalg, exp, log, pi
from numpy.linalg import inv
from numpy import *
from numpy.random import randn

def kf_predict(X, P, A, Q, B, U):
    X = dot(A, X) + dot(B, U)  # 预测下一个状态
    P = dot(A, dot(P, A.T)) + Q  # 更新协方差
    return (X, P)

def kf_update(X, P, Y, H, R):
    IM = dot(H, X)  # 预测测量值
    IS = R + dot(H, dot(P, H.T))  # 预测测量误差协方差
    K = dot(P, dot(H.T, inv(IS)))  # 计算卡尔曼增益
    X = X + dot(K, (Y - IM))  # 更新状态
    P = P - dot(K, dot(IS, K.T))  # 更新协方差
    LH = gauss_pdf(Y, IM, IS)  # 计算测量值的概率密度函数值
    return (X, P, K, IM, IS, LH)

def gauss_pdf(X, M, S):
    if M.shape[1] == 1:
        DX = X - tile(M, X.shape[1])
        E = 0.5 * sum(DX * (dot(inv(S), DX)), axis=0)
        E = E + 0.5 * M.shape[0] * log(2 * pi) + 0.5 * log(linalg.det(S))
        P = exp(-E)
    elif X.shape()[1] == 1:
        DX = tile(X, M.shape()[1]) - M
        E = 0.5 * sum(DX * (dot(inv(S), DX)), axis=0)
        E = E + 0.5 * M.shape()[0] * log(2 * pi) + 0.5 * log(linalg.det(S))
        P = exp(-E)
    else:
        DX = X - M
        E = 0.5 * dot(DX.T, dot(inv(S), DX))
        E = E + 0.5 * M.shape()[0] * log(2 * pi) + 0.5 * log(linalg.det(S))
        P = exp(-E)
    return (P[0], E[0])

#time step of mobile movement
dt = 0.1  # 移动的时间步长
X = array([[0.0], [0.0], [0.1], [0.1]])  # 初始状态向量
P = diag((0.01, 0.01, 0.01, 0.01))  # 初始状态协方差矩阵
A = array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]])  # 状态转移矩阵
Q = eye(X.shape[0])  # 状态过程噪声协方差矩阵
B = eye(X.shape[0])  # 控制输入矩阵
U = zeros((X.shape[0], 1))  # 控制输入向量

Y = array([[X[0, 0] + abs(randn(1)[0])], [X[1, 0] + abs(randn(1)[0])]])  # 测量值向量
H = array([[1, 0, 0, 0], [0, 1, 0, 0]])  # 测量矩阵
R = eye(Y.shape[0])  # 测量噪声协方差矩阵

N_iter = 50  # Kalman滤波器的迭代次数

# Applying the Kalman Filter
for i in arange(0, N_iter):
    (X, P) = kf_predict(X, P, A, Q, B, U)
    (X, P, K, IM, IS, LH) = kf_update(X, P, Y, H, R)
    Y = array([[X[0, 0] + abs(0.1 * randn(1)[0])], [X[1, 0] + abs(0.1 * randn(1)[0])]])  # 更新测量值

print("Final estimated state:")
print(X)


import matplotlib.pyplot as plt

# 初始化状态向量列表和时间步长列表
X_estimated = [X]  # 初始状态
time_steps = [0]

# Applying the Kalman Filter
for i in range(1, N_iter + 1):
    (X, P) = kf_predict(X, P, A, Q, B, U)
    (X, P, K, IM, IS, LH) = kf_update(X, P, Y, H, R)
    Y = array([[X[0, 0] + abs(0.1 * randn(1)[0])], [X[1, 0] + abs(0.1 * randn(1)[0])]])  # 更新测量值

    # 将估计的状态向量和时间步长保存在列表中
    X_estimated.append(X)
    time_steps.append(i * dt)

# 提取各个状态维度的变化值
X_estimated = array(X_estimated).squeeze()
X_pos_x = X_estimated[:, 0]
X_pos_y = X_estimated[:, 1]
X_vel_x = X_estimated[:, 2]
X_vel_y = X_estimated[:, 3]

# 绘制状态随时间的变化曲线
plt.figure(figsize=(12, 8))
plt.subplot(2, 1, 1)
plt.plot(time_steps, X_pos_x, label='X Position (x)')
plt.plot(time_steps, X_pos_y, label='Y Position (y)')
plt.title('Estimated Position')
plt.xlabel('Time')
plt.ylabel('Position')
plt.legend()

plt.subplot(2, 1, 2)
plt.plot(time_steps, X_vel_x, label='X Velocity (v_x)')
plt.plot(time_steps, X_vel_y, label='Y Velocity (v_y)')
plt.title('Estimated Velocity')
plt.xlabel('Time')
plt.ylabel('Velocity')
plt.legend()

plt.tight_layout()
plt.show()

四、代码详解

这段代码实现了一个基本的卡尔曼滤波器（Kalman Filter）用于估计移动物体的位置。以下是代码的中文解释：

导入所需的库和函数：

from numpy import dot, sum, tile, linalg, exp, log, pi
from numpy.linalg import inv

定义kf_predict函数，用于预测下一个时刻的状态和协方差：

def kf_predict(X, P, A, Q, B, U):
    X = dot(A, X) + dot(B, U)  # 预测下一个状态
    P = dot(A, dot(P, A.T)) + Q  # 更新协方差
    return (X, P)

定义kf_update函数，用于根据测量值更新状态和协方差：

def kf_update(X, P, Y, H, R):
    IM = dot(H, X)  # 预测测量值
    IS = R + dot(H, dot(P, H.T))  # 预测测量误差协方差
    K = dot(P, dot(H.T, inv(IS)))  # 计算卡尔曼增益
    X = X + dot(K, (Y - IM))  # 更新状态
    P = P - dot(K, dot(IS, K.T))  # 更新协方差
    LH = gauss_pdf(Y, IM, IS)  # 计算测量值的概率密度函数值
    return (X, P, K, IM, IS, LH)

定义gauss_pdf函数，用于计算高斯分布的概率密度函数值：

def gauss_pdf(X, M, S):
    if M.shape[1] == 1:
        DX = X - tile(M, X.shape()[1])
        E = 0.5 * sum(DX * (dot(inv(S), DX)), axis=0)
        E = E + 0.5 * M.shape()[0] * log(2 * pi) + 0.5 * log(linalg.det(S))
        P = exp(-E)
    elif X.shape()[1] == 1:
        DX = tile(X, M.shape()[1]) - M
        E = 0.5 * sum(DX * (dot(inv(S), DX)), axis=0)
        E = E + 0.5 * M.shape()[0] * log(2 * pi) + 0.5 * log(linalg.det(S))
        P = exp(-E)
    else:
        DX = X - M
        E = 0.5 * dot(DX.T, dot(inv(S), DX))
        E = E + 0.5 * M.shape()[0] * log(2 * pi) + 0.5 * log(linalg.det(S))
        P = exp(-E)
    return (P[0], E[0])

初始化所需的变量：

dt = 0.1  # 移动的时间步长
X = array([[0.0], [0.0], [0.1], [0.1]])  # 初始状态向量
P = diag((0.01, 0.01, 0.01, 0.01))  # 初始状态协方差矩阵
A = array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]])  # 状态转移矩阵
Q = eye(X.shape()[0])  # 状态过程噪声协方差矩阵
B = eye(X.shape()[0])  # 控制输入矩阵
U = zeros((X.shape()[0], 1))  # 控制输入向量

Y = array([[X[0, 0] + abs(randn(1)[0])], [X[1, 0] + abs(randn(1)[0])]])  # 测量值向量
H = array([[1, 0, 0, 0], [0, 1, 0, 0]])  # 测量矩阵
R = eye(Y.shape()[0])  # 测量噪声协方差矩阵

N_iter = 50  # Kalman滤波器的迭代次数

应用Kalman滤波器进行状态估计：

for i in arange(0, N_iter):
    (X, P) = kf_predict(X, P, A, Q, B, U)
    (X, P, K, IM, IS, LH) = kf_update(X, P, Y, H, R)
    Y = array([[X[0, 0] + abs(0.1 * randn(1)[0])], [X[1, 0] + abs(0.1 * randn(1)[0])]])  # 更新测量值

在每次迭代中，先进行预测步骤（kf_predict），然后进行更新步骤（kf_update）。预测步骤用于预测下一个时刻的状态和协方差，而更新步骤用于根据测量值来更新状态和协方差，同时计算卡尔曼增益等相关参数。循环迭代过程用于不断更新估计结果，从而逐步优化状态估计值。

五、结果展示

通过运行代码，我们将得到一个包含两个子图的图像，分别展示了估计的位置和速度随时间的变化情况。从图像中可以看出，Kalman滤波器能够很好地估计移动物体的位置和速度，同时考虑了测量噪声和过程噪声的影响，使得估计结果更加稳定和准确。

六、总结

Kalman滤波器是一种非常有用的估计方法，特别适用于线性动态系统的状态估计问题。它通过递归的方式将先验信息和测量信息相结合，得到最优的状态估计结果。本文通过一个简单的Python代码示例演示了Kalman滤波器的应用，希望读者通过本文对Kalman滤波器有一个初步的了解，并能在实际应用中使用它解决相关问题。

七、参考文献

https://www.researchgate.net/publication/222095635_Implementation_of_Kalman_Filter_with_Python_Language

基于卡尔曼滤波算法的轨迹跟踪
FPGA/MATLAB学习教程/源码/项目合作开发
05-20 7566
预测：根据系统的动态模型和先前的状态估计，预测系统的下一个状态。状态预测是利用系统的动态模型和先前的状态估计来预测系统的下一个状态。它的缺点是需要对系统的动态模型和测量噪声模型进行准确的建模，并且对于非线性系统和非高斯噪声模型，需要使用扩展卡尔曼滤波或无迹卡尔曼滤波等方法进行处理。利用目标的运动模型和先前的状态估计，预测目标的下一个状态。其中，P(t)是先前状态估计的协方差矩阵，Q是过程噪声的协方差矩阵，用于表示状态估计的不确定性。（2）预测：利用目标的运动模型和先前的状态估计，预测目标的下一个状态。
python实现简单的卡尔曼滤波，，预测小车的位移和速度（匀速）
DeepCBW的博客
04-21 1422
python实现简单的卡尔曼滤波，，预测小车的位移和速度（匀速）
kalman
L_fengzifei的博客
03-11 135
kalman 状态变量只有一个需要跟踪 例如x轴的位置 测量值不太准，预测值不太准，俩加一块不就准了吗 https://blog.csdn.net/qq_33797005/article/details/105770227 https://zhuanlan.zhihu.com/p/106582238 https://zhuanlan.zhihu.com/p/158751412 https://zhuanlan.zhihu.com/p/39912633 https://nbviewer.org/github/
rtklib使用kalman滤波进行高精度相对定位原理
热门推荐
从零开始
10-15 1万+
rtklib使用kalman滤波进行高精度相对定位中浮点解的计算，流程主要分为以下几个步骤： 1. 计算移动站概略坐标 2. 计算卫星位置、速度 3. 选择共视卫星 4. 计算非差残差项和双差 5. 使用kalman滤波计算浮点解 6. 重新进行双差计算 5. 计算固定解 各个步骤的关键算法如下： 1. 计算概略坐标     概略坐标的计算可以使用卡拉曼滤波方法或者加权
（实例演示）Kalman滤波器的应用解释2
kissgoodbye2012的博客
04-19 810
这里开始是实例了！理论看前一篇文章。1.普遍文章里温度的例子目的：估算k时刻的实际温度值输入：k-1时刻的温度值，预测模型的误差方差，测量模型的误差方差。计算过程：（1）预测模型和测量模型为：状态变量为温度值X，预测模型认为k时刻会延续k-1时刻的温度值，方差为w;测量值也为温度，所以和状态变量的关系为H=1，方差为v。（2）假设k-1时刻的温度为23度，预测标准差为5度。测量得到k时刻的温度为2......
boss:整个卡尔曼滤波器的简单案例——估计机器人位置
weixin_37522117的博客
04-22 1260
卡尔曼滤波器的简单案例——估计机器人位置
基于python的移动物体检测与跟踪算法_详解使用PyTorch实现目标检测与跟踪
weixin_33662430的博客
12-24 1759
python教程栏目介绍使用PyTorch实现目标检测与跟踪大量免费学习推荐，敬请访问python教程(视频)引言在昨天的文章中，我们介绍了如何在PyTorch中使用您自己的图像来训练图像分类器，然后使用它来进行图像识别。本文将展示如何使用预训练的分类器检测图像中的多个对象，并在视频中跟踪它们。图像中的目标检测目标检测的算法有很多，YOLO跟SSD是现下最流行的算法。在本文中，我们将使用YOLOv...
python卡尔曼滤波_OpenCV3-Python卡尔曼(Kalman)滤波器介绍
weixin_39969232的博客
11-27 730
卡尔曼滤波器主要由Rudolf Kalam在20世纪50年代末提出来的，它在众多领域得到应用，特别是交通工具的导航系统上经常用到。该方法认为物体的运动模型服从高斯模型，通过对输入数据流进行递归操作，预测目标的运动状态，然后通过与观察模型进行对比，根据误差来更新运动目标的状态。举例说明：假设桌子上有一个小红球，现场也有摄像头对着这一个场景。小球确定为被追踪的目标，用手指轻弹下小球，它会在桌子上滚动。...
基于kalman的单目标追踪，以及demo测试（Python and C++）
qq_46454669的博客
03-29 2613
我们可以在任何含有不确定信息的动态系统中的使用卡尔曼滤波，对系统的下一步动作做出有根据的猜测。猜测的依据是预测值和观测值，首先我们认为预测值和观测值都符合高斯分布且包含误差，然后我们预设预测值的误差Q和观测值的误差R，然后计算得到卡尔曼增益，最后利用卡尔曼增益来综合预测值和观测值的结果，得到我们要的最优估计。通俗的说卡尔曼滤波就是将算出来的预测值和观测值综合考虑的过程。
最优估计大作业：基于kalman滤波的组合导航
03-17
项目中包含的报告很可能会详细分析每种传感器的数据特性，解释如何构建合适的kalman滤波器结构，包括状态向量、系统矩阵、观测矩阵以及噪声协方差矩阵的设定。报告可能还会涉及滤波器的初始化、滤波过程中的误差分析...
Python+opencv3.4+kalman（卡尔曼）滤波实现视频中运动物体追踪和轨迹绘制
Miaosh999的博客
06-24 1万+
官方卡尔曼滤波目标追踪实例c++版，本文的opencv版本代码是由改代码改编而来 link kalman滤波相关知识，理解等 link hsv色彩对照表：（设置运动目标hsv阈值时要用到） 实现效果： 需要视频的话可以去我的下载里面找，有上面这个视频 实验环境： Python3.6+OpenCV3.4+pycharm2019 代码实现： 首先是一个简单的不用kalman滤波的运动目标追踪代码 这里可以根据需要进行摄像头运动目标识别，只要把 camera = cv2.VideoCapture('./vid
Python实现卡尔曼滤波器-递归状态估计技术详解
卡尔曼滤波器是一种有效的递归滤波器，它估计线性动态系统的状态，即使存在不确定性和噪声的情况下也能给出较为准确的估计。此估计器由Rudolf E. Kalman于1960年提出，它的出现不仅对控制理论领域有着划时代的意义，...
Python爬虫实战：研究进制流数据，实现逆向解密
ylfhpy的博客
05-14 467
许多网站和应用通过二进制流数据传输敏感信息，如视频、金融交易数据等。逆向工程进制流数据不仅有助于合法的数据获取与分析，还能帮助企业发现自身安全漏洞，提升数据保护能力。本研究主要针对特定类型的视频加密系统，对于采用更复杂加密机制（如硬件加密、动态密钥交换）的系统，可能需要进一步优化方法。以二进制形式传输的数据序列，通常包含文件、图像、音频或加密信息。如 ECB、CBC、CFB 等，定义加密算法如何处理数据块。拦截并修改通信双方数据的攻击方式，本研究用于合法数据捕获。
Python 基础语法与数据类型(八) - 函数参数：位置参数、关键字参数、默认参数、可变参数 (*args, **kwargs)
晨的技术博客
05-13 703
本篇博客详细介绍了 Python 函数参数的四种主要类型：位置参数、关键字参数、默认参数和可变参数 (*args, **kwargs)。位置参数要求实参按顺序传递，关键字参数则通过参数名指定值，提高了代码的可读性。默认参数允许为参数提供默认值，但需注意避免使用可变对象作为默认值。可变参数 *args 和 **kwargs 分别用于接收任意数量的位置参数和关键字参数，增强了函数的灵活性。通过合理使用这些参数类型，可以编写出更通用和易维护的函数。
YOLOv7训练时4个类别只出2个类别
最新发布
qq_48873240的博客
05-16 417
在使用YOLOv7进行训练和预测时，发现模型仅能预测出两个类别（LFM和SFM），而实际应有四个类别。经过检查，特征图大小和anchors设置均与YOLOv3一致，且YOLOv3表现正常。最终发现问题可能出在detect.py中的conf-thres参数设置过高，将其从0.5调整为0.1后，问题得到解决。此外，还需注意在NMS操作时不使用classes参数，并在train.py中根据实际类别数和图片大小调整hyp['cls']和hyp['obj']的缩放比例。这些调整有助于确保模型能够正确预测所有类别。
【AI】“CUDA” 到底是什么？（AI 计算民主化，第二部分）
u011808788的博客
05-14 948
似乎每个人都在去年开始谈论CUDA ：它是深度学习的支柱，是新型硬件难以竞争的原因，也是NVIDIA 护城河和飙升市值的核心。DeepSeek的出现，让我们获得了一个惊人的发现：它的突破是通过“绕过” CUDA，直接进入 PTX 层实现的……但这究竟意味着什么？似乎每个人都想打破这种锁定，但在制定计划之前，我们必须了解我们面临的是什么。本文是 Modular “ AI 计算民主化”系列文章的第二部分。更多信息，请参阅：第一部分：DeepSeek 对 AI 的影响第二部分：“CUDA”到底是什么？
maxtext开源程序是一个简单、高性能和可扩展的 Jax LLM！
struggle2025的博客
05-12 1400
MaxText 是一种高性能、高度可扩展的开源软件，以纯 Python/Jax LLM 编写，以 Google Cloud TPU 和 GPU 为目标，用于训练和推理。借助 Jax 和 XLA 编译器的强大功能，MaxText 实现了高 MFU 并从单个主机扩展到超大型集群，同时保持简单和“无优化”
Java零基础学习Day12——集合ArrayList
m0_68932052的博客
05-13 604
集合只存引用数据类型；长度可变数组可存基本数据类型、引用数据类型；长度固定。
SparkSQL操作Mysql
2301_82309776的博客
05-13 683
SparkSQL 提供了强大的功能来连接和操作 MySQL 数据库，支持读取数据、写入数据以及执行 SQL 查询。下面将详细介绍如何使用 SparkSQL 与 MySQL 进行交互，并提供完整的代码示例。，并将 JAR 文件添加到 Spark 的 classpath 中。在创建 SparkSession 时，通过。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

_养乐多_

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值