文章目录
- 运行结果
- 位置曲线和速度曲线
- 位置误差曲线和速度误差曲线
- 源代码
- 代码结构
- 源代码
- 目的
- 作者信息
- 代码结构与功能详细说明
- 修改建议
- 总结
运行结果
位置曲线和速度曲线
位置误差曲线和速度误差曲线
源代码
代码结构
源代码
% 无迹粒子滤波（Unscented Particle Filter）例程
% 一维直线上的滤波，状态量为位置和速度、观测量为位置
% V:matlabfilter
% 2024-10-16/Ver1
clear; %清空工作区
clc; %清空命令行
close all; %关闭所有窗口（主窗口除外）
rng(0);
%% 参数设置
num_particles = 200; % 粒子数量
num_steps = 500; % 时间步数
Q = 1*diag([1,1]); %过程噪声协方差
measurement_noise = 1; % 测量噪声
R = 10*diag([1]); %观测噪声协方差%% 初始化粒子状态
particles = zeros(num_particles, 2); % [位置, 速度]
weights = ones(num_particles, 1) / num_particles; % 粒子权重
fprintf('完整代码下载链接：https://gf.bilibili.com/item/detail/1106498012');
目的
本代码实现了一维直线上的无迹粒子滤波（UPF），用于估计状态量为位置和速度的动态系统。通过该例程，用户可以理解无迹粒子滤波的基本原理及其在状态估计中的应用。
作者信息
- 作者: V (matlabfilter)
- 日期: 2024-10-16
- 版本: Ver1
代码结构与功能详细说明
-
初始化部分:
- 环境清理：确保每次运行时环境干净，避免变量干扰。
- 随机种子设置：使用
rng(0)确保每次运行生成相同的随机数序列，便于调试和结果验证。
-
参数设置:
num_particles = 200; % 粒子数量 num_steps = 500; % 时间步数 Q = 1*diag([1,1]); % 过程噪声协方差 measurement_noise = 1; % 测量噪声 R = 10*diag([1]); % 观测噪声协方差- 粒子数量：设定滤波中使用的粒子数量，影响估计精度和计算量。
- 时间步数：定义滤波的迭代次数。
- 噪声参数：设置过程噪声和观测噪声的协方差矩阵，以模拟真实系统中的不确定性。
-
初始化粒子状态:
particles = zeros(num_particles, 2); % [位置, 速度] weights = ones(num_particles, 1) / num_particles; % 初始权重均匀分布- 粒子状态：初始化粒子的位置和速度，维度为2。
- 粒子权重：初始化每个粒子的权重为均匀分布。
-
状态转移函数:
state_transition = @(x) [x(1) + x(2); x(2)] + sqrt(Q)*randn(size(Q,1),1;- 状态转移：定义状态转移函数，结合当前状态更新位置和速度，并加入过程噪声。
-
测量函数:
- 测量函数：定义观测值的生成方式，即根据位置加上观测噪声。
-
初始化真实状态:
- 真实状态：设定初始位置为0，速度为1。
- 状态历史与观测值存储：为后续分析准备数据存储。
-
迭代计算:
- 真实状态生成：在每个时间步更新真实状态。
- 测量值生成：基于真实状态生成观测值。
- 预测步骤：更新每个粒子的状态。
- 更新步骤：根据观测值更新粒子的权重。
- 权重归一化：将所有权重归一化，使其和为1。
- 重采样：根据权重选择粒子，保持有效粒子，提高估计精度。
- 状态估计：通过计算粒子的加权平均来估计当前状态。
-
绘图:
- 状态估计结果可视化：绘制真实状态和估计状态的曲线，便于比较两者的接近程度。
-
误差计算:
- 误差分析：计算位置和速度的估计误差，并输出最大误差，帮助评估滤波效果。
-
误差绘图:
figure; plot(err_pos); xlabel('时间'); ylabel('位置误差');- 误差可视化：绘制位置和速度误差随时间变化的图形，直观展示滤波性能。
-
重采样函数:
function indices = resample(weights, num_particles)cumulative_sum = cumsum(weights);cumulative_sum(end) = 1; % 确保最后一个值为1indices = zeros(num_particles, 1);for i = 1:num_particlesr = rand();indices(i) = find(cumulative_sum >= r, 1, 'first');end end- 重采样算法：实现粒子的重采样过程，通过累积权重选择粒子，确保高权重粒子被多次选择，低权重粒子被淘汰。
修改建议
- 增加粒子数量：加大num_particles
总结
本代码通过实现无迹粒子滤波，展示了如何在一维状态空间中估计动态系统的状态。通过模拟真实状态和噪声，用户可以观察到滤波器在位置和速度估计上的效果，并通过可视化手段深入理解无迹粒子滤波的工作原理和性能。