matlab-fuse-inertial-sensors

适用场景

在 MATLAB Navigation Toolbox 中选择、实现或调整惯性传感器融合滤波器。

不适用场景

• 仅使用视觉或 LiDAR 的 SLAM（不含惯性传感器）：请改用 nav SLAM 函数
• 仅进行传感器仿真或数据生成：无需滤波器
• 基于 Simulink 模型的传感器融合

---

选择滤波器

步骤 1 — 配置式还是灵活式？

配置式滤波器恰好覆盖以下传感器组合：

• 仅加速度计 + 磁力计（无陀螺仪）
• 加速度计 + 陀螺仪（无磁力计、无 GPS）
• 加速度计 + 陀螺仪 + 磁力计
• 加速度计 + 陀螺仪 + 磁力计 + 高度计
• 加速度计 + 陀螺仪 + GPS（地面车辆，无磁力计）
• 加速度计 + 陀螺仪 + GPS + 单目视觉里程计（地面车辆）
• 加速度计 + 陀螺仪 + 磁力计 + GPS（同步采样率）
• 加速度计 + 陀螺仪 + 磁力计 + GPS（混合采样率或存在丢失样本）

Is your sensor set one of the above?
        │ No ─────────────────────────────────────────────────────► Step 2B (Flexible)
        │ Yes
        ▼
Need a custom motion model (constant-velocity, bicycle, etc.)?
        │ Yes ────────────────────────────────────────────────────► Step 2B (Flexible)
        │ No
        ▼
Need RTS smoothing, StateCovariance output, or optimizer-based tune()?
        │ Yes ────────────────────────────────────────────────────► Step 2B (Flexible)
        │ No
        ▼
Batch-only processing (no real-time predict/fuse loop needed)?
        │ Yes ────────────────────────────────────────────────────► Step 2B (Flexible)
        │ No
        ▼
        Step 2A (Configured)

---

步骤 2A — 选择配置式滤波器

What output do you need?
│
├─ Orientation only (no position)
│   │
│   ├─ Accel + Mag only (no gyro)
│   │   └──► ecompass   [function, not object; single-shot; useful for filter init]
│   │
│   ├─ Accel + Gyro (no mag)
│   │   └──► imufilter   [heading drifts over time without mag]
│   │
│   ├─ Accel + Gyro + Mag
│   │   ├──► ahrsfilter   [default; KF-based, statistically tunable]
│   │   └── lowest compute cost needed?
│   │       └──► complementaryFilter   [gain-based only; no KF, no tune()]
│   │
│   └─ Accel + Gyro + Mag + Altimeter
│       └──► ahrs10filter   [adds altitude + vertical velocity output]
│
└─ Full pose (orientation + position + velocity)
    │
    ├─ No motion constraints (aerial, or ground vehicle without side-slip constraint)
    │   ├─ Accel + Gyro + Mag + GPS, all sensors sync
    │   │   └──► insfilterMARG
    │   └─ Accel + Gyro + Mag + GPS, mixed rates or dropped samples
    │       └──► insfilterAsync
    │
    └─ Ground vehicle with zero side-slip constraint
        ├─ Accel + Gyro + GPS
        │   └──► insfilterNonholonomic
        └─ Accel + Gyro + GPS + monocular visual odometry
            └──► insfilterErrorState

---

步骤 2B — 选择灵活式滤波器

Need real-time fusion, StateCovariance output, or optimizer-based tune()?
        │ Yes ──► insEKF   [production use; full KF; real-time and batch APIs]
        │ No
        ▼
Batch-only processing, no covariance needed, quick setup (R2026a+)?
        └──► insCF   [complementary filter; gain-based; not for production]

---

数据准备

采样率

SampleRate（A 组滤波器）和 IMUSampleRate（B 组滤波器）必须与实际数据速率一致——不匹配会直接缩放噪声模型，并使所有滤波器输出失真。insfilterAsync、insEKF 和 insCF 由时间戳驱动，不设采样率属性。

先将时间戳转换为秒，再推断采样率：

tSec = rawTimestamp / 1e6;   % µs → s; divide by 1e3 for ms, or skip if already seconds
fs = 1 / median(diff(tSec));

如果时间戳不可用（全部相同，或计算结果非有限）：

if numel(unique(tSec)) < 2 || ~isfinite(fs)
    fs = 100;   % use the stated or datasheet rate
end

时间间隔

所有有状态滤波器都会在记录间隔（时间戳出现较大跳变）期间累积误差。在每个间隔处分割数据，并在每个数据段开始时重置滤波器。

gapThreshold = 5 / fs;                   % 5× the expected sample interval
gapIdx = find(diff(tSec) > gapThreshold);
segments = [1; gapIdx+1];                % start index of each segment

A 组 System 对象滤波器（imufilter、ahrsfilter、complementaryFilter）：在各数据段之间调用 release(filt)，这会重置状态，同时保留所有已调整的噪声属性。

B 组及其他滤波器：在每个数据段开始时重新构造滤波器对象。

---

实现

请查阅所选滤波器的参考文件：

滤波器组	参考
姿态滤波器	references/attitude-filters.md
导航滤波器	references/navigation-filters.md
insEKF	references/insekf.md
insCF	references/inscf.md

---

调整

