最优滤波 TP2：粒子滤波应用于目标跟踪

粒子滤波应用于目标跟踪

引言

粒子滤波是一种基于蒙特卡洛方法的贝叶斯估计工具，适用于非线性和非高斯动态系统。在目标跟踪中，粒子滤波可以用来根据雷达测量数据（带噪）来估计目标的运动轨迹。

目标跟踪旨在根据带噪声传感器的测量值估计移动目标的运动。我们重点关注雷达跟踪问题，雷达发射非常短的脉冲，这些脉冲以光速在大气中传播。当信号到达目标时，部分信号被反射回雷达天线，经过处理后，雷达计算出视线轴与参考轴之间的角度估计，记为 \theta
以及目标与雷达的距离，距离可以直接从信号反射回雷达的时间间隔 \Delta t 推导出来：

r = \frac{c \cdot \Delta t}{2}

![image-20240908143027948](/Users/zehua/Library/Application Support/typora-user-images/image-20240908143027948.png)

在此示意图中，雷达位于坐标系原点 (0,0)

TP目标

我们要通过带噪声的雷达测量数据，角度 \theta^m(t) 和距离 r^m(t)，来找出目标的轨迹，

即随着时间变化的目标坐标 (x(t), y(t))。观测模型因此表现为：

\begin{pmatrix} r^m(t) \\ \theta^m(t) \end{pmatrix} = \begin{pmatrix} r(t) \\ \theta(t) \end{pmatrix} + \begin{pmatrix} v_r(t) \\ v_\theta(t) \end{pmatrix}

符号说明

r(t) = h_r(x(t), y(t)) 表示目标与雷达之间的真实几何距离，它可以通过目标坐标 (x(t), y(t)) 表示:

r(k) = \sqrt{x(k)^2 + y(k)^2}

\theta(t) = h_\theta(x(t), y(t)) 是雷达视线轴与 x 轴之间的准确角度，同样可以用目标的坐标 (x(t), y(t)) 表示。

\theta(k) = \tan^{-1}\left(\frac{y(k)}{x(k)}\right)

接下来，我们将观察向量记作：

\mathbf{z}(t) = \begin{pmatrix} r^m(t) \\ \theta^m(t) \end{pmatrix}

因此带噪声的观测模型为：

\mathbf{z}(k) = \begin{pmatrix} r^m(k) \\ \theta^m(k) \end{pmatrix} = \begin{pmatrix} r(k) \\ \theta(k) \end{pmatrix} + \begin{pmatrix} v_r(k) \\ v_\theta(k) \end{pmatrix}

其中，v_r(k) 和 v_\theta(k) 分别是服从均值为 0，方差为 \sigma_r^2 和 \sigma_\theta^2 的高斯噪声。

假设两次雷达测量之间的时间间隔是常量，并记为 T。目标坐标的估计仅在离散时间点上进行，记作：

t_k = kT, \quad \text{其中} \ k \geq 0

为了简化，后续将使用如下形式表示

x(k) = x(kT), \ y(k) = y(kT),r_m(k), \ \theta_m(k)

初步问题

这一部分的目标是编写一些 Matlab 函数，这些函数将在TP的第二部分用于实现粒子滤波器。

为了建立状态模型，假设目标没有大的加速度：它没有在进行机动。在这种情况下，其加速度可以建模为白噪声：

\underline{\mathbf{u}}(k)=\begin{pmatrix} \ddot{x}(k) \\ \ddot{y}(k) \end{pmatrix} = \begin{pmatrix} u_x(k) \\ u_y(k) \end{pmatrix}\\

其中

\ E[u_x(k)] = E[u_y(k)] = 0, \quad E[u_x^2(k)] = E[u_y^2(k)] = \sigma_u^2

在这个假设下，指出最合适的状态向量 \underline{\mathbf{x}}(k) 来估计目标的运动。

通过使用泰勒展开，明确 x(k)、x(k-1)、\dot{x}(k) 和 \dot{x}(k-1) 之间的关系。
通过使用泰勒展开，明确 y(k)、y(k-1)、\dot{y}(k) 和 \dot{y}(k-1) 之间的关系。

从中推导出如下相应的状态模型矩阵表达式：

\underline{\mathbf{x}}(k) = \Phi(k, k-1) \underline{\mathbf{x}}(k-1) + G(k) \underline{\mathbf{u}}(k-1)

设定：

\underline{\mathbf{x}}(k) = \begin{pmatrix} x(k) \\ \dot{x}(k) \\ y(k) \\ \dot{y}(k) \\ \end{pmatrix}

