# 四元数基本定义
四元数是一个4维数，可以写成：
$$q = w + xi + yj + zk$$

或者用向量形式：
$$q = [w, x, y, z]^T$$



其中：
* $w$ 是实部（标量部分）
* $x, y, z$ 是虚部（向量部分）
* $i, j, k$ 是虚数单位，满足：$i^2 = j^2 = k^2 = ijk = -1$

可以把它看作：
一个标量 $w$（实部）+一个3维向量 $[x, y, z]$（虚部）

* 第1维（$w$）：旋转的"量"（标量部分）
* 第2-4维（$x, y, z$）：旋转的"方向"（向量部分）

# 四元数的几何意义
想象一个4维空间：
* 第1维：$w$（实部，标量）
* 第2-4维：$x, y, z$（虚部，向量）

单位四元数：
* 所有满足 $w^2 + x^2 + y^2 + z^2 = 1$ 的四元数
* 构成4维空间中的单位球面（3维球面）
* 类比：
    * 2D单位圆：$x^2 + y^2 = 1$
    * 3D单位球：$x^2 + y^2 + z^2 = 1$
    * 4D单位超球：$w^2 + x^2 + y^2 + z^2 = 1$

只有单位四元数才能表示旋转

# 如何用四元数表示旋转

给定：
旋转轴：$\mathbf{n} = [n_x, n_y, n_z]$（单位向量）；
旋转角度：$\theta$

对应的单位四元数：

$$q = [\cos(\theta/2), \sin(\theta/2) \cdot n_x, \sin(\theta/2) \cdot n_y, \sin(\theta/2) \cdot n_z]$$，四元数用"半角"表示旋转

或者：

$$q = [\cos(\theta/2), \sin(\theta/2) \cdot \mathbf{n}]$$

例:

* 旋转180度：$\theta = 180°$，所以 $\theta/2 = 90°$
四元数：$q = [\cos(90°), \sin(90°) \cdot \mathbf{n}] = [0, \mathbf{n}]$
* 绕Z轴旋转90度
    * 旋转轴：$\mathbf{n} = [0, 0, 1]$（Z轴方向）
    * 旋转角度：$\theta = 90°$
    * 计算：$\cos(45°) \approx 0.707$，$\sin(45°) \approx 0.707$
    * 四元数：$q = [0.707, 0, 0, 0.707]$
    * $$0.707^2 + 0^2 + 0^2 + 0.707^2 = 0.5 + 0.5 = 1$$
* 绕X轴旋转180度
    * 旋转轴：$\mathbf{n} = [1, 0, 0]$（X轴方向）
    * 旋转角度：$\theta = 180°$
    * 计算：$\cos(90°) = 0$，$\sin(90°) = 1$
    * 四元数：$q = [0, 1, 0, 0]$

# 四元数的模长
定义：$$||q|| = \sqrt{w^2 + x^2 + y^2 + z^2}$$
* 单位四元数： $||q|| = 1$


几何意义：
- 四元数到原点的距离
- 单位四元数在单位球面上
- 每个点对应一个旋转

# 四元数乘法（旋转复合）
* 符号： $\otimes$
* 含义： 先应用旋转 $q_1$，再应用旋转 $q_2$
* 公式：
$$q_1 \otimes q_2 = [w_1, \mathbf{v}1] \otimes [w_2, \mathbf{v}_2]$$
$$= [w_1w_2 - \mathbf{v}1 \cdot \mathbf{v}_2, w_1\mathbf{v}_2 + w_2\mathbf{v}_1 + \mathbf{v}_1 \times \mathbf{v}_2]$$

实部：$w_1w_2$ 减去向量点积

虚部：$w_1\mathbf{v}2 + w_2\mathbf{v}_1$ 加上向量叉积

例子：

$q_1 = [0.707, 0.707, 0, 0]$（绕Z轴转90度）

$q_2 = [0.707, 0, 0.707, 0]$（绕Y轴转90度）

计算 $q_1 \otimes q_2$：
* 实部：$0.707 \times 0.707 - (0.707, 0, 0) \cdot (0, 0.707, 0) = 0.5 - 0 = 0.5$
* 虚部：$0.707 \times (0, 0.707, 0) + 0.707 \times (0.707, 0, 0) + (0.707, 0, 0) \times (0, 0.707, 0)= (0, 0.5, 0) + (0.5, 0, 0) + (0, 0, 0.5) = (0.5, 0.5, 0.5)$
* 结果：$q_1 \otimes q_2 = [0.5, 0.5, 0.5, 0.5]$

# 为什么用四元数而不是旋转矩阵
* 旋转矩阵的问题：
    * 冗余：9个数字表示3个自由度
    * 约束多：必须是正交矩阵，行列式为1
    * 插值困难
* 四元数的优势：
    * 紧凑：4个数字表示3个自由度
    * 插值简单
    * 计算效率高

无人机 四元数

# 阶段1：初始位姿估计
## EPnP计算位姿：
* 核心思想是将任意个3D点表示为4个虚拟的控制点的加权组合，然后在控制点坐标系下表达投影关系，最后求解相机的初始位姿。



