2024数维杯小国赛大学生数学建模挑战赛A题

Cyrus_Wong

已于 2024-11-16 17:37:14 修改

阅读量1.6k

点赞数 11

分类专栏：数学建模算法及应用文章标签：数学建模

于 2024-11-07 23:03:56 首次发布

本文链接：https://blog.csdn.net/m0_74473137/article/details/143609116

版权

数学建模算法及应用专栏收录该内容

8 篇文章

订阅专栏

说明

针对问题1，我们首先建立了基于信号特性的分类模型，将机会信号分为TOA、TDOA、DFD、AOA、RSSI五类，并通过理论分析明确了每类信号单独及联合使用时定位能力的最低信号需求。

对于问题2，我们采用了无线定位模型[1]。通过RSSI和TDOA这两种机会信号结合的方式，建立一个无线定位的数学模型。通过数学模型将复杂方程转化为一个非线性优化问题。使用该方法求解出得到飞行器的位置估计。

针对问题3，我们通过精细的偏差识别与信号筛选机制，能够有效去除错误数据，最后基于TOA和TDOA信号机会，结合多源融合定位技术[2]，通过建立数学模型和优化算法来准确估计飞行器的位置

1、问题预处理

根据附件中的信息，计算两种飞行器接收情况，不同机会信号下从发送器到接收器的到达时间，计算其时间的平均值，以及标准差。平均值和标准差

2、问题1的模型建立与求解

我们根据题目提供的信息，总共有五种机会信号，分别是到达时间信息（TOA）、到达时间查信息（TDOA）、多普勒频率差信息（DFD）、到达角度信息（AOA）和接受强度指标信息（RSSI）。根据题目描述建立信号机会的数学表达式以及最少机会信号个数来确定飞行器位置如下：

2.1 机会信号数学表达式的建立

1.到达时间信息（TOA）的数学表达式：

TOA= Ts - Tf =d/c

其中

Ts——接收时间（s）

Tf——发射时间（s）

d——飞行器与发射源之间的距离（m）

c——从发射源到飞行器的传播速度,即光速（m/s）

2.到达时间差信息（TDOA）的数学表达式：

TDOA1,2 =( Ts,1 −Tf,1 )−(Ts,2 −Tf,2)=

其中

TDOA1,2——表示飞行器到两个不同发射源之间的距离（m）

3.多普勒频率差信息（DFD）的数学表达式：

其中

——是从发射源到接收源的方向单位向量

——发射源相对接受源的速度（m/s）

f——信号频率（Hz）

4. 到达角度信息（AOA）的数学表达式：

根据题目信息需要确定发射源相对接收源的位置坐标（x,y,z），我们可以根据接收到的信号计算出两个角度 $\alpha$ 和 $\beta$ ，分别代表信号在水平面（xOy平面）上的入射角和垂直于该平面的俯仰角。利用三角函数关系得出。

数学表达式：

水平角度（ $\alpha$ ）：代表信号方向与x轴正方向之间的夹角，可通过发射源的横向偏移(y坐标)与其纵向偏移(x坐标)的比值得出。

tan $\alpha$ =y/x

俯仰角度（ $\beta$ ）：表示信号方向相对于局部水平面的抬升或下降角度，通过发射源的垂直高度(z坐标)与其到接收器在水平面上投影的距离(即)的比值得出。

5.接收强度指标信息（RSSI）的数学表达式：

其中

Pnominal——标称信号强度

n——信道衰减系数

d——发射源与接收源的距离

d0——参考距离

2.2 不同信号机会确定飞行器位置

1.TOA确定飞行器位置：

由于TOA只提供时间信息，则至少需要4个不同位置的信号源来确定飞行器的3D位置。

2.TDOA确定飞行器位置：

使用TDOA可以确定飞行器到两个发射源的距离差，至少需要3个不同位置的信号源来确定飞行器的3D位置。

3.DFD确定飞行器位置：

使用DFD主要反映的是相对运动信息，并且DFD本身就很难直接确定位置信息，通常需要与其他类型的信号（如TOA或TDOA）联合使用来确定位置，因此DFD不能确定飞行器的3D位置。

4.AOA确定飞行器位置：

