PythonRobotics 定位（Localization）模块深度解析：基于贝叶斯滤波的机器人位姿估计实战指南

导读

本指南以 PythonRobotics 仓库中的 定位（Localization）模块文档 为骨架，系统讲解机器人定位的核心问题——如何利用 GNSS、里程计、陀螺仪与 RFID 地标等传感器信息，实时估计机器人的位置与姿态。文章完整继承文档中的五类贝叶斯滤波器（扩展卡尔曼滤波 EKF、无迹卡尔曼滤波 UKF、集合卡尔曼滤波 EnKF、直方图滤波、粒子滤波 PF）的算法推导、滤波设计与参数配置，并结合仓库源码逐一印证实现细节。读完本文，你将掌握每种定位算法的原理公式、状态/观测模型设计方法、关键参数调优方向，以及如何在本地运行对应仿真程序验证效果。

定位问题与贝叶斯滤波家族

什么是机器人定位

根据 定位模块文档 的定义，定位（Localization）是机器人借助 GNSS（全球导航卫星系统）等传感器感知自身位置与姿态的能力。定位是自主移动机器人最基础也最关键的能力之一——无论是路径规划、路径跟踪还是避障，都依赖一个可靠的位姿估计作为前提。

在实际系统中，传感器观测总是带有噪声：GNSS 定位精度受卫星几何与多径效应影响，里程计存在累积漂移，陀螺仪存在零偏。因此，仅靠单一传感器直接读数往往无法得到稳定的位姿估计，业界普遍采用**传感器融合（sensor fusion）**思路，将多个异质传感器的信息在概率框架下融合。文档明确指出：在定位领域，卡尔曼滤波器（Kalman filter）、直方图滤波器（histogram filter）与粒子滤波器（particle filter）等贝叶斯滤波器被广泛使用。

贝叶斯滤波的统一视角

从概率角度看，定位本质上是一个**贝叶斯滤波（Bayesian filter）**问题：给定到当前时刻为止的所有控制输入与观测，估计机器人状态的后验概率分布。PythonRobotics 定位模块中的五类算法，正是贝叶斯滤波在不同假设下的具体实现：

算法	状态分布表示	非线性处理方式	适用场景特点
扩展卡尔曼滤波 EKF	高斯分布（均值+协方差）	一阶泰勒展开（雅可比矩阵）	非线性较弱、需要在线实时计算的系统
无迹卡尔曼滤波 UKF	高斯分布（均值+协方差）	无迹变换（sigma 点）	强非线性系统，精度要求更高
集合卡尔曼滤波 EnKF	蒙特卡洛集合样本	集合统计量替代协方差传播	高维状态、无法解析计算雅可比
直方图滤波	离散网格概率分布	网格化离散化	任意分布、无需初值、计算量随网格爆炸
粒子滤波 PF	加权粒子集合	随机采样+重采样	强非线性、非高斯分布、全局定位

五种算法共享"预测（Predict）—更新（Update）"两步递归结构，区别在于用何种方式表达与传播概率分布。下文将逐一深入每个算法的文档内容与源码实现。

扩展卡尔曼滤波（EKF）定位：GNSS + 里程计的经典融合方案

仿真场景与代码入口

EKF 定位仿真的核心目标是实现传感器融合定位：机器人配备速度传感器、陀螺仪与 GNSS 传感器，EKF 将速度/角速度输入与 GNSS 的 x-y 位置观测融合，输出比任一单传感器都更平滑、更准确的轨迹估计。仿真结果图中（见 EKF 定位仿真图）：

• 蓝色线：真实轨迹（true trajectory）
• 黑色线：航位推算轨迹（dead reckoning trajectory）——仅用里程计/陀螺仪积分，随时间漂移
• 绿色点：定位观测（如 GPS）
• 红色线：EKF 估计轨迹
• 红色椭圆：EKF 估计的协方差椭圆（covariance ellipse）

核心估计函数为 ekf_estimation，定义在 Localization/extended_kalman_filter/extended_kalman_filter.py。

EKF 五步递推算法

文档给出了 EKF 定位的完整递推流程，核心思想是在非线性系统上对运动模型与观测模型做一阶线性化：

