【FEJ】type_base 成员的数学推导

📐 一、type_base 成员的数学推导

1. _value：当前状态估计值

在贝叶斯滤波框架下，状态估计是后验概率的最大值：

$${\hat{\mathbf{x}}}_k=\arg \underset{\mathbf{x}}{\max }p({\mathbf{x}}_k|{\mathbf{z}}_{1:k},{\mathbf{u}}_{1:k})$$

其中：

• ${\hat{\mathbf{x}}}_k$：时刻 k 的状态估计（对应 _value）
• ${\mathbf{z}}_{1:k}$：所有观测
• ${\mathbf{u}}_{1:k}$：所有控制输入（IMU 测量）

在 EKF 中：
$${\hat{\mathbf{x}}}_k=\mathbb{E}[{\mathbf{x}}_k|{\mathbf{z}}_{1:k}]$$

维度：_size 决定，对于不同类型：

• Vec(3)：$\mathbf{x}\in {\mathbb{R}}^3$
• JPLQuat：$\mathbf{q}\in \mathbb{H}$（4维）
• PoseJPL：$[\mathbf{q},\mathbf{p}{]}^T\in {\mathbb{R}}^7$
• IMU：$[\mathbf{q},\mathbf{p},\mathbf{v},{\mathbf{b}}_g,{\mathbf{b}}_a{]}^T\in {\mathbb{R}}^{16}$

---

2. _fej：First Estimate Jacobian

问题背景：EKF 线性化导致的不一致性

在 EKF 中，非线性函数 $f(\mathbf{x})$ 在点 $\bar{\mathbf{x}}$ 处泰勒展开：

$$f(\mathbf{x})\approx f(\bar{\mathbf{x}})+\mathbf{F}{|}_{\bar{\mathbf{x}}}(\mathbf{x}-\bar{\mathbf{x}})$$

其中雅可比矩阵：
$$\mathbf{F}=\frac{\partial f}{\partial \mathbf{x}}{|}_{\bar{\mathbf{x}}}$$

不一致性问题：如果每次更新都在新的 ${\hat{\mathbf{x}}}_k$ 处线性化，会导致：

$$\text{零空间不一致：}\mathcal{N}({\mathbf{H}}_1)\ne \mathcal{N}({\mathbf{H}}_2)$$

FEJ 解决方案：
$$\boxed{{\mathbf{F}}_{\text{FEJ}}=\frac{\partial f}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_0}}$$

固定在首次估计值 ${\hat{\mathbf{x}}}_0$ 处计算雅可比，存储在 _fej 中。

数学证明（简化）：

对于状态转移：
$${\mathbf{x}}_{k+1}=f({\mathbf{x}}_k,{\mathbf{u}}_k)$$

误差状态传播：
$${\tilde{\mathbf{x}}}_{k+1}={\mathbf{\Phi }}_k{\tilde{\mathbf{x}}}_k+{\mathbf{w}}_k$$

其中状态转移矩阵：
$${\mathbf{\Phi }}_k=\frac{\partial f}{\partial \mathbf{x}}{|}_{{\bar{\mathbf{x}}}_k}$$

使用 FEJ：
$${\mathbf{\Phi }}_k^{\text{FEJ}}=\frac{\partial f}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_{\text{fej}}}$$

保证：
$$\text{如果 }{\tilde{\mathbf{x}}}_0\in \mathcal{N}(\mathbf{H})\Rightarrow {\tilde{\mathbf{x}}}_k\in \mathcal{N}(\mathbf{H}),\forall k$$

---

3. _id：状态向量中的唯一标识

全局状态向量：
$${\mathbf{x}}_{\text{global}}=\left[\begin{array}{c}{\mathbf{x}}_{\text{IMU}}\\ {\mathbf{x}}_{{\text{clone}}_1}\\ ⋮\\ {\mathbf{x}}_{{\text{clone}}_N}\\ {\mathbf{x}}_{{\text{landmark}}_1}\\ ⋮\\ {\mathbf{x}}_{{\text{landmark}}_M}\end{array}\right]$$