在 x(k) 和 y(k) 方向上的运动分别可以表示为：。

x(k) = x(k-1) + T \dot{x}(k-1) + \frac{T^2}{2} \ddot{x}(k-1) \overset{\ddot{x}(k) = u_x(k)}{\Rightarrow} x(k-1) + T \dot{x}(k-1) + \frac{T^2}{2} u_x(k-1)

\dot{x}(k) = \dot{x}(k-1) + T \ddot{x}(k-1) \overset{\ddot{x}(k) = u_x(k)}{\Rightarrow} \dot{x}(k-1) + T u_x(k-1)

y(k) = y(k-1) + T \dot{y}(k-1) + \frac{T^2}{2} \ddot{y}(k-1) \overset{\ddot{y}(k) = u_y(k)}{\Rightarrow} y(k-1) + T \dot{y}(k-1) + \frac{T^2}{2} u_y(k-1)

\dot{y}(k) = \dot{y}(k-1) + T \ddot{y}(k-1) \overset{\ddot{y}(k) = u_y(k)}{\Rightarrow} \dot{y}(k-1) + T u_y(k-1)

\mathbf{x}(k) = \begin{pmatrix} x(k) \\ \dot{x}(k) \\ y(k) \\ \dot{y}(k) \end{pmatrix} = \begin{pmatrix} 1 & T & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & T \\ 0 & 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} x(k-1) \\ \dot{x}(k-1) \\ y(k-1) \\ \dot{y}(k-1) \end{pmatrix} + \begin{pmatrix} 0.5 T^2 & 0 \\ T & 0 \\ 0 & 0.5 T^2 \\ 0 & T \end{pmatrix} \begin{pmatrix} u_x(k) \\ u_y(k) \end{pmatrix}

1.编写一个 Matlab 函数，命名为 matrices_etat，该函数以时间间隔 T 作为输入，计算：

进化矩阵 \Phi(k, k-1)

\Phi(k, k-1) = \begin{pmatrix} 1 & T & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & T \\ 0 & 0 & 0 & 1 \end{pmatrix}

矩阵 G(k)

G(k) = \begin{pmatrix} 0.5 T^2 & 0 \\ T & 0 \\ 0 & 0.5 T^2 \\ 0 & T \end{pmatrix}

function [Phi, G] = matrices_etat(T)
    % 状态转移矩阵 Phi (4x4)
    Phi = [1, T, 0, 0;
           0, 1, 0, 0;
           0, 0, 1, T;
           0, 0, 0, 1];
    
    % 加速度影响矩阵 G (4x2)
    G = [0.5*T^2, 0;
         T, 0;
         0, 0.5*T^2;
         0, T];
end

给出将平面上一点 (x,y) 与雷达之间的几何距离 r 以及角度 θ 联系起来的函数表达式。这两种函数分别用 Matlab 创建，命名为 fonction_r 和 fonction_theta

r = \sqrt{x^2 + y^2}\\ \theta = \tan^{-1} \left( \frac{y}{x} \right)

function r = fonction_r(x, y)
    % 计算几何距离
    r = sqrt(x^2 + y^2);
end

function theta = fonction_theta(x, y)
    % 计算角度
    theta = atan2(y, x);  % atan2 处理四象限的角度
end

给出似然函数的表达式：

p(\underline{\mathbf{z}}(k)| \underline{\mathbf{x}}(k)) = p(r^m(k) | \underline{\mathbf{x}}(k)) p(\theta^m(k) | \underline{\mathbf{x}}(k))

然后编写一个 Matlab 函数 vraisemblance，使用函数 fonction_r 和 fonction_theta，使用以下输入量来计算输出

\left( \text{状态向量 } \underline{\mathbf{x}}(k) \right), \left( \text{测量值 } \underline{\mathbf{z}}(k) \right), \left( \text{距离噪声和角度噪声的方差 } \sigma_r^2 和 \sigma_\theta^2 \right) \quad \xrightarrow{\text{来计算似然函数}} \quad p(\underline{\mathbf{z}}(k) | \underline{\mathbf{x}}(k))

测量的误差服从高斯分布，因此我们可以使用高斯分布的概率密度函数来表示每次测量结果的似然函数 (观测值与真实值之间的概率关系)

均值(真实值)为 μ，方差为 σ^2的高斯分布（正态分布）的观测值 x概率密度函数:

p(x) = \frac{1}{\sqrt{2 \pi \sigma^2}} \exp \left( - \frac{(x - \mu)^2}{2 \sigma^2} \right)

