实验三 移动自组网络的时间校准与三维空间定位程序设计

1、掌握移动自组网络局部定位；

2、掌握移动自组网络全局定位

3、掌握对于空间分布的N个节点，且它们存在时间同步误差。现考虑从它们的相互测距数据中求得各节点的空间位置坐标。

4、锻炼编写代码的能力。

创建一个命名为sy3的主函数 1、清除命令窗口(clc)和从工作空间(清除所有)清除所有变量 将默认的图形背景色设置为白色，后续命令生成的所有图形都将具有白色背景。 图1 设置生成默认图形背景 2、设置用于定位的无线网络中的节点 定义光速(c)。它以矩阵(pos)的形式指定了三维空间中节点的位置，其中,每一行代表单个节点在x、y和z坐标中的位置。 该通过提取pos矩阵的行数来计算节点数(N)。代码为每个节点生成一个时间同步错误(延迟)向量，作为从平均值为零和标准差为1纳秒的正态分布中抽取的随机值。 参考节点的时间延迟设置为零，因为它是定位的锚节点。 图2 设置节点 3、计算无线网络真邻接矩阵(R) 初始化一个n × n的零矩阵。然后循环遍历所有对节点(ii和jj)，并使用函数计算它们在3D空间中位置之间的欧几里得距离。计算出的距离被分配给矩阵R中相应的条目。得到的矩阵R包含无线网络中节点之间的真实成对距离。 图3 计算真邻接矩阵(R) 4、计算测量的无线网络邻接矩阵(Rm) 代码初始化一个n × n的零矩阵。循环遍历所有对节点(ii和jj)，并计算它们位置之间的测量距离，作为它们的真实距离(存储在矩阵R中)和节点之间的时间同步误差之差的总和，乘以光速(c)。 这种计时误差之差表示由节点中的异步时钟引起的飞行时间测量误差。 计算出的距离被分配给矩阵Rm中相应的项。 结果矩阵Rm包含无线网络中节点之间的成对测量距离，其中包括时钟同步问题引入的误差。 图4 计算测量的邻接矩阵 5、求解每个节点与参考节点(第一个节点)之间的时间同步误差(delay1) 循环遍历所有节点，并计算每个节点到参考节点的测量距离(存储在Rm中)与参考节点到该节点的测量距离之间的时间差。这个时间差除以光速的两倍，就得到了节点之间的实际时间差。 结果向量delay1包含相对于参考节点的估计时间同步错误。 图5 求解时间同步误差 6、从测量的邻接矩阵(Rm)中去除时间同步误差，以解得真正的邻接矩阵(Rm1) 初始化一个n × n的零矩阵。然后循环遍历所有对节点(ii和jj)，并从Rm矩阵的相应条目中减去节点(存储在delay1向量中)与光速(c)之间的时间同步误差差。 该操作通过消除时钟异步带来的错误，恢复节点之间的真实距离。计算出的距离被分配给矩阵Rm1中相应的条目。 得到的矩阵Rm1包含无线网络中节点间的调整后的两两距离，且不存在时间同步误差。 图6 去除误差解得真正的邻接矩阵 7、根据网络中节点的成对距离求解节点的空间位置。 设置了一个n × 3矩阵(pos1)来存储节点的估计3D位置。 代码首先将参考节点(第一个节点)的位置初始化为原点([0,0,0])。然后根据距离和预定义的x轴方向将第二个节点的位置设置为(Rm(1,2)， 0,0)。第三和第四个节点的位置使用基于几何约束的类似方法确定。 对于其余节点，代码建立一个系数矩阵a，由参考节点和三个锚节点(2、3、4)的位置构造;然后，通过求解线性系统Ax=b，使用矩阵反演来确定第i个节点的位置，其中b的值是根据第i个节点与四个锚节点之间的测量距离计算的。 最终矩阵pos1包含基于飞行时间测量和锚节点已知位置的无线网络中所有节点的估计3D位置。 图7 求解节点的空间位置 8、计算估计节点位置(pos1)和节点子集(Y)的已知(或真值)位置之间的线性变换矩阵w。 代码选择与已知节点位置对应的索引(点)列表。矩阵X的每一行存储一个节点的估计位置以及一个常数项(即，一个总是等于1的额外维度)，这使得变换矩阵w中的平移操作成为可能。代码应用矩阵X的伪逆并将其乘以Y来计算w的最小均方误差解。 得到的变换矩阵w表示一个3 × 4矩阵，包括旋转和缩放分量以及平移向量。它有效地将三维位置从它们在pos1矩阵中的估计位置映射到它们在Y矩阵中的已知地面真值坐标。 最后，代码将计算的变换矩阵w应用于所有估计的节点位置pos1(包括那些在计算w时未使用的位置)，并将其与一列1连接起来，在使用真值位置进行全局优化后，生成每个节点的最终本地化位置pos_LS。 图8 计算线性变换矩阵w 9、实现的可视化的无线网络定位算法的结果代码 第一个figure()命令显示全局坐标系(pos矩阵)中所有节点真实位置的3D散点图 第二个figure()命令在局部坐标(pos1矩阵)中显示估计的节点间成对距离的3D散点图。 第三个图结合了这两个图，并使用地面真值位置(pos_LS)对线性变换后的位置进行了比较。 代码首先将所有节点真实位置的3D散点图(pos矩阵)用蓝色标记为“*”。然后用橙色的“o”标记覆盖转换后的节点位置(pos_LS)的3D散点图。 最后，代码添加了一个包含两个项目的图例:一个用于真实位置(pos)，另一个用于通过最小二乘过程(pos_LS)定位的位置。结果是可视化，提供了定位算法的性能和准确性的定性评估。 图9 绘图结果代码 实验结果图 图1 节点真实空间位置（全局坐标系） 图2 节点相对空间位置（局部坐标系） 图3 节点空间位置（全局坐标系）

