线性动态系统（LDS）的粒子滤波（Particle Filter）算法原理与状态估计过程
字数 3262 2025-12-06 05:15:53

线性动态系统（LDS）的粒子滤波（Particle Filter）算法原理与状态估计过程

1. 题目描述
粒子滤波（Particle Filter， PF），也称序贯重要性采样（Sequential Importance Sampling），是一种用于解决非线性、非高斯状态空间模型中状态估计问题的蒙特卡洛方法。它通过一组带有权重的随机样本（称为“粒子”）来近似表示系统的状态后验概率分布，并随着新观测数据的到来，递归地对粒子进行预测、更新、重采样，从而实现对系统隐藏状态的实时跟踪与估计。我们需要理解其如何解决卡尔曼滤波不适用的非线性/非高斯场景。

2. 核心模型：状态空间模型
粒子滤波基于以下状态空间模型：

  • 状态方程（过程模型）：\(x_t = f(x_{t-1}, u_t, w_t)\)。其中，\(x_t\) 是t时刻的隐藏状态，\(f\) 是（可能非线性的）状态转移函数，\(u_t\) 是控制输入，\(w_t\) 是过程噪声（分布可能非高斯）。
  • 观测方程：\(z_t = h(x_t, v_t)\)。其中，\(z_t\) 是t时刻的观测值，\(h\) 是（可能非线性的）观测函数，\(v_t\) 是观测噪声（分布可能非高斯）。

目标：给定到t时刻为止的所有观测 \(z_{1:t}\)，估计状态 \(x_t\) 的后验概率分布 \(p(x_t | z_{1:t})\)。

3. 核心思想：序列重要性采样与贝叶斯递推
粒子滤波的核心是利用一组带权重的粒子 \(\{ x_t^{(i)}, w_t^{(i)} \}_{i=1}^{N}\) 来近似后验分布：

\[p(x_t | z_{1:t}) \approx \sum_{i=1}^{N} w_t^{(i)} \delta(x_t - x_t^{(i)}) \]

其中 \(\delta(\cdot)\) 是狄拉克函数。递推过程遵循贝叶斯规则：

\[p(x_t | z_{1:t}) \propto p(z_t | x_t) \cdot \int p(x_t | x_{t-1}) \cdot p(x_{t-1} | z_{1:t-1}) dx_{t-1} \]

粒子滤波通过重要性采样来递归实现这个积分。

4. 算法步骤详解

步骤1：初始化（t=0）
从先验分布 \(p(x_0)\) 中随机抽取N个粒子 \(\{ x_0^{(i)} \}_{i=1}^{N}\)，并为每个粒子赋予相等的初始权重 \(w_0^{(i)} = 1/N\)。这组粒子近似表示了初始状态分布。

步骤2：对于每个时刻 t = 1, 2, ... 进行递归迭代

2.1 预测步（采样）
对于上一时刻（t-1）的每个粒子 \(x_{t-1}^{(i)}\)，通过状态方程进行传播（采样），生成t时刻的预测粒子（或称“建议粒子”）：

\[\tilde{x}_t^{(i)} \sim q(x_t | x_{t-1}^{(i)}, z_t) \]

这里 \(q(\cdot)\) 称为重要性密度函数。最简单也是最常用的选择是先验密度，即 \(q = p(x_t | x_{t-1}^{(i)})\)。此时，直接从状态转移概率中采样：

\[\tilde{x}_t^{(i)} \sim p(x_t | x_{t-1}^{(i)}) \]

这相当于将每个旧粒子 \(x_{t-1}^{(i)}\) 通过过程模型向前随机推进一步，得到新的可能状态。

2.2 更新步（计算重要性权重）
当获得新的观测 \(z_t\) 后，我们需要根据观测来评估每个预测粒子 \(\tilde{x}_t^{(i)}\) 的“好坏”。这是通过更新粒子的重要性权重来实现的。

重要性权重的递推公式为：