tune 有两种签名——混淆它们是常见错误：

• 签名 A（imufilter、ahrsfilter）——就地修改滤波器： tune(filt, sensorData, groundTruth)
• 签名 B（ahrs10filter、insfilterMARG、insfilterAsync、insfilterNonholonomic、insfilterErrorState、insEKF）——返回噪声结构体： tunedNoise = tune(filt, tunernoise(filt), sensorData, groundTruth)

无法通过 tune 调整的滤波器：

• ecompass、complementaryFilter——没有 tune 方法；请手动调整属性
• insCF——请改用 gainparts(filt, sensorName, value)

仅适用于 insEKF：tunerconfig 需要滤波器实例——应使用 tunerconfig(filt)，而非 tunerconfig('insEKF')

tunerconfig

属性	默认值	说明
MaxIterations	20	达到此迭代次数后停止
ObjectiveLimit	0.1	成本低于此值时停止
Display	"iter"	"iter" 会在每次迭代时显示进度；"none" 会禁止输出
Cost	"RMS"	"RMS" 会最小化 RMS 误差；"Custom" 会使用 CustomCostFcn
CustomCostFcn	[]	函数句柄；仅当 Cost = "Custom" 时生效

按滤波器分类的 tunernoise 字段

签名 B 滤波器需要先调用 tunernoise(filt)，再调用 tune。签名 A 滤波器（imufilter、ahrsfilter）不使用 tunernoise — tune 会直接就地修改它们的噪声属性。

滤波器	tunernoise 返回的字段
ahrs10filter	MagnetometerNoise、AltimeterNoise
insfilterMARG	MagnetometerNoise、GPSPositionNoise、GPSVelocityNoise
insfilterAsync	AccelerometerNoise、GyroscopeNoise、MagnetometerNoise、GPSPositionNoise、GPSVelocityNoise
insfilterNonholonomic	GPSPositionNoise、GPSVelocityNoise
insfilterErrorState	MVOOrientationNoise、MVOPositionNoise、GPSPositionNoise、GPSVelocityNoise
insEKF	由 filt.SensorNames 命名，并带有 Noise 后缀（例如 AccelerometerNoise、GPSNoise）

字段使用 ...Noise 后缀 — 忘记添加该后缀是常见错误。

可视化结果

tunerPlotPose 是一个 OutputFcn 回调 — 将它传给 tunerconfig，即可在每次调优迭代期间实时绘制位姿估计。它不是独立的事后绘图函数；直接向它传入滤波器和数据参数会报错（“输入参数过多”）。

config = tunerconfig(filt);
config.OutputFcn = @tunerPlotPose;
tunedNoise = tune(filt, tunernoise(filt), sensorData, groundTruth, config);

若要在调优后进行可视化，请提取滤波器状态，并手动与真实值对照绘图。已配置的滤波器（ahrs10filter、insfilterMARG、insfilterAsync、insfilterNonholonomic、insfilterErrorState）具有 pose() 方法 — 请使用该方法。insEKF 和 insCF 没有 pose() 方法 — 请改用 stateparts(filt, 'Orientation')、stateparts(filt, 'Position') 等。对于批处理工作流，请根据 estimateStates 输出的时间表绘图。

有关高级用法（控制 TunableParameters、自定义代价函数、用于日志记录或提前停止的 OutputFcn），请参阅 references/insekf.md 和 references/navigation-filters.md。

---

约定

• 参考坐标系：默认值为 "NED"。若要使用 ENU：

  • 已配置的滤波器对象（imufilter、ahrsfilter、complementaryFilter、ahrs10filter、insfilterMARG、insfilterAsync、insfilterNonholonomic、insfilterErrorState）：filt.ReferenceFrame = "ENU"
  • ecompass（函数，而非对象）：任何名称-值对之前都必须提供位置参数 orientationFormat — ecompass(a, m, 'quaternion', 'ReferenceFrame', 'ENU')。省略该参数会报错：“需要输入参数 3，FORMAT...”
  • insEKF/insCF：构造后 ReferenceFrame 为只读；请在构造时通过 insOptions/insCFOptions 设置

• 方向输出：默认使用 quaternion 对象。

• GPS 位置输入：LLA 向量，单位依次为度、度、米。

• 采样率属性：

  • imufilter、ahrsfilter、complementaryFilter：SampleRate
  • ahrs10filter、insfilterMARG、insfilterNonholonomic、insfilterErrorState：IMUSampleRate
  • insfilterAsync、insEKF、insCF：没有采样率属性（传感器各自带有时间戳）

• ReferenceLocation：在融合循环开始前，必须在 insfilterMARG、insfilterAsync、insfilterNonholonomic 和 insfilterErrorState 上设置此属性。对于 insEKF/insCF，请改为在 insGPS/insCFGPS 传感器对象上设置。

技能反馈

如果此技能的指导有误、不完整或不起作用，请在回复中输出一个纯文本反馈标记，然后继续执行任务。

格式：NAV-SKILL-FEEDBACK [matlab-fuse-inertial-sensors] [category]: 失败原因；你改用了什么方法 类别：wrong-command（命令错误或过时）、missing-guidance（遗漏步骤）、 discovery-failure（建议的方法不起作用）、platform-issue（环境特定问题）

版权所有 2026 The MathWorks, Inc.