代码

clc
clear all;
set(0,'defaultfigurecolor','w')
%% 节点设置
c = 3e8; %波速
% 节点位置
pos=[  -1.0000         0    5.0073
        -0.4600    0.7200    4.9792
        -0.4600   -0.7200    5.0010
        0.3900    1.0000    4.9631
        0.3900   -1.0000    4.9634
        1.0000    0.4800    4.9676
        1.0000   -0.4800    5.0373
        -0.0200         0    5.0392
        1.1000    1.4000    5.0175
        1.1000   -1.4000    5.0138 ];
N = size(pos,1);%节点数量
% 时间同步误差
delay = ones(N,1) .* normrnd( 0 , 1e-9 , N , 1 );%时间误差（每行一个节点）
delay(1)=0; %第一个节点为参考节点，时间误差为0

%% 真实临接矩阵
R = zeros(N,N);
for ii = 1 : N
    for jj = 1 : N
        R(ii,jj) = norm( pos(ii,:) - pos(jj,:) );%ii节点与jj节点的距离
    end
end
%% 测量临接矩阵（真实距离+时间同步造成的测距误差）
Rm = zeros(N,N);
for ii = 1 : N
    for jj = 1 : N
        Rm(ii,jj) = R(ii,jj) + ( delay(ii)-delay(jj) ) * c; % 测量距离 = 真实距离+时间同步造成的测距误差
    end
end
%% 求解时间误差
delay1 = zeros(N,1);%初始化
for ii = 1 : N
    delay1(ii,1) = (Rm(ii,1) - Rm(1,ii) ) / (2*c) ; %理由和第1个参考节点的距离，求出各节点对于第1个节点的时间差
end
%% 去除测量临接矩阵的时间误差
Rm1 = zeros(N,N);
for ii = 1 : N
    for jj = 1 : N
        Rm1(ii,jj) = Rm(ii,jj) - ( delay1(ii)-delay1(jj) ) * c; %去除时间同步误差，恢复成真实测距
    end
end
%% 求解空间定位
%初始化
pos1 = zeros(size(pos)); 

%第1个点设为原点
x1=0;
y1=0;
z1=0;
pos1(1,:)=[x1,y1,z1];

%第2个点设为x轴上点
x2 = Rm1(1,2);
y2 =0;
z2 =0;
pos1(2,:) = [x2,y2,z2];%选取1-2节点连线作为x轴

%第3个点设为xoy平面内点
x3 = ( Rm1(1,3)^2 - Rm1(2,3)^2 + x2^2 ) / ( 2 * x2 );
y3 = sqrt( Rm1(1,3)^2 - x3^2 ); %取根号取正值，即设定y方向
z3 = 0; 
pos1(3,:) = [ x3,y3,z3 ];

%第4个点由1~3个点确定位置
x4 = ( Rm1(1,4)^2 - Rm1(2,4)^2 + x2^2 ) / (2 * x2);
y4 = ( ( Rm1(1,4)^2- x4^2 )-( Rm1(3,4)^2 - (x4-x3)^2 ) + y3^2 ) / ( 2 * y3 );
z4 = sqrt( Rm1(1,4)^2 - x4^2 - y4^2 ); %取根号取正值，即设定z方向
pos1(4,:) = [ x4,y4,z4 ];


% 其他点都由 1~4 节点空间定位

% 系数矩阵 A
A = [ 2*(x1-x2) 2*(y1-y2) 2*(z1-z2)
      2*(x1-x3) 2*(y1-y3) 2*(z1-z3)
      2*(x1-x4) 2*(y1-y4) 2*(z1-z4) ];
% 求解其他节点的定位  
for ii = 5 : N
    %值域向量
    b1 = Rm1(2,ii)^2 - Rm1(1,ii)^2 - x2^2 + x1^2 - y2^2 + y1^2 - z2^2 + z1^2;
    b2 = Rm1(3,ii)^2 - Rm1(1,ii)^2 - x3^2 + x1^2 - y3^2 + y1^2 - z3^2 + z1^2;
    b3 = Rm1(4,ii)^2 - Rm1(1,ii)^2 - x4^2 + x1^2 - y4^2 + y1^2 - z4^2 + z1^2;
    b = [b1;b2;b3];
    % 求解第ii个节点的位置
    pos1(ii,:) = inv(A)*b;
end

%% 最小二乘求解坐标变换矩阵
point = [1 4 6 7]; %已知位置的点索引（至少4点）
Y = pos(point,:);
X = [ pos1(point,:), ones(length(point),1) ];
w = pinv(X)*Y; %最小二乘求解坐标变换矩阵
pos_LS = [pos1,ones(size(pos1,1),1)] * w;
%% 绘图
figure,plot3(pos(:,1),pos(:,2),pos(:,3),'o'),grid on,title('节点真实空间位置（全局坐标系）')
figure,plot3(pos1(:,1),pos1(:,2),pos1(:,3),'o'),grid on,title('节点相对空间位置（局部坐标系）')
figure,
plot3(pos(:,1),pos(:,2),pos(:,3),'*','markersize',10),hold on
plot3(pos_LS(:,1),pos_LS(:,2),pos_LS(:,3),'o','markersize',10),
legend('真实空间位置','LS解算的空间位置')
grid on,title('节点空间位置（全局坐标系）')