尝试使用AOA信息进行定位，我们实际上是在处理一个几何问题，需要至少三个非共线的AOA测量来确定三维空间的一个点，即飞行器的3D位置。同时飞行器的2D位置至少需要2个不同位置的信号源来确定。

5.RSSI确定飞行器位置：

单独使用RSSI很难确定位置，因为它主要反映的是信号强度随距离变化而增减，并且受环境因素影响较大。如果想要确定位置信息，还需要与其他信号技术结合使用。

综上所述，对于最少机会信号个数来唯一确定飞行器位置：

对于3D定位，采用TOA需要至少4个不同位置的信号源，采用TDOA需要至少3个不同位置的信号源，采用DFD则需要至少3个非共线的信号源；

对于2D定位，采用AOA至少需要2个不同位置的信号源。

3、问题2的模型建立与求解

3.1 无线定位模型的建立

无线定位模型本质上是基于RSSI和TDOA的结合，这种方法适合在没有精确时间同步或者很难获取精确到达时间（TOA）信息的场景下使用。以下是基于这两种信号特征的定位算法模型概述：

1.TDOA模型：

TDOA提供了两个或多个信号源之间到接收器的相对距离差信息。设飞行器接收到来自两个不同发射源和的信号，它们的到达时间差为,则飞行器到两信号源的距离差为：

其中

C——信号传播速度，即光速

RSSI模型

RSSI是一种基于信号强度的测距方法，虽然受到环境因素的严重影响，但在一定条件下可以作为辅助信息。RSSI与距离d0的关系一般是非线性的，通常采用经验模型如对数距离路径损耗模型：

其中

Pt——发射功率

n——路径损耗指数

d0——参考距离

η——阴影衰落或变量。

2.综合为定位

2.1设立参考源

选择一个作为参考的发射源（比如S1），基于其他发射源对S1的TDOA信息，建立一系列关于距离差的方程组。这组方程可以表示为飞行器位置向量的函数。

2.2利用RSSI进行距离估计

对每个发射源，根据接收到的RSSI值和预先建立的RSSI-距离关系模型，可以算出飞行器到该参考源的大致距离。

2.3构建优化问题

将TDOA方程和RSSI估计转化为一个非线性优化问题，目标是最小化所有估计距离与TDOA距离差之间的不一致性和RSSI估计距离与实际TDOA解耦合的距离之间的差异。这通常是一个非线性最小二乘问题：

其中

Wk——根据RSSI估计可靠性的权重

解决问题

使用数值优化方法，使用梯度下降算法求解上述优化问题，得到飞行器的位置估计P。

3.2无线定位模型求解

根据上述无线定位模型的建立，为了使得使用最小二乘法计算得到飞行器的位置，根据附件飞行器接收情况1，代入数学模型进行计算，得到飞行器的位置估计。下面是对建模过程分步处理。

步骤一：读取数据

读取飞行器和发射器的坐标数据，分别对应飞行器坐标以及发射源的坐标

步骤二：设定数据采样时间间隔：

假定所有数据样本之间的时间间隔为T=0.01秒

步骤三：计算接收与发射时间：

根据采样点数量和时间间隔计算每个接收时间点。

假设发射时间与接收时间相同，这可能是一个简化的假设，实际应用中需要考虑信号传播延迟。

步骤四：距离计算

计算飞行器坐标到三个预设基站坐标的欧几里得距离

步骤五：时间差计算

利用光速（大约3e8米/秒）将上述距离转换为信号传输时间，并进一步计算出相对于第一个基站的时间差（delta_t1,delta_2.delta_3）。

步骤六：位置估计

调用自定义的trilat函数，该函数接收基站坐标和各基站间的时间差作为输入，使用三边测量法计算飞行器的估计位置。

trilat函数内部通过构造线性方程组来解算未知的飞行器坐标。这是基于到达时间差（TDOA）原理，利用基站之间的相对位置和信号到达时间差来确定目标位置。

步骤七：提取估计的飞行器位置信息p。p点位置坐标(50,50,0)

步骤八：得到0~10秒内，发射源的每秒变化图2-2，飞行器的位置变化图2-3。

(图就不放了，后面有代码)

3.3结果

