FMT是一款基于模型设计的开源智驾仪，可被用来快速构建无人机，车，船，机器人等的无人控制系统。“Firmament意为苍穹，希望无人机未来可以自由翱翔于广阔天空。仰望苍穹，也表达了对于未知的探索和科技的敬畏。” 源码地址：https://github.com/Firmament-Autopilot

# FMT Base INS — Simulink 模型算法解析报告

> **模型名称**: INS.slx（Firmament Autopilot Base INS）
> **版权**: Copyright 2016-2023 The Firmament Autopilot. All Rights Reserved.
> **数据字典**: Share_Data.sldd
> **仿真模式**: Accelerator
> **解析日期**: 2026-07-11

---

## 一、模型总体架构

该模型是一个**多传感器融合惯性导航系统（INS）**，基于扩展卡尔曼滤波（EKF）/互补滤波（CF）框架，融合 IMU、磁力计、GPS、气压计、测距仪、光流、空速计、外部位置共 8 类传感器，输出完整的导航状态。

### 1.1 顶层拓扑

```
┌──────────────────────────────────────────────────────────────┐
│ FMT Base INS │
│ │
┌──────────────┐ │ ┌──────────────────┐ ┌──────────────┐ ┌─────────────┐ │
│ IMU │────────┼─►│ │ │ │ │ │ │
│ MAG │────────┼─►│ Sensor │ │ Data │ │ Bus │ │
│ Barometer │────────┼─►│ _PreProcess ├───►│ _Fusion ├───►│ _Constructor│──┼──► INS_Out
│ GPS_uBlox │────────┼─►│ (system_1169) │ │ (system_9096)│ │ (system_968)│ │
│ Rangefinder │────────┼─►│ │ │ │ │ │ │
│ Optical_Flow │────────┼─►│ 9 in → 1 out │ │ 1 in → 2 out│ │ 3 in → 1 out│ │
│ AirSpeed │────────┼─►│ │ │ │ │ │ │
│ External_Pos │────────┼─►│ │ │ │ │ │ │
└──────────────┘ │ └──────────────────┘ └──────┬───────┘ └─────────────┘ │
│ │ │
│ ┌────────────▼──────┐ │
│ │ Delay (1 sample) │ │
│ └────────┬──────────┘ │
│ │ Rotation_Data (feedback) │
│ └──────────────────────────────────┤
│ (反馈到 Sensor_PreProcess port 9) │
└───────────────────────────────────────────────────────────────┘
```

### 1.2 三大处理阶段

| 阶段 | 子系统 | SID | 功能 |
|------|--------|-----|------|
| **阶段一** | `Sensor_PreProcess` | 1169 | 传感器数据预处理：坐标变换、校准补偿、质量评估 |
| **阶段二** | `Data_Fusion` | 9096 | 核心融合算法：姿态解算（Rotation）+ 位置/速度估计（Translation） |
| **阶段三** | `Bus_Constructor` | 968 | 将融合结果打包为 29 通道 `INS_Out_Bus` 输出总线 |

---

## 二、传感器预处理（Sensor_PreProcess）

子系统 `Sensor_PreProcess` (SID=1169) 接收 9 路输入（8 路传感器 + 1 路姿态反馈），输出 1 路 `Sensor_Data` 总线。

```
IMU ───────────► IMU_PreProcess ──────────┐
MAG ───────────► MAG_PreProcess ──────────┤ (also receives GPS_Data + Rotation_Data)
Barometer ─────► Baro_PreProcess ─────────┤
GPS ───────────► GPS_PreProcess ──────────┤──► Bus Creator (8 elem) ──► Sensor_Data
Rangefinder ───► Rangefinder_PreProcess ──┤
Optical_Flow ──► OpticalFlow_PreProcess ──┤
AirSpeed ──────► AirSpeed_Process ────────┤
ExternalPos ───► ExternalPos_Process ─────┤
Rotation_Data ─┤ (feedback to MAG + Rangefinder + OpticalFlow)
```

### 2.1 各预处理模块

