【Sage Husa EKF代码】一维非线性状态、非线性观测的Sage Husa自适应EKF,MATLAB代码|订阅专栏后,可直接查看源代码

在这里插入图片描述

使用Sage Husa方法,给EKF添加未知Q和R情况下对QR的自适应。使用一维的状态量和观测量为基准,对比了滤波前、标准EKF和本文的的Sage Husa自适应EKF的误差。
订阅专栏后,可直接查看源代码,粘贴到MATLAB空脚本中即可直接运行

运行结果

状态对比曲线:
在这里插入图片描述
误差对比曲线:
在这里插入图片描述
命令行输出的误差统计特性:
在这里插入图片描述

MATLAB源代码

完整代码如下:

% 一维非线性状态、非线性观测的Sage Husa自适应EKF例程
% 作者:matlabfilter
% 2025-09-28/Ver1

clear; %清空工作区变量
clc;
<think> 我们被要求回答关于Sage-Husa自适应滤波的原理、应用和实现方法。根据之前的对话历史和站内引用,我们可以整理如下: ### 原理 Sage-Husa自适应滤波是一种扩展的卡尔曼滤波算法,用于处理系统噪声统计特性(均值和协方差)未知或时变的情况。其核心思想是通过在线估计噪声的统计特性(主要是观测噪声协方差$R_k$和过程噪声协方差$Q_k$),来调整卡尔曼滤波器的参数,从而提高滤波器在不确定环境中的鲁棒性。 主要原理包括: 1. **噪声统计特性的在线估计**:在标准卡尔曼滤波的基础上,利用历史观测数据(残差序列)来估计当前时刻的噪声统计特性。例如,观测噪声协方差$R_k$的估计公式为: $$\hat{R}_k = (1 - d_k) \hat{R}_{k-1} + d_k \left( r_k r_k^T - H_k P_{k|k-1} H_k^T \right)$$ 其中,$r_k = z_k - H_k \hat{x}_{k|k-1}$是预测残差,$d_k$是衰减因子(通常取$d_k = 1/k$或一个接近1的常数,如0.95~0.99)[^2][^3][^4]。 2. **渐消记忆指数加权**:为了避免历史数据权重过大导致自适应能力下降,采用指数渐消记忆的方法,即给历史数据赋予指数衰减的权重。衰减因子$d_k$的选择是关键,通常取固定值(如0.99)或随时间衰减($d_k = (1-b)/(1-b^k)$,其中$b$为遗忘因子,$0<b<1$)[^3][^4]。 3. **处理噪声均值非零**:如果噪声均值非零,可以通过状态增广(将噪声均值作为状态的一部分)来估计[^3]。 4. **稳定性保障**:由于自适应滤波引入了非线性(噪声协方差的估计依赖于观测值),可能导致滤波发散。因此,常采用以下策略: - 对估计的协方差矩阵进行正定性约束(如$R_k$不能小于某个阈值)[^3]。 - 采用序贯处理(逐个处理观测值)结合方差受限方法[^3]。 - 限制自适应参数的数量,优先自适应量测噪声(因为量测噪声通常更容易估计)[^3][^4]。 ### 应用 Sage-Husa自适应滤波适用于以下场景: - **系统噪声统计特性未知**:当无法事先准确获得$Q_k$和$R_k$时。 - **噪声时变**:如传感器噪声随环境变化(温度、湿度等)或目标运动模式改变(如机动目标跟踪)。 - **具体应用领域**: - 组合导航(如GPS/INS):当GPS信号受多路径效应影响时,观测噪声协方差会变化,自适应滤波可调整$R_k$以提高定位精度[^1]。 - 目标跟踪:在雷达或声呐系统中,目标机动或环境干扰导致过程噪声$Q_k$变化,自适应滤波能实时调整模型参数。 - 三维非线性系统状态估计:通过结合扩展卡尔曼滤波EKF)或无迹卡尔曼滤波(UKF),处理非线性系统的自适应滤波问题(如MATLAB示例中的三维非线性系统)[^1]。 ### 实现方法 1. **基本步骤**:在标准卡尔曼滤波的预测和更新步骤基础上,增加噪声协方差更新步骤: - **预测**: $$\hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1}$$ $$P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_{k-1}$$ - **更新**: $$r_k = z_k - H_k \hat{x}_{k|k-1}$$ $$S_k = H_k P_{k|k-1} H_k^T + R_k$$ $$K_k = P_{k|k-1} H_k^T S_k^{-1}$$ $$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k r_k$$ $$P_{k|k} = (I - K_k H_k) P_{k|k-1}$$ - **自适应更新**(以$R_k$为例): $$\hat{R}_k = (1 - d_k) \hat{R}_{k-1} + d_k \left( r_k r_k^T - H_k P_{k|k-1} H_k^T \right)$$ 然后对$\hat{R}_k$进行正定性修正(如对角线元素取绝对值,或设下限)。 2. **MATLAB示例**(基于引用[1]的三维EKF): ```matlab % 初始化 x_est = [0;0;0]; % 初始状态估计 P_est = eye(3); % 初始协方差矩阵 R = eye(3); % 初始观测噪声协方差(待自适应) Q = diag([0.1,0.1,0.1]); % 过程噪声协方差(可固定或自适应) d = 0.99; % 衰减因子 for k = 1:N % 预测步骤(使用非线性状态方程f和雅可比F_jac) x_pred = f(x_est); % 状态预测 F = F_jac(x_est); % 计算雅可比矩阵 P_pred = F * P_est * F' + Q; % 观测预测(使用非线性观测方程h和雅可比H_jac) z_pred = h(x_pred); H = H_jac(x_pred); % 实际观测值z_k r = z_k - z_pred; % 残差 % 更新步骤 S = H * P_pred * H' + R; K = P_pred * H' / S; x_est = x_pred + K * r; P_est = (eye(3) - K * H) * P_pred; % Sage-Husa自适应更新R R_adapt = (1-d)*R + d*(r*r' - H*P_pred*H'); % 确保正定性:对角线元素非负 R = diag(max(diag(R_adapt), 0.1)); end ``` 3. **注意事项**: - **计算复杂度**:对于高维系统,在线估计$Q_k$和$R_k$会增加计算负担,可能影响实时性[^2]。 - **稳定性**:当$r_k r_k^T < H_k P_{k|k-1} H_k^T$时,$R_k$可能非正定,需强制正定(如对角线加小常数)[^3][^4]。 - **参数选择**:衰减因子$d_k$需权衡历史记忆与当前信息($d_k$大则记忆长,自适应慢;$d_k$小则对新数据敏感,但波动大)[^3][^4]。 ### 总结 Sage-Husa自适应滤波通过在线估计噪声协方差,增强了卡尔曼滤波在不确定环境中的鲁棒性。其核心是渐消记忆加权和残差统计,适用于导航、跟踪等领域。实现时需注意计算复杂度、正定性约束和参数选择[^1][^2][^3][^4]。 §§相关问题§§ 1. Sage-Husa自适应滤波中衰减因子$d_k$如何选择? 2. 如何解决Sage-Husa自适应滤波中协方差矩阵可能出现的非正定问题? 3. Sage-Husa自适应滤波在组合导航中的具体应用案例有哪些? 4. 对于高维系统,如何降低Sage-Husa自适应滤波的计算复杂度? 5. Sage-Husa自适应滤波与变分贝叶斯自适应滤波有何区别? </think> ### Sage-Husa自适应滤波:原理、应用与实现方法 #### 1. **基本原理** Sage-Husa自适应滤波卡尔曼滤波的扩展,核心思想是通过**在线估计噪声统计特性**(主要是观测噪声协方差$R_k$和过程噪声协方差$Q_k$)来适应系统不确定性。其工作流程如下: 1. **状态预测与残差计算** 与传统卡尔曼滤波相同: $$ \begin{align*} \hat{x}_{k|k-1} &= F_k \hat{x}_{k-1|k-1} \\ P_{k|k-1} &= F_k P_{k-1|k-1} F_k^T + Q_k \end{align*} $$ 残差:$r_k = z_k - H_k \hat{x}_{k|k-1}$ 2. **噪声协方差在线估计** 采用**渐消记忆指数加权**更新$R_k$: $$ \hat{R}_k = (1 - d_k) \hat{R}_{k-1} + d_k \left( r_k r_k^T - H_k P_{k|k-1} H_k^T \right) $$ 其中$d_k$为**衰减因子**(通常取$0.95 \sim 0.99$),通过降低旧数据权重增强对当前噪声的适应性[^3][^4]。 3. **状态更新** 使用自适应$R_k$计算卡尔曼增益: $$ \begin{align*} K_k &= P_{k|k-1} H_k^T \left( H_k P_{k|k-1} H_k^T + \hat{R}_k \right)^{-1} \\ \hat{x}_{k|k} &= \hat{x}_{k|k-1} + K_k r_k \\ P_{k|k} &= (I - K_k H_k) P_{k|k-1} \end{align*} $$ **核心优势**: - 处理噪声统计特性未知或时变的系统 - 对观测噪声变化更敏感(因量测噪声方差$R_k$更易估计)[^3] - 通过状态增广可处理非零均值噪声[^4] #### 2. **关键技术与挑战** 1. **衰减因子选择** - 取值原则:接近1(如0.95~0.99) - 权衡:$d_k$越大 → 历史数据权重越高(稳定性好但自适应慢);$d_k$越小 → 对新数据敏感(易波动)[^3] - 经验取值:工程中常取$d_k=0.99$[^3] 2. **数值稳定性保障** - **问题**:当$r_k r_k^T < H_k P_{k|k-1} H_k^T$时,$\hat{R}_k$可能非正定 - **解决方案**: - 方差受限:设下限$\hat{R}_k = \max(\hat{R}_k, R_{\min})$ - 序贯量测处理:逐维度更新避免矩阵负定[^3] - 正则化:添加小扰动项确保正定性 3. **计算复杂度** - 复杂度:$O(n^3)$($n$为状态维度) - 高维系统优化:只自适应部分噪声参数(优先$R_k$),或采用分块矩阵运算[^2] #### 3. **典型应用场景** 1. **组合导航系统** - 场景:GPS/INS组合导航中处理城市峡谷效应导致的GPS噪声突变 - 效果:自适应调整$R_k$,定位误差降低40%+[^1] 2. **目标跟踪** - 场景:雷达跟踪机动目标时过程噪声$Q_k$变化 - 实现:联合自适应$Q_k$和$R_k$(需增加计算量) 3. **三维非线性系统估计** - 扩展形式:Sage-Husa自适应EKF(扩展卡尔曼滤波) ```matlab % MATLAB代码(三维EKF+Sage-Husa) for k = 1:N [x_pred, F_jac] = nonlinear_state_model(x_est); % 状态预测 P_pred = F_jac * P_est * F_jac' + Q; [z_pred, H_jac] = nonlinear_obs_model(x_pred); % 观测预测 r = z_k - z_pred; % 残差 R_adapt = (1-d)*R + d*(r*r' - H_jac*P_pred*H_jac'); R = max(R_adapt, R_min); % 方差受限 K = P_pred * H_jac' / (H_jac*P_pred*H_jac' + R); % 增益计算 x_est = x_pred + K*r; % 状态更新 P_est = (I - K*H_jac)*P_pred; end ``` *代码来源:引用[1]的MATLAB实现* #### 4. **工程实践建议** 1. **参数初始化** - $R_0$设置:略大于传感器标称噪声方差 - $d_k$选择:从0.95开始调试,根据残差收敛性调整 2. **稳定性增强策略** - 冷启动阶段:前$N$步(如$N=20$)采用固定$R_k$ - 异常值处理:当$\|r_k\| > 3\sigma$时暂停自适应 3. **适用性限制** - **推荐场景**:噪声统计特性缓变或分段平稳的系统[^4] - **慎用场景**:噪声统计特性剧变或高阶系统(易发散)[^2] #### 5. **与其他方法对比** | **方法** | **计算复杂度** | **实时性** | **适用场景** | |------------------|----------------|------------|---------------------------| | Sage-Husa | 中等 | 较好 | 线性/弱非线性系统 | | 变分贝叶斯自适应 || 较差 |非线性/非高斯系统[^1] | | 遗忘因子滤波 ||| 时变参数系统 | > **总结**:Sage-Husa自适应滤波通过**在线噪声协方差估计**提升了传统卡尔曼滤波的鲁棒性,在导航、跟踪等领域应用广泛。其实现关键在于**衰减因子选择**和**数值稳定性保障**,对中低维系统效果显著,但高维实时场景需谨慎使用[^2][^4]。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

打赏作者

MATLAB卡尔曼

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

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

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

打赏作者

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

抵扣说明:

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

余额充值