流程：用少数控制点（4个）表示所有3D点；通过求解控制点的相机坐标，间接得到位姿
```
输入：
  - N个3D点（世界坐标系）
  - N个2D点（图像坐标）
  - 相机内参（f_x, f_y, c_x, c_y）

步骤1：构建4个控制点（世界坐标系）
  ↓
步骤2：将3D点表示为控制点的线性组合
  ↓
步骤3：建立投影方程（关于控制点相机坐标的线性方程）
  ↓
步骤4：构建线性方程组 A·β = 0
  ↓
步骤5：SVD求解，得到控制点的相机坐标
  ↓
步骤6：从控制点恢复位姿 R, t
  ↓
输出：相机位姿（旋转矩阵R和平移向量t）
```
##  计算步骤
## 1：构建控制点（世界坐标系）——方法：基于特征分解的均匀分布选择方法
输入：N个3D点（世界坐标系，已知）

1. 计算质心
2. 中心化处理
3. 计算协方差矩阵并特征分解
4. 定义4个控制点$ C_1, C_2, C_3, C_4$

输出：4个控制点在世界坐标系的位置

## 2：将3D点表示为控制点的线性组合
对每个3D点 $P_i$：

$P_i=\sum_{j=1}^4 \alpha_{i j} C_j$

计算权重 $\alpha_{i j}$（这些权重是固定的，只依赖于3D点的几何关系）

## 3：建立投影方程
$C_j^c=R C_j+t \Rightarrow P_i^c=\sum_{j=1}^4 \alpha_{i j} C_j^c$
* 控制点在世界坐标系$ C_j$经过R、t变换到相机坐标系$C_j^c$，3D点在相机坐标系的位置是：
$ P_i^c = α_{i1}×C_1^c + α_{i2}×C_2^c + α_{i3}×C_3^c + α_{i4}×C_4^c$
 
 将$P_i^c$ 投影到图像：
 
 $$
u_i=f_x \frac{X_i^c}{Z_i^c}+c_x
$$


$$
v_i=f_y \frac{Y_i^c}{Z_i^c}+c_y
$$

代入 $P_i^c$ 的表达式，得到关于 $C_j^c$ 的线性方程：

$X_i^c=\sum_j^4 \alpha_{i j} X_j^c, \quad Y_i^c=\sum_j^4 \alpha_{i j} Y_j^c, \quad Z_i^c=\sum_j^4 \alpha_{i j} Z_j^c$

## 4：构建线性方程组
最终的投影关系是控制点在相机坐标系下的线性组合。联合上述公式，对每个点建立方程组:

$\begin{aligned} & \sum_{j=1}^4\left(f_x \alpha_{i j} X_j^c-\left(u_i-c_x\right) \alpha_{i j} Z_j^c\right)=0 \\ & \sum_{j=1}^4\left(f_y \alpha_{i j} Y_j^c-\left(v_i-c_y\right) \alpha_{i j} Z_j^c\right)=0\end{aligned}$

这些方程最终构成一个的线性方程组：
$A \cdot \beta=0$

其中：
  A: 系数矩阵（2N × 12）
  β: 未知向量（12 × 1），包含4个控制点的相机坐标
  
  ## 5：SVD求解
使用SVD分解：$A = U × Σ × V^T$

β 的解 = V 的最后一列（对应最小奇异值）

得到：4个控制点在相机坐标系的位置 $C_1^c, C_2^c, C_3^c, C_4^c$


## 6：从控制点恢复位姿
已知：
  - 控制点在世界坐标系：$C_1, C_2, C_3, C_4$
  - 控制点在相机坐标系：$C_1^c, C_2^c, C_3^c, C_4^c$

求解：
  - 旋转矩阵 R 和平移向量 t
  - 使得：$C_j^c = R × C_j + t$

通过最小二乘或直接计算得到 R 和 t，即求解得到相机的初始外参矩阵。

## 获取2D-3D对应关系
* 问题：图像中检测到LED标记的2D坐标，但不知道每个2D点对应哪个3D标记点。
例：
```
目标上有5个LED标记（编号：LED1, LED2, LED3, LED4, LED5）
图像中检测到5个光点（检测点：D1, D2, D3, D4, D5）

问题：图像中的D1对应哪个LED？D2对应哪个LED？...
```
而位姿估计需要建立2D-3D对应关系：

```
已知：
  - 3D点：LED_A在(0.0, 0.0, 0.0)
  - 2D点：图像中某点在(100, 150)
  
求解：
  - 相机的位姿（旋转R和平移t）
  
条件：
  - 必须知道(100, 150)这个2D点就是LED_A的投影
```
* 解决：需要匹配所有检测点与标记点

## 步骤1：枚举所有可能匹配
$N = C(n_d, 5) × P(n_l, 5)$
* 从检测点中选择5个
* 记点中选择5个进行排列
* 所有可能的匹配组合

## 步骤2：用EPnP验证每个匹配
对每个匹配组合：
### 1、PnP计算位姿

```
匹配组合1：
  检测点1(100, 150) → LED_A(0.0, 0.0, 0.0)
  检测点2(200, 150) → LED_B(0.1, 0.0, 0.0)
  检测点3(150, 250) → LED_C(0.05, 0.1, 0.0)
  检测点4(180, 200) → LED_D(0.08, 0.05, 0.0)
  检测点5(120, 180) → LED_E(0.02, 0.08, 0.0)
```
#### 1.1、选择4个点用于EPnP计算
从5个匹配中随机选择4个

