概率机器人_C3_卡尔曼滤波——噪声模型初始化问题

最新推荐文章于 2025-11-13 16:31:45 发布
原创 最新推荐文章于 2025-11-13 16:31:45 发布 · 641 阅读 · 1 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文探讨了噪声矩阵Q和R在系统收敛及收敛速度中的关键作用，通过数学基础链接深入解析其确定方法，并利用泰勒展开和状态转移矩阵概念说明系统相同条件下的动态变化。

问题：噪声矩阵Q,R 的确定，决定了系统的能否收敛，收敛速度。如何确定呢？

数学基础：https://wenku.baidu.com/view/0ba46c5280eb6294dc886c32.html

x 是关于t的函数， t 时刻开始， x(t) 泰勒展开为

t= 0, x(0)=bo

右边写成x(t+1)= x'= 

系统相同， 得到 b0*A=b1

带回，x(t), 

0--t 时间的状态转移矩阵为 括号内的数。可以记做

和 A 矩阵（动态矩阵）维度相同，如果是2*2，  如何表示状态误差呢？

 

 

卡尔曼滤波算法demo（python）
如果想成为中心，那么就到中心去吧。
08-08 388
freerop.py。
概率机器人——卡尔曼滤波
jcsm__的博客
06-04 1075
前一篇贝叶斯滤波中提到了贝叶斯滤波不是一种实用的方法，没有办法在计算机中进行表达。是因为贝叶斯是一种抽象的滤波器，其中的置信度表示都是抽象的，没有明确的表达式，没有办法计算积分。尽管如此，贝叶斯给我们提供了一种关于状态最优估计的方法，本篇及后续可能更新的方法皆是贝叶斯滤波的实现方式。 1.高斯滤波 在正式介绍卡尔曼之前，需要先对高斯滤波做一个说明。 高斯滤波是一种递归状态估计器，为了解决贝叶斯滤波器中置信度没有明确表示方法的缺陷，高斯滤波给出了一种置信度用多元正态分布表示的思路，这也是为什么称之为高斯
3 概率机器人 Probabilistic Robotics 卡尔曼滤波算法
fengyu19930920的博客
09-30 799
文章目录0 数学基础知识1 卡尔曼滤波所需的概率分布2 卡尔曼滤波器的算法流程3 实例4 公式推导4.1 预测公式推导4.2 测量更新公式推导4.3 总结 0 数学基础知识 这部分主要讲解卡尔曼滤波公式推导中所需要的数学基础，不关注具体推导过程的可以忽略这部分 高斯滤波器是连续空间中最早的可处理的贝叶斯滤波器 高斯滤波器的基本思想：将贝叶斯滤波器的置信度用多元正态分布表示：bel(x)=p(x)∼N(μ,Σ)bel(x)=p(x) \sim N(\mu, \Sigma)bel(x)=p(x)∼N(μ,Σ)
卡尔曼滤波的一阶、三阶应用实例，矩阵的初始化问题
xufuqiang2017的博客
10-17 6770
五个基本公式 最近学习卡尔曼滤波，本想着从卡尔曼的那篇论文学起，但是奈何看了很长时间不知道到底在干什么（本人着实太菜了），后来无奈之下开始从国内的文献看起，结合各位大佬的博客，总算是有点理解了，所以打算记录下来，也与各位共享。 关于五个公式的作用网上已经有很多人讲的很清楚了，这里不再赘述，大家可以参考这篇中文论文，里边关于五个公式及一些参数的初始化问题讲的比较透彻。这里仅将公式贴出： 首先必须要指出公式（7）中黄色标注的部分是单位矩阵I，而不是数字1，我再进行高阶滤波时在这里卡了很久。 各个参数的初始化及
概率机器人——卡尔曼滤波器
KongDaQing1290的博客
03-20 551
1、参考文章 1-1概率机器人(Probabilistic Robotics)——高斯滤波（卡尔曼滤波） 1-2 卡尔曼滤波推导 2、高斯分布乘法 2-1 高斯分布乘积 2-2 直观理解高斯分布相乘 2-3 高斯概率密度函数相乘仍然是高斯密度函数 2-4 高斯相乘引理及其证明 ...
（十三）卡尔曼滤波初始化
h320130的博客
12-14 1751
滤波初始化会严重影响到初始时刻的跟踪精度，因此只有初始化进行好了，才可以利用卡尔曼滤波对目标进行跟踪。本文针对四维、六维及九维条件下的卡尔曼滤波进行了初始化，对应的原理以及代码如文中所示。后续代码会在资源中上传。
7、卡尔曼滤波场景分析与算法推导
gg901的博客
08-26 26
本文详细介绍了卡尔曼滤波的基本原理、场景分析以及算法推导过程。通过单变量电路和下落纸团两个示例，分析了卡尔曼滤波在处理状态变量估计问题时的建模方法与不确定性处理机制。同时，推导了卡尔曼滤波的核心步骤，包括预测阶段和更新阶段的具体公式，并总结了其在导航、机器人、信号处理等领域的广泛应用与优势。
17、集合卡尔曼滤波算法详解
coffee的博客
08-30 54
本文详细介绍了扩展卡尔曼滤波器（EKF）和集合卡尔曼滤波器（EnKF）的原理、算法步骤及其在非线性系统状态估计中的应用。EKF通过泰勒级数近似处理非线性，适用于低维弱非线性系统；而EnKF通过维护状态估计集合避免高维协方差计算，更适合高维强非线性场景如天气预报和油井控制。文章还比较了两种滤波器的计算复杂度、精度与适用场景，并探讨了协方差膨胀、自适应噪声调整等优化方法，结合无人机定位案例分析其实际表现，为相关领域提供了理论支持与实践指导。
31、数据拟合与卡尔曼滤波方法详解
最新发布
star5的专栏
11-13 19
本文详细介绍了多种数据拟合方法，包括通用函数拟合、非线性最小二乘回归和数据变换，并深入探讨了卡尔曼滤波算法的原理及其在抛射体运动、导航系统、机器人和计算机视觉等领域的应用。文章还比较了不同拟合方法的优缺点，分析了卡尔曼滤波在实际应用中的挑战，如模型构建、噪声估计和初始值设定，最后展望了未来在数据分析与状态估计领域的发展方向。
30、数据拟合与卡尔曼滤波技术详解
qqq44的博客
11-13 20
本文详细介绍了数据拟合与卡尔曼滤波技术在数据分析中的应用。内容涵盖通用函数拟合、非线性最小二乘回归和数据转换等拟合方法，并通过MATLAB实例展示其实现过程与效果对比。同时深入讲解了卡尔曼滤波的原理、实现流程及其在抛射体跟踪中的应用，探讨了实际应用中面临的噪声估计和初始值设定等挑战及应对策略。最后对不同拟合方法进行了比较，提供了选择建议，并展望了未来发展方向。
概率机器人（3）-卡尔曼滤波
qq_36450426的博客
11-08 437
       卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包含系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。        卡尔曼用矩参数表示置信度：在时间,置信度用均值和方差表达。除了贝叶斯滤波的马尔可夫假设之外，还具有其他的三个特征，则后验就是高斯的。马尔可夫假设设定过去和未来数据都是独立的，即当前时刻的状态至于上一...