| 预处理模块 | SID | 输入 | 输出 | 核心处理 |
|-----------|-----|------|------|---------|
| **IMU_PreProcess** | 1407 | IMU_Bus | IMU_Data | 陀螺/加速度计校准（零偏、比例因子）、坐标旋转 |
| **MAG_PreProcess** | 1539 | MAG + Rotation + GPS (3路) | Mag_Data | 磁力计校准、磁倾角/磁偏角补偿、质量估计 (Inclination_Quality + Intensity_Quality) |
| **GPS_PreProcess** | 1293 | GPS_Bus | GPS_Data | GPS 数据解析、位置/速度提取、有效性校验 |
| **Baro_PreProcess** | 1208 | Baro_Bus | Baro_Data | 气压高度换算、温度补偿 |
| **Rangefinder_PreProcess** | 10403 | Rangefinder + Rotation | Rangefinder_Data | 测距仪倾角补偿（利用当前姿态修正斜距） |
| **OpticalFlow_PreProcess** | 10478 | Rangefinder_Data + Optical_Flow | OpticalFlow_Data | 光流速度解算（结合测距高度换算为实际速度） |
| **AirSpeed_Process** | 10965 | AirSpeed_Bus | AirSpeed_Data | 空速数据解析与滤波 |
| **ExternalPos_Process** | 11045 | ExternalPos_Bus | ExternalPos_Data | 外部位置参考（如视觉里程计、UWB 等）的坐标转换 |

---

## 三、数据融合（Data_Fusion）

子系统 `Data_Fusion` (SID=9096) 是核心算法引擎，将融合问题**解耦为旋转（姿态）和平移（位置/速度）两个子问题**。

```
┌──────────────────────────────────────────────┐
Sensor_Data ──────┤ │
│ ┌──────────────────┐ │
├──► Rotation_Filter ├──► Rotation_Data ─────┼──► Data_Fusion out
│ │ (system_12) │ │ (port 2)
│ │ AHRS (EKF-based) │ │
│ └────────┬─────────┘ │
│ │ │
│ ▼ │
│ ┌──────────────────┐ │
├──► Translation_Filter├──► Translation_Data ──┼──► Data_Fusion out
│ │ (system_2332) │ │ (port 1)
│ │ CF (双通道EKF) │ │
│ └──────────────────┘ │
└──────────────────────────────────────────────┘
```

### 3.1 旋转滤波器 — Rotation_Filter (AHRS)

子系统路径：`Data_Fusion/Rotation_Filter` → `RF_Data_PreProcess` → `AHRS`

#### 3.1.1 AHRS 结构：Correct → Update 闭环

```
RF_Data ────────────────────────────────────────┐
▼
┌─────────────────┐ ┌──────────┐ ┌──────────────────┐
Sensor ──►│ RF_Data_ │───►│ │───►│ │
Data │ PreProcess │ │ AHRS │ │ Bus_Constructor │──► Rotation_Data
└─────────────────┘ │ (system │ │ (system_9858) │
│ 9717) │ └──────────────────┘
│ │
│ Correct │ ┌──────────┐
│ (438) │────►│ Delay1 │──┐
│ ↓ │ │ (1 samp) │ │
│ Update │ └──────────┘ │ ┌──────────────┐
│ (822) │◄───────────────────┼──┤ Attitude │
│ │ ┌──────────┐ │ │ Update(829) │
│ │────►│ Delay2 │──┘ │ + Gyro_Bias │
└──────────┘ │ (1 samp) │ │ Update(910) │
└──────────┘ └──────────────┘
```

#### 3.1.2 Correct 阶段 — 误差校正

| 校正模块 | SID | 输入 | 输出 | 算法 |
|---------|-----|------|------|------|
| **Attitude_Correction** | 9146 | Sensor_Data + Rotation_Data + RF_Data | 校正后姿态 `att_B_correction` | **磁力计/加速度计辅助姿态校正**：利用加速度计测量重力方向 + 磁力计测量地磁方向，与当前估计姿态比较，计算姿态误差并补偿 |
| **Gyro_Bias_Correction** | 710 | `att_B_correction` + IMU_Data | `bias_g_B_correction` | **陀螺零偏估计**：利用校正后的姿态和原始陀螺数据，在线估计陀螺零偏漂移，并进行 Saturation 限幅保护 |

#### 3.1.3 Update 阶段 — 状态传播