ID 映射：
$$\text{\_id}:\text{Type}\to \mathbb{N}$$

使得：
$${\mathbf{x}}_{\text{type}}={\mathbf{x}}_{\text{global}}[\text{\_id}:\text{\_id}+\text{\_size}]$$

用途：

• 协方差矩阵索引：$\mathbf{P}[{\text{id}}_i:{\text{id}}_i+n,{\text{id}}_j:{\text{id}}_j+m]$
• 卡尔曼增益计算：$\mathbf{K}=\mathbf{P}{\mathbf{H}}^T(\mathbf{HP}{\mathbf{H}}^T+\mathbf{R}{)}^{-1}$

---

4. _size：状态维度

流形上的维度：

类型	流形	嵌入维度	切空间维度 (_size)
Vec(n)	${\mathbb{R}}^n$	n	n
JPLQuat	$SO(3)$	4	3
PoseJPL	$SE(3)$	7	6
IMU	$SE(3)\times {\mathbb{R}}^9$	16	15

误差状态维度：

对于四元数：
$$\mathbf{q}\leftarrow \mathbf{q}\otimes \left[\begin{array}{c}\frac{1}{2}\delta \mathbf{\theta }\\ 1\end{array}\right]$$

误差状态 $\delta \mathbf{\theta }\in {\mathbb{R}}^3$，而不是 ${\mathbb{R}}^4$

协方差矩阵大小：
$$\mathbf{P}\in {\mathbb{R}}^{N\times N},N=\sum _i{\text{\_size}}_i$$

---

📊 二、完整的运动学方程推导

1. IMU 运动学（连续时间）

状态定义：

$${\mathbf{x}}_{\text{IMU}}=\left[\begin{array}{c}{}^G{\mathbf{q}}_I\\ {}^G{\mathbf{p}}_I\\ {}^G{\mathbf{v}}_I\\ {\mathbf{b}}_g\\ {\mathbf{b}}_a\end{array}\right]\in {\mathbb{R}}^{16}$$

运动学微分方程：

(1) 姿态更新：
$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{\Omega }(\mathbf{\omega })\mathbf{q}$$

其中：
$$\mathbf{\Omega }(\mathbf{\omega })=\left[\begin{array}{cc}-[\mathbf{\omega }{]}_{\times }& \mathbf{\omega }\\ -{\mathbf{\omega }}^T& 0\end{array}\right],\mathbf{\omega }={\mathbf{\omega }}_m-{\mathbf{b}}_g-{\mathbf{n}}_g$$

(2) 位置更新：
$${}^G{\dot{\mathbf{p}}}_I={}^G{\mathbf{v}}_I$$

