目录

1.课题概述

2.系统仿真结果

3.核心程序或模型

4.系统原理简介

4.1 四旋翼飞行器动力学与状态空间模型

4.2 扩展卡尔曼滤波

4.3 EKF状态估计

初始化参数与状态

Simulink联合仿真获取真实状态与带噪测量值

时间更新（预测阶段）

测量更新（校正阶段）

5.完整工程文件

---

1.课题概述

       四旋翼飞行器是典型的欠驱动、强耦合非线性系统，其飞行状态包含位置、姿态、线速度与角速度等关键信息。在实际飞行中，GPS、陀螺仪等传感器会引入测量噪声，直接使用原始数据无法满足高精度控制需求。扩展卡尔曼滤波通过对非线性系统进行线性化近似，在噪声环境下实现对真实状态的最优估计，是四旋翼状态感知与稳定控制的核心技术。

2.系统仿真结果

3.核心程序或模型

版本：Matlab2024b

figure
subplot(131);
plot(t,St_est(1,:),'g','LineWidth',2);
hold on
plot(t,Yceli(1,:),'b--o','LineWidth',1);
hold on
plot(t,del_x_kp1(1,:),'r','LineWidth',2);
title('X轴高度估计');
xlabel('时间 (秒)');
ylabel('X轴高度 (米)');
legend('估计状态','带噪声测量值','真实状态');

subplot(132);
plot(t,St_est(2,:),'g','LineWidth',2);
hold on
plot(t,Yceli(2,:),'b--o','LineWidth',1);
hold on
plot(t,del_x_kp1(2,:),'r','LineWidth',2);
title('X轴高度估计');
xlabel('时间 (秒)');
ylabel('X轴高度 (米)');
legend('估计状态','带噪声测量值','真实状态');
ylim([-2,2]);

subplot(133);
plot(t,St_est(3,:),'g','LineWidth',2);
hold on
plot(t,Yceli(3,:),'b--o','LineWidth',1);
hold on
plot(t,del_x_kp1(3,:),'r','LineWidth',2);
title('Z轴高度估计');
xlabel('时间 (秒)');
ylabel('Z轴高度 (米)');
legend('估计状态','带噪声测量值','真实状态');

figure
subplot(131);
plot(t,St_est(4,:),'g','LineWidth',2);
hold on
plot(t,Yceli(4,:),'b--o','LineWidth',1);
hold on
plot(t,del_x_kp1(4,:),'r','LineWidth',2);
title('滚转角估计');
xlabel('时间 (秒)');
ylabel('滚转角 (弧度)');
legend('估计状态','带噪声测量值','真实状态');
ylim([-1,1]);

subplot(132);
plot(t,St_est(5,:),'g','LineWidth',2);
hold on
plot(t,Yceli(5,:),'b--o','LineWidth',1);
hold on
plot(t,del_x_kp1(5,:),'r','LineWidth',2);
title('俯仰角估计');
xlabel('时间 (秒)');
ylabel('滚转角 (弧度)');
legend('估计状态','带噪声测量值','真实状态');

subplot(133);
plot(t,St_est(6,:),'g','LineWidth',2);
hold on
plot(t,Yceli(6,:),'b--o','LineWidth',1);
hold on
plot(t,del_x_kp1(6,:),'r','LineWidth',2);
title('偏航角估计');
xlabel('时间 (秒)');
ylabel('偏航角 (弧度)');
legend('估计状态','带噪声测量值','真实状态');
ylim([-1,1]);

149

4.系统原理简介

4.1 四旋翼飞行器动力学与状态空间模型

       EKF以12维状态向量作为系统核心表征量，包含三维位置、三维姿态角、三维线速度和三维角速度，其向量形式为：

四旋翼在机体坐标系与地理坐标系转换下，动力学呈现强非线性，连续时间状态方程可表示为：

系统可观测变量为位置与姿态角速度，观测方程为线性形式：

4.2 扩展卡尔曼滤波

       标准卡尔曼滤波仅适用于线性系统，而无人机系统存在三角函数与耦合项，必须使用EKF。其核心思想是在每一步滤波周期内，对非线性函数f(x,u)在当前状态估计点进行一阶泰勒展开，舍弃高阶项，将非线性系统近似为线性系统，再执行卡尔曼预测与更新。

EKF依赖过程噪声与测量噪声的统计特性，过程噪声协方差Q表征模型不确定性，测量噪声协方差R表征传感器精度，程序中分别对 GPS 与陀螺仪设置不同噪声强度，贴合实际硬件特性。

4.3 EKF状态估计

初始化参数与状态

设定无人机质量、转动惯量、采样时间、总仿真时间，初始化状态向量与协方差矩阵：

Simulink联合仿真获取真实状态与带噪测量值

通过调用四旋翼动力学模型，获取真实状态xtrue，并叠加高斯噪声生成传感器测量值：

时间更新（预测阶段）

测量更新（校正阶段）

5.完整工程文件

v v

关注后，GZH回复关键词：a33

或回复关键词：EKFUAV