\underline{\mathbf{z}}(t) = \begin{pmatrix} r^m(t) \\ \theta^m(t) \end{pmatrix} = \begin{pmatrix} r(t) \\ \theta(t) \end{pmatrix} + \begin{pmatrix} v_r(t) \\ v_\theta(t) \end{pmatrix} \quad \overset{r(t) = h_r(x(t), y(t))\ , \ \theta(t) = h_\theta(x(t), y(t)) }{\Longrightarrow} \quad \begin{pmatrix} r^m(k) = h_r(x(k), y(k)) + v_r(k) \\ \theta^m(k) = h_\theta(x(k), y(k)) + v_\theta(k) \end{pmatrix}

重写一下，测量模型可以写为：

\underline{\mathbf{z}}(k) = \begin{pmatrix} r^m(k) \\ \theta^m(k) \end{pmatrix} = \begin{pmatrix} h_r(x(k), y(k)) \\ h_\theta(x(k), y(k)) \end{pmatrix} + \begin{pmatrix} v_r(k) \\ v_\theta(k) \end{pmatrix}

其中：

h_r(x(k), y(k)) = \sqrt{x(k)^2 + y(k)^2} 是真实几何距离。
h_\theta(x(k), y(k)) = \tan^{-1}\left(\frac{y(k)}{x(k)}\right) 是真实角度。
v_r(k) \sim \mathcal{N}(0, \sigma_r^2) 是距离测量误差。
v_\theta(k) \sim \mathcal{N}(0, \sigma_\theta^2) 是角度测量误差。

测量值的概率取决于这些噪声的分布，因此似然函数的表达式来自于噪声的概率密度函数。

距离的似然函数

测量值 r^m(k) 是由真实距离 h_r(x(k), y(k)) 加上测量误差 v_r(k) 得到的：

r^m(k) = h_r(x(k), y(k)) + v_r(k)

假设噪声 v_r(k) \sim \mathcal{N}(0, \sigma_r^2)，则测量值 r^m(k) 的概率密度为：

p(r^m(k) | \underline{\mathbf{x}}(k)) = \frac{1}{\sqrt{2\pi\sigma_r^2}} \exp \left( - \frac{\left( r^m(k) - h_r(x(k), y(k)) \right)^2}{2\sigma_r^2} \right)

这评估在目标状态 \underline{\mathbf{x}}(k) 下，实际测量值 r^m(k) 偏离理论值 h_r(\underline{\mathbf{x}}(k)) 的可能性。偏离越小，似然值越大。

r^m(k) 是实际测量值。
h_r(x(k), y(k)) 是根据状态 \underline{\mathbf{x}}(k) 计算得到的理论值。
\sigma_r^2 是测量误差的方差。

角度的似然函数

测量值 \theta^m(k) 是由真实角度 h_\theta(x(k), y(k)) 加上测量误差 v_\theta(k) 得到的：

\theta^m(k) = h_\theta(x(k), y(k)) + v_\theta(k)

假设噪声 v_\theta(k) \sim \mathcal{N}(0, \sigma_\theta^2)，则测量值 \theta^m(k) 的概率密度为：

p(\theta^m(k) | \underline{\mathbf{x}}(k)) = \frac{1}{\sqrt{2\pi\sigma_\theta^2}} \exp \left( - \frac{\left( \theta^m(k) - h_\theta(x(k), y(k)) \right)^2}{2\sigma_\theta^2} \right)

这评估在目标状态 \underline{\mathbf{x}}(k) 下，实际测量值 \theta^m(k) 偏离理论值 h_\theta(\underline{\mathbf{x}}(k)) 的可能性。

\theta^m(k) 是实际测量值。
h_\theta(x(k), y(k)) 是根据状态 \underline{\mathbf{x}}(k) 计算得到的理论值。
\sigma_\theta^2 是测量误差的方差。

组合似然函数：

由于距离和角度的噪声是中心高斯噪声（相互独立），因此可以直接相乘：

p(\underline{\mathbf{z}}(k) | \underline{\mathbf{x}}(k)) = p(r^m(k) | \underline{\mathbf{x}}(k)) \times p(\theta^m(k) | \underline{\mathbf{x}}(k))

即:

p(\underline{\mathbf{z}}(k) | \underline{\mathbf{x}}(k)) = \frac{1}{\sqrt{2 \pi \sigma_r^2}} \exp\left(-\frac{\left(r^m(k) - h_r(\underline{\mathbf{x}}(k))\right)^2}{2 \sigma_r^2}\right) \cdot \frac{1}{\sqrt{2 \pi \sigma_\theta^2}} \exp\left(-\frac{\left(\theta^m(k) - h_\theta(\underline{\mathbf{x}}(k))\right)^2}{2 \sigma_\theta^2}\right)

[!NOTE]

为什么是条件下的概率密度函数？

条件似然函数是基于“给定状态下”的测量值分布，以概率密度的形式来体现。即在给定真实值状态 \underline{\mathbf{x}}(k) 的情况下，测量值的概率分布。因为我们总是基于某个假设的状态，去估计观测到数据的概率。

输入变量为:

x, y：目标的坐标，用于计算理论值 r 和 \theta。
z_r, z_\theta：带噪声的测量值，分别是距离和角度。
\sigma_r, \sigma_\theta：测量噪声的标准差，决定高斯分布的形状。

function p = vraisemblance(x, y, z_r, z_theta, sigma_r, sigma_theta)
    % 使用 fonction_r 和 fonction_theta 计算真实的距离和角度
    r = fonction_r(x, y);  % 计算几何距离
    theta = fonction_theta(x, y);  % 计算角度
    
    % 计算距离的似然函数部分
    p_r = (1 / sqrt(2 * pi * sigma_r^2)) * exp(-(z_r - r)^2 / (2 * sigma_r^2));
    
    % 计算角度的似然函数部分
    p_theta = (1 / sqrt(2 * pi * sigma_theta^2)) * exp(-(z_theta - theta)^2 / (2 * sigma_theta^2));
    
    % 总的似然函数是两者的乘积
    p = p_r * p_theta;
end

粒子滤波流程概述

粒子表示和初始化

粒子表示：
- 每个粒子是状态空间中的一个假设值（状态向量 \underline{\mathbf{x}}）。
- 粒子滤波器通过多个粒子来近似状态的概率分布。
- 在数据中，我们有一个表格，每列表示一个粒子，每行是粒子的某个状态变量（例如位置、速度等）。
初始化：
- 在滤波开始时，根据初始状态的先验分布随机生成一组粒子，每个粒子赋予相等的初始权重。

每个时间步的更新

粒子的传播

目标：使用状态模型生成每个粒子的下一时刻的状态。

步骤：

A) 对每个粒子，使用状态转移模型：

\underline{\mathbf{x}}(k) = \Phi(k, k-1) \underline{\mathbf{x}}(k-1) + G(k) \underline{\mathbf{u}}(k-1)

B) 其中：
- \Phi 是状态转移矩阵。
- G 是控制输入矩阵（反映噪声对状态的影响）。
- \underline{\mathbf{u}}(k-1) 是加速度噪声，从高斯分布中随机生成。
C) 每个粒子通过状态模型产生一个新的粒子，更新状态。
计算权重

目标：根据测量值 \underline{\mathbf{z}}(k)，计算每个粒子的权重，反映粒子状态与真实观测值的匹配程度。

步骤：

A) 对每个粒子，计算其似然函数：

\text{权重} = \text{前一时刻权重} \times \text{似然值}

B) 似然值的计算公式：

p(\underline{\mathbf{z}}(k) | \underline{\mathbf{x}}(k)) = p(r^m(k) | \underline{\mathbf{x}}(k)) \cdot p(\theta^m(k) | \underline{\mathbf{x}}(k))

C) 计算粒子的似然值（匹配程度）。

归一化权重：对所有粒子的权重进行归一化，使得它们的总和为1：

w_i(k) \leftarrow \frac{w_i(k)}{\sum_{j} w_j(k)}

**状态估计 **

目标：根据粒子的分布和权重，估计目标状态。
步骤：将所有粒子的位置加权平均，得到当前时间步的状态估计

\hat{\underline{\mathbf{x}}}(k) = \sum_{i=1}^N w_i(k) \cdot \underline{\mathbf{x}}_i(k)

输出的状态估计值是 \hat{\underline{\mathbf{x}}}(k) 加权后的粒子分布中心。

重采样

目标：解决粒子退化问题，即避免某些粒子的权重过小导致采样效率降低。
步骤：

A) 如果粒子的权重分布极端（大多数粒子权重接近零），那就重采样。

B) 根据粒子的权重，重新抽样：
- 权重大概率高的粒子会被重复采样。
- 权重小的粒子会被丢弃。
C) 重新生成的粒子表格具有均匀的权重，更能真实反映目标的后验概率。