\[w_t^{(i)} \propto w_{t-1}^{(i)} \cdot \frac{p(z_t | \tilde{x}_t^{(i)}) p(\tilde{x}_t^{(i)} | x_{t-1}^{(i)})}{q(\tilde{x}_t^{(i)} | x_{t-1}^{(i)}, z_t)} \]

  • 如果选择先验密度作为重要性密度（\(q = p(x_t | x_{t-1}^{(i)})\)），则权重更新简化为：

\[w_t^{(i)} \propto w_{t-1}^{(i)} \cdot p(z_t | \tilde{x}_t^{(i)}) \]

  • \(p(z_t | \tilde{x}_t^{(i)})\) 是观测似然，衡量了在粒子状态 \(\tilde{x}_t^{(i)}\) 下，实际观测 \(z_t\) 出现的可能性。似然值越高，权重越大。

计算未归一化的权重后，进行归一化：

\[\tilde{w}_t^{(i)} = \frac{w_t^{(i)}}{\sum_{j=1}^{N} w_t^{(j)}} \]

归一化权重 \(\tilde{w}_t^{(i)}\) 反映了每个粒子在表征当前后验分布中的相对重要性。

2.3 状态估计输出
当前时刻的状态估计（如最小均方误差估计）可以近似为粒子的加权和：

\[\hat{x}_t = E[x_t | z_{1:t}] \approx \sum_{i=1}^{N} \tilde{w}_t^{(i)} \tilde{x}_t^{(i)} \]

后验分布则由带权重的粒子集合 \(\{ \tilde{x}_t^{(i)}, \tilde{w}_t^{(i)} \}\) 近似。

2.4 重采样（Resampling）
这是粒子滤波避免粒子退化的关键步骤。经过几轮迭代后，大部分粒子权重会变得非常小（趋近于0），只有少数粒子有显著权重，导致计算资源浪费在无效粒子上。

重采样的目的：根据粒子的权重，有放回地从当前粒子集合中抽取N次，生成一个新的、等权重的粒子集合 \(\{ x_t^{(j)}, 1/N \}_{j=1}^{N}\)。权重大的粒子更可能被多次选中，权重小的粒子很可能被淘汰。

重采样过程（以系统重采样为例）：

  1. 计算归一化权重的累积分布函数（CDF）。
  2. 生成N个在[0,1]区间均匀分布的随机数 \(\{ u_j \}\)。
  3. 对于每个 \(u_j\)，找到满足CDF值大于 \(u_j\) 的最小索引对应的粒子，将其复制到新集合中。

重采样后，粒子集合重新变为等权重 \(1/N\)，并作为下一时刻迭代的初始粒子集 \(\{ x_t^{(i)} \}\)。然后令 \(t = t+1\)，回到步骤2.1。

5. 算法总结与关键点

  • 核心优势：适用于任意非线性、非高斯的状态空间模型，通用性强。
  • 关键挑战：
    1. 粒子退化：重采样是标准解决方案，但引入了新问题——粒子贫化（样本多样性丧失，大量粒子重复）。
    2. 重要性密度选择：先验密度易于实现，但未考虑最新观测，效率较低。最优重要性密度是后验密度本身，但通常难以采样。工程中常通过扩展卡尔曼滤波或无迹卡尔曼滤波生成重要性密度（如无迹粒子滤波UPF），以接近最优。
  • 计算本质：用离散的、带权重的随机样本（粒子）来“模拟”连续的概率分布，并通过蒙特卡洛方法计算期望。

6. 与卡尔曼滤波的对比
卡尔曼滤波是粒子滤波在线性高斯模型下的最优解析解。当模型为线性、噪声为高斯时，后验分布也是高斯分布，可以用均值和协方差精确描述。粒子滤波则通过大量粒子来近似任意分布，在非线性/非高斯情况下更强大，但计算成本远高于卡尔曼滤波。

