【滤波跟踪】基于拓展卡尔曼滤波EKF的四旋翼无人机姿态估计附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法       神经网络预测       雷达通信      无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机

⛄ 内容介绍

使用EKF(Extended Kalman Filter, 扩展卡尔曼滤波)算法来对四旋翼无人机的姿态进行滤波和估计，姿态包括：俯仰角、滚转角、偏航角的角度值和角速度值。前提：角度值无法直接通过传感器直接测得，角速度值可以测得。

⛄ 部分代码

clear all;

N=200;

bsx(1)=1;

p(1)=10;

Z=randn(1,N)+25;

R = std(Z).^2;

w=randn(1,N);

Q = std(w).^2;

for t=2:N;

   x(t)=bsx(t-1);

   p1(t)=p(t-1)+Q;

   kg(t)=p1(t)/(p1(t)+R);

   bsx(t)=x(t)+kg(t)*(Z(t)-x(t));

   p(t)=(1-kg(t))*p1(t);

end

t=1:N;

plot(t,bsx,'r', t,Z,'g', t,x,'b');              % 红色线最优化估算结果滤波后的值，%绿色线观测值，蓝色线预测值

legend('Kalman滤波结果','观测值','预测值');

⛄ 运行结果

⛄ 参考文献

[1]段敏, 赵凌, 周莹. 基于扩展卡尔曼滤波的四旋翼无人机姿态估计方法[J]. 现代信息科技, 2022(006-004).

[2]柏植, 许海峰, 郭凯,等. 基于自适用扩展卡尔曼滤波算法的无人机姿态估计[J]. 黑龙江工业学院学报：综合版, 2021, 21(9):54-60.

⛳️ 完整代码

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料