预测（Predict）

• 状态预测：x_Pred = F·x_t + B·u_t
• 协方差预测：P_Pred = J_f · P_t · J_fᵀ + Q

更新（Update）

• 观测预测：z_Pred = H·x_Pred
• 新息（innovation）：y = z − z_Pred
• 新息协方差：S = J_g · P_Pred · J_gᵀ + R
• 卡尔曼增益：K = P_Pred · J_gᵀ · S⁻¹
• 状态修正：x_{t+1} = x_Pred + K·y
• 协方差修正：P_{t+1} = (I − K·J_g)·P_Pred

其中 J_f 是运动模型的雅可比矩阵，J_g 是观测模型的雅可比矩阵——这正是"扩展（Extended）"的含义：对非线性函数在当前估计点做一阶泰勒展开。在源码中，上述流程被紧凑地实现为 ekf_estimation，J_f 由 jacob_f 计算，J_g 由 jacob_h 给出。

滤波设计：状态、输入与观测向量

状态向量（4 维）

$$\textbf{x}_t = [x_t,\ y_t,\ \phi_t,\ v_t]$$

其中 x, y 为二维位置坐标，φ 为朝向角，v 为速度。在代码中状态向量名为 xEst，在 main 函数 中初始化为 4×1 零向量。

输入向量（2 维）

$$\textbf{u}_t = [v_t,\ \omega_t]$$

机器人配备速度传感器与陀螺仪，因此每一步的输入是线速度 v 与角速度 ω（yaw rate）。仿真中 calc_input 固定使用 v = 1.0 m/s、yaw_rate = 0.1 rad/s。

观测向量（2 维）

$$\textbf{z}_t = [x_t,\ y_t]$$

机器人配备 GNSS 传感器，每一步可以直接观测 x-y 位置。输入与观测向量都叠加了传感器噪声——源码中的 observation 函数 负责生成带噪的观测与输入。

运动模型与雅可比矩阵

文档给出的连续运动学模型为：

$$\dot{x} = v\cos\phi,\qquad \dot{y} = v\sin\phi,\qquad \dot{\phi} = \omega$$

离散化后得到线性化运动模型 x_{t+1} = F·x_t + B·u_t，其中（Δt 为时间间隔，仿真中 DT = 0.1 s）：

$$F = \begin{bmatrix} 1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&0\end{bmatrix},\qquad B = \begin{bmatrix} \cos\phi\,\Delta t&0\\ \sin\phi\,\Delta t&0\\ 0&\Delta t\\ 1&0\end{bmatrix}$$

对应的运动函数为：

$$\begin{bmatrix} x'\\y'\\\phi'\\v'\end{bmatrix} = f(\textbf{x}, \textbf{u}) = \begin{bmatrix} x + v\cos\phi\,\Delta t\\ y + v\sin\phi\,\Delta t\\ \phi + \omega\,\Delta t\\ v\end{bmatrix}$$

其雅可比矩阵 J_f 为：

$$J_f = \begin{bmatrix} 1&0&-v\sin\phi\,\Delta t&\cos\phi\,\Delta t\\ 0&1&v\cos\phi\,\Delta t&\sin\phi\,\Delta t\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix}$$

源码 jacob_f 的注释逐项列出了四个偏导数（dx/dyaw = -v·dt·sin(yaw) 等），与文档公式完全对应。

观测模型与雅可比矩阵

GNSS 观测模型为线性形式 z_t = g(x_t) = H·x_t，其中：

$$H = \begin{bmatrix} 1&0&0&0\\ 0&1&0&0\end{bmatrix}$$

观测函数直接取状态的前两维 [x, y]ᵀ，其雅可比 J_g 与 H 相同，源码见 jacob_h。

噪声协方差参数（可调）

源码顶部集中定义了 EKF 仿真的全部噪声参数，这些是实际调参时的核心旋钮：

Q = np.diag([
    0.1,              # x 方向位置方差
    0.1,              # y 方向位置方差
    np.deg2rad(1.0),  # 偏航角方差
    1.0               # 速度方差
]) ** 2               # 预测（过程）状态协方差
R = np.diag([1.0, 1.0]) ** 2        # 观测 x,y 位置协方差
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2   # 输入传感器噪声
GPS_NOISE = np.diag([0.5, 0.5]) ** 2                  # GPS 观测噪声
DT = 0.1      # 时间步长 [s]
SIM_TIME = 50.0  # 仿真总时长 [s]

