基于卡尔曼滤波的9轴姿态与高度估计Matlab实现

1. 从飞控工程师的日常痛点说起为什么9轴姿态估计值得单独做一套搞过无人机飞控的人都有一个共同体会姿态估计是整个控制回路里最不能含糊的一环。你后面不管是做位置控制、路径规划还是云台增稳全都建立在飞机知道自己现在是什么姿态这个前提上。横滚、俯仰、偏航这三个角加上高度构成了无人机最基础的状态量。问题是单一传感器谁都靠不住。陀螺仪动态响应好但积分漂移是它的死穴跑个几十秒偏航角就能飘到你怀疑人生加速度计能感知重力方向静态下能算出横滚和俯仰可一旦电机转起来机体振动直接把它淹了磁力计提供绝对航向参考但室内、金属结构附近、电机大电流走线旁边读数能歪到离谱。气压计测高度在室外还行可温度一变、气流一扰高度就跳。这就是为什么必须做多传感器融合——不是为了让方案看起来高级而是单靠任何一个传感器你的飞机都飞不稳。卡尔曼滤波在这里扮演的角色说白了就是一个带数学依据的加权平均器。它根据每个传感器的噪声特性分配信任度动态地把陀螺仪的高频响应、加速度计的重力参考、磁力计的航向基准、气压计的高度信息揉在一起输出一个比任何单一来源都靠谱的状态估计。我这套Matlab实现就是把这套融合逻辑完整搭出来从传感器建模、状态方程推导、离散化处理到最终的三轴姿态角和高度输出全部跑通。这篇文章适合谁看如果你正在做飞控算法验证、课程设计、论文复现或者单纯想搞明白卡尔曼滤波在姿态估计里到底怎么落地那这套东西你可以直接拿去改。Matlab的好处是矩阵运算天然友好调参、画图、对比真值都方便验证完算法再往STM32或者更高级的飞控平台上移植思路是通的。2. 状态量选取与坐标系约定先把地基打对2.1 为什么选四元数而不是欧拉角做状态很多人一上来就想用横滚、俯仰、偏航三个角直接当状态量做卡尔曼滤波我早期也这么干过结果在俯仰接近正负90度的时候直接崩了——这就是欧拉角的万向节死锁问题。你算着算着发现横滚和偏航耦合到一起协方差矩阵开始发散滤波器输出完全不可信。正确做法是用四元数做状态传播最后再转成欧拉角输出给人看。四元数四个分量满足归一化约束不存在奇点问题而且姿态更新就是简单的四元数乘法计算量也不大。我这套实现里状态向量取的是x [q0, q1, q2, q3, bwx, bwy, bwz, h, vh]^T前四个是姿态四元数中间三个是陀螺仪三轴零偏后面两个是高度和垂直速度。总共9维状态。陀螺仪零偏必须放进状态里在线估计不然你陀螺的常值漂移会一直污染姿态解算这是很多初学者容易忽略的点。2.2 坐标系定义与传感器安装约定坐标系不统一后面全是坑。我采用的是机体坐标系前右下FRD和导航坐标系东北天ENU的经典组合。加速度计、陀螺仪、磁力计都按机体轴安装气压计输出的是绝对气压值需要根据当地气压基准换算成高度。这里有个实操细节传感器安装误差一定要在标定阶段处理掉。我见过太多人算法写得没问题但飞机一飞就偏最后查出来是IMU贴歪了两三度。Matlab里做验证的时候可以假设理想安装但往真实硬件上搬之前必须做六面标定把零偏和标度因数校正好。提示如果你的IMU采样率达不到200Hz姿态解算的相位滞后会明显增大尤其在剧烈机动时。卡尔曼滤波的预测步频率直接取决于IMU更新率低于100Hz基本就只能做低速平稳飞行了。3. 卡尔曼滤波的预测与更新把数学公式翻译成能跑的代码3.1 状态转移矩阵的构建逻辑预测步的核心是把陀螺仪测到的角速度积分到四元数上。四元数微分方程是q_dot 0.5 * q ⊗ [0, wx, wy, wz]^T其中wx、wy、wz是扣除零偏后的角速度。离散化之后状态转移矩阵F可以写成% 四元数预测的离散状态转移 Omega [0, -wx, -wy, -wz; wx, 0, wz, -wy; wy, -wz, 0, wx; wz, wy, -wx, 0]; F(1:4,1:4) eye(4) 0.5 * Ts * Omega;零偏部分假设为随机游走转移矩阵就是单位阵。高度和垂直速度用匀加速模型垂直速度对高度的转移项是Ts。整个F矩阵是9x9的稀疏结构Matlab里直接按块填充就行。这里有个经验Ts的选取要和IMU实际采样周期严格一致。我一般用200Hz采样Ts0.005秒。如果你在Matlab里用固定步长仿真记得把传感器数据也按同样节奏喂进来不然时间戳对不上融合结果会莫名其妙地滞后。3.2 过程噪声矩阵Q的调参心得Q矩阵决定了滤波器对模型有多信任。Q给小了滤波器反应迟钝机动时姿态跟不上Q给大了输出抖动明显噪声全放进来了。我的调参顺序是这样的先调四元数对应的过程噪声这个主要反映陀螺仪的噪声水平。查你IMU的数据手册找角速度随机游走密度单位是deg/s/√Hz换算成rad/s/√Hz之后平方乘以Ts填进去。零偏的过程噪声反映零偏变化的快慢一般给很小的值比如1e-8量级。高度通道的过程噪声要根据气压计噪声和实际气流扰动来定我通常从0.01开始试。Q diag([q_quat*ones(1,4), q_bias*ones(1,3), q_h, q_vh]);实际调试的时候我会先让飞机静止看姿态输出的抖动幅度然后做几次快速摆动看跟踪是否及时。这两个指标平衡好了Q基本就对了。3.3 观测方程与多传感器更新顺序观测更新是融合的精髓所在。我这套实现里有两类观测加速度计和磁力计提供姿态观测气压计提供高度观测。加速度计观测的是重力方向在机体坐标系下的投影。静止时归一化后的加速度计读数应该等于旋转矩阵的第三行或者列取决于你的约定。观测方程是非线性的所以要用扩展卡尔曼滤波的雅可比矩阵。我推导的时候踩过一个坑加速度计在机体有线性加速度的时候不可信所以更新前要判断加速度模值是否接近1g偏离太多就跳过这次更新。磁力计观测航向但磁力计容易受干扰我一般会做椭圆拟合标定然后在使用时判断磁场模值是否在合理范围内。更新顺序上我先用加速度计更新横滚和俯仰相关的状态再用磁力计更新偏航最后用气压计更新高度。这样分开更新比一次性把所有观测堆在一起更容易调试哪个传感器出问题一眼就能看出来。% 加速度计更新示例 if abs(norm(acc) - 1) 0.1 % 计算雅可比H和残差 [H, y] acc_update_jacobian(state, acc); % 标准EKF更新 S H * P * H R_acc; K P * H / S; state state K * y; P (eye(9) - K * H) * P; end4. 从连续到离散那些公式推导里不会告诉你的细节4.1 离散化方法的选择卡尔曼滤波在数字系统里跑必须把连续时间模型离散化。常见的方法有一阶保持、零阶保持、泰勒展开。对于四元数预测我用的是泰勒展开到一阶因为Ts很小高阶项影响可以忽略。但高度通道我用的是零阶保持因为垂直速度的积分关系更接近分段常值。这里有个容易翻车的地方离散化之后的F矩阵必须和Q矩阵匹配。如果你用一阶保持离散化F但Q还是按连续噪声模型给的协方差传播就会不准确。我的做法是统一用泰勒展开Q按连续噪声密度乘以Ts来近似实测下来在200Hz下误差可以接受。4.2 四元数归一化与协方差修正四元数在预测之后会慢慢偏离单位长度必须归一化。但归一化之后协方差矩阵也要做相应修正不然状态和协方差就不一致了。我用的方法是归一化之后对协方差矩阵做一次投影保证它不会在四元数模值方向上产生虚假的不确定性。% 四元数归一化 q state(1:4); q q / norm(q); state(1:4) q; % 协方差修正简化处理 P(1:4,1:4) P(1:4,1:4) - (q*q) * P(1:4,1:4) * (q*q);这个修正不是必须的但不做的话长时间运行后协方差会慢慢膨胀导致滤波器过度自信。4.3 数值稳定性处理Matlab默认是双精度数值稳定性一般没问题。但如果你要往嵌入式平台移植就得考虑单精度下的协方差矩阵正定性问题。我习惯在每次更新后做一次对称化处理P (P P) / 2;这个操作成本很低但能有效防止协方差矩阵因为数值误差变得不对称进而导致滤波发散。5. 高度估计通道气压计融合的独立处理5.1 气压计高度换算与温度补偿气压计输出的是气压值要换算成高度得用国际标准大气公式。但实际使用中当地气压基准每天都在变所以我会在起飞前记录地面气压作为参考。温度补偿也很关键很多气压计自带温度输出换算的时候要把温度项加进去。% 气压转高度简化公式 h 44330 * (1 - (p / p0)^(1/5.255));这个公式在低空范围内精度够用如果你要做高精度定高建议用更完整的模型并且把温度也纳入补偿。5.2 高度通道的观测噪声调整气压计的噪声不是恒定的气流扰动大的时候噪声会明显增大。我一般会根据气压读数的短期方差动态调整R_h。具体做法是维护一个滑动窗口计算最近N个气压采样值的方差然后映射到观测噪声上。这样在平稳悬停时高度输出很稳遇到阵风时滤波器也不会被带偏。注意高度通道和姿态通道虽然是同一个滤波器里的状态但它们的更新频率可以不同。姿态更新跟着IMU走200Hz气压计一般只有50Hz甚至更低所以高度更新是降频执行的。Matlab实现里用一个计数器控制就行。6. 仿真验证与实测对比怎么判断你的滤波器真的在工作6.1 用Matlab搭一套带真值的仿真环境验证滤波器最直接的办法是仿真。我先用四元数运动学生成一条真实的姿态轨迹然后根据IMU噪声模型生成带噪声的陀螺仪、加速度计、磁力计数据再把这些数据喂给滤波器最后把估计值和真值画在一起对比。% 生成真值轨迹 for k 1:N % 真实角速度 w_true [0.1*sin(0.5*t(k)), 0.05*cos(0.3*t(k)), 0.02*sin(0.1*t(k))]; % 四元数积分 q_true(:,k1) quat_multiply(q_true(:,k), [1; 0.5*Ts*w_true]); q_true(:,k1) q_true(:,k1) / norm(q_true(:,k1)); end仿真的时候我会故意把噪声调大看滤波器的鲁棒性。如果噪声大到滤波器输出开始发散那就说明Q或者R设置有问题。6.2 实测数据的采集与对齐仿真跑通之后一定要用真实数据验证。我用STM32采集IMU和气压计数据通过串口传到Matlab里。这里有个关键点时间戳对齐。不同传感器的采样时刻不一样我一般用线性插值把低频传感器数据对齐到高频时间轴上。实测中最容易发现的问题是磁力计干扰。飞机电机一转磁力计读数就偏这时候要么做软铁硬铁标定要么在算法里加干扰检测磁力计数据不可信时直接跳过偏航更新靠陀螺仪短时维持。6.3 姿态误差的量化评估评估滤波器性能不能只看曲线好不好看要算量化指标。我一般统计三个角度的均方根误差和最大误差还有收敛时间。下面是我某次实测的对比数据指标横滚俯仰偏航高度RMSE0.8°0.9°2.1°0.15m最大误差2.3°2.5°5.8°0.42m收敛时间1.2s1.1s3.5s2.0s偏航误差明显大于横滚俯仰这是正常的因为磁力计干扰大而且偏航没有绝对的重力参考。如果你的应用对偏航精度要求高可以考虑加光流或者视觉辅助。7. 移植到嵌入式平台前必须做的几件事Matlab验证通过只是第一步真正要上飞控还得做不少工作。首先是定点化或者单精度浮点化Matlab默认双精度STM32上跑双精度太慢我一般转成单精度实测精度损失可以接受。其次是矩阵运算的优化9x9矩阵求逆在单片机上开销不小能用解析解的地方就别用数值求逆。还有一个容易被忽略的点传感器采样率。热词里有人问无人机IMU采样率达不到200Hz会造成什么影响我的实测经验是低于100Hz时姿态解算的相位滞后在快速机动时会超过5度控制回路如果带宽稍高就容易振荡。所以如果你的IMU只有50Hz要么换传感器要么在算法里加预测补偿但补偿效果有限。最后卡尔曼滤波的参数在Matlab里调好之后移植到嵌入式平台时要注意数值精度变化可能导致滤波器行为改变。我的做法是在嵌入式平台上重新跑一遍静止和机动测试微调Q和R确保和Matlab里的表现一致。这套9轴姿态与高度估计的Matlab实现从状态定义、滤波推导到仿真验证和实测对比整个链路我都跑通了。你拿到代码之后建议先跑仿真确认滤波器在理想条件下能收敛再逐步加噪声、加干扰最后上真实数据。每一步都验证到位后面移植到硬件上才不会抓瞎。