| 更新模块 | SID | 输入 | 输出 | 算法 |
|---------|-----|------|------|------|
| **Gyro_Bias_Update** | 910 | `bias_g_correction` + `gyr_B_radPs` | `bias_g` (3轴) | **一阶马尔可夫过程**：陀螺零偏随时间漂移建模 |
| **Sum2** | 965 | `att_B_correction` + `bias_g` | 补偿后角速度 | 将校正后的偏置与当前零偏估计相加得到最终角速度补偿量 |
| **Attitude_Update** | 829 | `att_reset` + `quat_0` + 补偿后角速度 | `att_quat` + `DCM_OB` | **四元数传播**：使用 Runge-Kutta 或 Euler 积分更新姿态四元数，并输出方向余弦矩阵（DCM） |

#### 3.1.4 AHRS 输出

| 输出信号 | 含义 | 单位 |
|---------|------|------|
| `att_quat` | 姿态四元数 [q0, q1, q2, q3] | — |
| `DCM_OB` | 机体坐标系→导航坐标系的方向余弦矩阵 | — |
| `bias_g` | 陀螺零偏估计 [gx, gy, gz] | rad/s |

---

### 3.2 平移滤波器 — Translation_Filter (CF)

子系统路径：`Data_Fusion/Translation_Filter` → `TF_Data_PreProcess` → `CF`

CF（Complementary Filter）将平移估计解耦为**水平（Horizontal）** 和**垂直（Vertical）** 两个独立的 EKF 通道。

```
┌─────────────────────────────────────┐
TF_Data ─────────────┤ │
Rotation_Data ───────┤ CF (Complementary Filter) │
│ │
│ ┌──────────────────────┐ │
├──► Horizontal_Filter ├──► 水平状态 │
│ │ (system_7656) │ [6×1] │
│ │ Predict → Correct │ │
│ └──────────────────────┘ │
│ │
│ ┌──────────────────────┐ │
├──► Verticle_Filter ├──► 垂直状态 │
│ │ (system_8125) │ [3×1] │
│ │ Predict → Correct │ + rf_bias│
│ └──────────────────────┘ │
│ │
│ ┌──────────────────────┐ │
└──► Bus_Constructor ├──► Translation_Data
│ (system_8465) │
└──────────────────────┘
```

#### 3.2.1 Horizontal_Filter — 水平位置/速度估计

- **状态向量维度**: 6 ([6×1], 经 Reshape 重塑)
- **架构**: `Predict` → `Correct` → `Reshape`（反馈闭环）
- **算法类型**: EKF（扩展卡尔曼滤波）
- **状态变量推测**（标准无人机水平导航）:
- 位置：`[p_N, p_E]`（北向/东向位置）
- 速度：`[v_N, v_E]`
- 附加：可能包含加速度计零偏 `[ba_x, ba_y]`

| 模块 | SID | 功能 |
|------|-----|------|
| **Predict** | 7891 | 使用 IMU 加速度 + 姿态信息进行状态一步预测（运动学模型） |
| **Correct** | 7972 | 使用 GPS 位置/速度、光流速度等外部观测进行量测更新 |

#### 3.2.2 Verticle_Filter — 垂直高度/速度估计

- **状态向量维度**: 3 ([3×1])
- **架构**: `Predict` → `Correct` → `Reshape`（反馈闭环）
- **额外输出**: `rf_bias`（测距仪偏置估计）
- **状态变量推测**:
- 高度：`h`
- 垂直速度：`v_D`
- 气压计偏置或加速度计零偏

| 模块 | SID | 功能 |
|------|-----|------|
| **Predict** | 8206 | 垂直运动学预测（气压高度 + IMU 垂直加速度积分） |
| **Correct** | 8128 | 多源高度融合校正：GPS 高度、气压高度、测距仪 AGL 高度 |

#### 3.2.3 反馈闭环

Translation_Filter 存在两个反馈路径：
1. **内部反馈**：CF 输出经 `Delay(1)` 反馈至 `TF_Data_PreProcess`（port 3），作为下一时刻的先验
2. **外部反馈**：`Rotation_Data` 同时输入到 `TF_Data_PreProcess`（port 2），用于加速度的坐标旋转（机体→导航系）

---

## 四、输出总线构建（Bus_Constructor）

子系统 `Bus_Constructor` (SID=968) 将三个子系统的结果汇总为 29 通道 `INS_Out_Bus`：