```
用于EPnP的4个点：
  点1: (100, 150) → (0.0, 0.0, 0.0)
  点2: (200, 150) → (0.1, 0.0, 0.0)
  点3: (150, 250) → (0.05, 0.1, 0.0)
  点4: (180, 200) → (0.08, 0.05, 0.0)
```
#### 1.2 构建控制点
* 计算质心
* 中心化处理
* 计算协方差矩阵并特征分解
* 定义4个控制点
*  将3D点表示为控制点的线性组合
#### 1.3 建立投影方程

对每个2D-3D对应点，建立两个方程：
对点1: (100, 150) → (0.0, 0.0, 0.0)

$$Σ_{j=1}^4 (f_x × α_{1j} × X_j^c - (100 - c_x) × α_{1j} × Z_j^c) = 0$$

$$Σ_{j=1}^4 (f_y × α_{1j} × Y_j^c - (150 - c_y) × α_{1j} × Z_j^c) = 0$$

对点2, 3, 4 类似...
#### 1.4 构建线性方程组
将所有方程组合：A · β = 0

其中：
* A: 8×12 矩阵（4个点 × 2个方程/点 = 8个方程）
* β: 12×1 向量（4个控制点 × 3个坐标 = 12个未知数）
   = $$[X_1^c, Y_1^c, Z_1^c, X_2^c, Y_2^c, Z_2^c, X_3^c, Y_3^c, Z_3^c, X_4^c, Y_4^c, Z_4^c]^T$$
#### 1.5 SVD求解
#### 1.6 从控制点恢复位姿
已知：
  - 控制点在世界坐标系：$$C_1, C_2, C_3, C_4$$
  - 控制点在相机坐标系：$$C_1^c, C_2^c, C_3^c, C_4^c$$（从β得到）

求解：
  - 旋转矩阵 R 和平移向量 t
  - 使得：$$C_j^c = R × C_j + t$$

通过最小二乘或直接计算得到 R 和 t，即结果：得到位姿 (R, t)
### 2、位姿将剩余3D点投影到图像
假设剩余的点是第5个点：

```
剩余点：LED_E(0.02, 0.08, 0.0) 在世界坐标系
```
#### 2.1：转换到相机坐标系
$$P_E^c = R × P_E + t= R × (0.02, 0.08, 0.0) + t= (X_E^c, Y_E^c, Z_E^c)$$
#### 2.2：投影到图像平面
$$u_E' = f_x × (X_E^c / Z_E^c) + c_x$$

$$v_E' = f_y × (Y_E^c / Z_E^c) + c_y$$

结果：投影点 $$(u_E', v_E')$$ = (125, 185)假设
### 3、投影误差
* 实际检测点：检测点5 = (120, 180)
* 投影点：$$(u_E', v_E') $$= (125, 185)

投影误差 = √[(120-125)² + (180-185)²]
         = √[25 + 25]
         = √50
         ≈ 7.07 像素
### 4、判断是否可能是正确匹配
如果误差小 → 可能是正确匹配
* 如果投影误差 < 阈值（例如5像素）：
     * 该匹配可能是正确的，记录该匹配关系
  
* 如果投影误差 ≥ 阈值：
  * 该匹配可能是错误的，丢弃
## 步骤3：频率与概率直方图筛选
问题：
* 即使重投影误差小，仍可能因噪声导致错误匹配
* 需要更可靠的筛选机制

思路：
* 统计每个匹配关系出现的频率
* 频率高的匹配更可能是正确的

 
### 计算匹配频率
 假设有600个候选匹配组合，对每个组合：
* 用EPnP计算位姿
* 计算剩余点的重投影误差
* 如果误差 < 5像素 → 记录该匹配关系
* 记录统计次数

```
匹配关系统计：
  LED_A ↔ 检测点1: 出现了 50 次
  LED_A ↔ 检测点2: 出现了 10 次
  LED_A ↔ 检测点3: 出现了 5 次
  LED_A ↔ 检测点4: 出现了 2 次
  
  LED_B ↔ 检测点1: 出现了 8 次
  LED_B ↔ 检测点2: 出现了 45 次  ← 这个匹配出现最频繁！
  LED_B ↔ 检测点3: 出现了 12 次
  
  LED_C ↔ 检测点3: 出现了 48 次
  LED_C ↔ 检测点4: 出现了 15 次
  
  ...（其他匹配关系）
```
构建频率直方图 H(L_i, D_j)

### 计算匹配概率
$H_p\left(L_i, D_j\right)=\frac{H\left(L_i, D_j\right)}{\sum_{i=1}^{n_l} H\left(L_i\right)} \cdot \frac{H\left(L_i, D_j\right)}{\sum_{j=1}^{n_d} H\left(D_j\right)}$

上述公式实际上是计算两个条件概率的乘积：

$$H_p(L_i, D_j) = P(D_j | L_i) × P(L_i | D_j)$$

其中：
$$P(D_j | L_i) = H(L_i, D_j) / Σ_{k=1}^{n_l} H(L_k)$$
              = 给定$$LED_i$$，观察到检测点_j的概率

$$P(L_i | D_j) = H(L_i, D_j) / Σ_{m=1}^{n_d} H(D_m)$$
              = 给定检测点_j，它来自$$LED_i$$的概率

* 双向验证：同时考虑“LED→检测点”和“检测点→LED”的概率
* 归一化：消除某些LED或检测点频繁匹配的影响
* 置信度：概率越高，匹配越可靠
### 选择最优匹配
#### 1：按概率排序