(3) 速度更新：
$${}^G{\dot{\mathbf{v}}}_I={}^G{\mathbf{a}}_I={}_G^I\mathbf{R}({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a)+{}^G\mathbf{g}$$

(4) 偏置更新（随机游走）：
$${\dot{\mathbf{b}}}_g={\mathbf{n}}_{bg},{\dot{\mathbf{b}}}_a={\mathbf{n}}_{ba}$$

完整形式：

$$\boxed{\begin{array}{rl}\dot{\mathbf{q}}& =\frac{1}{2}\mathbf{\Omega }({\mathbf{\omega }}_m-{\mathbf{b}}_g)\mathbf{q}\\ \dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{R}({\mathbf{a}}_m-{\mathbf{b}}_a)+\mathbf{g}\\ {\dot{\mathbf{b}}}_g& ={\mathbf{n}}_{bg}\\ {\dot{\mathbf{b}}}_a& ={\mathbf{n}}_{ba}\end{array}}$$

---

2. 误差状态运动学

定义误差状态：
$$\tilde{\mathbf{x}}=\left[\begin{array}{c}\delta \mathbf{\theta }\\ \delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta {\mathbf{b}}_g\\ \delta {\mathbf{b}}_a\end{array}\right]\in {\mathbb{R}}^{15}$$

误差状态微分方程：

$$\boxed{\dot{\tilde{\mathbf{x}}}={\mathbf{F}}_c\tilde{\mathbf{x}}+{\mathbf{G}}_c\mathbf{n}}$$

其中连续时间雅可比：

$${\mathbf{F}}_c={\left[\begin{array}{ccccc}-[\mathbf{\omega }{]}_{\times }& 0& 0& -{\mathbf{I}}_3& 0\\ 0& 0& {\mathbf{I}}_3& 0& 0\\ -\mathbf{R}[\mathbf{a}{]}_{\times }& 0& 0& 0& -\mathbf{R}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]}_{15\times 15}$$

$${\mathbf{G}}_c={\left[\begin{array}{cccc}-{\mathbf{I}}_3& 0& 0& 0\\ 0& 0& 0& 0\\ 0& -\mathbf{R}& 0& 0\\ 0& 0& {\mathbf{I}}_3& 0\\ 0& 0& 0& {\mathbf{I}}_3\end{array}\right]}_{15\times 12}$$

噪声向量：
$$\mathbf{n}=\left[\begin{array}{c}{\mathbf{n}}_g\\ {\mathbf{n}}_a\\ {\mathbf{n}}_{bg}\\ {\mathbf{n}}_{ba}\end{array}\right]\sim \mathcal{N}(0,{\mathbf{Q}}_c)$$

---

3. 离散化（用于 EKF）

状态转移矩阵：
$${\mathbf{\Phi }}_k=\exp ({\mathbf{F}}_c\Delta t)\approx \mathbf{I}+{\mathbf{F}}_c\Delta t+\frac{1}{2}({\mathbf{F}}_c\Delta t{)}^2$$

离散噪声协方差：
$${\mathbf{Q}}_d={\int }_0^{\Delta t}\mathbf{\Phi }(\tau ){\mathbf{G}}_c{\mathbf{Q}}_c{\mathbf{G}}_c^T{\mathbf{\Phi }}^T(\tau )d\tau$$

预测步骤：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_{k+1}^{-}& =f({\hat{\mathbf{x}}}_k,{\mathbf{u}}_k)\\ {\mathbf{P}}_{k+1}^{-}& ={\mathbf{\Phi }}_k{\mathbf{P}}_k{\mathbf{\Phi }}_k^T+{\mathbf{Q}}_d\end{array}$$

---

📷 三、观测方程推导

1. 相机投影模型

路标点 ${}^G{\mathbf{p}}_f$ 在相机坐标系下：

$${}^{C_i}{\mathbf{p}}_f={}_G^{C_i}\mathbf{R}({}^G{\mathbf{p}}_f-{}^G{\mathbf{p}}_{C_i})$$

针孔相机投影：
$$\left[\begin{array}{c}u\\ v\end{array}\right]=\pi ({}^{C_i}{\mathbf{p}}_f)=\left[\begin{array}{c}{f}_x\frac{X}{Z}+c_x\\ f_y\frac{Y}{Z}+c_y\end{array}\right]$$

其中 ${}^{C_i}{\mathbf{p}}_f=[X,Y,Z{]}^T$

---

2. 观测方程

$${\mathbf{z}}_k=h({\mathbf{x}}_k)+{\mathbf{v}}_k$$

其中：

• ${\mathbf{z}}_k$：图像特征点坐标 $[u,v{]}^T$
• $h(\cdot )$：投影函数
• ${\mathbf{v}}_k\sim \mathcal{N}(0,\mathbf{R})$：观测噪声

完整形式：
$$\boxed{\mathbf{z}=\pi \left({}_I^C\mathbf{R}\cdot {}_G^I\mathbf{R}({}^G{\mathbf{p}}_f-{}^G{\mathbf{p}}_I)-{}^C{\mathbf{p}}_I\right)}$$

---

3. 观测雅可比矩阵

对状态求偏导：
$$\mathbf{H}=\frac{\partial h}{\partial \mathbf{x}}{|}_{\hat{\mathbf{x}}}$$

分解：
$$\mathbf{H}=\underset{{\mathbf{J}}_{\pi }}{\underbrace{\frac{\partial \pi }{\partial {}^C\mathbf{p}}}}\cdot \underset{{\mathbf{J}}_{\text{trans}}}{\underbrace{\frac{\partial {}^C\mathbf{p}}{\partial \mathbf{x}}}}$$

(1) 投影雅可比：
$${\mathbf{J}}_{\pi }={\left[\begin{array}{ccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}\end{array}\right]}_{2\times 3}$$

(2) 对姿态的雅可比：
$$\frac{\partial {}^C\mathbf{p}}{\partial \delta \mathbf{\theta }}=-{}_G^C\mathbf{R}[{}^G{\mathbf{p}}_f-{}^G{\mathbf{p}}_I{]}_{\times }$$

(3) 对位置的雅可比：
$$\frac{\partial {}^C\mathbf{p}}{\partial {}^G{\mathbf{p}}_I}=-{}_G^C\mathbf{R}$$

(4) 对路标的雅可比：
$$\frac{\partial {}^C\mathbf{p}}{\partial {}^G{\mathbf{p}}_f}={}_G^C\mathbf{R}$$

完整观测矩阵：
$$\mathbf{H}={\mathbf{J}}_{\pi }\left[\begin{array}{cccccc}{\mathbf{H}}_{\theta }& {\mathbf{H}}_p& 0& 0& 0& {\mathbf{H}}_f\end{array}\right]$$

---

🔧 四、FEJ 使用推导

1. 不一致性问题分析

可观测性理论：

系统的零空间（不可观测子空间）：
N(H)={x~:Hx~=0}\mathcal{N}(\mathbf{H}) = \{\tilde{\mathbf{x}} : \mathbf{H}\tilde{\mathbf{x}} = \mathbf{0}\}N(H)={x~:Hx~=0}

对于 VIO 系统，理论上不可观测量：

• 全局位置（3 DOF）
• 全局偏航角（1 DOF）

问题：标准 EKF 线性化会导致：
$$\dim (\mathcal{N}({\mathbf{H}}_{\text{EKF}}))<\dim (\mathcal{N}({\mathbf{H}}_{\text{true}}))$$

即错误地观测到本不可观测的量，导致过度自信。

---

2. FEJ 数学证明

定理：如果使用 FEJ，则：
$$\mathcal{N}({\mathbf{H}}_{\text{FEJ}})=\mathcal{N}({\mathbf{H}}_{\text{linearized}})$$

证明（简化版）：

考虑全局平移 $\delta {\mathbf{p}}_{\text{global}}$：

标准 EKF：
$${\mathbf{H}}_k=\frac{\partial h}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_k}$$

