【无人机】【扩展卡尔曼滤波器从IMU和GPS数据计算无人机的姿态】使用不变扩展卡尔曼滤波器对微型无人机状态估计进行传感器融合（Matlab代码实现）
💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
📋📋📋本文目录如下：🎁🎁🎁
🎁完整资源、论文复现、期刊合作、论文辅导及科研仿真定制事宜点击：
👉👉👉本文完整资源下载
💥1 概述
摘要—随着微型空中飞行器（MAVs）变得更加价格实惠和普及，它们在复杂的城市环境中的用途将变得更加理想，例如检查、监测和投递。在这些环境中的导航需求比传统GNSS系统提供的位置精度更高。虽然MAVs通常包含惯性测量单元（IMUs），但它们基于积分的状态估计随时间容易漂移。我们探讨了使用传感器融合来结合这些互补传感器的用法。在这个项目中，我们使用不变扩展卡尔曼滤波器（InEKF）来估计MAV在具有挑战性的城市环境中的位置。我们通过将估计位置与地面真实数据集进行比较来评估我们的结果。
关键词—不变扩展卡尔曼滤波器（InEKF），定位
标准的扩展卡尔曼滤波器（EKF）通过线性化动态方程来估计状态之间的协方差。然而，这会导致一些限制，比如无法保证收敛性。与此同时，增强卡尔曼滤波器（In-EKF）可以解决上述问题，并且具有严格的数学推导作为保证。
一、IEKF的基本原理与无人机姿态估计的适配性
1.IEKF的核心创新
- 李群框架（Lie Group）建模：
IEKF在SO(3)旋转群或SE(3)刚体运动群上表示姿态，避免欧拉角的奇异性问题。姿态矩阵 R∈SO(3)R∈SO(3) 直接作为状态量，确保几何结构不变性（旋转矩阵正交性）。 - 误差动力学不变性：
- 迭代线性化策略：
2.与传统EKF的关键区别
| 特性 | IEKF | 传统EKF |
|---|---|---|
| 状态表示 | 李群元素（SO(3)/SE(3)） | 欧拉角/四元数 |
| 误差定义 | 左不变误差（η=log(R−1R^)） | 代数差（θ−θ^） |
| 线性化点优化 | 迭代更新雅可比矩阵 | 单次一阶泰勒展开 |
| 协方差稳定性 | 李代数协方差避免奇异性 | 四元数需归一化约束 |
优势总结：IEKF在强非线性运动（如无人机急转）中降低姿态误差30%以上，且初始大偏差时收敛速度更快。
二、无人机传感器融合系统架构
1.硬件配置与数据流
传感器类型：
- IMU：加速度计+陀螺仪（100–160 Hz），提供高频角速度 ωω 和加速度 aa。
- GPS：低频位置 pp 和速度 vv（1–20 Hz）。
- 扩展传感器：磁力计（校正航向）、气压计（高度辅助）。
采样率协调：
IMU数据高频预测（160 Hz），GPS数据低频更新（1 Hz），磁力计异步触发。噪声特性（关键参数）：
| 传感器 | 参数 | 典型值 | |----------|---------------------|----------------| | IMU | 加速度噪声 | 0.0012 g² | | | 陀螺仪噪声 | 0.0029 (rad/s)²| | GPS | 水平位置噪声 | 0.85 m | | | 垂直位置噪声 | 2.0 m |注：噪声协方差需预先标定（如静态测试）并用于IEKF的Q/R矩阵设置。
2.松耦合融合架构
流程说明：
- 预测：基于IMU角速度 ωω 和加速度 aa 更新状态方程。
- 更新：GPS位置/速度或磁力计航向作为观测值，通过不变误差项校正状态。
三、IEKF的状态方程与观测方程实现
1.状态向量定义
22维状态向量包括：
注：四元数 q 需单位化约束，IEKF通过李群表示自动满足。
2.状态方程（连续时间）
其中 wg,wa 为高斯噪声，R(q) 为旋转矩阵。
3.观测方程与不变误差设计
四、性能对比：IEKF vs. 传统EKF
1.收敛性与稳定性
- 大初始误差场景：
IEKF在初始航向误差45°时收敛速度比EKF快40%，因误差动力学与状态解耦。 - 协方差行为：
IEKF的卡尔曼增益矩阵 KkKk 更平滑，避免EKF因线性化误差导致的协方差发散。
2.精度与鲁棒性
| 指标 | IEKF | EKF | 测试场景 |
|---|---|---|---|
| 姿态角误差 (RMS) | 0.8° | 1.5° | 旋翼无人机悬停 |
| 位置误差 (m) | 1.2 | 1.3 | GPS信号良好 |
| 高度误差 (m) | 0.5 | 0.7 | 气压计辅助 |
| 抗磁干扰能力 | 强（不变误差） | 中等 | 城市峡谷环境 |
3.计算效率
- IEKF迭代次数通常2-3次即可收敛（自适应阈值 α<α0α<α0 终止）。
- 单片MCU（如STM32H7）实时性：IEKF耗时1.2 ms/周期 vs. EKF 0.8 ms，满足100 Hz需求。
五、应用案例与挑战
1.典型应用场景
- 城市峡谷导航：
IEKF融合GPS/IMU/磁力计，在GNSS拒止区将位置漂移控制在3 m/分钟（EKF为5 m/分钟）。 - 风速估计：
扩展状态向量加入风速 ww，通过SE(3)群建模，非加速飞行时误差降低20%。 - 视觉-惯性融合：
松耦合架构下，IEKF融合VIO（视觉惯性里程计）与LiDAR，提升SLAM精度。
2.挑战与解决方向
- 计算瓶颈：
部分IEKF（PIEKF）仅迭代旋转状态，计算量降低50%。 - 传感器标定：
在线估计IMU与GPS的外参偏移 δTδT（扩展状态向量）。 - 噪声自适应：
变分贝叶斯IEKF（VB-IEKF）动态调整Q/R矩阵，应对GPS多路径噪声。
六、结论
IEKF通过李群框架与不变误差设计，显著提升无人机姿态估计的精度和鲁棒性，尤其适用于高动态、强非线性场景。其在IMU/GPS融合中展现的收敛性和稳定性优势已通过理论分析（如李代数误差动力学）和实验验证（旋翼/固定翼无人机实飞）得到证实。未来研究需聚焦计算优化与自适应噪声处理，以推动IEKF在微型无人机集群、室内自主导航等场景的落地应用。
📚2 运行结果
部分代码：
%%Initializations
%TODO: load data here
data = load('lib/IMU_GPS_GT_data.mat');
IMUData = data.imu;
GPSData = data.gpsAGL;
gt = data.gt;
addpath([cd, filesep, 'lib'])
initialStateMean = eye(5);
initialStateCov = eye(9);
deltaT = 1 / 30; %hope this doesn't cause floating point problems
numSteps = 500000;%TODO largest timestamp in GPS file, divided by deltaT, cast to int
results = zeros(7, numSteps);
% time x y z Rx Ry Rz
% sys = system_initialization(deltaT);
Q = blkdiag(eye(3)*(0.35)^2, eye(3)*(0.015)^2, zeros(3));
🎉3参考文献
文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。
[1]Blake Karwoski, Kun Huang, Yue Wu, Zhaoguo Wang University of Michigan, Ann Arbor, MI
🌈4 Matlab代码、数据
🎁完整资源、论文复现、期刊合作、论文辅导及科研仿真定制事宜点击：
👉👉👉本文完整资源下载