```
将所有匹配按概率从高到低排序：

1. LED_A ↔ 检测点1: 概率 = 0.0230  ← 最高！
2. LED_C ↔ 检测点3: 概率 = 0.0212
3. LED_B ↔ 检测点2: 概率 = 0.0186
4. ...
```
#### 2：构建最优匹配组合

```
步骤1：选择概率最高的：LED_A ↔ 检测点1
步骤2：在剩余匹配中，选择概率最高的：LED_C ↔ 检测点3
步骤3：继续选择，直到所有LED都有匹配
```
#### 3：计算匹配组合的总置信度

```
对每个完整的匹配组合：
  总置信度 = 所有匹配概率的乘积（或加权和）
  
选择总置信度最高的组合作为最优匹配
```

## 最终实现的目标
1. 相机拍摄图像 → 检测到5个LED光点
2. 不知道每个光点对应哪个LED标记
3. 运行本流程：
   - 枚举600个可能匹配
   - EPnP验证每个匹配
   - 频率概率筛选
   - 得到最优匹配 + 初始位姿
4. 输出：目标在相机前方1.2米，偏右15度，得到目标在3D空间中的准确位置和姿态

**解决了2D-3D匹配问题：自动确定图像中的每个检测点对应目标上的哪个LED标记**

**计算了初始位姿：得到相机（或目标）在3D空间中的位置和姿态**

```
旋转矩阵 R = [r11, r12, r13;
              r21, r22, r23;
              r31, r32, r33]

平移向量 t = [tx, ty, tz]^T
```
R 和 t 描述了相机相对于目标（或目标相对于相机）的位置和朝向，例如：目标在相机前方1米，向右偏30度

提供了准确的初始位姿也为后续的粒子滤波跟踪提供准确的起点

```
重投影误差 = 实际检测点与投影点的距离
步骤1：用位姿将3D点投影到图像
  LED_A (0.0, 0.0, 0.0) → 用位姿投影 → (105, 155)

步骤2：与实际检测点比较
  实际检测点：(100, 150)
  投影点：(105, 155)
  
步骤3：计算距离
  误差 = √[(100-105)² + (150-155)²] = 7.07像素

---------
最优位姿（最小重投影误差）：
  R_optimal, t_optimal  ← 作为基准位姿

其他可信位姿（误差在容忍范围内，范围假设为5像素）：
  R_1, t_1  ← 粒子1
  R_2, t_2  ← 粒子2
  R_3, t_3  ← 粒子3
  ...
  
 例子：
 最优位姿（基准）：
  R_optimal, t_optimal
  重投影误差 = 2.1像素
  匹配：LED_A↔D1, LED_B↔D2, LED_C↔D3, LED_D↔D4, LED_E↔D5
  
其他可信位姿1：
  R_1, t_1
  重投影误差 = 3.5像素
  匹配：LED_A↔D1, LED_B↔D2, LED_C↔D3, LED_D↔D5, LED_E↔D4

其他可信位姿2：
  R_2, t_2
  重投影误差 = 4.2像素
  匹配：LED_A↔D2, LED_B↔D1, LED_C↔D3, LED_D↔D4, LED_E↔D5

其他可信位姿3：
  R_3, t_3
  重投影误差 = 4.8像素
  匹配：LED_A↔D1, LED_B↔D3, LED_C↔D2, LED_D↔D4, LED_E↔D5
```
* 基准位姿：最可能是正确的位姿
    * 作为粒子滤波的"中心粒子"
    * 作为其他粒子的参考点
    * 如果其他粒子都失效，至少有一个可靠的基准 
* 其他可信位姿
    *  作为粒子滤波的初始粒子
    *  覆盖不确定性范围
    *  提供多个假设，提高鲁棒性 


 # 阶段2：粒子滤波逐帧跟踪
 粒子滤波初始化， 使用阶段1的输出：
 * 最优位姿 → 基准粒子
 *  其他可信位姿 → 初始粒子    
 *   在基准周围生成更多粒子   
 
```
粒子1：R_optimal, t_optimal（基准，误差2.1）← 最重要
粒子2：R_1, t_1（误差3.5）
粒子3：R_2, t_2（误差4.2）
粒子4：R_3, t_3（误差4.8）
粒子5：R_optimal + 小噪声（在基准周围）
粒子6：R_optimal + 小噪声（在基准周围）
...
粒子100：R_optimal + 小噪声（在基准周围）
```
初始化完成：100个粒子就绪 
## 预测的信息：目标的位姿（位置+姿态）
位置（Position）：
  - X坐标：目标在3D空间中的X位置（米）
  - Y坐标：目标在3D空间中的Y位置（米）
  - Z坐标：目标在3D空间中的Z位置（米）

姿态（Orientation）：
  - 旋转角度：目标相对于某个坐标系的旋转
  - 通常用旋转矩阵R或欧拉角表示

## 具体预测过程
### 基于常速度模型预测：