### 4.1 输出信号一览（29 通道）

| 序号 | 信号名 | 来源 | 含义 | 单位 |
|------|--------|------|------|------|
| 1 | `timestamp` | Counter(1004) | 时间戳计数器 | sample |
| 2 | `phi` | Rotation_Output | 滚转角 | rad |
| 3 | `theta` | Rotation_Output | 俯仰角 | rad |
| 4 | `psi` | Rotation_Output | 偏航角 | rad |
| 5 | `quat` | Rotation_Output | 姿态四元数 [q0,q1,q2,q3] | — |
| 6 | `p` | Rotation_Output | 机体 x 轴角速度 | rad/s |
| 7 | `q` | Rotation_Output | 机体 y 轴角速度 | rad/s |
| 8 | `r` | Rotation_Output | 机体 z 轴角速度 | rad/s |
| 9 | `ax` | Rotation_Output | 机体 x 轴加速度 | m/s² |
| 10 | `ay` | Rotation_Output | 机体 y 轴加速度 | m/s² |
| 11 | `az` | Rotation_Output | 机体 z 轴加速度 | m/s² |
| 12 | `vn` | Translation_Output | 北向速度 | m/s |
| 13 | `ve` | Translation_Output | 东向速度 | m/s |
| 14 | `vd` | Translation_Output | 地向速度 | m/s |
| 15 | `airspeed` | Translation_Output | 空速 | m/s |
| 16 | `lat` | Translation_Output | 纬度 | rad |
| 17 | `lon` | Translation_Output | 经度 | rad |
| 18 | `alt` | Translation_Output | 海拔高度 | m |
| 19 | `lat_0` | Translation_Output | 参考原点纬度 | rad |
| 20 | `lon_0` | Translation_Output | 参考原点经度 | rad |
| 21 | `alt_0` | Translation_Output | 参考原点高度 | m |
| 22 | `dx_dlat` | Translation_Output | 纬度-北向位置转换因子 | m/rad |
| 23 | `dy_dlon` | Translation_Output | 经度-东向位置转换因子 | m/rad |
| 24 | `x_R` | Translation_Output | 北向位置（相对原点） | m |
| 25 | `y_R` | Translation_Output | 东向位置（相对原点） | m |
| 26 | `h_R` | Translation_Output | 相对高度 | m |
| 27 | `h_AGL` | Translation_Output | 离地高度（Above Ground Level） | m |
| 28 | `flag` | Status_Output | 状态标志位 | — |
| 29 | `status` | Status_Output | 系统状态字 | — |

---

## 五、核心算法总结

### 5.1 算法架构总览

```
┌──────────────────────────────────────────────────────────────────────────┐
│ FMT Base INS 算法框架 │
│ │
│ ┌─────────────────────────────────────────────────────────────────┐ │
│ │ 传感器预处理层 │ │
│ │ ┌──────┐ ┌──────┐ ┌──────┐ ┌──────┐ ┌──────────┐ ┌───────┐ │ │
│ │ │ IMU │ │ MAG │ │ GPS │ │Baro │ │Rangefind │ │Optic │ ...│ │ │
│ │ │校准 │ │校准 │ │解析 │ │换算 │ │倾角补偿 │ │Flow │ │ │ │
│ │ └──┬───┘ └──┬───┘ └──┬───┘ └──┬───┘ └────┬─────┘ └──┬────┘ │ │
│ └─────┼────────┼────────┼────────┼──────────┼──────────┼──────────┘ │
│ │ │ │ │ │ │ │
│ ┌─────▼────────▼────────▼────────▼──────────▼──────────▼──────────┐ │
│ │ 融合估计层 │ │
│ │ │ │
│ │ ┌─────────────────────────┐ ┌──────────────────────────┐ │ │
│ │ │ Rotation_Filter │ │ Translation_Filter │ │ │
│ │ │ (AHRS - 姿态解算) │ │ (CF - 位置/速度估计) │ │ │
│ │ │ │ │ │ │ │
│ │ │ Correct 阶段: │ │ ┌─ Horizontal_Filter │ │ │
│ │ │ · 磁力计/加计姿态校正 │ │ │ Predict → Correct │ │ │
│ │ │ · 陀螺零偏在线估计 │ │ │ 状态维度: 6 │ │ │
│ │ │ │ │ └──────────────────────│ │ │
│ │ │ Update 阶段: │ │ │ │ │
│ │ │ · 四元数姿态传播 │ │ ┌─ Verticle_Filter │ │ │
│ │ │ · 陀螺零偏马尔可夫更新 │ │ │ Predict → Correct │ │ │
│ │ │ · DCM 矩阵输出 │ │ │ 状态维度: 3 │ │ │
│ │ └───────────┬─────────────┘ │ └──────────────────────│ │ │
│ │ │ └────────────┬─────────────┘ │ │
│ │ └───────────┬───────────────────┘ │ │
│ └───────────────────────────┼──────────────────────────────────────┘ │
│ │ │
│ ┌───────────────────────────▼──────────────────────────────────────┐ │
│ │ 输出层 │ │
│ │ INS_Out_Bus: 姿态 + 位置 + 速度 + 状态 │ │
│ │ (29 通道，phi/theta/psi, quat, lat/lon/alt, vn/ve/vd)│ │
│ └──────────────────────────────────────────────────────────────────┘ │
└──────────────────────────────────────────────────────────────────────────┘
```

