记录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 三大处理阶段

二、传感器预处理（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 各预处理模块

三、数据融合（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 阶段 — 误差校正

3.1.3 Update 阶段 — 状态传播

3.1.4 AHRS 输出

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]

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

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

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 通道）

五、核心算法总结

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 算法类型识别

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

六、模型统计

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