$\begin{gathered}\hat{\xi}_{k+1}=\xi_k+\Delta t\left(\xi_k-\xi_{k-1}\right) \\ \Delta t=\left\{\begin{array}{c}0, n_p=1 \\ \left(t_{k+1}-t_k\right) /\left(t_k-t_{k-1}\right), n_p \geq 2\end{array}\right.\end{gathered}$

* 第k-1帧：位姿 $$ξ_{k-1} = (位置, 姿态)$$
* 第k帧：位姿 $$ξ_k = (位置, 姿态)$$

计算速度：
  速度 = $$(ξ_k - ξ_{k-1})$$ / 时间差

预测第k+1帧：
$$  ξ̂_{k+1} = ξ_k + Δt × (ξ_k - ξ_{k-1})$$

```
第1帧（t=0秒）：
  目标位置：(1.0, 2.0, 3.0) 米
  目标姿态：旋转10度

第2帧（t=0.1秒）：
  目标位置：(1.1, 2.1, 3.1) 米
  目标姿态：旋转12度

计算速度：
  位置速度 = (0.1, 0.1, 0.1) 米/0.1秒 = (1.0, 1.0, 1.0) 米/秒
  角度速度 = 2度/0.1秒 = 20度/秒

预测第3帧（t=0.2秒）：
  预测位置 = (1.1, 2.1, 3.1) + 0.1秒 × (1.0, 1.0, 1.0)
           = (1.2, 2.2, 3.2) 米
  预测姿态 = 12度 + 0.1秒 × 20度/秒 = 14度
```

### 对每个粒子独立预测
$P^u=T_k \cdot P_k \cdot W_k, W_k \in\left[\begin{array}{cc}R & t \\ 0 & 1\end{array}\right]$
* 对粒子1：
  * 当前位姿：P_1
  * 预测位姿：$$P_1' = T_k × P_1 × W_k$$
  （T_k是预测变换，W_k是随机噪声）

* 对粒子2：
     * 当前位姿：P_2
     * 预测位姿：$$P_2' = T_k × P_2 × W_k$$
  （独立预测，可能得到不同结果）
  
  ...
  
每个粒子代表一个可能的位姿，独立预测保持多样性，即使部分粒子预测错误，其他粒子可能正确
## 投影点与检测点的比较

**位姿 = 旋转 + 平移**

在齐次坐标下，位姿用4×4矩阵表示：

```
P'' = [R  t]  = [r11  r12  r13  tx]
      [0  1]    [r21  r22  r23  ty]     R：3×3旋转矩阵（描述姿态）
                [r31  r32  r33  tz]
                [0    0    0    1 ]     t：3×1平移向量（描述位置
```
对每个投影点：
### 1. 计算投影点到所有检测点的距离
* 检测点（实际观测）
    * 来源：从实际拍摄的图像中检测得到，直接在图片上进行计算得到LED像素点
    * 方法：图像处理（阈值分割、轮廓检测等）
    * 结果：LED标记在图像中的实际像素位置 
* 投影点（理论预测）
    * 来源：粒子估计的位姿，将真实世界坐标的LED标记投影得到
    * 方法：位姿变换（世界系到相机系） + 相机投影（像素平面）
    * 结果：如果粒子位姿正确，投影点应该接近检测点

```
粒子的独立估计结果
已知：LED_A在世界坐标系 (0.0, 0.0, 0.0) 米

粒子1假设位姿：P_1''
  → 用P_1''投影LED_A → (400, 400)  ← 这是理论预测

粒子2假设位姿：P_2''
  → 用P_2''投影LED_A → (421, 392)  ← 不同的假设，不同的预测
---------  
  独立匹配结果
  粒子1（位姿较准确）：
  投影点：(400, 400)
  检测点：(402, 398)
  距离：d_min = 2.83 像素  ← 很小！
  → 匹配很好 → 权重高 ✓

 粒子2（位姿有误差）：
  投影点：(421, 392)
  检测点：(402, 398)
  距离：d_min = 19.9 像素  ← 很大！
  → 匹配很差 → 权重低 ✗
```
### 2. 选择最小距离 d_min

```
图像中检测到的LED点：
  检测点1: (100, 150) 像素
  检测点2: (200, 150) 像素
  检测点3: (150, 250) 像素
  检测点4: (180, 200) 像素
  检测点5: (120, 180) 像素

投影点LED_A: (105, 155)
  到检测点1的距离 = √[(105-100)² + (155-150)²] = √50 ≈ 7.07 像素
  到检测点2的距离 = √[(105-200)² + (155-150)²] = √9026 ≈ 95.0 像素
  到检测点3的距离 = √[(105-150)² + (155-250)²] = √10906 ≈ 104.4 像素
  ...
  
  最小距离 d_min = 7.07 像素（对应检测点1）
```
### 3. 判断 d_min ≤ λ_rPF（匹配阈值）
*  d_min ≤ λ_rPF：
  → 该投影点匹配成功，计入 n_p
  
* 否则：
  → 该投影点匹配失败，不计入 n_p
  
  
```
假设 λ_rPF = 5 像素（匹配半径阈值）

投影点LED_A: d_min = 7.07 像素
  7.07 > 5 → 匹配失败 

投影点LED_B: d_min = 3.2 像素
  3.2 ≤ 5 → 匹配成功 

投影点LED_C: d_min = 4.8 像素
  4.8 ≤ 5 → 匹配成功 
```
过滤明显错误的匹配，只考虑合理的投影点
### 4、统计有效投影数量 n_p
n_p = 所有满足 d_min ≤ λ_rPF 的投影点数量

反映该粒子位姿能匹配到多少个LED，n_p 越大，位姿假设越可能正确
### 5、计算粒子权重 w
$$w = n_p · n_l + Σ_{i=1}^{n_p} ((λ_rPF - d_min,i) / λ_rPF)² - 3 · n_so$$
### 6、检查权重是否大于阈值 λ_w
* 如果至少有一个粒子权重 > λ_w：
  → 进行重采样
* 否则：
  → 重复更新粒子权重
### 7、权重归一化
$$ w̃_t^(i) = w_t^(i) / Σ_{j=1}^N w_t^(j)$$

将权重转换为概率，便于重采样时按概率选择

```原始权重：
  粒子1: w_1 = 20.48
  粒子2: w_2 = 7.01
  粒子3: w_3 = 12.5
  粒子4: w_4 = 18.2
  总和 = 58.19

归一化后：
  粒子1: w̃_1 = 20.48 / 58.19 = 0.352
  粒子2: w̃_2 = 7.01 / 58.19 = 0.120
  粒子3: w̃_3 = 12.5 / 58.19 = 0.215
  粒子4: w̃_4 = 18.2 / 58.19 = 0.313
  总和 = 1.0
```
### 8、重采样
高权重粒子 → 被复制多份，低权重粒子 → 被淘汰

```
假设有100个粒子需要重采样：

粒子1：权重0.352 → 被复制35份
粒子2：权重0.120 → 被复制12份
粒子3：权重0.215 → 被复制22份
粒子4：权重0.313 → 被复制31份
（总共100份）

结果：下一帧的100个粒子中，大部分来自高权重粒子
```
意义：
* 去除不可能的粒子（低权重）
* 聚焦到更可能的状态（高权重）
* 防止粒子退化（保持多样性）



实现了什么，估计出位姿可以用来干什么

阅读_粒子估计位姿

# CPU的命令如何实现对设备的控制
![image.png](/static/img/697f9380411ebdc920dc188f4c3a7787.image.webp)
CPU无法直接控制设备的机械部分（如磁盘的转动、打印机的打印头），所以需要I/O控制器来翻译和执行CPU的命令，从而实现对设备的控制
## I/O控制器的三大组成部分
### 1、CPU与控制器的接口（芯片内部接口）
* CPU与控制器的接口（芯片内部接口）
* 组成部分：
    *  数据线（数据总线）：用于在CPU和控制器之间传输数据（输入/输出）
    * 地址线：CPU通过它指明要操作的是控制器中的哪个寄存器。
    * 控制线：CPU通过它向控制器发出具体命令（如读、写命令）

内部寄存器：
* 数据寄存器：暂存数据
* 控制寄存器：存放CPU命令
* 状态寄存器：记录设备状态
### 2、I/O逻辑
   * 接收和识别CPU命令：对地址线传来的信号进行译码，识别CPU要执行的操作。
   * 向设备发出命令：根据CPU的指令，向连接的设备发出控制信号。
### 3、控制器与设备的接口
* 功能：实现控制器与设备本身之间的通信。
* 传输信息：通过该接口，控制器与设备之间会传递三种信息：
    * 数据：实际要输入/输出的数据。
    * 状态：设备当前的状态信息。
    * 控制信息：控制器发给设备的控制命令。
  
**CPU只与I/O控制器“对话”，I/O控制器再与设备“对话”**




操作系统_磁盘

采样 Xrand → 找 Xnear → 朝它生成 Xnew → 线段碰撞检测 → 成功则“加入树与边” → 若 Xnew 距目标 < t 则输出路径

1、初始化：把起点作为根节点加入树；设定边界（采样范围）、步长 S、到达阈值 t、最大迭代次数 N、障碍物表示与碰撞检测函数。



2、循环扩展
* 1） 随机采样 q_rand，是临时点，只用来决定树往哪儿伸一步，它本身不进树
* 2） 最近邻搜索：在树中找离 q_rand 最近的 q_near（即q_near是“已长出的树”的节点）
* 3） 生成新点：从 q_near 朝 q_rand 方向走固定步长 S 得 q_new
* 4） 检查线段 q_near→q_new 是否碰障碍；碰了就丢弃，不碰就把 q_new 加进树，并连一条蓝线 
* 5）更新“当前最好节点”：如果 q_new 比历史上任何节点都更接近目标，就把“最好节点”改成 q_new，并把“起点→q_new”的父指针链画成红线
* 6）终止判定： 如果 q_new 距目标小于阈值 t（认为“够近了”），结束并回溯父指针得到路径，红线就是最终路径；否则继续

3、输出路径：从末节点沿父指针回溯到起点得到折线；可再做“捷径平滑”（随机挑两点直连、B样条等）

* 红线：从起点到“当前最有希望的那个树节点”的父指针路径。这个“最有希望”在多数演示里指的是“树里离目标最近的节点”。每出现一个更靠近目标的新节点，红线就会改为通向它的那条链。
* 浅蓝线：整棵“搜索树”的所有边。它显示了算法迄今为止尝试过、并且无碰撞的延伸。
![image.png](/static/img/f7ab47910f7a612e00d2da712d226bc5.image.webp)


![image.png](/static/img/61e521a241c3dc8adeebd2309ca9481e.image.webp)

当出现了一个距离目标更近的节点，红线就改为通向这个新节点的父链——这会让红线向目标大方向推进

发现蓝色树另一支存在离目标更近的节点，轨迹重新规划：
![image.png](/static/img/4dd9bbe94ebafc5e7e621c4148593c1e.image.webp)

树的覆盖范围很大了（蓝线）

![image.png](/static/img/7b465aaf7187908f4cc34c6bf90c5e5d.image.webp)

一旦某次 q_new 落在距离目标 < t 的圆内，红线就定格成最终路径

![image.png](/static/img/0142dc9424a40070d8fb41f906b06747.image.webp)

对RRT的理解


## 一、数据准备阶段
### 1、收集无人船姿态数据
无人船姿态数据包括：
* 横摇角（Roll）：船体左右倾斜角度
* 纵摇角（Pitch）：船体前后倾斜角度
* 艏摇角（Yaw）：船体航向角度
* 位置信息：x、y、z坐标（可选）
* 速度信息：线速度、角速度（可选）
* 其他状态：如波浪高度、风速等环境参数（可选）

数据格式：时间序列，每个时间步记录上述所有状态值。

#### 时间步基本概念
时间步是时序数据中的一个采样点，代表某个时刻的状态

假设每0.1秒采集一次无人船的姿态数据：

```
时间步0：t=0.0秒，记录 [Roll=5°, Pitch=3°, Yaw=45°]
时间步1：t=0.1秒，记录 [Roll=5.2°, Pitch=3.1°, Yaw=45.5°]
时间步2：t=0.2秒，记录 [Roll=5.5°, Pitch=3.3°, Yaw=46°]
时间步3：t=0.3秒，记录 [Roll=5.8°, Pitch=3.6°, Yaw=46.5°]
时间步4：t=0.4秒，记录 [Roll=6.0°, Pitch=3.8°, Yaw=47°]
...
```

这里：
* 每个时间步 = 一个采样时刻
* 时间步编号 = 0, 1, 2, 3, 4, ...
* 实际时间 = 0.0秒, 0.1秒, 0.2秒, 0.3秒, 0.4秒, ...
### 2. 数据组织策略

核心思想：用历史姿态预测未来姿态
#### 确定时间窗口
* 历史窗口长度（seq_length）：用过去多少个时间步的数据，例如10、20、30

seq_length = 10 表示：
用过去10个时间步的数据作为输入，如果采样间隔是0.1秒，那么10个时间步 = 1.0秒的历史数据
    
* 预测步数（pred_length）：预测未来几个时间步的姿态

#### 确定样本

样本的定义：

* 样本”是从原始时序数据中切分出的一个训练单元，包含：
   * 序列：历史数据（例如10个时间步）
   * 标签：要预测的目标（例如未来1个时间步）
    
* **从原始数据创建样本**

假设采集了1000个时间步的无人船姿态数据：

```
时间步0:  [Roll₀, Pitch₀, Yaw₀]
时间步1:  [Roll₁, Pitch₁, Yaw₁]
时间步2:  [Roll₂, Pitch₂, Yaw₂]
...
时间步999: [Roll₉₉₉, Pitch₉₉₉, Yaw₉₉₉]
```
**创建样本**（假设seq_length=10，pred_length=1）：10个时间步的输入 + 1个时间步的目标

单个样本**预测**过程（假定网络是训练好的）：
![image.png](/static/img/4bd10d1e43c5af6af087ca327b607d75.image.webp)
样本1：
* 输入：时间步0-9的数据（10个时间步的历史），样本中时间步的数据在训练时，按时间顺序，从x₀到x₉依次作为当前输入xt，参与隐藏状态和细胞状态的更新。
* 输出：时间步10的数据（要预测的目标）

```
x₀ → h₀, c₀
  ↓
x₁ + h₀, c₀ → h₁, c₁
  ↓
x₂ + h₁, c₁ → h₂, c₂
  ↓
...
  ↓
x₉ + h₈, c₈ → h₉, c₉

最终输出：
使用最后一个时间步的隐藏状态 h₉
h₉包含了整个序列（x₀到x₉）的信息
```
样本2：
* 输入：时间步1-10的数据
* 输出：时间步11的数据

样本3：
* 输入：时间步2-11的数据
* 输出：时间步12的数据

...

样本990：
* 输入：时间步989-998的数据
* 输出：时间步999的数据

结果：从1000个时间步的数据中，可以创建990个样本

* **批次（Batch）的含义**：“批次”是一次训练中使用的样本集合

批次大小=32（一次处理多个样本，利用并行计算）：

从990个样本中随机选择32个，这32个样本组成一个批次，用这个批次进行一次参数更新

```
样本1, 样本5, 样本10, ..., 样本100（随机选择32个）
```
下一个批次：
再选择32个样本（可能与之前有重叠），
进行下一次参数更新

```
样本3, 样本7, 样本15, ..., 样本200（再随机选择32个）
```

**知道了一个样本是如何得到最后一个隐藏状态的，32个样本共享权值，并行工作时，批次处理的完整过程：**

输入：[32, 10, 3]
* 时间步0（32个样本同时处理）：
 
```
样本1：x₀¹ → LSTM → h₀¹, c₀¹
样本2：x₀² → LSTM → h₀², c₀²
...
样本32：x₀³² → LSTM → h₀³², c₀³²

输出：h₀ = [32, 64], c₀ = [32, 64]
```
* 时间步1（32个样本同时处理）：

```
样本1：x₁¹ + h₀¹, c₀¹ → LSTM → h₁¹, c₁¹
样本2：x₁² + h₀², c₀² → LSTM → h₁², c₁²
...
样本32：x₁³² + h₀³², c₀³² → LSTM → h₁³², c₁³²

输出：h₁ = [32, 64], c₁ = [32, 64]
```
最终输出：
使用h₉ = [32, 64]（32个样本，每个样本最后一个时间步的隐藏状态）

## 二、模型设计阶段
### 1、模型输入形状
#### 标准输入形状
* 三维张量：[batch_size, seq_length, features]

详细维度说明：
* batch_size：批次大小，例如32、64、128
* seq_length：历史时间步数，例如10、20、30
* features：每个时间步的特征数

例子：只预测3个姿态角
* 批次大小 = 32
* 序列长度 = 10
* 特征数 = 3（Roll、Pitch、Yaw）

输入形状：[32, 10, 3]
* 含义：32个样本，每个样本包含10个历史时间步，每个时间步有3个姿态角值



### 2、LSTM层：学习时序模式
输入形状：[32, 10, 3]

#### 逐个时间步处理过程：
##### 时间步0的处理
 输入到LSTM单元：
 * 当前输入 xt：样本中时间步0的数据，形状 [3]（例如：[Roll₀, Pitch₀, Yaw₀]）
 * 前一时刻隐藏状态 ht-1：初始化为全零，形状 [hidden_size]（例如：[0, 0, ..., 0]）
  * 前一时刻细胞状态 ct-1：初始化为全零，形状 [hidden_size]

LSTM内部计算:
* 步骤1：拼接输入

```
concat_input = [ht-1, xt]
              = [64维隐藏状态, 3维输入]
              = 67维向量
```
* 步骤2
    * 计算遗忘门、输入门和候选细胞状态，更新细胞状态，计算输出门和隐藏状态结果：64维向量（新的隐藏状态，短期记忆）
 
时间步0的输出：
* 隐藏状态 h₀：[64]（包含时间步0的信息）
* 细胞状态 c₀：[64]（长期记忆载体）

##### 时间步1的处理
 输入到LSTM单元：
 * 当前输入 xt：样本中时间步1的数据，
 * 前一时刻隐藏状态 ht-1：时间步0的隐藏状态 h₀，形状 [64]
  * 前一时刻细胞状态 ct-1：时间步0的细胞状态 c₀，形状 [64]

LSTM内部计算:
* 步骤1：拼接输入

```
concat_input = [ht-1, xt]
              = [64维隐藏状态, 3维输入]
              = 67维向量
```
* 步骤2：LSTM内部计算
 
时间步0的输出：
* 隐藏状态 h₀：[64]（包含时间步0的信息）
* 细胞状态 c₀：[64]（长期记忆载体）

#### LSTM层的输出
若只输出最后一个时间步（每个样本只取最后一个时间步（时间步9）的隐藏状态）：
输出形状：[32, 64]
* 32：32个样本
* 64：最后一个时间步的隐藏状态维度

```
样本1：时间步0-9 → h₉ → [64维向量]
样本2：时间步0-9 → h₉ → [64维向量]
...
样本32：时间步0-9 → h₉ → [64维向量]

最终：32个样本，每个样本一个64维向量
形状：[32, 64] 
```
### 3、全连接层：映射到姿态角预测
全连接层目的：将抽象的隐藏状态映射到具体的姿态角预测值

输入：LSTM的隐藏状态
* 输入形状：[32, 64]

过程：
* 变换：output = W · hidden_state + b
* 激活函数

输出：
* [Roll_pred, Pitch_pred, Yaw_pred]  （3维预测值）
### 4、模型输出形状
输出形状取决于预测任务，例：预测未来1个时间步的姿态
* 输出形状：[batch_size, features] 或 [batch_size, 1, features]


例子：
* 输入：[32, 10, 3]
* 输出：[32, 3]（32个样本，每个样本预测3个姿态角）

```
样本1的预测：
  未来时间步10: [Roll₁₀, Pitch₁₀, Yaw₁₀]

样本2的预测：
  未来时间步11: [Roll₁₁, Pitch₁₁, Yaw₁₁]
```

## 三、如果模型没训练好，那么训练阶段是如何完成的
### 1、前向传播：历史姿态 → LSTM → 预测姿态
#### 训练阶段：使用真实数据作为标签
数据准备：假设采集了1000个时间步的无人船姿态数据

1、创建训练样本

样本1：
* 输入（X）：时间步0-9的数据（10个时间步的历史）
* 输出（y，标签）：时间步10的数据（这是采集到的真实数据），不是模型预测的

```
  y = [6.2, 4.0, 47.5]  ← 真实采集的数据
```
样本2：
* 输入（X）：时间步1-10的数据
* 输出（y，标签）：时间步11的数据（真实采集的数据）

...

2、前向传播

```
输入：样本1的时间步0-9数据
  ↓
LSTM处理
  ↓
全连接层
  ↓
模型预测：y_pred = [6.1, 3.9, 47.3]  ← 这是模型预测的值
```

#### 2、计算损失：预测值与真实值比较（MSE）

```
真实标签：y_true = [6.2, 4.0, 47.5]  ← 这是采集到的真实数据
模型预测：y_pred = [6.1, 3.9, 47.3]  ← 这是模型预测的值

损失 = MSE(y_pred, y_true)
     = (6.1-6.2)² + (3.9-4.0)² + (47.3-47.5)²
     = 0.01 + 0.01 + 0.04
     = 0.06
```
#### 3、反向传播：更新权重
#### 4、迭代优化：多个epoch直到收敛

