👨🎓个人主页
💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
📋📋📋本文目录如下：🎁🎁🎁
目录
2.4 采用无迹卡尔曼滤波对无人机俯仰角、原始角、偏航角进行估计
💥1 概述
随着无人机技术的发展，固定翼无人机由于其成本低、飞行速度快、高度高、可按规划航迹自主飞行等特点，广泛应用于军事和民用领域，进而对无人机飞行时完整的状态反馈提出了更高的要求。但同时，大多数无人机配置着由低成本微机电系统、磁力计、GPS 及气压传感器组成的低成本飞控系统，这些传感器在测量过程中会产生持续的噪声和漂移，从而造成严重后果。且利用这样的飞控系统研究无人机自主飞行控制问题，虽可以明显降低其研究成本，但随着时间的增加，传感器的测量误差被叠加，严重影响无人机的控制精度。因此本文从低成本传感器入手，研究固定翼无人机自主飞行控制问题。由于无人机系统的内部动态规律无法直接测量，状态估计作为传感器测量与控制器解算的中间层，以可获取的测量数据作为输入估算出无人机当前状态，反馈至控制器形成闭
环，其准确性直接影响无人机的定位精度和控制效果。
【状态估计】卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、库图尔卡尔曼滤波、M-估计、鲁棒立方卡尔曼滤波器实现无人机位置跟踪、迎角和俯仰角跟踪， 方向角度跟踪等研究（Matlab代实现）
卡尔曼滤波及其变种在无人机状态估计中的研究综述
一、卡尔曼滤波（KF）基本原理与局限性
卡尔曼滤波是一种基于贝叶斯框架的递归算法，通过预测和更新两个步骤对动态系统状态进行最优估计。其核心假设是系统线性且噪声服从高斯分布。算法通过状态转移方程和观测方程的最小均方误差准则实现状态估计，具有无偏性和方差最小的特性。然而，KF仅适用于线性系统，对无人机飞行中的非线性动力学（如姿态角变化、空气动力学耦合）无法直接应用。
二、扩展卡尔曼滤波（EKF）与非线性的初步处理
EKF通过一阶泰勒展开对非线性系统进行局部线性化，将雅可比矩阵引入状态转移和观测模型中。例如在无人机迎角跟踪中，EKF可将非线性气动力方程线性化，但会引入截断误差，导致姿态角估计精度受限。研究显示，EKF在GPS/IMU融合定位中位置误差均方根（RMSE）约为0.5m，而俯仰角误差在强机动下可达2°以上。
三、无迹卡尔曼滤波（UKF）与非线性精度提升
UKF采用无迹变换（UT）解决非线性问题，通过2n+1个Sigma点精确捕获状态分布的高阶矩。以无人机方向角跟踪为例，UKF通过UT传播四元数状态，避免了欧拉角奇异性问题，姿态角RMSE较EKF降低30%。实验表明，在动态飞行中，UKF的俯仰角误差可控制在0.8°以内，而EKF为1.2°。此外，自适应UKF（AUKF）通过实时调整噪声协方差，在复杂气流扰动下位置跟踪精度提升28.9%。
四、库图尔卡尔曼滤波（iUBIKF）的协方差优化
iUBIKF针对高维非线性系统，通过最小化预测误差协方差矩阵的迹来优化卡尔曼增益。在无人机集群协同导航中，iUBIKF通过约束协方差矩阵上界，解决了多机状态耦合导致的数值不稳定问题。仿真显示，其在100-200s时间段的轨迹跟踪误差比传统联邦卡尔曼滤波降低40%。
五、M-估计的鲁棒性增强机制
M-估计通过引入Huber、Cauchy等鲁棒损失函数，减小异常测量值对状态估计的影响。在无人机视觉定位中，M-估计可有效抑制UWB测距异常值，将位置跳变概率从15%降至3%以下。研究对比显示，采用Biweight M-估计的EKF，在50%数据污染情况下，俯仰角估计误差仍能保持在1.5°，而标准EKF误差超过4°。
六、鲁棒立方卡尔曼滤波器（RCKF）的综合优化
RCKF融合了协方差约束和鲁棒权重机制，通过惩罚参数μ调节估计偏差与方差的平衡。在无人机悬停实验中，传统EKF因UWB异常值导致位置波动达±1.2m，而RCKF将其控制在±0.3m内。算法通过投影统计（PS）识别异常值，并采用迭代加权最小二乘（IRLS）更新状态，在复杂电磁干扰下方向角跟踪误差降低58%。
七、应用场景性能对比分析
| 方法 | 计算复杂度 | 位置误差（RMSE） | 俯仰角误差（RMSE） | 抗异常值能力 |
|---|---|---|---|---|
| EKF | O(n²) | 0.5-1.2m | 1.2-2.5° | 低 |
| UKF | O(n³) | 0.3-0.8m | 0.5-1.0° | 中 |
| iUBIKF | O(n³) | 0.2-0.6m | 0.4-0.9° | 高（协方差约束） |
| M-EKF | O(n²) | 0.4-1.0m | 0.8-1.5° | 高 |
| RCKF | O(n³) | 0.2-0.5m | 0.3-0.7° | 极高 |
八、典型应用案例
-
位置跟踪：
- UKF在UWB动态定位中，通过加权最小二乘优化，位置精度提升28.9%。
- RCKF在Crazyflie无人机上实现±0.2m的悬停精度，显著优于传统EKF。
-
迎角/俯仰角跟踪：
- 四元数UKF（QKF）解决了欧拉角奇异性，在60°大迎角机动下误差<0.6°。
- 基于ESKF-MPC的方法，通过前馈风扰补偿，俯仰角跟踪误差降低至0.013m。
-
方向角跟踪：
- 改进UKF（IUKF）在水下目标跟踪中，方向角误差比EKF降低40%。
- 联邦RCKF在输电线巡检中，航向角估计误差<0.5°，满足±3°安全阈值。
九、未来研究方向
- 混合架构：结合UKF的非线性精度与RCKF的鲁棒性，开发自适应混合滤波器。
- 计算优化：针对嵌入式系统设计降阶iUBIKF，如采用Cholesky分解降低O(n³)复杂度。
- 多源融合：集成M-估计的视觉/激光雷达数据与IMU，提升复杂环境下的状态估计可靠性。
十、结论
在无人机状态估计中，KF系列算法的选择需权衡非线性强度、噪声特性及计算资源：
- 强非线性系统（如大迎角机动）优先选用UKF或iUBIKF；
- 异常值频发场景（如城市GNSS遮挡）适用M-估计或RCKF；
- 高维协同导航可采用联邦iUBIKF架构。实验表明，RCKF在综合性能上展现最优潜力，其位置误差可控制在0.2-0.5m，姿态角误差<0.7°，且抗干扰能力突出。
📚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.
扫一扫
1055
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
