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.
解析日期: 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 存在两个反馈路径：
- 内部反馈：CF 输出经
Delay(1)反馈至TF_Data_PreProcess（port 3），作为下一时刻的先验 - 外部反馈：
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 关键设计特点
-
旋转-平移解耦：姿态估计（AHRS）与位置/速度估计（CF）分离，降低计算复杂度，提高数值稳定性
-
级联反馈：
- 旋转估计结果反馈给平移滤波器（加速度坐标变换）
- 姿态反馈给传感器预处理（MAG 磁偏角补偿、测距仪倾角补偿）
-
在线零偏估计：
- 陀螺零偏：通过 Magn_Correct 与 Gyro_Bias_Correction 在线估计
- 测距仪偏置：Verticle_Filter 的 Correct 阶段估计
rf_bias
-
多源高度融合：GPS 高度 + 气压高度 + 测距仪 AGL，通过 EKF 量测更新实现加权融合
-
质量评估机制：MAG 预处理包含
Inclination_Quality_Estimate和Intensity_Quality_Estimate，对磁力计数据质量进行评估，可能用于自适应调整量测噪声协方差 -
姿态重置机制：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 种传感器融合。*
扫一扫
327
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