其中 Q 控制对运动模型不确定度的信任程度：Q 越大，滤波器越"相信"观测而弱化运动模型；R 控制对观测噪声的建模，R 越大则滤波结果越平滑但响应越慢。

进阶：带速度比例因子修正的 EKF

文档还介绍了 EKF 定位的一个重要变体——速度比例因子修正（Kalman Filter with Speed Scale Factor Correction），实现于 Localization/extended_kalman_filter/ekf_with_velocity_correction.py。

动机：车辆速度传感器可能因车轮磨损等因素引入比例因子误差（scale factor error），导致速度测量系统性偏差。为此，状态向量在原有 4 维基础上增加一维速度比例因子 s：

$$\textbf{x}_t = [x_t,\ y_t,\ \phi_t,\ v_t,\ s_t]$$

xEst 初始化为 5×1 向量且 xEst[4,0] = 1.0（初始比例因子为 1），而真实仿真中设定 true_scale_factor = 0.9，即真实速度仅为测量值的 90%（见 main 函数）——EKF 的任务就是在定位过程中把 s 从 1.0 逐步修正到 0.9。

修改后的运动模型为：

$$\dot{x} = vs\cos\phi,\qquad \dot{y} = vs\sin\phi,\qquad \dot{\phi} = \omega$$

对应的 B 矩阵变为：

$$B = \begin{bmatrix} \cos\phi\,\Delta t\,s&0\\ \sin\phi\,\Delta t\,s&0\\ 0&\Delta t\\ 1&0\\ 0&0\end{bmatrix}$$

J_f 扩展为 5×5，新增对 s 的偏导数（如 dx/ds = dt·v·cos(yaw)），见源码 jacob_f。观测模型 H 同样扩展为 2×5，只观测前两维位置。其余流程与标准 EKF 完全一致。

运行该脚本时，动画窗口会实时显示 True Velocity Scale Factor 与 Estimated Velocity Scale Factor 两行文本（见 ekf_with_velocity_correction.py），可以直观看到估计的比例因子逐渐收敛到真实值。该变体的仿真结果图见 ekf_with_velocity_correction_1_0.png。

无迹卡尔曼滤波（UKF）定位：sigma 点替代雅可比

UKF 的核心思想

文档指出：UKF 是一种非线性状态估计技术，与使用雅可比矩阵线性化的 EKF 不同，UKF 采用无迹变换（unscented transform）——通过确定性采样的一组 sigma 点来捕捉状态分布的均值与协方差。这样做的好处是避免了 EKF 中繁琐且易出错的雅可比计算，并能在强非线性系统中获得更高精度。

核心估计函数为 ukf_estimation，定义在 Localization/unscented_kalman_filter/unscented_kalman_filter.py，仿真场景与 EKF 完全一致（蓝线真实轨迹、黑线航位推算、绿点 GPS 观测、红线 UKF 估计、红椭圆协方差）。

UKF 递推算法

预测（Predict）

1. 在当前状态估计周围生成 sigma 点：
   • χ₀ = x_t
   • χᵢ = x_t + γ·√P_tᵢ（i = 1…n）
   • χᵢ = x_t − γ·√P_tᵢ₋ₙ（i = n+1…2n）
   • 其中 γ = √(n + λ)，λ = α²(n + κ) − n
2. 将 sigma 点通过运动模型传播：χᵢ⁻ = f(χᵢ, u_t)
3. 计算预测均值：x⁻_{t+1} = Σ wᵐᵢ·χᵢ⁻
4. 计算预测协方差：P⁻_{t+1} = Σ wᶜᵢ(χᵢ⁻ − x⁻_{t+1})(χᵢ⁻ − x⁻_{t+1})ᵀ + Q

更新（Update）