这些步骤的循环进行，使粒子滤波器能够动态估计目标的轨迹，同时适应非线性和非高斯系统。

粒子滤波器的编程

下载文件 TP_particulaire.zip。加载测量数据文件 mesures_radar.mat，其中包含一个矩阵 Z ，行数为2，列数为目标轨迹的测量点数量。因此， Z(1,k) 和 Z(2,k) 分别表示第 k 个点的距离和角度测量值。

使用 bootstrap (SIR) 粒子滤波器从雷达测量值中估计目标轨迹。首先，创建一个函数 simu_modele_etat

输入：

第 k-1 时刻的状态向量：\underline{\mathbf{x}}(k-1)
状态矩阵：\Phi(k, k-1) 和 G(k)
状态噪声的方差：\sigma_u^2

输出：

当前时刻的状态向量：\underline{\mathbf{x}}(k)

\underline{\mathbf{x}}(k) = \Phi(k, k-1) \underline{\mathbf{x}}(k-1) + G(k) \underline{\mathbf{u}}(k-1)

function x_current = simu_modele_etat(x_prev, Phi, G, sigma_u)
    % 生成一个状态的可能下一个状态
    u = randn(2, 1) * sigma_u;  % 随机加速度噪声
    x_current = Phi * x_current + G * u;
end

在初始阶段，选取 N=1000 个粒子，并正确初始化滤波器，将所有粒子设为 t=1 时刻目标的真实位置（包含在文件 trajectoire_reelle.mat 中）。参数设置如下。同时使用 TP_particulaire.zip 提供的 resampling 程序进行粒子的重采样。

\begin{array}{c|c} \sigma_u & 2 \, \text{m/s}^2 \\ \sigma_r & 50 \, \text{m} \\ \sigma_\theta & \frac{\pi}{100} \\ T & 1 \, \text{s} \end{array}

close all
clc
clear all

%% 参数定义
T = 1;  % 时间步长
N = 1000;  % 粒子数
sigma_u = [2; 2];  % 过程噪声的标准差,表示目标运动过程中存在的加速度噪声.这里假设加速度在 x 和 y 方向的噪声标准差均为 2。
sigma_r = 50;  % 距离中心化高斯噪音的标准
sigma_theta = pi / 100;  % 角度中心化高斯噪音的标准差
% sigma_theta = deg2rad(0.01);  % 角度中心化高斯噪音的标准差

%% 加载真实的初始状态和测量数据
load('mesures_radar.mat');  % 雷达测量数据,其中包含一个矩阵 Z,横坐标2行代表距离和角度，纵坐标200列代表测了200次(时间步)

load('trajectoire_reelle.mat');  % 真实的轨迹数据Xvrai,表示目标的真实位置和状态

%% 粒子滤波流程
num_steps = size(Z, 2);  % 获取测量数据 Z 的列数，表示一共有多少个时间步

% 初始化粒子为真实值的第一个状态，也就是起点，把他复制N次，不要紧，因为会不断更新覆盖后面的值，这里没有添加额外噪声
x_particles = Xvrai(:, 1) * ones(1, N);  % 目标的初始真实状态重复 N 次，生成 N 个粒子

% 初始化权重
weights = ones(1, N) / N;

% 存储估计值尺度和真实值对齐
estimated_states = zeros(4, num_steps);

% 计算具体值下的状态转移矩阵和加速度影响矩阵
[Phi, G] = matrices_etat(T);

%% 粒子滤波主循环
for k = 1:num_steps %到最后一次时间步长，对应估计轨迹和真实轨迹维度
    % 预测每个粒子的下一个状态
    for i = 1:N
        x_particles(:, i) = simu_modele_etat(x_particles(:, i),Phi,G,sigma_u);
    end
        % 基于似然函数更新权重
    for i = 1:N
        weights(i) = vraisemblance(x_particles(1,i),x_particles(3,i),Z(1,k),Z(2,k), sigma_r, sigma_theta);
        % 似然函数根据粒子的状态x y 和真实值z_r  z_theta 以及 对应的标准来计算对应状态的概率 
        % 该值存储在 weights(i) 中，表示第 i 个粒子的权重。
    end

        
    % 归一化权重
    weights = weights / sum(weights); % 所有粒子的权重总和为 1
        
    % 估计状态（粒子的加权平均）
    estimated_states(:, k) = x_particles * weights'; % 粒子的初始状态乘权重==>得到当前时间步长下的估计状态
    
    % 计算有效粒子数
    Neff = 1 / sum(weights.^2);
        
    % 设置阈值，比如 0.5 * N
    if Neff <  0.95*N
       % 当有效粒子数低于阈值时进行重采样
       [x_particles, weights] = resampling(x_particles, weights);
    end