线性动态系统（LDS）的粒子滤波（Particle Filter）算法原理与状态估计过程 1. 题目描述 粒子滤波（Particle Filter， PF），也称序贯重要性采样（Sequential Importance Sampling），是一种用于解决 非线性、非高斯 状态空间模型中状态估计问题的蒙特卡洛方法。它通过一组带有权重的随机样本（称为“粒子”）来近似表示系统的状态后验概率分布，并随着新观测数据的到来，递归地对粒子进行预测、更新、重采样，从而实现对系统隐藏状态的实时跟踪与估计。我们需要理解其如何解决卡尔曼滤波不适用的非线性/非高斯场景。 2. 核心模型：状态空间模型 粒子滤波基于以下状态空间模型： 状态方程（过程模型） ：\( x_ t = f(x_ {t-1}, u_ t, w_ t) \)。其中，\( x_ t \) 是t时刻的隐藏状态，\( f \) 是（可能非线性的）状态转移函数，\( u_ t \) 是控制输入，\( w_ t \) 是过程噪声（分布可能非高斯）。 观测方程 ：\( z_ t = h(x_ t, v_ t) \)。其中，\( z_ t \) 是t时刻的观测值，\( h \) 是（可能非线性的）观测函数，\( v_ t \) 是观测噪声（分布可能非高斯）。 目标：给定到t时刻为止的所有观测 \( z_ {1:t} \)，估计状态 \( x_ t \) 的后验概率分布 \( p(x_ t | z_ {1:t}) \)。 3. 核心思想：序列重要性采样与贝叶斯递推 粒子滤波的核心是利用一组带权重的粒子 \(\{ x_ t^{(i)}, w_ t^{(i)} \} {i=1}^{N}\) 来近似后验分布： \[ p(x_ t | z {1:t}) \approx \sum_ {i=1}^{N} w_ t^{(i)} \delta(x_ t - x_ t^{(i)}) \] 其中 \( \delta(\cdot) \) 是狄拉克函数。递推过程遵循贝叶斯规则： \[ p(x_ t | z_ {1:t}) \propto p(z_ t | x_ t) \cdot \int p(x_ t | x_ {t-1}) \cdot p(x_ {t-1} | z_ {1:t-1}) dx_ {t-1} \] 粒子滤波通过 重要性采样 来递归实现这个积分。 4. 算法步骤详解 步骤1：初始化（t=0） 从先验分布 \( p(x_ 0) \) 中随机抽取N个粒子 \(\{ x_ 0^{(i)} \}_ {i=1}^{N}\)，并为每个粒子赋予相等的初始权重 \( w_ 0^{(i)} = 1/N \)。这组粒子近似表示了初始状态分布。 步骤2：对于每个时刻 t = 1, 2, ... 进行递归迭代 2.1 预测步（采样） 对于上一时刻（t-1）的每个粒子 \( x_ {t-1}^{(i)} \)，通过状态方程进行传播（采样），生成t时刻的 预测粒子 （或称“建议粒子”）： \[ \tilde{x} t^{(i)} \sim q(x_ t | x {t-1}^{(i)}, z_ t) \] 这里 \( q(\cdot) \) 称为 重要性密度函数 。最简单也是最常用的选择是 先验密度 ，即 \( q = p(x_ t | x_ {t-1}^{(i)}) \)。此时，直接从状态转移概率中采样： \[ \tilde{x} t^{(i)} \sim p(x_ t | x {t-1}^{(i)}) \] 这相当于将每个旧粒子 \( x_ {t-1}^{(i)} \) 通过过程模型向前随机推进一步，得到新的可能状态。 2.2 更新步（计算重要性权重） 当获得新的观测 \( z_ t \) 后，我们需要根据观测来评估每个预测粒子 \( \tilde{x}_ t^{(i)} \) 的“好坏”。这是通过更新粒子的 重要性权重 来实现的。 重要性权重的递推公式为： \[ w_ t^{(i)} \propto w_ {t-1}^{(i)} \cdot \frac{p(z_ t | \tilde{x}_ t^{(i)}) p(\tilde{x} t^{(i)} | x {t-1}^{(i)})}{q(\tilde{x} t^{(i)} | x {t-1}^{(i)}, z_ t)} \] 如果选择先验密度作为重要性密度（\( q = p(x_ t | x_ {t-1}^{(i)}) \)），则权重更新简化为： \[ w_ t^{(i)} \propto w_ {t-1}^{(i)} \cdot p(z_ t | \tilde{x}_ t^{(i)}) \] \( p(z_ t | \tilde{x}_ t^{(i)}) \) 是 观测似然 ，衡量了在粒子状态 \( \tilde{x}_ t^{(i)} \) 下，实际观测 \( z_ t \) 出现的可能性。似然值越高，权重越大。 计算未归一化的权重后，进行 归一化 ： \[ \tilde{w} t^{(i)} = \frac{w_ t^{(i)}}{\sum {j=1}^{N} w_ t^{(j)}} \] 归一化权重 \( \tilde{w}_ t^{(i)} \) 反映了每个粒子在表征当前后验分布中的相对重要性。 2.3 状态估计输出 当前时刻的状态估计（如最小均方误差估计）可以近似为粒子的加权和： \[ \hat{x} t = E[ x_ t | z {1:t}] \approx \sum_ {i=1}^{N} \tilde{w}_ t^{(i)} \tilde{x}_ t^{(i)} \] 后验分布则由带权重的粒子集合 \(\{ \tilde{x}_ t^{(i)}, \tilde{w}_ t^{(i)} \}\) 近似。 2.4 重采样（Resampling） 这是粒子滤波避免 粒子退化 的关键步骤。经过几轮迭代后，大部分粒子权重会变得非常小（趋近于0），只有少数粒子有显著权重，导致计算资源浪费在无效粒子上。 重采样的目的 ：根据粒子的权重， 有放回地 从当前粒子集合中抽取N次，生成一个新的、等权重的粒子集合 \(\{ x_ t^{(j)}, 1/N \}_ {j=1}^{N}\)。权重大的粒子更可能被多次选中，权重小的粒子很可能被淘汰。 重采样过程 （以系统重采样为例）： 计算归一化权重的累积分布函数（CDF）。 生成N个在[ 0,1]区间均匀分布的随机数 \(\{ u_ j \}\)。 对于每个 \( u_ j \)，找到满足CDF值大于 \( u_ j \) 的最小索引对应的粒子，将其复制到新集合中。 重采样后，粒子集合重新变为等权重 \( 1/N \)，并作为下一时刻迭代的初始粒子集 \(\{ x_ t^{(i)} \}\)。然后令 \( t = t+1 \)，回到步骤2.1。 5. 算法总结与关键点 核心优势 ：适用于任意非线性、非高斯的状态空间模型，通用性强。 关键挑战 ： 粒子退化 ：重采样是标准解决方案，但引入了新问题—— 粒子贫化 （样本多样性丧失，大量粒子重复）。 重要性密度选择 ：先验密度易于实现，但未考虑最新观测，效率较低。最优重要性密度是后验密度本身，但通常难以采样。工程中常通过扩展卡尔曼滤波或无迹卡尔曼滤波生成重要性密度（如无迹粒子滤波UPF），以接近最优。 计算本质 ：用离散的、带权重的随机样本（粒子）来“模拟”连续的概率分布，并通过蒙特卡洛方法计算期望。 6. 与卡尔曼滤波的对比 卡尔曼滤波是粒子滤波在 线性高斯模型 下的最优解析解。当模型为线性、噪声为高斯时，后验分布也是高斯分布，可以用均值和协方差精确描述。粒子滤波则通过大量粒子来近似任意分布，在非线性/非高斯情况下更强大，但计算成本远高于卡尔曼滤波。