1. 围绕预测状态重新生成 sigma 点
2. 将 sigma 点通过观测模型传播：Zᵢ = h(χᵢ)
3. 计算观测预测均值：z⁻_{t+1} = Σ wᵐᵢ·Zᵢ
4. 计算新息协方差：S_t = Σ wᶜᵢ(Zᵢ − z⁻_{t+1})(Zᵢ − z⁻_{t+1})ᵀ + R
5. 计算互相关矩阵：P_xz = Σ wᶜᵢ(χᵢ − x⁻_{t+1})(Zᵢ − z⁻_{t+1})ᵀ
6. 计算卡尔曼增益：K = P_xz·S_t⁻¹
7. 更新状态：x_{t+1} = x⁻_{t+1} + K(z_t − z⁻_{t+1})
8. 更新协方差：P_{t+1} = P⁻_{t+1} − K·S_t·Kᵀ

Sigma 点权重公式

均值权重与协方差权重分别为：

$$w^m_0 = \frac{\lambda}{n+\lambda},\qquad w^m_i = \frac{1}{2(n+\lambda)}\ (i=1\ldots 2n)$$

$$w^c_0 = \frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta),\qquad w^c_i = \frac{1}{2(n+\lambda)}\ (i=1\ldots 2n)$$

参数含义：

• α（ALPHA）：控制 sigma 点围绕均值的散布程度，通常 0.001 ≤ α ≤ 1，值越小 sigma 点越靠近均值
• β（BETA）：融入分布先验知识，对高斯分布取 β = 2 为最优
• κ（KAPPA）：次级缩放参数，通常取 0 或 3 − n（n 为状态维度）
• n：状态向量维度

这些参数共同决定 λ = α²(n + κ) − n，进而影响 sigma 点散布与权重。源码中 setup_ukf 实现了权重与 γ 的计算，仿真采用的具体取值为 ALPHA = 0.001、BETA = 2、KAPPA = 0。

滤波设计细节

UKF 仿真的状态、输入、观测向量及运动/观测模型与标准 EKF 完全相同（4 维状态 [x, y, φ, v]、输入 [v, ω]、观测 [x, y]、Δt = 0.1 s），因此下文仅列出文档与源码中给出的具体数值参数：

过程噪声协方差 Q（文档给出，与源码一致）：

$$Q = \begin{bmatrix} 0.1^2&0&0&0\\ 0&0.1^2&0&0\\ 0&0&(0.017)^2&0\\ 0&0&0&1.0^2\end{bmatrix}$$

观测噪声协方差 R：

$$R = \begin{bmatrix} 1.0^2&0\\ 0&1.0^2\end{bmatrix}$$

sigma 点生成使用 scipy.linalg.sqrtm 计算协方差矩阵平方根，见 generate_sigma_points。

UKF 相对 EKF 的优势

文档总结了 UKF 的五个优势：无需计算雅可比矩阵（对复杂非线性系统更易实现、更不易出错）、精度更高（对均值与协方差的逼近达到二阶，对高斯分布可达三阶，而 EKF 仅有一阶精度）、对非线性变换后的分布逼近更好、实现更简单（不需要解析导数）、数值稳定性更好（尤其对强非线性系统）。

集合卡尔曼滤波（EnKF）定位：基于 RFID 地标的蒙特卡洛方法

算法背景与仿真场景

EnKF 是卡尔曼滤波的一种蒙特卡洛方法（Monte Carlo approach）：与经典卡尔曼滤波解析传播均值与协方差不同，EnKF 用一组集合成员（ensemble members）的样本来估计状态及其不确定性。文档强调，EnKF 的独特之处在于假设机器人可以测量到 RFID 地标的距离与方位角（bearing angle），这些测量被用于 EnKF 定位。

核心函数为 enkf_localization，实现于 Localization/ensemble_kalman_filter/ensemble_kalman_filter.py。仿真图中蓝线为真实轨迹、黑线为航位推算、红线为 EnKF 估计、红椭圆为协方差椭圆，同时以 *k 星号标出 4 个 RFID 地标位置（见 main 中的 RF_ID 定义）。

EnKF 算法流程

预测（Predict）——对每个集合成员 i = 1, …, N：

