基于DKF延迟卡尔曼滤波器的无人机的状态估计研究（Matlab代码实现）

荔枝科研社

于 2025-06-08 09:55:24 发布

阅读量820

点赞数 11

文章标签：无人机 matlab 开发语言

本文链接：https://blog.csdn.net/weixin_46039719/article/details/148509663

版权

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

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

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

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

目录

⛳️赠与读者

💥1 概述

基于DKF延迟卡尔曼滤波器的无人机的状态估计研究

摘要

1. 引言

2. 系统架构与传感器介绍

2.1 系统架构

2.2 IMU介绍

2.3 实时动态GPS单元介绍

3. 基于DKF的状态估计方法

3.1 DKF原理

3.2 状态空间模型

3.3 传感器数据融合

3.4 算法实现

4. 实验验证与结果分析

4.1 实验设置

4.2 结果分析

5. 结论与展望

📚2 运行结果

2.1 算例1-dkf

2.2 算例2-dkf-alexander

2.3 算例3-dkf-larsen

2.4 算例4-dkf-merwe

🎉3 参考文献

🌈4 Matlab代码实现

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

基于DKF延迟卡尔曼滤波器的无人机的状态估计研究

摘要

本文介绍了一种通过集成惯性测量单元（IMU）和低成本实时动态GPS单元（考虑时间延迟）来估计四旋翼无人机六自由度位置和姿态的方案。该方案在已知时间延迟的条件下，使用延迟卡尔曼滤波器（DKF）将GPS的时滞位置测量与IMU的同步姿态和角速度测量进行融合，以实现精确的状态估计。通过数值例子和实验数据验证了所提出方法的有效性。

1. 引言

无人机在各个领域得到了广泛应用，其自主导航和控制能力的关键在于对自身状态的精确估计。无人机状态通常包括位置、速度、姿态等信息，这些信息会受到各种噪声和干扰的影响，例如传感器测量误差、气流扰动、GPS信号衰落等。为了获得可靠的状态估计，需要利用有效的滤波算法对传感器数据进行处理和融合。

卡尔曼滤波器作为一种最优估计方法，因其能够有效处理线性高斯系统中的噪声，并具有较低的计算复杂度，成为了无人机状态估计的理想选择。然而，传统的卡尔曼滤波器在实际应用中存在输出延迟的问题，这会导致状态估计的误差。为了解决这一问题，研究人员提出了延迟卡尔曼滤波器（DKF）。

本文将进一步研究基于DKF的无人机状态估计方法，通过集成IMU和考虑时间延迟的实时动态GPS单元，实现四旋翼无人机六自由度位置和姿态的精确估计。

2. 系统架构与传感器介绍

2.1 系统架构

本系统主要包括IMU、实时动态GPS单元、数据处理单元和无人机飞行控制系统。IMU提供无人机的加速度和角速度信息，实时动态GPS单元提供无人机的位置信息（考虑时间延迟），数据处理单元负责融合传感器数据并进行状态估计，无人机飞行控制系统根据状态估计结果进行飞行控制。

2.2 IMU介绍

IMU是Inertial Measurement Unit的缩写，是一种集成了加速度计和陀螺仪的装置。IMU可以提供物体的三维加速度和三维角速度信息。这些信息可以通过结合加速度数据和陀螺仪数据来确定物体的姿态，从而获得实时的运动追踪。在无人机中，IMU的主要作用是帮助无人机维护平衡和姿态。

2.3 实时动态GPS单元介绍

实时动态GPS单元是一种能够提供高精度位置信息的传感器。然而，由于信号传播延迟、接收设备处理时间等因素，实时动态GPS单元提供的位置信息往往存在一定的时间延迟。在本系统中，我们考虑GPS的时间延迟，并将其与IMU数据进行融合以实现更精确的状态估计。

3. 基于DKF的状态估计方法

3.1 DKF原理

延迟卡尔曼滤波器（DKF）是对传统卡尔曼滤波器的改进和扩展，它通过引入延迟状态和测量数据来减小滤波器的延迟。具体而言，DKF使用滑动窗口的方式来存储最近的状态和测量数据，然后通过对窗口内的数据进行加权平均来估计当前的系统状态。

3.2 状态空间模型

无人机的状态空间模型通常包括位置、速度、姿态等状态变量。在本系统中，我们采用六自由度状态空间模型来描述无人机的运动状态。状态变量包括无人机的三维位置、三维速度以及姿态角（俯仰角、滚转角、偏航角）。

3.3 传感器数据融合

