基于EKF扩展卡尔曼滤波的四旋翼飞行器状态估计simulink建模与仿真

1.课题概述

2.系统仿真结果

3.核心程序或模型

4.系统原理简介

[4.1 四旋翼飞行器动力学与状态空间模型](#4.1 四旋翼飞行器动力学与状态空间模型)

[4.2 扩展卡尔曼滤波](#4.2 扩展卡尔曼滤波)

[4.3 EKF状态估计](#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