基于扩展卡尔曼滤波（EKF）结合到达时间（TDOA）、到达频率（FDOA）实现多无人机无源定位，MATLAB代码

原创

于 2025-04-09 10:51:18 发布 · 1.1k 阅读

22 ·

CC 4.0 BY-SA版权

文章标签：

#无人机 #matlab #开发语言

在这里插入图片描述

本文给出一个基于扩展卡尔曼滤波（EKF）结合时差定位（TDOA）和频差定位（FDOA）实现多无人机给无源目标定位的Matlab代码。该代码涉及TDOA/FDOA模型设计、EKF非线性滤波逻辑及多传感器数据融合方法。

文章目录

运行结果
代码框架
代码说明
结论

运行结果

在这里插入图片描述

代码框架

% 基于扩展卡尔曼滤波（EKF）结合到达时间（TDOA）、到达频率（FDOA）实现多无人机给单目标定位
% 作者：matlabfilter
% 2025-04-07/Ver1

%% 参数初始化
clear;clc;close all;
T = 1;          % 采样周期
N = 10;        % 总采样次数
num_drones = 4; % 无人机数量
sigma_T = 1e-3; % TDOA测量噪声标准差
sigma_F = 1e-2;% FDOA测量噪声标准差

% 无人机初始位置和速度（示例数据）
drone_pos = [0, 0, 0;
    5, 0, 10;
    0, 5, 6;
    5, 5, 0]; % 四台无人机的位置（3D）
drone_vel = [0.1, 0.1, 0;
    0.1, -0.1, 0;
    -0.1, 0.1, 0;
    -0.1, -0.1, 0]; % 速度向量
% 目标初始状态（位置和速度）
x_true = [0; 0; 0; 0.5; 0.5; 0.1]; % [x, y, z, vx, vy, vz]
P = diag([10, 10, 10, 1, 1, 1]);     % 初始协方差矩阵
Q = diag([0.1, 0.1, 0.1, 0.01, 0.01, 0.01]); % 过程噪声协方差
R = diag([sigma_T^2, sigma_F^2,sigma_T^2, sigma_F^2,sigma_T^2, sigma_F^2]);     % 观测噪声协方差
    x_est = x_true + 0*randn(6,1); % 初始状态估计
    estimated_pos = zeros(6, N);

for k = 1:N
    % 预测步骤
    [x_pred, F] = ekf_predict(x_est, T, Q, P);
    x_true = F*x_true;
    X_true(:,k) = x_true;

    % 生成仿真数据（TDOA和FDOA测量值）
    [z_TDOA, z_FDOA] = generate_measurements(x_true, drone_pos, drone_vel, N, sigma_T, sigma_F);

    % 更新步骤（使用TDOA和FDOA观测值）
    H = compute_jacobian(x_pred, drone_pos, drone_vel); % 计算雅可比矩阵
    z_meas = [z_TDOA(