记录simulink无人机开源代码-开发者社区

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`（统一时间步长）