end

    
% 绘图：真实轨迹 vs. 粒子滤波估计
figure;
plot(estimated_states(1, :), estimated_states(3, :), 'r-', 'LineWidth', 2);  % 粒子滤波估计轨迹
hold on;
plot(Xvrai(1, :), Xvrai(3, :), 'bo');  % 真实位置
legend('Estimated trajectory', 'True trajectory');
xlabel('X position');
ylabel('Y position');
title('Particle Filter: Estimated Trajectory vs True Position');
grid on;

子函数

function [Xr,wr]=resampling(X,w)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Entrées
%  X : tableau contenant les particules à rééchantillonner
%  X(:,k) est un vecteur colonne correspondant à la k-ième particule
%  w : vecteur contenant les poids associés aux particules
%  w(k) : poids de la k-ième particule
%  Sorties
%  Xr : tableau contenant les particules après rééchantillonnage
%  X(:,k) est un vecteur colonne correspondant à la k-ième particule
%  wr : vecteur contenant les poids après rééchantillonnage
%  wr(k) : poids de la k-ième particule

N1=size(X,2);
N2=N1;

ind_select = zeros(1,N2);

% sample the particles 

u=zeros(1,N2);
N_sons=zeros(1,N1);
dist=cumsum(w);

aux=rand(1);
u=aux:1:(N2-1+aux);
u=u./N2;

j=1;
for i=1:N2
   while (u(1,i)>=dist(1,j))
      j=j+1;
   end
   N_sons(1,j)=N_sons(1,j)+1;
end

ind=1;

for i=1:N1,
   
   % if copy then keep it here
   if (N_sons(1,i)>0)
       
      for j=ind:ind+N_sons(1,i)-1
         ind_select(j) = i;
      end
      
   end
   
   ind=ind+N_sons(1,i);
   
end

Xr = X(:,ind_select);
wr = ones(1,N2)/N2;

引入重采样的意义何在？是否有必要在每个时间步都进行重采样？

避免粒子退化现象，即随着时间的推移，许多粒子的权重会逐渐接近于零，只有少数粒子的权重较大。这种退化现象会导致大部分粒子失去作用，进而影响粒子滤波器的估计精度。

重采样的代价是减少了粒子的多样性（即粒子趋向于集中在高权重区域），如果过于频繁重采样，会导致估计陷入局部最优解，丧失对真实状态的多样性描述。
改变初始化和粒子数量，观察到什么？为论证你的回答，可以绘制估计的均方误差（真实轨迹在文件 trajectoire_reelle.mat 中作为参考）。

初始化对滤波结果的影响

粒子滤波最初始的粒子分布(先验分布)对于后续滤波效果非常重要。如果初始化偏差过大，且粒子数量不足或观测噪声大，则粒子滤波需要较多时间步才能逐渐收敛到目标状态附近，甚至可能发生发散。
- (1) 初始化过于集中且偏离真实值
  
  若所有粒子都初始化在一个和真实目标位置相差较大的位置（且方差很小），会导致在最初数个迭代里，很多粒子的似然函数都非常低；权重集中在少数“偶然离目标更近”的粒子上，一旦这些粒子也不够准确，可能会出现滤波较晚收敛或滤波准确度不足的情况。
- (2) 初始化分布较分散
  
  如果初始化时给定较大的方差(例如给x(0)和y(0)赋予较大随机偏移)，意味着先验粒子能够在更大范围内搜索目标位置，这对于不确定性较大的情况可能有利。但若太分散，则在初始若干步中，真正落在目标附近的粒子可能也比较有限，需要时间来慢慢“筛选”出高权重粒子，这种情况下算法初期的估计结果抖动会较大。
总之，初始化分布的选取会显著影响粒子滤波的收敛速度与最终准确度。
若实验需要：
1. 可以修改初始粒子的均值(比如真值加上一个偏移)和方差(粒子在一个大范围均匀采样或高斯采样)，观察不同初始化下前几个时刻的滤波表现对比。
2. 若想要定量比较，可记录每个时刻的均方误差(MSE)随时间的演化曲线。
粒子数量对滤波结果的影响

粒子数N越多，对真实分布的近似越好，但计算量也越大。

估计均方误差(MSE)

在每个时间步k，计算如下均方误差(以位置坐标为例):

