matlab粒子滤波的代码（三维）

程序设计

1. 介绍

粒子滤波是一种用于动态系统状态估计的先进方法，广泛应用于机器人定位、目标跟踪和金融预测等领域。该算法通过一组粒子及其权重来表示系统状态的概率分布，能够有效处理非线性和非高斯的系统。在每个时间步中，粒子滤波首先随机初始化粒子的位置和权重，然后根据系统的状态方程对粒子进行预测，接着计算每个粒子与实际观测值的匹配程度以更新权重。为了避免粒子退化，算法执行重采样步骤，根据权重选择新的粒子集合，最后通过加权平均得到当前时刻的状态估计。通过图形化展示真实状态、观测值和估计结果，粒子滤波能够在含噪声的环境中提供精准的状态估计，展现其在复杂动态环境中的优秀性能。

2. 系统模型

2.1 状态方程

系统的状态可以通过以下非线性方程进行描述:

$$X_k=f\left(X_{k-1}\right)+w_k$$

其中： $\square$

• $X_k$ 表示在时间 $k$ 的状态向量。
• $f(\cdot )$ 是状态转移函数，定义为:

$$f\left(X_{k-1}\right)=\left[\begin{array}{c}{X}_{1,k-1}+\frac{2.5\cdot X_{1,k-1}}{1+X_{1,k-1}^2}+8\cdot \cos (1.2\cdot (k-1))\\ X_{2,k-1}+1\\ X_{3,k-1}\end{array}\right]$$

• $w_k$ 是过程噪声，假设其符合高斯分布 $w_k\sim \mathcal{N}(0,Q)$ 。
  2.2 观测方程

观测模型通过以下方程描述:

$$Z_k=h\left(X_k\right)+v_k$$

其中:

• $Z_k$ 是在时间 $k$ 的观测值。
• $h(\cdot )$ 是观测函数，定义为:

$$h\left(X_k\right)=\left[\begin{array}{c}\frac{X_{1,k}^2}{X_{2,k}^2}\\ X_{3,k}\end{array}\right]$$

• $v_k$ 是观测噪声，假设其符合高斯分布 $v_k\sim \mathcal{N}(0,R)$ 。

3. 算法步骤

3.1 初始化

1. 设置粒子数量 $N$ 和时间步 $t$ 。
2. 初始化粒子的位置 $P$ :

$$P(:,i)=X(:,1),\forall i\in [1,N]$$

3. 计算初始观测值 $Z_P$ 并计算权重 $w$ :

$$w(:,i)=\frac{1}{\sqrt{2\pi R}}\cdot \exp \left(-\frac{{\Vert Z_P(:,i)-Z(:,1)\Vert }^2}{2R}\right)$$
3.2 预测步骤

在每个时间步 $k$ 中:

1. 对每个粒子进行状态预测:

$$P(:,i)=f\left(X_{pf}(k-1)\right)+w_{pf,k}$$

2. 计算预测观测值 $Z_P$ :

$$Z_P(:,i)=h(P(:,i))$$

3.3 更新权重

1. 计算每个粒子的权重:

$$w(:,i)=\frac{1}{\sqrt{2\pi R}}\cdot \exp \left(-\frac{{\Vert Z_P(:,i)-Z_k\Vert }^2}{2R}\right)$$

2. 归一化权重:

$$w(:,i)=\frac{w(:,i)}{{\sum }_{j=1}^{N}w(:,j)}$$

3.4 重采样步骤

1. 根据权重重采样，生成新的粒子集合 $P_{\text{next }}$ :

$$P_{\text{next }}(:,i)=P(:,\text{ index })$$

3.5 状态估计

最终的状态估计为所有粒子的加权平均:

$$X_{pf}(k)=\frac{1}{N}\sum _{i=1}^{N}P(:,i)$$

源代码

% PF三维滤波效果对比
% author:Evand
% 作者联系方式VX：matlabfilter（除前期达成一致外，咨询需付费）
% 2024-9-2/Ver1
% 2024-10-01/Ver2：添加逐行注释
clear; clc; close all; % 清空工作空间、命令窗口和关闭所有图形窗口
rng(0); % 设置随机数生成器的种子，确保结果可复现
%% 参数设置
N = 100; % 粒子总数
t = 1:1:1000; % 时间向量，从1到1000
Q = 1*diag([1,1,1]); % 过程噪声的协方差矩阵
w_pf = sqrt(Q) * randn(size(Q,1), length(t)); % 生成过程噪声，维度与状态一致
R = 1*diag([1,1,1]); % 观测噪声的协方差矩阵
v_pf = sqrt(R) * randn(size(R,1), length(t)); % 生成观测噪声，维度与观测一致
P0 = 1 * eye(3); % 初始状态的协方差矩阵
X = zeros(3, length(t)); % 初始化真实状态矩阵
X_ekf = zeros(3, length(t)); % 初始化扩展卡尔曼滤波状态矩阵
X_ekf(:, 1) = X(:, 1); % 设置初始状态
Z = zeros(3, length(t)); % 定义观测值矩阵
Z(:, 1) = [X(1, 1)^2 / 20; X(2, 1); X(3, 1)] + v_pf(:, 1); % 计算初始观测值

% 设定变量维度
X_ = zeros(3, length(t)); % 初始化未滤波状态矩阵

剩余代码下载链接：
https://mall.bilibili.com/neul-next/index.html?page=mall-up_itemDetail&noTitleBar=1&itemsId=1106357012&from=items_share&msource=items_share

运行结果

三维状态量的曲线:

三维状态量误差曲线：

三维误差的CDF图像：

滤波前后误差的统计特性（命令行截图）：

更多程序，详见专栏：卡尔曼讲解与各种典型进阶MATLAB编程