FMT是一款基于模型设计的开源智驾仪可被用来快速构建无人机车船机器人等的无人控制系统。“Firmament意为苍穹希望无人机未来可以自由翱翔于广阔天空。仰望苍穹也表达了对于未知的探索和科技的敬畏。” 源码地址https://github.com/Firmament-Autopilot# FMT Base INS — Simulink 模型算法解析报告 **模型名称**: INS.slxFirmament 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 (SID1169) 接收 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_DataRangefinder ───► 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 (SID9096) 是核心算法引擎将融合问题**解耦为旋转姿态和平移位置/速度两个子问题**。┌──────────────────────────────────────────────┐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 → CFCFComplementary 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_PreProcessport 3作为下一时刻的先验2. **外部反馈**Rotation_Data 同时输入到 TF_Data_PreProcessport 2用于加速度的坐标旋转机体→导航系---## 四、输出总线构建Bus_Constructor子系统 Bus_Constructor (SID968) 将三个子系统的结果汇总为 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_bias4. **多源高度融合**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---## 六、模型统计| 指标 | 数值 ||------|------|| 顶层输入端口 | 8IMU, MAG, Barometer, GPS_uBlox, Rangefinder, Optical_Flow, AirSpeed, External_Pos || 顶层输出端口 | 1INS_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 种传感器融合。*