无人机卡尔曼滤波多传感器高度融合系统

资 源 简 介

本项目利用MATLAB软件平台，针对无人机飞行过程中由于传感器噪声导致高度测量不准的问题，开发了一套基于卡尔曼滤波（Kalman Filter）的多传感器数据融合算法。主要功能是融合来自气压计（Barometer）、GPS模块以及惯性测量单元（IMU）中加速度计的数据。程序通过建立无人机垂直运动的线性状态空间模型，将加速度二次积分作为系统预测模型，并将气压计和GPS的高度数据作为观测值，利用卡尔曼滤波的“预测-更新”迭代机制，对状态协方差矩阵进行实时计算和修正。该系统能够有效消除气压计的随机噪声、抑制加速度计积分带来的漂移误差，从而输出平滑、精确的无人机实时高度和垂直速度估计值。此外，项目还可以比较单纯依赖气压计、单纯依赖加速度计积分与采用卡尔曼滤波融合后的效果差异，并提供均方根误差（RMSE）分析图表，为无人机定高飞行控制提供可靠的数据支撑。

详 情 说 明

基于MATLAB的无人机卡尔曼滤波高度数据融合系统

项目简介

本项目实现了一套基于卡尔曼滤波（Kalman Filter）的多传感器数据融合算法仿真系统。针对无人机在垂直飞行过程中遇到的传感器噪声和漂移问题，程序通过融合气压计（Barometer）、GPS和惯性测量单元（IMU）中的加速度计数据，实现了对无人机高度和垂直速度的精确估计。该系统能够有效抑制气压计和GPS的高频随机噪声，同时消除加速度计直接积分产生的累积漂移误差。

主要功能特性

• 多传感器仿真与融合：模拟产生气压计、GPS和加速度计的三路传感器数据，并利用卡尔曼滤波算法将它们融合处理。
• 轨迹生成器：内置符合物理规律的无人机垂直运动轨迹生成逻辑，包含起飞、悬停、扰动、下降和着陆等多个飞行阶段。
• 误差对比分析：实时计算并对比单纯依赖气压计、单纯依赖GPS、加速度计直接积分以及卡尔曼滤波融合后的高度误差。
• 漂移演示：直观展示加速度计在通过二次积分计算位置时产生的严重零偏漂移现象，凸显数据融合的必要性。
• 可视化图表：提供包含高度对比、速度估计、误差分布及协方差收敛过程的详细数据图表，并自动计算均方根误差（RMSE）。

系统要求

• MATLAB R2016a 或更高版本
• 无需额外的工具箱（Toolbox），仅基于基础MATLAB函数实现

使用方法

直接运行主脚本即可启动仿真。程序将按照以下步骤执行：

1. 初始化系统参数和清理工作环境。
2. 生成60秒的真实飞行轨迹和含噪声的传感器观测数据。
3. 执行卡尔曼滤波迭代循环，逐时刻进行状态预测和更新。
4. 在控制台输出各传感器的RMSE误差统计结果。
5. 弹出两个图形窗口展示详细的波形对比和分析结果。

详细算法实现与代码逻辑

本项目的主程序代码严格遵循线性的离散卡尔曼滤波流程，具体实现逻辑如下：

1. 系统参数与轨迹仿真

• 时间设置：设定采样频率为100Hz，总时长60秒。
• 运动模型：通过分段函数定义不同时间段的加速度（起飞+1.0、悬停-1.0/0.0、正弦扰动、下降-0.5等），并利用欧拉积分生成真实的垂直速度和高度真值。
• 传感器模拟：

* 气压计与GPS：在真实高度基础上叠加标准差分别为1.2m和3.5m的高斯白噪声。 * 加速度计：在真实加速度基础上叠加0.05 m/s²的常值零偏（Bias）和0.2 m/s²的随机噪声。

2. 卡尔曼滤波系统建模

程序建立了双状态的线性状态空间模型，状态向量 x 包含 [高度; 垂直速度]。

• 状态转移矩阵 (A)：基于物理运动学公式构建，利用当前时刻的高度和速度预测下一时刻状态。

* 高度 = 上一时刻高度 + 速度 × dt * 速度 = 上一时刻速度

• 控制输入矩阵 (B)：将加速度计的测量值作为系统的控制输入（u）。矩阵系数对应 $0.5 times dt^2$ 和 $dt$，将加速度对位移和速度的影响引入预测模型。
• 观测矩阵 (H)：构建了一个 2x2 的观测矩阵。系统同时将气压计高度和GPS高度作为观测向量 z 传入。矩阵设计表明这两个传感器都直接观测状态向量中的“高度”项。
• 噪声协方差矩阵：

* Q (过程噪声)：基于加速度计噪声方差，通过离散化白噪声模型计算得到，反映了系统预测的不确定性。 * R (观测噪声)：构建对角矩阵，分别填入气压计和GPS的方差，表示两个传感器相互独立且具不同的噪声水平。

3. 滤波迭代核心流程

在主循环中，程序并行执行两条路径以进行对比：

• 路径一：纯加速度计积分（死算航位）

直接对带有噪声和零偏的加速度数据进行累加积分得到速度，再对速度积分得到高度。此过程不进行任何校正，用于演示随时间发散的漂移误差。

• 路径二：卡尔曼滤波（预测-更新）

* 预测步骤 (Time Update)：利用状态转移矩阵A和控制输入B计算先验状态估计。代码在此处显式减去了加速度计的固定零偏漂移值，模拟了已知的传感器校准过程。同时计算先验误差协方差 P_pred。 * 更新步骤 (Measurement Update)： 1. 构建当前时刻的观测向量 $z_k = [Z_{baro}; Z_{gps}]$。 2. 计算新息（Measurement Residual）：观测值减去预测值。 3. 计算卡尔曼增益 (K)：权衡预测不确定性和观测不确定性。 4. 修正状态估计：利用增益和新息更新高度和速度。 5. 更新后验协方差矩阵 P：降低系统的不确定性。

4. 误差分析与结果输出

仿真结束后，程序计算以下四种数据的均方根误差（RMSE）：

1. 纯气压计测量值
2. 纯GPS测量值
3. 纯加速度计积分值（通常误差巨大）
4. 卡尔曼滤波融合后的估计值

控制台将打印上述指标，证明滤波算法能够获得比单一传感器更小的误差。

5. 可视化模块

程序生成两幅图表：

• 综合分析图 (Figure 1)：

* 子图1：绘制真实高度、GPS观测点、气压计观测点和KF融合曲线，直观展示滤波后的平滑效果。 * 子图2：对比真实垂直速度与KF估计出的速度，展示滤波对速度的观测能力（即便没有直接的速度传感器）。 * 子图3：单独绘制气压计误差与KF融合误差的波形，展示噪声幅度的衰减。 * 子图4：绘制协方差矩阵 P 对角线元素的平方根，展示高度和速度估计的不确定性随时间收敛的过程。

• 漂移对比图 (Figure 2)：

单独的一个窗口，专门用于对比“真实高度”、“KF估计高度”与“纯加速度计积分高度”。由于加速度计积分通常会产生巨大的偏离，单独绘图可以避免压缩其他曲线的显示比例，清晰展示漂移现象。