阅读_MPC模型

阅读_卡尔曼滤波

# 四元数基本定义
四元数是一个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控制器再与设备“对话”**




基于模型预测控制的无人机轨迹跟踪原理详解

整体理解

MPC的核心思想

基于扩展卡尔曼滤波的视觉惯性融合定位详解

一、背景与问题

1.1 核心问题

四元数基本定义

阶段1：初始位姿估计

EPnP计算位姿：

CPU的命令如何实现对设备的控制