基于对无线定位模型的建立，通过将数学模型转化为一个非线性最小二乘问题，同时利用可视化绘制出0至10秒内发射源与飞行器位置的散点图，观察位置变化明显，通过发现飞行器的位置估计与发射源不在一个可视化图像内，合理推断计算模型的数据范围，得出飞行器0至10秒内的位置估计值。

4、问题3的模型建立与求解

4.1筛选偏差较大机会信号的方法

1.定义偏差识别标准

我们使用统计学的方法来识别异常值

对于TOA和TDOA：计算多个连续样本的平均值和标准差，如果某次测量值与平均值的偏差超

过一定倍数的标准差，则认为该测量值存在较大偏差。

对于DFD：考虑到多普勒频移受相对速度影响大，且在理想条件下相对稳定，同样可采用平均值加标准差的方法判断。

对于AOA：由于角度测量通常较为稳定，除非飞行器快速机动，否则大幅度跳变可视为异常。

对于RSSI：受环境影响较大，但持续显著低于或高于预期水平的值可能指示偏差。

2.实施信号筛选

对每个时间点的机会信号，依据上述标准进行筛选。对于每个机会信号类型，计算飞行器接受情况1的TOA、TDOA、DFD、AOA、RSSI所有样本的平均值和标准差，然后结合平均值±3σ范围的值两两对比每种情况选出偏差较小的机会信号。

通过可视化的箱线图观察对每种机会信号的偏差进行对比。下面是抽取TOA与RSSI，TOA与TDOA，两组结果的箱线图对比。

通过筛选计算，我们选择TOA和TDOA机会信号进行计算。

4.2多源融合定位模型的建立

1.TOA方法

TOA定位是基于信号从发射源到接收器的传播时间来确定位置。如果已知信号传播速度（通常为光速c在空气中），并且有至少三个不同位置的接收器记录到信号到达的时间，就可以通过以下公式计算出发射源的位置：

其中，ti 是信号到达第i个接收器的时间，t0 是一个参考时间（通常是第一个接收器接收到信号的时间或者发射时间），dx,i,dy,i,dz,i 分别是从发射源到第i个接收器在x、y、z方向上的距离，N是接收器的数量。

2.TDOA方法

TDOA定位则是基于信号到达不同接收器之间的时间差来确定位置。如果有三个接收器，可以形成两个独立的时间差，通过这两个时间差可以解算出一个双曲线方程组，进而确定源位置。对于三维空间中的任意一对接收器i和j，TDOA定位的基本公式为：

其中，C是信号传播速度，和分别是第i个和第j个接收器的位置向量，是发射源的位置向量。

3.结合TOA和TDOA的模型

结合TOA和TDOA方法，我们可以构建一个系统来求解发射源位置

。以三个接收器为例，建立方程组，来近似求解的最优值。

4.通过梯度下降的优化算法来求解发射源的位置坐标的算法流程。

初始化参数：选择一个初始点作为参数的起始值，通常可以是随机值或者全零向量，记θ(0)。

计算梯度：在在当前参数值θ(t)处计算目标函数

关于参数的梯度（即偏导数向量），记作。

更新参数：根据梯度和学习率（步长） $\eta$ 更新参数值，公式为：

其中， $\eta$ 控制着每一步移动的大小，太大可能导致震荡不收敛，太小则收敛速度慢。

重复迭代：

重复步骤2和3直到满足停止条件。

4.3多源融合定位模型求解

根据筛选最大信号机会的方法，通过计算每种信号机会的飞行器的接收情况的平均值以及标准差，对比每种信号情况偏离标准差的情况，筛选出偏离值最小的一种飞行器信号机会。通过两两数据的对比可视化退箱线图对比筛选出，TDOA和TOA信号机会方式。

步骤一：初始化数据

确定接收器的位置，获取N个接收器在三维空间中的确切位置向量集合

,其中每个

设置初始条件，确定一个θ(0)位置坐标作为作为发射源的初始位置；设置学习率（步长） $\eta$ ，以及定义停止条件（即迭代次数）。

步骤二：构建目标函数

根据数据的预处理计算出各个信号机会的平均值和标准差。将数据带入到多源融合模型当中。将TOA和TDOA的预处理信息，构建一个综合误差函数，根据初始发射源的位置，使用MATLAB编程计算得到TOA和TDOA值与实际测量的偏差可视化图像。计算每个接收器的TOA和TDOA误差平方和的最小化，即标准差。