### 5.2 算法类型识别

| 滤波子系统 | 算法类型 | 状态维度 | 核心观测 |
|-----------|---------|---------|---------|
| **Rotation (AHRS)** | 误差状态 EKF / 互补滤波融合 | 7 (4 四元数 + 3 陀螺零偏) | 加速度计（重力参考）、磁力计（地磁参考） |
| **Horizontal_Filter** | EKF（扩展卡尔曼滤波） | 6 | GPS 位置/速度、光流速度 |
| **Verticle_Filter** | EKF（扩展卡尔曼滤波） | 3 | GPS 高度、气压高度、测距仪 AGL 高度 |

### 5.3 关键设计特点

1. **旋转-平移解耦**：姿态估计（AHRS）与位置/速度估计（CF）分离，降低计算复杂度，提高数值稳定性

2. **级联反馈**：
- 旋转估计结果反馈给平移滤波器（加速度坐标变换）
- 姿态反馈给传感器预处理（MAG 磁偏角补偿、测距仪倾角补偿）

3. **在线零偏估计**：
- 陀螺零偏：通过 Magn_Correct 与 Gyro_Bias_Correction 在线估计
- 测距仪偏置：Verticle_Filter 的 Correct 阶段估计 `rf_bias`

4. **多源高度融合**：GPS 高度 + 气压高度 + 测距仪 AGL，通过 EKF 量测更新实现加权融合

5. **质量评估机制**：MAG 预处理包含 `Inclination_Quality_Estimate` 和 `Intensity_Quality_Estimate`，对磁力计数据质量进行评估，可能用于自适应调整量测噪声协方差

6. **姿态重置机制**：AHRS Update 阶段通过 `att_reset` 和 `quat_0` 支持姿态的强制重置（如初始对准或 GPS 航向初始化）

### 5.4 信号流总结

```
传感器 → 预处理（校准+坐标变换+质量评估）→ 旋转解算（AHRS EKF）
├─► 姿态输出（quat/DCM/欧拉角）
└─► 平移解算（CF 双通道 EKF）
├─► 水平位置/速度输出
└─► 垂直高度/速度输出
└─► 29 通道 INS_Out_Bus
```

---

## 六、模型统计

| 指标 | 数值 |
|------|------|
| 顶层输入端口 | 8（IMU, MAG, Barometer, GPS_uBlox, Rangefinder, Optical_Flow, AirSpeed, External_Pos） |
| 顶层输出端口 | 1（INS_Out, 29 通道总线） |
| 子系统文件 (.xml) | 约 165 个 |
| 最大子系统深度 | 约 8 层（如 Correct → Gyro_Bias_Correction → Saturation） |
| 核心算法块 | AHRS Correct/Update, Horizontal/Verticle Predict/Correct |
| 数据字典 | Share_Data.sldd（共享参数定义） |
| 采样时间 | `INS_CONST.dt`（统一时间步长） |

---

*解析完成。该模型为 Firmament 开源飞控的基准 INS 实现，采用解耦的旋转-平移 EKF 架构，支持 8 种传感器融合。*