【MATLAB例程】二维非线性系统的自适应粒子滤波（APF），基于SageHusa原理，与标准PF、未滤波值进行对比|附代码下载链接-优快云博客

在这里插入图片描述

本代码实现了一种基于Sage-Husa自适应调节机制的自适应粒子滤波算法，用于解决非线性系统的状态估计问题。该算法结合了传统粒子滤波的蒙特卡洛模拟优势和自适应噪声估计能力，能在系统噪声特性未知或时变的情况下实现准确的状态跟踪

文章目录

程序详解
运行结果
MATLAB源代码

程序详解

算法概述

本代码实现了一种基于Sage-Husa自适应调节机制的自适应粒子滤波算法，用于解决非线性系统的状态估计问题。该算法结合了传统粒子滤波的蒙特卡洛模拟优势和自适应噪声估计能力，能在系统噪声特性未知或时变的情况下实现准确的状态跟踪。

核心特性

双重滤波架构

标准粒子滤波(PF)：作为基准对比，使用固定噪声协方差
自适应粒子滤波(APF)：集成Sage-Husa自适应机制，实时估计和调整噪声参数

自适应机制

过程噪声自适应(Q_sh)：动态调整状态转移过程中的不确定性
观测噪声自适应(R_sh)：实时修正观测模型中的噪声水平
遗忘因子(b=0.95)：平衡历史信息与当前观测的权重

非线性系统建模
系统采用经典的非线性状态空间模型：

状态方程：包含复杂的非线性函数和周期性扰动
观测方程：采用平方关系，体现强非线性特性

技术亮点

粒子滤波核心组件

重要性采样：基于状态转移密度生成候选粒子
权重计算：利用观测似然函数评估粒子质量
系统重采样：有效解决粒子退化问题
有效粒子数监测：动态触发重采样过程

自适应更新策略

基于新息序列实时估计噪声统计特性
采用指数加权平均平滑参数变化
保证协方差矩阵的正定性

性能评估体系

代码提供了全面的性能分析工具：

可视化分析

状态轨迹对比：真实值、滤波估计、未滤波状态的时空演变
误差曲线分析：各维度估计误差的时域特性
累积分布函数(CDF)：误差统计分布的直观展示
绝对误差对比：滤波效果的定量比较

统计指标

最大误差：评估滤波器的鲁棒性
均值误差：反映估计的系统偏差
标准差：衡量估计的精度和稳定性

应用价值

该自适应粒子滤波算法特别适用于：

噪声统计特性未知的非线性系统
时变环境下的状态估计问题
对传统EKF线性化误差敏感的应用场景
需要高精度状态跟踪的工程系统

运行结果

各方法得到的状态对比：
在这里插入图片描述
各维度误差对比：

误差曲线对比：
在这里插入图片描述

误差绝对值对比：
在这里插入图片描述

MATLAB源代码

部分代码如下：

% 自适应粒子滤波（APF）实现，基于Sage-Husa自适应调节QR
% 作者:matlabfilter
% 2025-10-19/Ver1

clear; clc; close all;% 清除变量、命令行和图形窗口
rng(0); % 设置随机数种子

%% 滤波模型初始化
t = 1:1:100;
n = 2;
T = length(t);

Q = 0.1 * diag([1,1]);      % 真正的过程噪声协方差
R = 1 * diag([1,1]);      % 真正的观测噪声协方差
P0 = 0.1 * diag([1,1]); 
% 生成噪声序列
w = 0.1*chol(Q)' * randn(n, T);   % 过程噪声
v = 0.1*chol(R)' * randn(n, T);   % 观测噪声

% 粒子滤波参数
N = 100;                    % 粒子数
X_pf = zeros(n, T);         % 标准PF估计
X_apf = zeros(n, T);        % 自适应PF估计

% 粒子集初始化
particles_pf = zeros(n, N, T);
particles_apf = zeros(n, N, T);
weights_pf = ones(N, T) / N;
weights_apf = ones(N, T) / N;

% Sage-Husa 初始为矩阵形式
Q_sh = Q/2;
R_sh = R/2;

% 遗忘因子
b = 0.95;
eps_min = 1e-6;  % 用于保持正定

% 存储自适应参数
Q_num = zeros(n,n,T); Q_num(:,:,1) = Q;
R_num = zeros(n,n,T); R_num(:,:,1) = R;

%% 运动模型
X = zeros(2, length(t)); % 真实状态
X_ = zeros(2, length(t)); % 带噪声的状态
Z = zeros(n, length(t)); % 观测值