步骤三：梯度下降的迭代设置

使用梯度下降算法，设置迭代数目。给定参数θ(t)计算目标函数关于参数的梯度

，对误差函数进行偏导数计算。

参数更新：使用梯度下降公式更新参数

步骤四：迭代判断：

检查是否达到预设的停止条件，如果未达到停止条件，则返回重复步骤2和3直到满足停止条件。

步骤五：根据数学模型利用梯度下降算法进行合理计算，推算出合理的飞行器位置坐标估计如图：3-3

图：3-3

步骤五：根据位置推断，绘制出不同速度向量情况下0至10秒内的飞行器的位置变化轨迹如图[0; 0; 8]速度向量情况下的位置轨迹如图3-4，[20; 0; 0]速度向量情况下的位置轨迹如图3-5，[0; 0; 0]速度向量情况下的位置轨迹如图3-6。

5、结果

根据问题筛选出合理的数据情形，同时结合TOA和TDOA机会信号的特点建立数学模型，根据将两个数据模型的糅合，引用多源融合定位模型，结合梯度下降算法计算出0至10秒的飞行器坐标位置估计，同时通过更改速度向量的方式，绘制出不同速度向量情况下，飞行器的位置轨迹，根据接收信号的信息，为信号传播减少失真奠定了基础，实现简单的三维定位轨迹模拟。

数据预处理

clc
clear
% 读取Excel文件
filename = '飞行器接收情况1.xlsx'; %filename = '飞行器接收情况2.xlsx';
[data, ~, ~] = xlsread(filename);

% 将科学计数法表示的数据转换为常规格式
data = convertToNumericFormat(data);

% 提取所需的列
TOA_source2 = data(:, 1);
TOA_source3 = data(:, 2);
TDOA_source3_source4 = data(:, 3);
DFD_source1_source2 = data(:, 4);
DFD_source3_source4 = data(:, 5);
AOAtan_alpha = data(:, 6);
AOAtan_beta = data(:, 7);
RSSI_source1 = data(:, 8);
RSSI_source3 = data(:, 9);
RSSI_source4 = data(:, 10);

% 计算平均值和标准差
TOA_mean_source2 = mean(TOA_source2);
TOA_std_source2 = std(TOA_source2);

TOA_mean_source3 = mean(TOA_source3);
TOA_std_source3 = std(TOA_source3);

TDOA_mean_source3_source4 = mean(TDOA_source3_source4);
TDOA_std_source3_source4 = std(TDOA_source3_source4);

DFD_mean_source1_source2 = mean(DFD_source1_source2);
DFD_std_source1_source2 = std(DFD_source1_source2);

DFD_mean_source3_source4 = mean(DFD_source3_source4);
DFD_std_source3_source4 = std(DFD_source3_source4);

AOA_mean_alpha = mean(AOAtan_alpha);
AOA_std_alpha = std(AOAtan_alpha);

AOA_mean_beta = mean(AOAtan_beta);
AOA_std_beta = std(AOAtan_beta);

RSSI_mean_source1 = mean(RSSI_source1);
RSSI_std_source1 = std(RSSI_source1);

RSSI_mean_source3 = mean(RSSI_source3);
RSSI_std_source3 = std(RSSI_source3);

RSSI_mean_source4 = mean(RSSI_source4);
RSSI_std_source4 = std(RSSI_source4);

% % 绘制直方图
% figure;
% subplot(2, 1, 1);
% histogram(TOA_source2);
% title('TOA(发射源2) 直方图');
% 
% subplot(2, 1, 2);
% histogram(TOA_source3);
% title('TOA(发射源3) 直方图');

% 绘制箱线图
figure;
set(gcf, 'Color', 'white');
subplot(2, 1, 1);
boxplot(TOA_source2);
title('TOA(发射源2) 箱线图');

subplot(2, 1, 2);
boxplot(TOA_source3);
title('TOA(发射源3) 箱线图');

subplot(2, 1, 2);
boxplot(TDOA_source3_source4);
title('TDOA(发射源3, 发射源4) 箱线图');

