【MATLAB代码】基于扩展卡尔曼滤波（Extended Kalman Filter, EKF）的 SAGE-HUSA 滤波【自适应R】

原创 已于 2025-09-28 20:07:39 修改 · 466 阅读 · 3 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#matlab #开发语言

于 2024-12-04 07:39:14 首次发布

在这里插入图片描述

本文给出一个MATLAB代码，基于扩展卡尔曼滤波器（EKF）的Sage-Husa滤波算法编写，主要用于对非线性系统状态的估计。以下是代码的详细介绍，包括主要功能、实现步骤及其应用场景。

文章目录

代码运行结果

估计值对比：
在这里插入图片描述

估计误差对比：

在这里插入图片描述

绝对误差的对比曲线：

在这里插入图片描述

误差平均值输出：

在这里插入图片描述

源代码

下面的源代码复制粘贴到MATLAB空脚本中，即可运行：

% 基于EKF的sage-husa滤波例程
=% 2024-11-12/Ver1
了解本专栏 订阅专栏 解锁全文
【老生谈算法】matlab实现卡尔曼滤波算法源码——卡尔曼滤波
m0_53407570的博客
06-23 3197
卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。卡尔曼滤波中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。卡尔曼几个重要公
自适应卡尔曼滤波的噪声方差怎么推的_一种基于观测噪声方差未知的自适应卡尔曼滤波方法与流程...
weixin_36261385的博客
01-17 2047
本发明属于线性系统的目标跟踪领域，涉及过程噪声方差未知的自适应卡尔曼滤波方法。背景技术：卡尔曼滤波算法自诞生以来，便在诸多领域得到广泛的应用。与维纳滤波相比，kalman滤波能够解决了非平稳随机信号或状态的最优估计问题。但是在非线性系统中就不能直接使用kalman滤波。通常人们使用扩展kalman滤波，即对非线性方程做泰勒级数展开，以进行线性化近似。在线性化误差较小的系统中使用ekf具有较高的估计...
Sage-Husa滤波器以及matlab仿真
qq_48082931的博客
04-11 969
Sage-Husa滤波器如同EKF滤波器一样，利用非线性函数的局部线性化特性，将非线性模型局部线性化，在线性化的系统中，状态转移矩阵Ф(k|k-1)和观测矩阵H(k-1)分别是有f和h的雅可比矩阵代替。Sage-Husa滤波器是一种经典的滤波器设计，它是由Charles A. Sage和Alfred M. Husa于1959年提出的。这个算法主要针对的是不确定的系统噪声协方差Q和观测噪声协方差R，并且在后续的迭代中会持续的对于Q和R矩阵进行更新。
Sage-Husa滤波器的主要参数调节方法【经验，无理论推导】
985工科博士毕业，专攻定位、导航和滤波等算法研究。从业10年，主要使用MATLAB
12-04 1042
功能：alpha 值在滤波更新中决定了新观测值对当前状态估计的权重。较大的 alpha 值意味着对新数据的响应更敏感，而较小的 alpha 值则意味着对新数据的响应较慢。调节 Sage-Husa 滤波器的参数，包括过程噪声协方差矩阵QQQ、观测噪声协方差矩阵RRR和 alpha 值，是提升滤波性能的关键步骤。通过实施初始校准，可以有效减少由于初始参数设置不当造成的影响。结合数据驱动校准与模型校准的方法，能够提高系统的准确性和鲁棒性。
MATLAB编写的EKF和UKF滤波程序源代码 扩展卡尔曼滤波、无迹卡尔曼滤波的MATLAB程序
最新发布
2504_94312458的博客
11-29 418
这个文件里塞了扩展卡尔曼滤波和无迹卡尔曼滤波的完整实现，还自带三维轨迹误差对比图。方位角观测的非线性特性在物体靠近原点时会急剧变化，这时候UKF的泰勒展开阶数更高，优势就出来了。这时候EKF的轨迹估计会像醉汉走路，UKF虽然也晃但还能保持大体方向。误差曲线图上UKF的蓝色线条明显更贴近零点，但在第25秒左右有个突然的抖动。，这模拟了现实中的运动阻力。当物体速度越快，减速效果越明显，这种非线性特性正是考验滤波器的关键。扩展卡尔曼滤波、无迹卡尔曼滤波的MATLAB程序，有误差对比图像和最大误差数字的显示。
扩展卡尔曼滤波代码
03-05
扩展卡尔曼滤波代码，详细的matlab代码
扩展卡尔曼滤波MATLAB代码
01-30
扩展卡尔曼滤波MATLAB代码 % UKF Unscented Kalman Filter for nonlinear dynamic systems % [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P
扩展卡尔曼滤波项目C++代码
03-14
本资源提供了一个自动驾驶汽车程序启动扩展卡尔曼滤波项目C++代码。所谓扩展卡尔曼滤波器，就是适用于非线性系统的卡尔曼滤波器，可以更广泛的应用在项目中。
拓展卡尔曼滤波（Kalman）附Matlab代码
matlab_dingdang的博客
11-18 2577
根据传统的扩展卡尔曼厚度算法在多普勒测量目标情况下估计精度低的，提出了扩展卡尔曼厚度跟踪优化算法。估计量测的扩展卡算法推广到包含普勒量测的提高目标跟踪位置精确度。仿真结果主动，算法以多方均方根的精度和方根的速度精度，可以很容易地实现精度良好地提高目标跟踪的精确度，可有效持续跟踪目标中的情况。
【信号处理】扩展卡尔曼滤波EKF（Matlab代码实现）
weixin_46039719的博客
11-19 2421
为解决传统EKF算法中其状态向量维度过大导致该算法运行效率变慢、识别精度下降等问题，本文提出了一种改进的EKF算法，引入模态坐标变换，利用模态坐标转换对传统EKF的状态向量进行处理，构建以模态坐标初始值和结构损伤参数的状态向量。[2]胡文强,胡建鹏,吴飞,陆雯霞.基于EKF的初始状态自适应室内融合定位算法[J].传感器与微系统,2022,41(11):147-151.DOI:10.13873/J.1000-9787(2022)11-0147-05.%x方向的过程噪声向量//相当于Q。行百里者，半于九十。
精选资源
MATLAB 代码实现了基于扩展卡尔曼滤波（Extended Kalman Filter, EKF）的 SAGE-HUSA 滤波
11-12
MATLAB代码，实现了基于扩展卡尔曼滤波（Extended Kalman Filter, EKF）的SAGE-HUSA滤波算法，用于估计一个非线性动态系统的状态。该程序通过模拟一个简单的非线性系统，比较了普通 EKF 和SAGE-HUSA EKF在状态估计中...
数据融合matlab代码-Extended-Karman-Filter:扩展卡尔曼滤波器
05-22
数据融合matlab代码扩展卡曼滤波器 郑磊2018年1月 这是一个根据激光雷达数据和雷达数据估算汽车位置的项目。 激光雷达数据使用卡尔曼滤波器。 雷达数据使用扩展卡曼滤波器，因为雷达数据是由非线性数据（rho，rho-dot，phi）提供的。 然后，我们将两种类型的传感器数据放在一起，以获得更准确的估计。 解决方案就是所谓的传感器融合。 以下gif图片是从模拟器记录的。 蓝色圆圈：雷达测量 红圈：激光测量 绿色圆圈：EKF估计位置 1，为carnd-term创建环境2 我没有Mac，并且可以正常使用的PC正在安装Windows7 所以，这是选项 Windows7的专业+ vmware（ubuntu）的，不起作用 Windows7的专业+码头工人，无法正常工作 纯的ubuntu，它的工作原理 所以，这是建议，如果您使用的是Windows7。 请勿尝试任何操作。 这会浪费您的时间。 购买计算机或借用计算机，然后安装ubuntu （版本16.4使用） 2.在空的计算机上使用Simulator运行代码 2.1安装ubuntu （前提条件：您已经有另一台Windows PC） 在Windows
扩展卡尔曼滤波python代码
11-19
某物体在XY平面做运动，采样周期为1s，该运动系统的状态方程如式 (2-1) 所示， 其中,为系统的状态向量，各状态变量对应地分别表示方向的位置、方向速度、方向的位置、方向的速度。 为零均值高斯白噪声, 。 采用方位角传感器测量运动系统的方位角，作为系统的输出。系统的输出方程如式（2-2）所示： 其中 是零均值高斯白噪声，。 假设系统的初始状态, ，=0.02。 试利用扩展卡尔曼滤波理论求出的最优估计。 要求： （1）利用Matlab或Python 编写仿真程序。 （2）给出各状态变量的真值和估计值曲线变化图。 （3）分别给出的真值与估计值之间的误差曲线变化图，并求出误差的均值和方差。 （4）对滤波效果进行分析。
扩展卡尔曼滤波代码和数据
10-26
基于扩展卡尔曼滤波的车辆追踪项目，C++实现，CTRV模型，激光雷达和雷达传感器融合，详情见博客http://blog.youkuaiyun.com/adamshan/article/details/78359048
扩展卡尔曼滤波器的matlab代码
03-06
扩展卡尔曼滤波器的matlab仿真。希望能对大家有所帮助。
EKF扩展卡尔曼滤波器代码实现
03-31
以匀速直线运动为例，设计了基于距离的目标跟踪算法，即状态量为X、Y轴的位置和速度，观测值为物体到观测站的距离，具体实现过程见代码
精选资源
AKF-master_sage_husa_sagehusa_sage_sage-husa_sagekalman.zip
10-10
AKF 可能包含了扩展 Kalman 滤波（Extended Kalman Filter, EKF）或无迹卡尔曼滤波（Unscented Kalman Filter, UKF）等技术，这些技术在处理非线性问题时比标准 Kalman 滤波器更为有效。 在源码库中，我们可能会...
扩展卡尔曼滤波算法Python代码
Scarlett的博客
06-16 1883
# 生成汽车行驶的真实数据 # 汽车从以初速度v0，加速度a行驶10秒钟，然后匀速行驶20秒 # x0:initial distance, m # v0:initial velocity, m/s # a:acceleration，m/s^2 # t1:加速行驶时间，s # t2:匀速行驶时间，s # dt:interval time, s import numbers import numpy as np import matplotlib.pyplot as plt import math def
扩展卡尔曼滤波_扩展卡尔曼滤波(EKF)代码实战
weixin_39833687的博客
12-14 1946
一、本文简介通过一组数据，来实践扩展卡尔曼滤波（EKF）的代码，未来也可能利用这组数据来实践粒子滤波等非线性滤波方法来对比结果。二、环境语言：Julia 1.5第三方包：DataFrames, CSV, LinearAlgebra, Plots, PyPlot三、引用多传感器数据融合及其应用－杨万海－西安电子科技大学出版社机器人学中的状态估计－高翔等译－西安交通大学出版社四、理论理论这部分网上已经...
扩展卡尔曼滤波(EKF)图
12-29
### 关于扩展卡尔曼滤波(EKF)的图表与图形化解释 #### 扩展卡尔曼滤波的工作原理概述 扩展卡尔曼滤波是一种用于处理非线性系统的改进型卡尔曼滤波算法。当面对的是非线性模型时，标准的线性卡尔曼滤波不再适用，因此需要采用EKF来近似解决这个问题[^3]。 #### 图形化展示EKF流程 为了更好地理解EKF的过程，可以通过一系列图像来进行说明： 1. **初始化阶段** 初始化意味着设定初始状态向量\( \mathbf{x}_0 \)，以及对应的协方差矩阵P_0 。这一步骤对于任何类型的卡尔曼滤波都是必要的。此时并没有涉及到具体的测量值或预测操作。 2. **预测步骤** ![Prediction Step](https://miro.medium.com/max/700/1*GZJfUWjKX9QrV8gHkDvY_g.png) 在此期间，利用上一时刻的状态估计和当前控制输入u_k 来预测下一刻的状态\(\hat{\mathbf{x}}_{k|k-1}\) 和相应的不确定性 P_{k|k-1} 。由于系统可能是非线性的，所以这里会应用到雅各比矩阵 J_f ，它是对非线性函数 f() 的局部线性化的结果[^4]。 3. **更新(校正)步骤** ![Update Step](https://miro.medium.com/max/700/1*jnLqBmTtOyNwMhAaFpIcCw.png) 接收到新的观测数据 z_k 后，计算卡尔曼增益 K_k 并据此修正之前的预测得到更精确的状态估值 \( \mathbf{x}_{k|k} \) 及其不确定度 P_{k|k} 。同样地，在这个过程中也会用到另一个雅各比矩阵 H_j 对应于观测模型 h() 的线性化形式。 4. **Sage-Husa 自适应机制（可选）** Sage-Husa 方法进一步增强了传统EKF的功能，特别是在应对变化多端的实际环境中表现更为出色。该方法通过引入自适应参数 alpha 动态调整观测噪声 R_sage ,从而提高滤波精度[^2]。 这些图片展示了如何在一个典型的应用场景下运用EKF完成目标跟踪的任务。每张图都对应着上述提到的一个具体环节，并且清晰地标明了各个变量之间的关系及其演变规律。 ```python import matplotlib.pyplot as plt from filterpy.kalman import ExtendedKalmanFilter # 假设已经定义好了rk对象和其他必要参数... plt.figure(figsize=(10, 6)) for i in range(len(measurements)): # 进行一次完整的EKF循环... # 绘制真实轨迹、测量点位及估计位置 plt.scatter(true_positions[i][0], true_positions[i][1], c='blue', marker='o') plt.plot([true_positions[i][0]], [measurements[i][0]], 'rx') plt.plot([ekf_estimates[i][0]], [ekf_estimates[i][1]], 'go') plt.title('Comparison of True Position vs Measurement and EKF Estimate') plt.xlabel('Position X') plt.ylabel('Position Y') plt.legend(['True Pos.', 'Measurement', 'EKF Estimation']) plt.show() ``` 这段代码片段用来绘制比较实际位置、传感器读数和由EKF得出的位置估算三者之间差异的散点图。注意这里的`true_positions`, `measurements` 和 `ekf_estimates` 是假设存在的列表，分别存储每次迭代的真实坐标、观察到的数据点以及经过EKF处理后的最佳猜测值。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

打赏作者

MATLAB卡尔曼

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

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

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

打赏作者

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

抵扣说明：

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

余额充值