在已知时间延迟的条件下，我们使用DKF将GPS的时滞位置测量与IMU的同步姿态和角速度测量进行融合。融合过程包括预测步骤和更新步骤：在预测步骤中，根据无人机的动力学模型和上一时刻的状态估计预测当前时刻的状态；在更新步骤中，利用GPS和IMU的测量数据对预测状态进行校正。

3.4 算法实现

算法实现过程中需要考虑以下几个方面：

状态预测方程的建立：根据无人机的动力学模型建立状态预测方程。
协方差预测方程的建立：根据状态预测方程和噪声统计特性建立协方差预测方程。
卡尔曼增益的计算：根据协方差预测方程和测量噪声统计特性计算卡尔曼增益。
状态更新方程的建立：利用卡尔曼增益和测量数据对预测状态进行校正。
协方差更新方程的建立：根据状态更新方程和卡尔曼增益更新协方差矩阵。

4. 实验验证与结果分析

4.1 实验设置

为了验证所提出方法的有效性，我们进行了数值仿真实验和室外自主飞行实验。数值仿真实验中，我们模拟了无人机的飞行过程并添加了噪声和延迟；室外自主飞行实验中，我们使用了一架四旋翼无人机并集成了IMU和实时动态GPS单元进行状态估计。

4.2 结果分析

数值仿真实验和室外自主飞行实验的结果均表明，所提出的方法能够有效地融合IMU和GPS数据，实现无人机六自由度位置和姿态的精确估计。与传统的卡尔曼滤波器相比，DKF能够显著减小状态估计的误差，提高估计的精度和鲁棒性。

5. 结论与展望

本文介绍了一种基于DKF的无人机状态估计方法，通过集成IMU和考虑时间延迟的实时动态GPS单元，实现了四旋翼无人机六自由度位置和姿态的精确估计。实验结果表明，所提出的方法具有较高的估计精度和鲁棒性。

未来工作可以进一步探索更高级的滤波算法，如无迹卡尔曼滤波器（UKF）或粒子滤波器（Particle Filter），以进一步提高无人机状态估计的性能。此外，还可以考虑将视觉传感器等其他类型的传感器与IMU和GPS进行融合，以实现更加全面和准确的状态估计。

📚2 运行结果

2.1 算例1-dkf

2.2 算例2-dkf-alexander

2.3 算例3-dkf-larsen

2.4 算例4-dkf-merwe

部分代码：

addpath('common_functions/'); % includes hat, vee, etc
%% Define variables
tf = 20; % final time
freq = 100; % frequency of the system (Hz)
freq_gps = 5; % frequency of the GPS update (Hz)

% Parameters
R_fv = eye(3);
R_nv = eye(3);
R_bi = [cos(pi/4), -sin(pi/4), 0;
sin(pi/4), cos(pi/4), 0;
0, 0, 1];
R_bi = eye(3);

gps_delay = 0.2; % GPS measurement delay (this code is hard coded
% such that this delay must be a multiplication of freq_gps)

% Variances of w_k
V_a = 1e-1*diag([0.1 0.1 0.1]).^2; % acceleration
V_W = 1e-1*diag([0.5 0.5 0.5]).^2; % angular velocity
V_b_a = 0.01^2; % acclerometer z bias

Q = blkdiag(V_a, V_W, V_b_a);

% Covariances of measurement error
V_R_imu = diag([0.01 0.01 0.01]).^2;
V_x_gps = diag([0.01 0.01 0.01]).^2;
V_v_gps = diag([0.01 0.01 0.01]).^2;

V_x_lidar = 0.01^2;

% Initial covariances of x
P_x = diag([1^2, 1^2, 1^2]); % position
P_v = diag([1^2, 1^2, 1^2]); % velocity
P_eta = diag([0.1^2, 0.1^2, 0.1^2]); % attitude
P_b_a = 1^2; % accelerometer z bias

P = blkdiag(P_x, P_v, P_eta, P_b_a);
[m, ~] = size(P);

%% Calculate time related parameters
N = tf*freq + 1;
t = linspace(0,tf,N);
h = t(2) - t(1);

%% Create empty arrays to save data
% tru: simulated true data
% imu: simulated IMU measurement with noise
% vcn: simulated Vicon data with noise
% est: estimated data
% gps: simulated GPS data with noise
% ldr: simulated Lidar data with noise
[tru, imu, vcn, est, gps, ~] = create_arrays_to_save_data(N, m);

%% Initial estimates
est.x(:,1) = [0, 0, 0]';
est.v(:,1) = [0, 0, 0]';