\text{MSE}(k) = \frac{1}{K} \sum_{i=1}^K \bigl[(\hat x(k) - x^\mathrm{true}(k))^2 + (\hat y(k) - y^\mathrm{true}(k))^2\bigr]
- 其中\hat x(k), \hat y(k)分别是粒子滤波在第k步估计的x,y坐标；
- x^\mathrm{true}(k), y^\mathrm{true}(k)是真实值(可从trajectoire_reelle.mat加载)；
- K为总的时间步数(或也可以在每一步就分别计算当前瞬时误差)。
```
mse_vals = zeros(1, num_steps);
for k = 1:num_steps
    est_x = estimated_states(1,k);  
    est_y = estimated_states(3,k);
    true_x = Xvrai(1,k);
    true_y = Xvrai(3,k);
    mse_vals(k) = (est_x - true_x)^2 + (est_y - true_y)^2; 
end
figure; 
plot(mse_vals, 'LineWidth',2); 
xlabel('Time step'); ylabel('MSE'); grid on;
title('MSE vs Time step');
```
改变状态噪声和测量噪声的标准差值，研究算法对这些参数的敏感性。为此，你可以使用文件 mesures_radar_non_bruitees.mat，该文件包含目标与雷达之间的真实几何距离和真实角度，并可以加入你想要的测量噪声。评论所得结果。为了解决你发现的问题，有哪些解决方案？

改变状态噪声\sigma_u
- 过小的过程噪声会使粒子不能及时跟上目标机动，出现跟踪延迟和偏差增大的情况。
- 很大的过程噪声，会导致粒子在状态空间大范围散布，滤波结果变抖，甚至可能出现不必要的估计发散(权重分布很分散)，估计精度不高。
改变测量噪声(\sigma_r, \sigma_\theta)

如果测量噪声过小，在计算似然时，只有非常接近观测值的粒子才可能得到较大权重

如果测量噪声过大，会不信任测量，粒子会更依赖状态模型的预测而非观测更新

“距离噪声”\sigma_r=30 (m) 和“角度噪声”\sigma_\theta=\frac{\pi}{50}，并基于无噪声测量数据来生成模拟测量值。然后，将这些带噪声测量值传入粒子滤波算法中，观察粒子滤波在不同噪声下的表现和敏感性。

使用无噪声的雷达数据`mesures_radar_non_bruitees.mat`，改变角度加速度标准差近乎于0 --- 纯理想情况

%% 第四题示例：向无噪声测量添加自定义测量噪声并进行粒子滤波

close all;
clc;
clear all;

%========== 1) 设定你想加入的测量噪声（示例值） ======================
sigma_r_test = 20;          % 距离噪声标准差 (单位: m)
sigma_theta = deg2rad(0.01); % 角度噪声标准差 (单位: rad)

%========== 2) 加载无噪声测量 ============================================
load('mesures_radar_non_bruitees.mat'); 
% 假设其中包含 Z，维度为 2×(时间步数)
% Z(1,:) = 无噪声距离
% Z(2,:) = 无噪声角度

%========== 3) 添加噪声，生成 Z_bruite ====================================
Z_bruite = zeros(size(Z));
for k = 1:size(Z, 2)
    r_true = Z(1, k);     
    theta_true = Z(2, k); 
    
    % 添加测量噪声
    r_noise = r_true + randn(1)*sigma_r_test;
    theta_noise = theta_true + randn(1)*sigma_theta;
    
    % 生成带噪声测量
    Z_bruite(:, k) = [r_noise; theta_noise];
end

%========== 4) 加载真实轨迹（用于对比/计算误差） ========================
load('trajectoire_reelle.mat'); 
% 假设 trajectoire_reelle.mat 中有 Xvrai (4×timeSteps):
% Xvrai(1,:) = 真实 x
% Xvrai(2,:) = 真实 vx
% Xvrai(3,:) = 真实 y
% Xvrai(4,:) = 真实 vy

%========== 5) 粒子滤波主程序 =============================================
% 你也可以自行改动过程噪声、粒子数等参数
T = 1;                % 时间步长
N = 1000;             % 粒子数
sigma_u = [2; 2];     % 过程噪声标准差 (x、y方向加速度噪声)

% 此处使用我们刚才定义的测量噪声参数
sigma_r = sigma_r_test;
sigma_theta = sigma_theta;

% 载入我们已经写好的函数：matrices_etat、vraisemblance、simu_modele_etat、resampling
% 假设它们都在同一路径下或已添加路径

% 获取 Z_bruite 的时间步
num_steps = size(Z_bruite, 2);

