【状态估计】卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、库图尔卡尔曼滤波、M-估计、鲁棒立方卡尔曼滤波器实现无人机位置跟踪、迎角和俯仰角跟踪， 方向角度跟踪等研究（Matlab代实现）

 👨‍🎓个人主页：研学社的博客  

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

2.1 采用扩展卡尔曼滤波的无人机

2.2 容积卡尔曼滤波角度跟踪方向 

​

 2.3 M估计鲁棒容积卡尔曼滤波

 2.4 采用无迹卡尔曼滤波对无人机俯仰角、原始角、偏航角进行估计

2.5 利用卡尔曼滤波进行无人机位置跟踪 

🎉3 参考文献

🌈4 Matlab代码实现

---

💥1 概述

随着无人机技术的发展 ， 固定翼无人机由于其成本低 、 飞行速度快、 高度高 、 可按规划航迹自主飞行等特点 ， 广泛应用于军事和民用领域， 进而对无人机飞行时完整的状态反馈提出了更高的要求。 但同时 ， 大多数无人机配置着由低成本微机电系统、 磁力计 、 GPS 及气压传感器组成的低成本飞控系统 ， 这些传感器在测量过程中会产生持续的噪声和漂移， 从而造成严重后果。 且利用这样的飞控系统研究无人机自主飞行控制问题 ，虽可以明显降低其研究成本， 但随着时间的增加 ， 传感器的测量误差被叠加， 严重影响无人机的控制精度 。 因此本文从低成本传感器入手， 研究固定翼无人机自主飞行控制问题 。由于无人机系统的内部动态规律无法直接测量， 状态估计作为传感器测量与控制器解算的中间层， 以可获取的测量数据作为输入估算出无人机当前状态， 反馈至控制器形成闭

环 ， 其准确性直接影响无人机的定位精度和控制效果 。

【状态估计】卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、库图尔卡尔曼滤波、M-估计、鲁棒立方卡尔曼滤波器实现无人机位置跟踪、迎角和俯仰角跟踪， 方向角度跟踪等研究（Matlab代实现）

📚2 运行结果

2.1 采用扩展卡尔曼滤波的无人机

2.2 容积卡尔曼滤波角度跟踪方向 

 2.3 M估计鲁棒容积卡尔曼滤波

 2.4 采用无迹卡尔曼滤波对无人机俯仰角、原始角、偏航角进行估计

2.5 利用卡尔曼滤波进行无人机位置跟踪 

部分代码：

%%Target Tracking
clc;
clear all;
%plane is flying but we will be considering its position only in the x
%direction to make it a 2-d case.
%initial state
xo=4000;
vox=280;
%Observations 
X=[4000 4260 4550 4860 5410 5600 5990 6400 6790 7000];
V=[280 282 285 286 290 292 294 296 299 302];
%Process Errors in Process Covaiance Matrix
del_px=20; %initial covariance matrix is choosen intuitively
del_pv=5;
%initial conditions
acc_x=2;
del_t=1;
vx=280;
del_x=25; %uncertainity in the measurement
%Observation Error
del_X=25;
del_VX=6;
Xk=[];
%The Predicted State
A=[1 del_t;0 1];
B=[(0.5*((del_t).^2));del_t];
p=[];
for k=1:1:length(X)-1;
Xk_= [X(k);V(k)];
uk=acc_x;
Xkp1=((A*Xk_));
Xkp2=((B*uk));   
Xkp=(Xkp1+Xkp2);%this is our first estimation
p=[p;Xkp];
%Initialising Process Covariance Matrix
Pk_=[((del_px).^2) 0;0 ((del_pv).^2)];
%Predicted Process Covariance Matrix
Pkp1=((A)*(Pk_));
Pkp2=((Pkp1)*(A'));
pkp=(Pkp2-[0 Pkp2(2);Pkp2(3) 0]); %since the 2nd and 3rd term are not imp.
%Calculating the Kalman gain
R=[((del_X)^2) 0;0 ((del_VX)^2)];
H=[1 0 ; 0 1];
K=((pkp)*H')/((H*pkp*H')+R);
%The New Observation
k=k+1;
Ykm=[X(k);V(k)];
C=[1 0;0 1];
Yk=C*Ykm;
%Calculating the Current State
Xk=[Xk; Xkp + K*(Yk-(H*(Xkp)))];
%Updating the process covariance matrix
Pk1=((eye)-(K*H))*pkp;
pk=(Pk1-[0 Pk1(3);Pk1(2) 0]);
k=k+1;
end
Xkf=[X(1)];
Vkf=[V(1)];
for k=1:2:(length(Xk)-1)
    Xkf=[Xkf;Xk(k)];
end
for k=2:2:(length(Xk))
    Vkf=[Vkf;Xk(k)];
end
prx=[X(1)];
prv=[V(1)];
for i=1:2:(length(p)-1)
    prx=[prx;p(i)];
end
for i=2:2:(length(p))
    prv=[prv;p(i)];
end

Y1=[1200 1300 1480 1590 1700 1800 1990 2090 2200 2300];
V1=[20 22 23 25 27 29 32 34 37 40];
%Process Errors in Process Covaiance Matrix
del_py=20; %initial covariance matrix is choosen intuitively
del_pv1=5;
%initial conditions
acc_y=2;
del_t=1;
vy=280;
del_y=25; %uncertainity in the measurement
%Observation Error
del_Y=25;
del_VY=6;
Yk=[];
%The Predicted State
A=[1 del_t;0 1];
B=[(0.5*((del_t).^2));del_t];
q=[];
for k=1:1:length(Y1)-1;
Yk_= [Y1(k);V1(k)];
uk2=acc_x;
Ykp1=((A*Yk_));
Ykp2=((B*uk2));   
Ykp=(Ykp1+Ykp2);%this is our first estimation

🎉3 参考文献

部分理论来源于网络，如有侵权请联系删除。

[1]王文丽,何博.基于扩展卡尔曼滤波器的无人机状态估计[J].现代信息科技,2022,6(04):118-122.DOI:10.19850/j.cnki.2096-4706.2022.04.031.

[2]李雨石. 基于卡尔曼滤波器的无人机地面目标跟踪[D].中国民航大学,2012.

🌈4 Matlab代码实现