【MATLAB代码介绍】无迹粒子滤波（Unscented Particle Filter），运动目标滤波定位

程序介绍

程序介绍

本程序实现了一个基于**无迹粒子滤波（Unscented Particle Filter, UPF）**的一维目标状态估计示例，用于对直线运动目标的位置进行滤波与精度分析。系统以一维匀速运动为背景，传感器提供带噪位置观测，滤波器利用无迹变换与粒子采样相结合的方式，对目标状态进行递推估计，并与原始观测结果进行对比。

在目标运动建模中，设目标在离散时刻 (k) 的状态为
$$x_k$$
其状态转移方程可表示为
$$x_k=f(x_{k-1})+w_{k-1}$$
其中，$f(\cdot )$为状态转移函数，$w_{k-1}$为过程噪声，满足
$$w_{k-1}\sim \mathcal{N}(0,Q)$$
程序中目标真实位置按照匀速模型更新，即
$$x_k=x_{k-1}+v\Delta t$$
其中，$v$为目标速度，$\Delta t$为采样周期。

UPF 的核心思想是在粒子滤波框架下引入无迹变换。对于每个粒子，不再简单依靠先验分布进行随机传播，而是先利用当前粒子的均值与协方差构造 Sigma 点，再通过无迹变换获得更优的建议分布，从而提高粒子传播质量。设当前粒子状态维数为 $n$，则第$i$个 Sigma 点可写为
$${\chi }_0=x$$
$${\chi }_i=x+\sqrt{n+\lambda }\left(\sqrt{P}\right)\ast i,i=1,\dots ,n$$
$$\chi \ast i+n=x-\sqrt{n+\lambda }{\left(\sqrt{P}\right)}_i,i=1,\dots ,n$$
其中，$\lambda$为尺度参数，$P$为当前粒子的协方差矩阵。

Sigma 点经状态方程传播后，可得到预测均值与预测协方差：
$$\hat{x}\ast k^{-}=\sum \ast {i=0}^{2n}{w}_i^{(m)}{\chi }_{i,k}^{-}$$
$$P_k^{-}=\sum _{i=0}^{2n}{w}_i^{(c)}\left({\chi }_{i,k}^{-}-\hat{x}\ast k^{-}\right){\left(\chi \ast {i,k}^{-}-{\hat{x}}_k^{-}\right)}^{\mathrm{T}}+Q$$
其中，$w_i^{(m)})和(w_i^{(c)}$分别为均值权重和协方差权重。程序中通过该预测结果为每个粒子重新构造采样分布，并完成粒子更新。

在获得新粒子后，依据当前观测值计算各粒子的似然权重。
在高斯测量噪声条件下，粒子权重按似然函数更新为
$$w_k^{(i)}\propto \exp \left(-\frac{1}{2}{\tilde{y}}_k^{(i)\mathrm{T}}{R}^{-1}{\tilde{y}}_k^{(i)}\right)$$
随后对权重进行归一化。为避免粒子退化，程序进一步采用重采样方法保留高权重粒子，并剔除低权重粒子。

最终，滤波估计值由所有粒子的加权均值或重采样后的样本均值给出。该程序中采用粒子均值作为状态估计

修改建议

• 增加粒子数量：加大num_particles
  

运行结果

滤波前后误差曲线对比

估计误差的曲线

代码结构：

源代码

部分代码

% 无迹粒子滤波（Unscented Particle Filter）例程
% 一维直线上的滤波，状态量为位置和速度、观测量为位置
%% 初始化
clc; clear; close all;
rng(0);

% 时间步长和总时间
dt = 0.1;          % 时间步长 (秒)
T_total = 200;    % 模拟总时间 (秒)
N = T_total / dt;  % 总时间步数

% 目标初始位置和速度
true_pos = 500;  % 目标初始位置 (x)
true_vel = 1;      % 目标速度向量 (vx)

% 导弹初始状态
est_pos = true_pos + 5;   % 初始估计位置
est_speed = true_vel + 0.1;      % 初始速度向量 (vx, vy, vz)

% UPF参数
alpha = 1e-3;   % 控制Sigma点分布的参数
beta = 2;       % 包含高阶矩信息
kappa = 2;      % 调节协方差矩阵
lambda = 3-1;  % 复合参数
% 数据存储
true_trajectory = zeros(N, 1);
est_trajectory = zeros(N, 1);
distance_est = zeros(N, 1);
distance_z = zeros(N, 1);
z_measure = zeros(N, 1);

% 协方差矩阵初始化
Q = 0.01;  % 过程噪声协方差
R = 1e1;              % 观测噪声协方差

%% 状态转移与测量模型
state_transition = @(x) [x(1)]; % 匀速模型

measurement_function = @(x) x(1); % 直接观测位置

%% 初始化粒子

完整代码：https://blog.youkuaiyun.com/callmeup/article/details/143080337?spm=1011.2415.3001.5331

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者