% subplot(2, 1, 2);
% boxplot(RSSI_source1);
% title('RSSI(发射源1) 箱线图');

% 显示结果
disp('平均值和标准差：');
disp(['TOA(发射源2) 平均值: ', num2str(TOA_mean_source2), ', 标准差: ', num2str(TOA_std_source2)]);
disp(['TOA(发射源3) 平均值: ', num2str(TOA_mean_source3), ', 标准差: ', num2str(TOA_std_source3)]);
disp(['TDOA(发射源3, 发射源4) 平均值: ', num2str(TDOA_mean_source3_source4), ', 标准差: ', num2str(TDOA_std_source3_source4)]);
disp(['DFD(发射源1, 发射源2) 平均值: ', num2str(DFD_mean_source1_source2), ', 标准差: ', num2str(DFD_std_source1_source2)]);
disp(['DFD(发射源3, 发射源4) 平均值: ', num2str(DFD_mean_source3_source4), ', 标准差: ', num2str(DFD_std_source3_source4)]);
disp(['AOA atan(alpha) 平均值: ', num2str(AOA_mean_alpha), ', 标准差: ', num2str(AOA_std_alpha)]);
disp(['AOA atan(beta) 平均值: ', num2str(AOA_mean_beta), ', 标准差: ', num2str(AOA_std_beta)]);
disp(['RSSI(发射源1) 平均值: ', num2str(RSSI_mean_source1), ', 标准差: ', num2str(RSSI_std_source1)]);
disp(['RSSI(发射源3) 平均值: ', num2str(RSSI_mean_source3), ', 标准差: ', num2str(RSSI_std_source3)]);
disp(['RSSI(发射源4) 平均值: ', num2str(RSSI_mean_source4), ', 标准差: ', num2str(RSSI_std_source4)]);

function data = convertToNumericFormat(data)
    for i = 1:numel(data)
        if ischar(data(i))
            data(i) = str2double(data(i));
        end
    end
end


添加下面代码绘制出飞行器接收情况的平均值和方差的可视化图像
% 创建数据
means = [TOA_mean_source2, TOA_mean_source3, TDOA_mean_source3_source4, ...
    DFD_mean_source1_source2, DFD_mean_source3_source4, AOA_mean_alpha, ...
    AOA_mean_beta, RSSI_mean_source1, RSSI_mean_source3, RSSI_mean_source4];
stds = [TOA_std_source2, TOA_std_source3, TDOA_std_source3_source4, ...
    DFD_std_source1_source2, DFD_std_source3_source4, AOA_std_alpha, ...
    AOA_std_beta, RSSI_std_source1, RSSI_std_source3, RSSI_std_source4];
labels = {'TOA(发射源2)', 'TOA(发射源3)', 'TDOA(发射源3, 发射源4)', ...
    'DFD(发射源1, 发射源2)', 'DFD(发射源3, 发射源4)', 'AOA atan(alpha)', ...
    'AOA atan(beta)', 'RSSI(发射源1)', 'RSSI(发射源3)', 'RSSI(发射源4)'};

% 准备数据
means = [TOA_mean_source2, TOA_mean_source3, TDOA_mean_source3_source4, ...
         DFD_mean_source1_source2, DFD_mean_source3_source4, ...
         AOA_mean_alpha, AOA_mean_beta, ...
         RSSI_mean_source1, RSSI_mean_source3, RSSI_mean_source4];

std_devs = [TOA_std_source2, TOA_std_source3, TDOA_std_source3_source4, ...
            DFD_std_source1_source2, DFD_std_source3_source4, ...
            AOA_std_alpha, AOA_std_beta, ...
            RSSI_std_source1, RSSI_std_source3, RSSI_std_source4];

% 计算纵轴范围
y_min = min(means - std_devs);
y_max = max(means + std_devs);
y_range = max(y_max - y_min, 0.1); % 确保纵轴范围至少为0.1

