FMT是一款基于模型设计的开源智驾仪可被用来快速构建无人机车船机器人等的无人控制系统。“Firmament意为苍穹希望无人机未来可以自由翱翔于广阔天空。仰望苍穹也表达了对于未知的探索和科技的敬畏。”源码地址https://github.com/Firmament-AutopilotFMT Base INS — Simulink 模型算法解析报告模型名称: INS.slxFirmament 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_PreProcess1169传感器数据预处理坐标变换、校准补偿、质量评估阶段二Data_Fusion9096核心融合算法姿态解算Rotation 位置/速度估计Translation阶段三Bus_Constructor968将融合结果打包为 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_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_PreProcess1407IMU_BusIMU_Data陀螺/加速度计校准零偏、比例因子、坐标旋转MAG_PreProcess1539MAG Rotation GPS (3路)Mag_Data磁力计校准、磁倾角/磁偏角补偿、质量估计 (Inclination_Quality Intensity_Quality)GPS_PreProcess1293GPS_BusGPS_DataGPS 数据解析、位置/速度提取、有效性校验Baro_PreProcess1208Baro_BusBaro_Data气压高度换算、温度补偿Rangefinder_PreProcess10403Rangefinder RotationRangefinder_Data测距仪倾角补偿利用当前姿态修正斜距OpticalFlow_PreProcess10478Rangefinder_Data Optical_FlowOpticalFlow_Data光流速度解算结合测距高度换算为实际速度AirSpeed_Process10965AirSpeed_BusAirSpeed_Data空速数据解析与滤波ExternalPos_Process11045ExternalPos_BusExternalPos_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→AHRS3.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_Correction9146Sensor_Data Rotation_Data RF_Data校正后姿态att_B_correction磁力计/加速度计辅助姿态校正利用加速度计测量重力方向 磁力计测量地磁方向与当前估计姿态比较计算姿态误差并补偿Gyro_Bias_Correction710att_B_correction IMU_Databias_g_B_correction陀螺零偏估计利用校正后的姿态和原始陀螺数据在线估计陀螺零偏漂移并进行 Saturation 限幅保护3.1.3 Update 阶段 — 状态传播更新模块SID输入输出算法Gyro_Bias_Update910bias_g_correctiongyr_B_radPsbias_g(3轴)一阶马尔可夫过程陀螺零偏随时间漂移建模Sum2965att_B_correctionbias_g补偿后角速度将校正后的偏置与当前零偏估计相加得到最终角速度补偿量Attitude_Update829att_resetquat_0 补偿后角速度att_quatDCM_OB四元数传播使用 Runge-Kutta 或 Euler 积分更新姿态四元数并输出方向余弦矩阵DCM3.1.4 AHRS 输出输出信号含义单位att_quat姿态四元数 [q0, q1, q2, q3]—DCM_OB机体坐标系→导航坐标系的方向余弦矩阵—bias_g陀螺零偏估计 [gx, gy, gz]rad/s3.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功能Predict7891使用 IMU 加速度 姿态信息进行状态一步预测运动学模型Correct7972使用 GPS 位置/速度、光流速度等外部观测进行量测更新3.2.2 Verticle_Filter — 垂直高度/速度估计状态向量维度: 3 ([3×1])架构:Predict→Correct→Reshape反馈闭环额外输出:rf_bias测距仪偏置估计状态变量推测:高度h垂直速度v_D气压计偏置或加速度计零偏模块SID功能Predict8206垂直运动学预测气压高度 IMU 垂直加速度积分Correct8128多源高度融合校正GPS 高度、气压高度、测距仪 AGL 高度3.2.3 反馈闭环Translation_Filter 存在两个反馈路径内部反馈CF 输出经Delay(1)反馈至TF_Data_PreProcessport 3作为下一时刻的先验外部反馈Rotation_Data同时输入到TF_Data_PreProcessport 2用于加速度的坐标旋转机体→导航系四、输出总线构建Bus_Constructor子系统Bus_Constructor(SID968) 将三个子系统的结果汇总为 29 通道INS_Out_Bus4.1 输出信号一览29 通道序号信号名来源含义单位1timestampCounter(1004)时间戳计数器sample2phiRotation_Output滚转角rad3thetaRotation_Output俯仰角rad4psiRotation_Output偏航角rad5quatRotation_Output姿态四元数 [q0,q1,q2,q3]—6pRotation_Output机体 x 轴角速度rad/s7qRotation_Output机体 y 轴角速度rad/s8rRotation_Output机体 z 轴角速度rad/s9axRotation_Output机体 x 轴加速度m/s²10ayRotation_Output机体 y 轴加速度m/s²11azRotation_Output机体 z 轴加速度m/s²12vnTranslation_Output北向速度m/s13veTranslation_Output东向速度m/s14vdTranslation_Output地向速度m/s15airspeedTranslation_Output空速m/s16latTranslation_Output纬度rad17lonTranslation_Output经度rad18altTranslation_Output海拔高度m19lat_0Translation_Output参考原点纬度rad20lon_0Translation_Output参考原点经度rad21alt_0Translation_Output参考原点高度m22dx_dlatTranslation_Output纬度-北向位置转换因子m/rad23dy_dlonTranslation_Output经度-东向位置转换因子m/rad24x_RTranslation_Output北向位置相对原点m25y_RTranslation_Output东向位置相对原点m26h_RTranslation_Output相对高度m27h_AGLTranslation_Output离地高度Above Ground Levelm28flagStatus_Output状态标志位—29statusStatus_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_FilterEKF扩展卡尔曼滤波6GPS 位置/速度、光流速度Verticle_FilterEKF扩展卡尔曼滤波3GPS 高度、气压高度、测距仪 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六、模型统计指标数值顶层输入端口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 种传感器融合。*