% 初始化粒子 (将真实的初始状态复制 N 份)
x_particles = Xvrai(:, 1) * ones(1, N);

% 初始化权重
weights = ones(1, N) / N;

% 用于存储各时刻的估计结果
estimated_states = zeros(4, num_steps);

% 获取状态转移矩阵和加速度影响矩阵
[Phi, G] = matrices_etat(T);

%========== 粒子滤波循环 =============================================
for k = 1 : num_steps
    
    %----(1) 预测每个粒子的下一个状态
    for i = 1 : N
        x_particles(:, i) = simu_modele_etat(x_particles(:, i), Phi, G, sigma_u);
    end
    
    %----(2) 基于似然函数更新权重
    for i = 1 : N
        weights(i) = vraisemblance( ...
            x_particles(1, i), x_particles(3, i), ...
            Z_bruite(1, k),    Z_bruite(2, k), ...
            sigma_r, sigma_theta);
    end
    
    %----(3) 归一化权重
    weights = weights / sum(weights);
    
    %----(4) 计算加权平均作为估计结果
    estimated_states(:, k) = x_particles * weights';
    
    %----(5) 判断是否进行重采样
    Neff = 1 / sum(weights.^2);
    if Neff < 0.95 * N
       [x_particles, weights] = resampling(x_particles, weights);
    end
    
end

%========== 6) 绘图：粒子滤波估计轨迹 vs. 真实轨迹 =======================
figure;
plot(estimated_states(1, :), estimated_states(3, :), 'r-', 'LineWidth', 2); 
hold on;
plot(Xvrai(1, :), Xvrai(3, :), 'bo');  
legend('Estimated trajectory', 'True trajectory');
xlabel('X position');
ylabel('Y position');
title('Particle Filter (With Custom Noise)');
grid on;

%========== 7) 计算并绘制MSE，查看不同时间步的误差 =======================
mse_vals = zeros(1, num_steps);
for k = 1 : num_steps
    est_x = estimated_states(1, k);
    est_y = estimated_states(3, k);
    true_x = Xvrai(1, k);
    true_y = Xvrai(3, k);
    mse_vals(k) = (est_x - true_x)^2 + (est_y - true_y)^2;
end

figure; 
plot(mse_vals, 'LineWidth', 2); 
xlabel('Time step'); 
ylabel('MSE'); 
grid on;
title('MSE vs Time step (With Custom Noise)');

![image-20240908224940953](/Users/zehua/Library/Application Support/typora-user-images/image-20240908224940953.png)	![image-20240908224930834](/Users/zehua/Library/Application Support/typora-user-images/image-20240908224930834.png)

这部分有很多不理解，最大的不理解就是两份雷达文件一份有噪音一份无噪音的区别是什么，是什么噪音？我们之前定义的噪音又是什么噪音？噪音的标准差是做什么用的？为什么加速度可以被简化成噪音？以及为什么有时候调节标准差过小的时候会不显示结果？

菜单

分享

最优滤波 TP2：粒子滤波应用于目标跟踪

粒子滤波应用于目标跟踪

引言

TP目标

符号说明

初步问题

粒子滤波流程概述

粒子滤波器的编程

使用无噪声的雷达数据`mesures_radar_non_bruitees.mat`，改变角度加速度标准差近乎于0 --- 纯理想情况

评论

优化理论（一）：导论、希尔伯特空间与凸性

Hello Halo

延迟系统稳定化的谱方法（研讨会）

置信函数理论在图像处理与多源融合中的应用

信念函数理论导论（研讨会报告）

心血管模型比较与 HRV 相干性分析（Master ATSI 团队项目）

医学信号处理 TP：相干函数性质证明与脑连接性实验

医学信号处理 TP：第 2 部分问题解答（LMS 自适应噪声消除）

医学信号处理 TP：自适应滤波去眼部伪差、ICA 与脑连接性（完整解答）

医学信号处理 TP（ENSEIRB-MATMECA 2023–2024）：实践工作题目

分享

最优滤波 TP2：粒子滤波应用于目标跟踪

粒子滤波应用于目标跟踪

引言

TP目标

符号说明

初步问题

粒子滤波流程概述

粒子滤波器的编程

使用无噪声的雷达数据mesures_radar_non_bruitees.mat，改变角度加速度标准差近乎于0 --- 纯理想情况

评论

使用无噪声的雷达数据`mesures_radar_non_bruitees.mat`，改变角度加速度标准差近乎于0 --- 纯理想情况