% 绘制误差条形图
figure;
set(gcf, 'Color', 'white'); % 设置背景颜色为白色
bar(means);
hold on;
errorbar(1:numel(means), means, std_devs, '.', 'LineWidth', 1.5, 'Color', 'red');
xticklabels({'TOA(发射源2)', 'TOA(发射源3)', 'TDOA(发射源3, 发射源4)', ...
             'DFD(发射源1, 发射源2)', 'DFD(发射源3, 发射源4)', ...
             'AOA atan(alpha)', 'AOA atan(beta)', ...
             'RSSI(发射源1)', 'RSSI(发射源3)', 'RSSI(发射源4)'});
xtickangle(45);
ylabel('平均值');
title('平均值和标准差');
legend('平均值', '标准差', 'Location', 'northwest');

% 设置纵轴范围
ylim([y_min - 0.1 * y_range, y_max + 0.1 * y_range]);

hold off;

位置求解，图像代码

clc;
clear;

% 读取 Excel 文件
[data, ~, ~] = xlsread('飞行器接收情况1.xlsx');

% 计算数据采样时间间隔（假设是相等的）
T = 0.01; % 更改为新的时间间隔

% 飞行器坐标
x = data(:, 2);
y = data(:, 3);
z = data(:, 4);

% 发射源坐标
xs = data(:, 5);
ys = data(:, 6);
zs = data(:, 7);

% 接收时间（根据新的时间间隔计算）
receive_time = (0:numel(x)-1)' * T;

% 发射时间（假设与接收时间相同）
transmit_time = receive_time;

% 计算信号传输时间
transmission_time = receive_time - transmit_time;

% 假设有三个基站，每个基站位置已知
% 基站1坐标
x1 = 0; y1 = 0; z1 = 0;
% 基站2坐标
x2 = 100; y2 = 0; z2 = 0;
% 基站3坐标
x3 = 0; y3 = 100; z3 = 0;

% 计算飞行器到各个基站的距离
d1 = sqrt((x - x1).^2 + (y - y1).^2 + (z - z1).^2);
d2 = sqrt((x - x2).^2 + (y - y2).^2 + (z - z2).^2);
d3 = sqrt((x - x3).^2 + (y - y3).^2 + (z - z3).^2);

% 计算时间差
delta_t1 = d1 / 3e8;
delta_t2 = d2 / 3e8;
delta_t3 = d3 / 3e8;

% 三个基站的位置
base_stations = [x1, y1, z1; x2, y2, z2; x3, y3, z3];

% 计算位置估计值
position_estimate = trilat(base_stations, [delta_t1, delta_t2, delta_t3]);

% 提取x,y,z坐标估计值
x_estimate = position_estimate(1);
y_estimate = position_estimate(2);
z_estimate = position_estimate(3);

% 绘制飞行器位置
figure;
plot3(x, y, z, 'b', 'LineWidth', 1.5);
hold on;
plot3(xs, ys, zs, 'r--', 'LineWidth', 1.5); % 绘制发射源位置
scatter3(x_estimate, y_estimate, z_estimate, 100, 'filled', 'MarkerFaceColor', 'g'); % 绘制位置估计值
xlabel('X');
ylabel('Y');
zlabel('Z');
title('飞行器位置');
legend('飞行器', '发射源', '位置估计');
grid on;
hold off;
hold on;
scatter3(x_estimate, y_estimate, z_estimate, 100, 'filled', 'MarkerFaceColor', 'g'); % 绘制位置估计值
text(x_estimate, y_estimate, z_estimate, sprintf('(%d,%d,%d)', x_estimate, y_estimate, z_estimate), 'FontSize', 12, 'HorizontalAlignment', 'left');


% 打印0秒至10秒的位置估计值
fprintf('0秒至10秒的位置估计值为：\n');
disp([x_estimate, y_estimate, z_estimate]);

% 定义三边定位函数
function estimated_position = trilat(base_stations, delta_ts)
    n = size(base_stations, 1);
    A = zeros(n-1, 3);
    b = zeros(n-1, 1);
    for i = 1:(n-1)
        A(i, :) = 2 * (base_stations(i+1, :) - base_stations(1, :));
        b(i) = (norm(base_stations(i+1, :))^2 - norm(base_stations(1, :))^2) ...
                - (delta_ts(i+1)^2 - delta_ts(1)^2);
    end
    estimated_position = (A' * A) \ (A' * b);
end


问题3

clc
clear