《probabilistic robotics》学习笔记——卡尔曼滤波
m0_54358435的博客
08-01 1082
《probabilistic robotics》学习笔记——卡尔曼滤波
概率机器人总结——(扩展)卡尔曼滤波先实践再推导
weixin_44580210的博客
05-22 2417
概率机器人总结——卡尔曼滤波先实践再推导概率机器人总结——(扩展)卡尔曼滤波先实践再推导卡尔曼、扩展卡尔曼、粒子滤波到底什么关系？扩展卡尔曼滤波的实践卡尔曼滤波的推导 概率机器人总结——(扩展)卡尔曼滤波先实践再推导 为什么要把扩展两个字加个括号呢，因为本文的实践过程是以扩展卡尔曼为例，但是推导过程主要是参考博客卡尔曼滤波 – 从推导到应用(一)，相较于《概率机器人》上的推导过程更容易理解，而《概...
卡尔曼滤波
JunJun
05-11 6876
卡尔曼滤波适用于线性高斯系统，其基于马尔可夫假设，认为当前状态只与上次的状态相关（与之相反的是非线性优化，其认为当前状态与过去的所有状态相关），能够根据多个传感器的测量数据和当前的运动学模型估计出当前机器人的最优状态。
kalman简单例子——初始化参数对kalman性能的影响
hello world
03-07 2378
参考：
概率机器人_阅读3_c6感知概率模型
chepwavege的专栏
08-01 708
REF: 书本教材：http://probabilistic-robotics.informatik.uni-freiburg.de/ppt/ 读书笔记：https://blog.youkuaiyun.com/daqianc/article/category/8250671 Abbr: TBD 牢记 重点 感知：（中文版page130，英文P149） 所有测量值都是不准确的。所有测量都...
Kalman滤波参数、调整原则
热门推荐
weixin_45834800的博客
02-14 2万+
卡尔曼滤波参数调整原则
Kalman滤波中状态方程初始化参数设置
曾记否@
01-31 5065
这个矩阵是卡尔曼滤波中比较难确定的一个量，一般有两种思路：一是在某些稳定的过程可以假定它是固定的矩阵，通过寻找最优的Q值使滤波器获得更好的性能，这是调整滤波器参数的主要手段，Q一般是对角阵，且对角线上的值很小，便于快速收敛；对于可观测性较强的状态分量，对应的状态初值和均方误差阵设置偏差容许适当大些，它们随着滤波更新将会快速收敛，如果均方误差阵设置太小，则会使收敛速度变慢。：表示状态转移矩阵，是n×n阶方阵，它是算法对状态变量进行预测的依据，状态转移矩阵如果不符合目标模型有可能导致滤波发散。
卡尔曼滤波确定初值x0与p0？——KF第二篇笔记
985工科博士毕业，专攻定位、导航和滤波等算法研究。从业10年，主要使用MATLAB
11-24 6357
这是关于卡尔曼滤波系列的第二篇笔记。笔记内容不按照算法学习的顺序来，所以这不是一个教程，而是对待某一特定问题的解决方法探讨。 所有笔记放在卡尔曼专栏里面。欢迎私信或email:850051568@qq.com讨论。 初值x0x_0x0​和P0P_0P0​ 这里说的初值，仅包括状态量的初值x0x_0x0​和协方差矩阵P0P_0P0​，R和Q其实严格来说不算是初值，关于R和Q的取值在这里：https://blog.youkuaiyun.com/callmeup/article/details/121489224 这里的初值
好的，我们已经做出了关键的战略选择：采用策略一，专注于在“理想化检测”的基础上，开发核心的传感器融合与轨迹预测算法。 这是您整个项目的“下半场”，也是最能体现您论文核心贡献的部分。现在，我们将搭建整个系统的“大脑”——Perception_Algorithm 模块。 下面，我将为您详细阐述构建这个模块的思路和步骤。 核心目标：创建一个“贝叶斯大脑” 这个 Perception_Algorithm 模块的核心是一个状态估计器。它的职责是： 输入: 来自 Robot_Platform_Simulator 的、带噪声的、异步的、多模态的传感器“观测数据” (imu_out, camera_out, lidar_out)。 输出: 对球体在世界坐标系下运动状态的最优估计 (Estimated_Ball_State)，以及对这个估计的置信度 (Covariance)。 我们将使用扩展卡尔曼滤波器 (Extended Kalman Filter, EKF) 作为这个“大脑”的底层算法框架。整个 Perception_Algorithm 模块的内部工作，就是一个不断循环的EKF流程。 第一步：创建 Perception_Algorithm 的框架 在您的项目中，创建一个新的 matlab.System 对象文件，命名为 Perception_Algorithm.m。 1. 定义输入 (Inputs): imu_out: 包含角速度和线加速度的结构体。 camera_out: 包含2D检测框、深度和可见性标志的结构体。 lidar_out: 包含点云和检测标志的结构体。 robot_pose: 这是至关重要的新增输入！ 我们的感知算法需要知道机器人当前的位姿，才能进行坐标变换。在初期，我们将直接使用来自Robot_Platform_Simulator的真值 robot_pose_out。 2. 定义输出 (Outputs): estimated_position: [3x1] 向量，球的估计位置。 estimated_velocity: [3x1] 向量，球的估计速度。 covariance: [6x6] 矩阵，描述了对位置和速度估计的不确定性。 is_initialized: 一个布尔标志，表示滤波器是否已经成功初始化。 3. 定义内部状态 (DiscreteState): State: [6x1] 列向量 [x; y; z; vx; vy; vz]。这是EKF的核心——我们对球体状态的当前最佳估计。 Covariance: [6x6] 矩阵 P。 IsInitialized: 布尔值，初始为false。 last_update_time: 记录上一次EKF循环的时间戳。 第二步：实现 EKF 的初始化逻辑 (setupImpl, resetImpl) 滤波器不能凭空开始工作，它需要一个“第一推动力”。 思路: 滤波器保持“未初始化”状态 (IsInitialized = false)，直到它收到第一次有效的外部观测数据（来自相机或LiDAR）。 使用这第一次测量，来设定状态向量 State 和协方差 Covariance 的初始值。 resetImpl 方法: 将 IsInitialized 设为 false。 将 State 设为零或一个预设的默认值。 将 Covariance 设为一个非常大的对角矩阵。这表示“在开始时，我对球的状态一无所知，不确定性极大”。 第三步：实现 EKF 的核心循环 (stepImpl) 这是整个算法的心脏。每一次 stepImpl 的调用，都代表着一次完整的“预测-更新”循环。 stepImpl 的内部逻辑流程: 计算时间差 dt: 用当前时间减去 last_update_time，得到时间间隔dt。这是所有物理模型积分的基础。 处理初始化: 检查 IsInitialized 是否为 false: 如果是，则进入“初始化模式”。 检查 camera_out 或 lidar_out 是否有效 (Is_Ball_Visible 或 Is_Ball_Detected 为 true)。 如果收到有效数据： a. 处理测量: 调用一个辅助函数，将相机或LiDAR的原始读数（在传感器坐标系下）转换到世界坐标系，得到一个初始的位置测量 Z_pos。（这个转换过程需要用到输入的 robot_pose）。 b. 初始化状态: State(1:3) = Z_pos。 State(4:6) = [0; 0; 0] (速度初始假定为零)。 c. 初始化协方差: 将 Covariance 设为一个较小的初始值，表示“我对这个初始位置比较有信心”。 d. 设置标志: IsInitialized = true。 e. 更新时间戳: last_update_time = current_time。 f. 提前返回: 本次循环只做初始化，不做预测和更新。 如果没有收到有效数据，则什么都不做，直接返回。 预测步骤 (Prediction Step) - “我猜下一刻在哪”: 如果 IsInitialized 为 true: 应用运动模型: 根据我们之前为BallDynamicsEngine设计的恒定速度+重力模型，来更新State和Covariance。 State_predicted = F * State_old + G * u (其中F是状态转移矩阵，G*u是重力项) Covariance_predicted = F * Covariance_old * F' + Q (Q是过程噪声协方差) IMU的作用: 在更高级的模型中，IMU测量的机器人加速度可以被认为是外部输入，影响球的“相对运动”，但这会使模型非常复杂。 在当前阶段，IMU的主要作用是为独立的机器人位姿估计器提供数据。在我们的球体跟踪EKF中，可以暂时不直接使用IMU读数。 更新步骤 (Update Step) - “用测量来修正猜测”: 依次检查每个传感器的数据: 如果收到有效的相机数据 (camera_out.Is_Ball_Visible == true): a. 处理测量: 调用辅助函数，将相机数据 [bbox, depth] 转换到世界坐标系下的位置测量 Z_camera。 b. 计算观测噪声 R_camera: 设计一个函数，根据depth值来动态计算一个 3x3 的协方差矩阵 R_camera。距离越远，R越大。 c. 执行EKF更新: 调用一个ekf_update辅助函数，输入 State_predicted, Covariance_predicted, Z_camera, R_camera，输出更新后的 State_updated, Covariance_updated。 如果收到有效的LiDAR数据 (lidar_out.Is_Ball_Detected == true): a. 处理测量: 调用辅助函数，将LiDAR点云 point_cloud 的中心点转换到世界坐标系下的位置测量 Z_lidar。 b. 计算观测噪声 R_lidar: LiDAR的噪声R_lidar可以设为一个较小的固定值，或与距离轻微相关。 c. 执行EKF更新: 再次调用 ekf_update 函数，但这次使用LiDAR的测量和噪声模型。如果之前已经被相机更新过，就在相机更新后的结果上再次更新。 最终化: 将最终更新后的 State_updated 和 Covariance_updated 写回到 obj.State 和 obj.Covariance。 更新时间戳 last_update_time = current_time。 将 obj.State 和 obj.Covariance 作为模块的输出。 第四步：设计辅助函数 为了让 stepImpl 保持清晰，您需要创建几个关键的内部辅助方法 (private methods)： measurement_camera_to_world(camera_out, robot_pose): 负责将相机原始读数转换成世界系下的位置坐标。 measurement_lidar_to_world(lidar_out, robot_pose): 负责将LiDAR原始读数转换成世界系下的位置坐标。 calculate_camera_noise(depth): 负责动态计算相机的观测噪声协方差矩阵 R。 ekf_update(state_pred, cov_pred, measurement, measurement_noise): 封装了标准的卡尔曼滤波更新公式。 总结：您的感知算法开发路线图 创建 Perception_Algorithm.m 框架，定义好输入、输出和内部状态。 实现初始化逻辑，让滤波器可以从第一次有效的测量中“冷启动”。 实现预测步骤，使用恒定速度+重力模型来推断球的运动。 实现更新步骤，这是核心。分别为相机和LiDAR编写“处理测量 -> 计算噪声 -> 执行更新”的逻辑链。 搭建Simulink模型: 将 Robot_Platform_Simulator 的所有传感器输出和机器人位姿输出，连接到 Perception_Algorithm 模块的输入。 将 Perception_Algorithm 的输出 estimated_position 和 BallDynamicsEngine 的真值 position 同时连接到 Scope 或 To Workspace。 运行与调试: 运行仿真，对比估计轨迹和真实轨迹，观察误差。通过调整过程噪声Q和观测噪声R（这被称为滤波器调参 Tuning），来让估计的轨迹尽可能地逼近真实轨迹。 完成以上步骤，您就拥有了一个功能完备、基于EKF的多传感器融合感知系统，这将是您论文中技术含量最高、最核心的贡献之一。 这个思路是否合理？？？？暂时不需要写代码，帮我分析一下
11-13
% 优先使用LiDAR初始化（精度更高） if lidar_data.Is_Ball_Detected pos = transform_lidar_to_world(lidar_data.BallPosition, robot_pose); obj.State(1:3) = pos; obj.Covariance = diag([0.1, 0.1, 0.1, 1...
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
表情包 代码片
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

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

余额充值