在不同时刻，线性化点不同 → 零空间不一致

FEJ：
$${\mathbf{H}}_k^{\text{FEJ}}=\frac{\partial h}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_0}$$

固定线性化点 → 保持零空间一致性

---

3. FEJ 使用时机

场景	是否使用 FEJ	原因
状态预测 $\mathbf{\Phi }$	✅ 使用	保持传播一致性
观测更新 $\mathbf{H}$	✅ 使用	保持可观测性
首次初始化	❌ 不用	_fej = _value
路标三角化	✅ 使用	固定锚点位姿
边缘化	✅ 使用	保持先验一致性

代码中的体现：

// Propagation with FEJ
Eigen::Matrix<double,3,3> R_fej = imu_Rot_fej(imu);  // Use FEJ value
Phi = compute_Phi(R_fej, ...);  // Compute state transition

// Update with FEJ
Eigen::Matrix<double,3,1> p_fej = imu_pos_fej(imu);
H = compute_H(p_fej, ...);  // Compute measurement Jacobian

---

4. FEJ 更新策略

初始化：

void imu_init(type_imu* imu) {
    // _fej = _value at initialization
    type_set_fej(imu->base, &initial_value);
}

更新策略：

策略	何时更新 _fej	优缺点
Never Update	永不更新	✅ 完美一致性 ❌ 大偏差时精度下降
Update on Init	仅初始化时	⚖️ 平衡方案（常用）
Periodic Update	周期性更新	⚖️ 适应大运动 ⚠️ 需谨慎

推荐方案（本代码采用）：

