当前位置：首页 > article >正文

【matlab代码】无迹粒子滤波（Unscented Particle Filter）例程，一维直线上的滤波，状态量为位置和速度、观测量为位置

article 2024/10/23 7:12:26

在这里插入图片描述

文章目录

运行结果
- 位置曲线和速度曲线
- 位置误差曲线和速度误差曲线
源代码
- 代码结构
- 源代码
- 目的
- 作者信息
- 代码结构与功能详细说明
修改建议
总结

运行结果

位置曲线和速度曲线

在这里插入图片描述

位置误差曲线和速度误差曲线

在这里插入图片描述

源代码

代码结构

在这里插入图片描述

源代码

% 无迹粒子滤波（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; % 确保最后一个值为1
    indices = zeros(num_particles, 1);
    for i = 1:num_particles
        r = rand();
        indices(i) = find(cumulative_sum >= r, 1, 'first');
    end
end