1. 给控制输入叠加随机噪声：uⁱ = u + ε_u，其中 ε_u ~ N(0, R)
2. 预测状态：xⁱ_pred = f(xⁱ_t, uⁱ)
3. 预测观测：zⁱ_pred = h(xⁱ_pred) + ε_z，其中 ε_z ~ N(0, Q)

更新（Update）：

1. 计算状态集合均值：x̄ = (1/N)·Σ xⁱ_pred
2. 计算观测集合均值：z̄ = (1/N)·Σ zⁱ_pred
3. 计算状态偏差：X' = xⁱ_pred − x̄
4. 计算观测偏差：Z' = zⁱ_pred − z̄
5. 计算协方差矩阵：U = (1/(N−1))·X'Z'ᵀ、V = (1/(N−1))·Z'Z'ᵀ
6. 计算卡尔曼增益：K = U·V⁻¹
7. 更新每个集合成员：xⁱ_{t+1} = xⁱ_pred + K(z_obs − zⁱ_pred)
8. 最终状态估计：x_est = (1/N)·Σ xⁱ_{t+1}
9. 协方差估计：P_est = (1/N)·Σ (xⁱ_{t+1} − x_est)(xⁱ_{t+1} − x_est)ᵀ

上述第 5–9 步在源码中对应 enkf_localization 的 x_dif/z_dif 偏差矩阵计算、U/V 协方差计算与 px_hat = px + K@(...) 集合更新。

滤波设计

• 状态向量（4 维）：x_t = [x_t, y_t, φ_t, v_t]ᵀ，与 EKF/UKF 相同
• 集合规模：N = 20（源码中 NP = 20，见 ensemble_kalman_filter.py）
• 输入向量：u_t = [v_t, ω_t]ᵀ（速度传感器 + 陀螺仪），输入噪声协方差 R = diag(σ_v², σ_ω²)
• 观测向量：z_t = [d_t, θ_t, x_lm, y_lm]，其中 d_t 为到地标的距离、θ_t 为地标方位角、(x_lm, y_lm) 为已知地标位置；观测噪声协方差 Q = diag(σ_d², σ_θ²)
• 最大观测范围：MAX_RANGE = 20.0 m（超出范围的地标不可见）

运动模型：与 EKF/UKF 相同的自行车式模型 ẋ = v·cosφ、ẏ = v·sinφ、φ̇ = ω、v̇ = 0，离散化后 F、B 矩阵与 EKF 完全一致。

观测模型：对位于 (x_lm, y_lm) 的每个地标，根据观测距离 d 与方位角 θ 反推其在全局系下的期望位置：

$$\begin{bmatrix} x_{lm,obs}\\ y_{lm,obs}\end{bmatrix} = \begin{bmatrix} x + d\cos(\phi+\theta)\\ y + d\sin(\phi+\theta)\end{bmatrix}$$

随后将其与地标真实位置比较完成更新。源码中 observe_landmark_position 实现了该投影。

EnKF 的优势

文档总结了 EnKF 的四点优势：无需雅可比矩阵（对非线性系统更易实现）；能处理非高斯分布（集合表示可以捕捉状态分布的非高斯特征）；计算高效（对高维系统，维护集合样本比维护完整协方差矩阵更省）；易于并行化（每个集合成员可以独立传播）。

直方图滤波（Histogram Filter）定位：无需初值的离散网格方法

仿真场景

直方图滤波定位是一个 2D 定位示例：红色十字为真实位置，黑色点为 RFID 位置，蓝色网格显示直方图滤波的位置概率。该仿真假设机器人的偏航角（yaw）与 RFID 位置已知，但 x、y 位置未知，滤波器使用速度输入与来自 RFID 的距离观测进行定位，且不需要初始位置信息。

核心函数为 histogram_filter_localization，定义在 Localization/histogram_filter/histogram_filter.py。注意该仿真的网格参数与文档提到的 gaussian_grid_map（高斯网格地图）存在衔接关系：当初始位置以高斯分布形式给出时，可参考 Mapping/gaussian_grid_map/gaussian_grid_map.py 生成初始概率分布。

四步滤波流程

直方图滤波是连续空间中的离散贝叶斯滤波器（discrete Bayes filter）：用规则网格管理机器人存在的概率，网格概率越高，机器人越可能位于该格。文档将整个流程拆解为 4 步：