% 读取Excel文件
filename = '飞行器接收情况1.xlsx';
[data, ~, ~] = xlsread(filename);

% 提取所需的列
TOA_source1 = data(:, 1);
TOA_source4 = data(:, 2);
TDOA_source1_source2 = data(:, 3);
TDOA_source3_source4 = data(:, 4);

% 初始化参数
theta = [0; 0; 0]; % 初始位置估计 (x, y, z)
learning_rate = 0.01; % 学习率
max_iterations = 1000; % 最大迭代次数
epsilon = 1e-6; % 停止条件：梯度范数小于 epsilon

% 信号传播速度
C = 3e8; % 光速

% 定义目标函数
J = @(theta) objectiveFunction(theta, TOA_source1, TOA_source4, TDOA_source1_source2, TDOA_source3_source4, C);

% 梯度下降算法
for iter = 1:max_iterations
    % 计算梯度
    gradient = computeGradient(J, theta);
    
    % 更新参数
    theta = theta - learning_rate * gradient;
    
    % 检查停止条件
    if norm(gradient) < epsilon
        disp('Converged.');
        break;
    end
end

% 输出结果
disp(['飞行器的位置估计: (x, y, z) = (' num2str(theta(1)) ', ' num2str(theta(2)) ', ' num2str(theta(3)) ')']);

% 定义目标函数
function cost = objectiveFunction(theta, TOA_source1, TOA_source4, TDOA_source1_source2, TDOA_source3_source4, C)
    x = theta(1);
    y = theta(2);
    z = theta(3);
    N = length(TOA_source1);
    
    cost = 0;
    for i = 1:N
        t1 = TOA_source1(i);
        t4 = TOA_source4(i);
        t1_sq = t1^2;
        t4_sq = t4^2;
        tdoa12 = TDOA_source1_source2(i);
        tdoa34 = TDOA_source3_source4(i);
        
        d1_sq = (x^2 + y^2 + z^2);
        d4_sq = ((x + 200)^2 + (y + 100)^2 + (z - 1050)^2);
        
        cost = cost + ((t1_sq - t4_sq) - (tdoa12^2 - tdoa34^2) - ((d1_sq - d4_sq) / C)^2);
    end
end

% 计算梯度
function gradient = computeGradient(J, theta)
    delta = 1e-6;
    gradient = zeros(size(theta));
    for i = 1:length(theta)
        delta_vec = zeros(size(theta));
        delta_vec(i) = delta;
        gradient(i) = (J(theta + delta_vec) - J(theta - delta_vec)) / (2 * delta);
    end
end

#飞行器的位置估计: (x, y, z) = (0.00010255, 5.1273e-05, -0.00053837)

问题三图像代码

clc
clear

theta = [0; 0; 0]; % 初始位置估计 (x, y, z)
% 初始位置
initial_position = theta; % 使用估计的位置作为初始位置

% 飞行器速度向量
% velocity = [0; 0; 8]; 
% velocity = [20; 0; 0]; 
velocity = [0; 0; 0]; 
% 信号传播速度
C = 3e8; % 光速

% 采样间隔时间
sampling_interval = 0.01; % 秒

% 计算时间步数
num_steps = 10 / sampling_interval; % 从0秒到10秒

% 初始化位置记录数组
positions = zeros(num_steps, 3); % 每行记录一个时间步长的位置信息

% 更新位置
current_position = initial_position;
for step = 1:num_steps
    % 记录当前位置
    positions(step, :) = current_position';
    
    % 更新位置（假设速度恒定）
    current_position = current_position + velocity * sampling_interval;
end

% 加入新的位置坐标
new_position = [20.0001, 5.0882e-05, 7.9995];
positions = [positions; new_position];

% 绘制飞行器轨迹
figure;
set(gcf, 'Color', 'white'); % 设置背景颜色为白色
plot3(positions(:, 1), positions(:, 2), positions(:, 3), 'b', 'LineWidth', 2);
hold on;
plot3(new_position(1), new_position(2), new_position(3), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
xlabel('X (meters)');
ylabel('Y (meters)');
zlabel('Z (meters)');
title('Navigation Trajectory of the Aircraft (0 to 10 seconds)');
grid on;
legend('Trajectory', 'New Position', 'Location', 'Best');