// FEJ is set once at initialization or anchor change
if (is_new_landmark || anchor_changed) {
    landmark_set_fej(landmark, &current_estimate);
}
// Thereafter, _fej remains fixed for Jacobian computation

---

📈 五、完整 EKF 流程

预测步骤：

$$\begin{array}{rl}{\hat{\mathbf{x}}}_k^{-}& =f({\hat{\mathbf{x}}}_{k-1}^{+},{\mathbf{u}}_k)\\ {\mathbf{P}}_k^{-}& ={\mathbf{\Phi }}_k^{\text{FEJ}}{\mathbf{P}}_{k-1}^{+}({\mathbf{\Phi }}_k^{\text{FEJ}}{)}^T+{\mathbf{Q}}_k\end{array}$$

更新步骤：

$$\begin{array}{rl}{\mathbf{S}}_k& ={\mathbf{H}}_k^{\text{FEJ}}{\mathbf{P}}_k^{-}({\mathbf{H}}_k^{\text{FEJ}}{)}^T+{\mathbf{R}}_k\\ {\mathbf{K}}_k& ={\mathbf{P}}_k^{-}({\mathbf{H}}_k^{\text{FEJ}}{)}^T{\mathbf{S}}_k^{-1}\\ {\tilde{\mathbf{x}}}_k& ={\mathbf{K}}_k({\mathbf{z}}_k-h({\hat{\mathbf{x}}}_k^{-}))\\ {\hat{\mathbf{x}}}_k^{+}& ={\hat{\mathbf{x}}}_k^{-}⊞{\tilde{\mathbf{x}}}_k\\ {\mathbf{P}}_k^{+}& =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k^{\text{FEJ}}){\mathbf{P}}_k^{-}\end{array}$$

注意：所有雅可比 $\mathbf{\Phi }$ 和 $\mathbf{H}$ 都在 _fej 值处计算，而状态更新使用 _value。

---

🎯 总结

1. _value：当前最优估计，持续更新
2. _fej：固定的线性化点，保证一致性
3. _id：全局状态向量索引
4. _size：误差状态流形维度

关键洞察：FEJ 通过牺牲少量精度（固定线性化点）来换取系统的长期一致性，避免滤波器过度自信和发散。

基础结构体的物理含义和关系：

2. 📊 类型系统架构图

type_base (基类)
    │
    ├─→ type_vec (向量)
    │       │
    │       └─→ 被以下类型使用：
    │           ├─ type_pose_jpl._p (位置)
    │           ├─ type_imu._v (速度)
    │           ├─ type_imu._bg (陀螺仪偏置)
    │           ├─ type_imu._ba (加速度计偏置)
    │           └─ type_landmark._v (路标坐标)
    │
    ├─→ type_jpl_quat (四元数)
    │       │
    │       └─→ 被以下类型使用：
    │           └─ type_pose_jpl._q (姿态)
    │
    ├─→ type_pose_jpl (位姿 = 姿态 + 位置)
    │       │
    │       └─→ 被以下类型使用：
    │           └─ type_imu._pose (IMU位姿)
    │
    ├─→ type_imu (IMU状态)
    │
    └─→ type_landmark (路标点)

---

1️⃣ type_base - 基础类型

typedef struct type_base_s {
    Eigen::MatrixXd _fej;
    Eigen::MatrixXd _value;
    int _id;
    int _size;
} type_base;

物理含义：所有类型的基类，提供状态管理的通用框架

成员含义：

• _value: 当前状态估计值
• _fej: First Estimate Jacobian，用于保持 EKF 一致性的固定雅可比值
• _id: 在状态向量中的唯一标识符
• _size: 状态维度大小

---

2️⃣ type_vec - 向量类型

typedef struct type_vec_s {
    type_base* base;
} type_vec;

物理含义：通用 N 维向量，用于表示各种线性量

应用场景：

• 3D 位置 (x, y, z)
• 3D 速度 (vx, vy, vz)
• 3D 偏置 (bx, by, bz)
• 路标点坐标

特点：维度可变，初始化时指定

---

3️⃣ type_jpl_quat - JPL 四元数