Step 1：滤波器初始化

直方图滤波不需要初始位置信息——此时可将每个网格的概率初始化为相同值（均匀分布）。若已知初始位置，则可依据它设置初始概率；若初始位置以高斯分布给出，可借助 gaussian_grid_map 生成初始分布。

Step 2：运动预测概率

机器人移动到下一个网格时，所有网格的概率信息沿运动方向整体平移，以表达概率分布随运动的变化。运动之后，分布还需反映运动带来的估计误差：文档对比了两张图——有观测时位置概率呈尖峰状（下图左），无观测时概率迅速弥散变宽（下图右）：

仿真中使用 SciPy 的 scipy.ndimage.gaussian_filter 为概率分布叠加高斯噪声（对应源码 histogram_filter.py 的导入与 MOTION_STD = 1.0 参数）。

Step 3：观测更新概率

所有概率由观测按贝叶斯更新公式更新，具体公式因传感器模型而异。本仿真采用距离观测模型（range observation model）：

$$p_t = p_{t-1}\cdot h(z),\qquad h(z) = \frac{\exp\left(-(d-z)^2/2\right)}{\sqrt{2\pi}}$$

其中 p_t 为时刻 t 的概率，h(z) 为观测 z 的观测概率，d 为从 RFID 到网格中心的已知距离。文档给出了 d = 3.0 时 h(z) 的分布曲线图，以及观测到一个 RFID 时观测概率呈圆环状分布的示意图：

源码中 observation_update 对每个观测遍历所有网格并逐格乘以 calc_gaussian_observation_pdf 计算的似然，随后调用 normalize_probability 归一化。

Step 4：从概率估计位置

每个时间步可从当前概率分布计算最终机器人位置，文档给出两种方式：

1. 取概率最大网格的位置（MAP 估计）
2. 按概率加权求平均的网格位置（期望估计）

关键网格参数

源码顶部的参数定义了网格地图的规模与噪声特性，调整这些参数直接影响定位精度与计算量：

EXTEND_AREA = 10.0   # [m] 网格地图扩展长度
SIM_TIME = 50.0      # 仿真时间 [s]
DT = 0.1             # 时间步长 [s]
MAX_RANGE = 10.0     # 最大观测范围 [m]
MOTION_STD = 1.0     # 运动高斯分布标准差
RANGE_STD = 3.0      # 观测高斯分布标准差
XY_RESOLUTION = 0.5  # 网格分辨率 [m]
MIN_X, MIN_Y = -15.0, -5.0   # 地图边界
MAX_X, MAX_Y = 15.0, 25.0
NOISE_RANGE = 2.0    # 距离噪声 1σ [m]
NOISE_SPEED = 0.5    # 速度噪声 1σ [m/s]

其中 XY_RESOLUTION 决定网格粗细：分辨率越小定位越精细，但网格数量按平方增长、计算开销显著增大。

粒子滤波（Particle Filter）定位：RFID 距离观测的加权粒子方法

仿真场景

粒子滤波定位同样是一种传感器融合定位：蓝线为真实轨迹、黑线为航位推算、红线为 PF 估计轨迹。假设机器人可以测量到 RFID 地标的距离，这些测量被用于 PF 定位。与 EnKF 类似，PF 也属于基于采样的方法，但通过**重采样（re-sampling）**机制维持粒子多样性，能表达非高斯分布。

核心函数为 pf_localization，定义在 Localization/particle_filter/particle_filter.py。

由粒子计算协方差矩阵

文档重点给出了如何从粒子信息计算协方差矩阵的公式。对协方差矩阵元素 Ξ_{j,k}（第 j 行第 k 列）：

$$\Xi_{j,k} = \frac{1}{1 - \sum_{i=1}^{N}(w^i)^2}\sum_{i=1}^{N} w^i (x^i_j - \mu_j)(x^i_k - \mu_k)$$

其中：

• w^i：第 i 个粒子的权重
• x^i_j：第 i 个粒子的第 j 个状态分量
• μ_j：所有粒子第 j 个状态分量的均值

源码中 calc_covariance 逐项实现了该公式，其分母 1/(1 − pw@pw.T) 正是上式中的 1/(1 − Σ(w^i)²) 因子。

