文章目录
程序介绍
程序介绍
本程序实现了一个基于**无迹粒子滤波（Unscented Particle Filter, UPF）**的一维目标状态估计示例，用于对直线运动目标的位置进行滤波与精度分析。系统以一维匀速运动为背景，传感器提供带噪位置观测，滤波器利用无迹变换与粒子采样相结合的方式，对目标状态进行递推估计，并与原始观测结果进行对比。
在目标运动建模中，设目标在离散时刻 (k) 的状态为
x k x_k xk
其状态转移方程可表示为
x k = f ( x k − 1 ) + w k − 1 x_k = f(x_{k-1}) + w_{k-1} xk=f(xk−1)+wk−1
其中， f ( ⋅ ) f(\cdot) f(⋅)为状态转移函数， w k − 1 w_{k-1} wk−1为过程噪声，满足
w k − 1 ∼ N ( 0 , Q ) w_{k-1} \sim \mathcal{N}(0,Q) wk−1∼N(0,Q)
程序中目标真实位置按照匀速模型更新，即
x k = x k − 1 + v Δ t x_k = x_{k-1} + v \Delta t xk=xk−1+vΔt
其中， v v v为目标速度， Δ t \Delta t Δt为采样周期。
UPF 的核心思想是在粒子滤波框架下引入无迹变换。对于每个粒子，不再简单依靠先验分布进行随机传播，而是先利用当前粒子的均值与协方差构造 Sigma 点，再通过无迹变换获得更优的建议分布，从而提高粒子传播质量。设当前粒子状态维数为 n n n，则第 i i i个 Sigma 点可写为
χ 0 = x \chi_0 = x χ0=x
χ i = x + n + λ ( P ) ∗ i , i = 1 , ... , n \chi_i = x + \sqrt{n+\lambda}\left(\sqrt{P}\right)*i,\quad i=1,\dots,n χi=x+n+λ (P )∗i,i=1,...,n
χ ∗ i + n = x − n + λ ( P ) i , i = 1 , ... , n \chi*{i+n} = x - \sqrt{n+\lambda}\left(\sqrt{P}\right)_i,\quad i=1,\dots,n χ∗i+n=x−n+λ (P )i,i=1,...,n
其中， λ \lambda λ为尺度参数， P P P为当前粒子的协方差矩阵。
Sigma 点经状态方程传播后，可得到预测均值与预测协方差：
x ^ ∗ k − = ∑ ∗ i = 0 2 n w i ( m ) χ i , k − \hat{x}*k^- = \sum*{i=0}^{2n} w_i^{(m)} \chi_{i,k}^- x^∗k−=∑∗i=02nwi(m)χi,k−
P k − = ∑ i = 0 2 n w i ( c ) ( χ i , k − − x ^ ∗ k − ) ( χ ∗ i , k − − x ^ k − ) T + Q P_k^- = \sum_{i=0}^{2n} w_i^{(c)}\left(\chi_{i,k}^- - \hat{x}*k^-\right)\left(\chi*{i,k}^- - \hat{x}_k^-\right)^{\mathrm T} + Q Pk−=i=0∑2nwi(c)(χi,k−−x^∗k−)(χ∗i,k−−x^k−)T+Q
其中， w i ( m ) ) 和 ( w i ( c ) w_i^{(m)}) 和 (w_i^{(c)} wi(m))和(wi(c)分别为均值权重和协方差权重。程序中通过该预测结果为每个粒子重新构造采样分布，并完成粒子更新。
在获得新粒子后，依据当前观测值计算各粒子的似然权重。
在高斯测量噪声条件下，粒子权重按似然函数更新为
w k ( i ) ∝ exp ( − 1 2 y ~ k ( i ) T R − 1 y ~ k ( i ) ) w_k^{(i)} \propto \exp\left(-\frac{1}{2}\tilde{y}_k^{(i)\mathrm T}R^{-1}\tilde{y}_k^{(i)}\right) wk(i)∝exp(−21y~k(i)TR−1y~k(i))
随后对权重进行归一化。为避免粒子退化，程序进一步采用重采样方法保留高权重粒子，并剔除低权重粒子。
最终，滤波估计值由所有粒子的加权均值或重采样后的样本均值给出。该程序中采用粒子均值作为状态估计
修改建议
- 增加粒子数量：加大num_particles
运行结果
滤波前后误差曲线对比
估计误差的曲线
代码结构：
源代码
部分代码
matlab
% 无迹粒子滤波（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.csdn.net/callmeup/article/details/143080337?spm=1011.2415.3001.5331
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者