typedef struct type_jpl_quat_s {
    type_base* base;
    Eigen::Matrix<double, 3, 3> _R;
    Eigen::Matrix<double, 3, 3> _Rfej;
} type_jpl_quat;

物理含义：表示 3D 旋转的四元数（JPL 约定）

成员含义：

• base->_value: 4×1 四元数 [qx, qy, qz, qw]
• _R: 当前旋转矩阵（由四元数转换得到）
• _Rfej: FEJ 版本的旋转矩阵

JPL vs Hamilton：

• JPL：quat = [qx, qy, qz, qw]，向量在前，标量在后
• Hamilton：quat = [qw, qx, qy, qz]，标量在前

维度：4 维（但旋转自由度为 3）

---

4️⃣ type_pose_jpl - JPL 位姿

typedef struct type_pose_jpl_s {
    type_base* base;
    type_jpl_quat* _q;
    type_vec* _p;
}  type_pose_jpl;

物理含义：SE(3) 群上的刚体位姿 = 姿态 + 位置

成员含义：

• _q: 姿态（旋转），4 维四元数
• _p: 位置（平移），3 维向量

物理意义：

变换：P_world = R * P_local + t
其中：
  R = 旋转矩阵（由 _q 计算）
  t = 位置向量 (_p)

总维度：7 维（4 + 3）

---

5️⃣ type_imu - IMU 状态

typedef struct type_imu_s {
    type_base* base;
    type_pose_jpl* _pose;
    type_vec* _v;
    type_vec* _bg;
    type_vec* _ba;
} type_imu;

物理含义：IMU 的完整运动状态 + 传感器偏置

组成成分：

成员	物理量	维度	单位	说明
_pose	位姿	7	-	IMU 在世界坐标系中的姿态和位置
_pose->_q	姿态	4	-	旋转四元数
_pose->_p	位置	3	m	3D 坐标
_v	速度	3	m/s	线速度
_bg	陀螺仪偏置	3	rad/s	角速度测量偏差
_ba	加速度计偏置	3	m/s²	线加速度测量偏差

总维度：16 维（但四元数约束后实际 15 自由度）

运动学关系：

IMU 传播方程（连续时间）：
┌─────────────────────────────────────┐
│ dR/dt = R * [ω_m - bg]×            │  (姿态)
│ dp/dt = v                          │  (位置)
│ dv/dt = R*(a_m - ba) + g           │  (速度)
│ dbg/dt = ng  (白噪声)              │  (陀螺仪偏置)
│ dba/dt = na  (白噪声)              │  (加速度计偏置)
└─────────────────────────────────────┘

其中：
  ω_m = 陀螺仪测量值
  a_m = 加速度计测量值
  g = 重力加速度
  [·]× = 反对称矩阵

---

6️⃣ type_landmark - 路标点

typedef struct type_landmark_s {
  type_base* base;
  type_vec* _v;
  size_t _featid;
  int _unique_camera_id;

  int _anchor_cam_id;

  double _anchor_clone_timestamp;

  bool has_had_anchor_change;

  /// Boolean if this landmark should be marginalized out
  bool should_marg;

  int update_fail_count;

  Eigen::Vector3d uv_norm_zero;
  /// First estimate normalized uv coordinate bearing of this measurement (used for single depth representation)
  Eigen::Vector3d uv_norm_zero_fej;
  type_landmark_representation feat_representation;

} type_landmark;

物理含义：环境中的 3D 特征点（路标）

关键成员：

成员	含义	说明
_v	路标坐标	根据表示方式不同，可以是 3D 坐标或逆深度
_featid	特征 ID	唯一标识符
_anchor_cam_id	锚点相机 ID	第一次观测到该特征的相机
_anchor_clone_timestamp	锚点时间戳	首次观测时刻
feat_representation	表示方式	见下表
uv_norm_zero	归一化图像坐标	首次观测的方向向量

路标表示方式 (feat_representation)：