滤波参数

Q = np.diag([0.2]) ** 2                # 距离误差
R = np.diag([2.0, np.deg2rad(40.0)]) ** 2  # 输入误差（速度+角速度）
Q_sim = np.diag([0.2]) ** 2            # 仿真距离噪声
R_sim = np.diag([1.0, np.deg2rad(30.0)]) ** 2  # 仿真输入噪声
DT = 0.1          # 时间步长 [s]
SIM_TIME = 50.0   # 仿真时间 [s]
MAX_RANGE = 20.0  # 最大观测范围 [m]
NP = 100          # 粒子数量
NTh = NP / 2.0    # 重采样阈值：有效粒子数低于该值时触发重采样

NP = 100 是粒子数，NTh = NP/2.0 表示当有效粒子数（1/Σ(wⁱ)²）低于粒子总数一半时执行重采样——这是 PF 防止粒子退化（particle degeneracy）的核心机制。

五种定位算法的选型对比

综合文档与源码，可以从以下几个维度给出选型建议：

维度	EKF	UKF	EnKF	直方图滤波	粒子滤波
分布假设	高斯	高斯	集合近似	任意离散	任意（采样近似）
非线性处理	一阶线性化	无迹变换	集合统计	网格离散	随机采样
是否需要雅可比	需要	不需要	不需要	不需要	不需要
初始位置	需要（高斯先验）	需要	需要	不需要	可以全局初始化
计算复杂度	低	中	中	随网格数平方增长	随粒子数线性增长
典型传感器	GNSS+里程计+陀螺仪	同左	RFID 距离/方位	RFID 距离	RFID 距离

从源码结构看（Localization/ 目录下五个独立目录），这五套实现共享相同的运动模型（F、B 矩阵）与仿真框架（observation、calc_input、main 中的可视化循环），差异集中在概率表示的载体与更新数学上——这恰好印证了文档"贝叶斯滤波器家族"的统一视角。

运行与验证

运行仿真

所有定位示例均为可直接执行的 Python 脚本，进入对应目录后直接运行即可，例如：

python Localization/extended_kalman_filter/extended_kalman_filter.py
python Localization/extended_kalman_filter/ekf_with_velocity_correction.py
python Localization/unscented_kalman_filter/unscented_kalman_filter.py
python Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
python Localization/histogram_filter/histogram_filter.py
python Localization/particle_filter/particle_filter.py

运行环境依赖见 requirements/requirements.txt（numpy、scipy、matplotlib 等），也可直接使用 requirements/environment.yml 创建 Conda 环境。仿真动画中可按 Esc 键提前退出（各脚本 main 中均有 key_release_event 监听）。

自动化测试

仓库为每个定位算法提供了单元测试（位于 tests/），例如 tests/test_extended_kalman_filter.py、tests/test_ensemble_kalman_filter.py、tests/test_histogram_filter.py、tests/test_particle_filter.py、tests/test_unscented_kalman_filter.py，直接调用各模块的估计函数并断言状态估计的收敛性。修改滤波器实现后，可通过这些测试快速回归验证。

小结

围绕 定位模块总览文档，PythonRobotics 提供了从经典卡尔曼族（EKF、UKF、EnKF）到非参数方法（直方图滤波、粒子滤波）的完整定位算法谱系。所有示例共享统一的"预测—更新"贝叶斯框架与运动学模型，差异仅在概率分布的表达方式：

• EKF 以雅可比线性化处理非线性，计算最轻，适合弱非线性在线融合（GNSS + 里程计），并提供速度比例因子修正变体应对车轮磨损等标定误差；
• UKF 用 sigma 点无迹变换逼近非线性，免去雅可比推导，精度更高；
• EnKF 以蒙特卡洛集合近似分布，适合高维、免雅可比场景；
• 直方图滤波以离散网格表达任意分布且无需初始位置，适合全局定位；
• 粒子滤波以加权粒子 + 重采样应对强非线性与非高斯分布。

对照文档中每个算法的数学推导阅读 Localization/ 下的源码，再运行仿真观察轨迹与协方差椭圆的收敛行为，即可快速建立起对机器人概率定位的完整实战认知。