typedef enum {
    GLOBAL_3D,
    GLOBAL_FULL_INVERSE_DEPTH,
    ANCHORED_3D,
    ANCHORED_FULL_INVERSE_DEPTH,
    ANCHORED_MSCKF_INVERSE_DEPTH,
    ANCHORED_INVERSE_DEPTH_SINGLE,
    UNKNOWN
} type_landmark_representation;

表示方式	维度	参数化	说明
GLOBAL_3D	3	[x, y, z]	全局 3D 坐标
GLOBAL_FULL_INVERSE_DEPTH	6	[x, y, z, θ, φ, ρ]	全局逆深度（5+1）
ANCHORED_3D	3	[dx, dy, dz]	相对锚点的 3D 坐标
ANCHORED_FULL_INVERSE_DEPTH	3	[θ, φ, ρ]	相对锚点的逆深度
ANCHORED_MSCKF_INVERSE_DEPTH	1	[ρ]	MSCKF 风格单参数逆深度
ANCHORED_INVERSE_DEPTH_SINGLE	1	[ρ]	简化的单参数逆深度

---

7️⃣ type_wrapper - 类型包装器

struct type_wrapper {
    type_base* base;
    void* specific_data;  // Points to specific type_imu, type_pose_jpl, etc.
    int type_id;          // Identifies specific type
    bool is_c_version;    // Identifies if it's C version
};

功能含义：统一接口包装器，用于多态处理

Type ID 枚举：

enum TypeID {
    TYPE_BASE = 0,
    TYPE_VEC = 1,
    TYPE_JPL_QUAT = 2,
    TYPE_POSE_JPL = 3,
    TYPE_IMU = 4,
    TYPE_LANDMARK = 5
};

---

🔗 与 type_imu 的关系总结

type_imu (15 DOF)
    │
    ├─→ _pose (type_pose_jpl) - 7 维
    │       │
    │       ├─→ _q (type_jpl_quat) - 4 维
    │       │   └─→ base (type_base) - 存储四元数值
    │       │
    │       └─→ _p (type_vec) - 3 维
    │           └─→ base (type_base) - 存储位置
    │
    ├─→ _v (type_vec) - 3 维
    │   └─→ base (type_base) - 存储速度
    │
    ├─→ _bg (type_vec) - 3 维
    │   └─→ base (type_base) - 存储陀螺仪偏置
    │
    └─→ _ba (type_vec) - 3 维
        └─→ base (type_base) - 存储加速度计偏置

---

📐 物理关系与数学模型

VIO/SLAM 系统中的协作关系：

┌─────────────────────────────────────────────────────┐
│                    SLAM 状态向量                     │
├─────────────────────────────────────────────────────┤
│                                                     │
│  [IMU 状态]   [路标状态]   [相机克隆状态]            │
│  type_imu     type_landmark  type_pose_jpl[]       │
│     │              │               │                │
│     │              │               │                │
│     ▼              ▼               ▼                │
│  ┌─────┐      ┌────────┐      ┌──────┐             │
│  │R,p,v│◄────►│ 3D点   │◄────►│相机位姿│             │
│  │bg,ba│      │        │      │      │             │
│  └─────┘      └────────┘      └──────┘             │
│                                                     │
│  通过 EKF/ESKF 融合：                               │
│  - IMU 预测（高频）                                  │
│  - 视觉更新（低频）                                  │
│  - 路标三角化                                        │
└─────────────────────────────────────────────────────┘

测量模型：

1. IMU 测量：

   ω_m = ω_true + bg + nω
   a_m = R^T(a_true - g) + ba + na
   

2. 视觉测量（重投影）：

   z = h(x_imu, x_landmark)
     = π(R^T * (p_landmark - p_imu))
   
   其中 π 是相机投影函数
   

---

🎯 设计要点

1. 分层设计：基于 type_base 的继承体系（用 C 结构体模拟）
2. FEJ 支持：每个状态都有 FEJ 版本，保证 EKF 一致性
3. 模块化：各类型独立，便于扩展
4. 纯 C 风格：使用结构体指针和函数指针模拟面向对象

这个类型系统完整实现了 VIO/SLAM 所需的所有状态表示，支持高效的状态估计和优化。