问题描述
- matlab实现一个质点随机运动无果
-
我是做定位算法的，但是在展示上需要仿真一个质点在区域内运动，然后我对其进行定位。我的思路是先随机产生一个点，然后由这个点的坐标加一个随机产生的向量，得到下一个点的位置。在此同时用我的算法对该点进行定位。但是实现不出来，下面是我的代码，急求大神帮忙修改，我17号就要答辩了。谢谢。！！
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~myshow~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~节点自定位~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
% BorderLength-----正方形区域的边长，单位：m
% NodeAmount-------网络节点的个数
% BeaconAmount---信标节点数
% Sxy--------------用于存储节点的序号，横坐标，纵坐标的矩阵
%Beacon----------信标节点坐标矩阵;BeaconAmount*BeaconAmount
%UN-------------未知节点坐标矩阵;2*UNAmount
% Distance------未知节点到信标节点距离矩阵;2*BeaconAmount
%h---------------节点间初始跳数矩阵
%X---------------节点估计坐标初始矩阵,X=[x,y]'
% R------------------节点的通信距离，一般为10-100
BorderLength=100;
NodeAmount=100;
BeaconAmount=20;%信标节点数量
UNAmount=NodeAmount-BeaconAmount;%未知节点数量
R=50;%通信半径
MinNode=5;%计算平均每跳距离的时候删除的节点
n=3%最后计算的时候删除的节点
Sxy=initialxy(BorderLength,NodeAmount)%产生随机分布的节点
Beacon=[Sxy(2,1:BeaconAmount);Sxy(3,1:BeaconAmount)];%随机分布的节点中，信标节点的坐标
UN=[Sxy(2,(BeaconAmount+1):NodeAmount);Sxy(3,(BeaconAmount+1):NodeAmount)];%假定的未知节点的坐标
h=initialhop(Sxy,NodeAmount,R)%初始化任意两个节点之间的跳数
Dall=initialDall(NodeAmount,Sxy)%在假定已知任意节点坐标的基础上计算出每个点之间的距离。
h=caculatehop(NodeAmount,h)%计算出每个节点之间的最小跳数%估计出了位置节点i的横坐标和纵坐标分别为Y（1，i）和Y（2，i）
%通过产生一些点，对该点的运动进行定位，并且把定位的误差率进行比较。%
%具体过程：1.先随机产生一个点a；
%2.随机产生一个向量，与时间有关系，起点a，指向终点b。
%3.重复2.
%4.设定一个限制条件，使得物体走出范围以后，对其不进行定位。
%~~~~~~~~~~~~~~~~~~~~~~~~~~首先产生一个随机的点，这个点应该在区域内部~~~~~~~~~~~~~~~~~~~~%
%ttime表示一个质点一共经历了几段位移
%a表示运动的点被检测的时候的位置
a=zeros(2,ttime)
a(1,0)=rand(1)*BorderLength;%先产生一个随机的点x坐标
a(2,0)=rand(1)*BorderLength;%先产生一个随机的点y坐标
%产生多个向量模拟质点做一个个步骤的运动，即用多个直线运动来模拟质点的曲线运动
Line=zeros(1,ttime)
%产生ttime个向量
for i=1:ttime
Line(1,i)=rand(1);
Line(2,i)=rand(1);
end
%t函数和定位的物体运动速度有关系
for i=1:ttime
a(1,i)=a(1,(i-1))+Line(1,i)*t;
a(2,i)=a(2,(i-1))+Line(2,i)*t;
end
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~第一步，首先对目标自节点自己进行定位，得出未知节点的坐标~~~~~~~~~~~~~~~~~~~~%
Distance=dvhopdeveloped(BeaconAmount,h,Dall,NodeAmount,UNAmount,MinNode)%计算未知节点和锚节点之间的距离
Y=caculatedvhopdevelop(h,BeaconAmount,NodeAmount,Distance,Dall,Beacon,n)%估算未知节点的坐标
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~第二步，把原来定位出来的结果（已知节点和未知节点的数据载入）~~~~~~~~~~~~~~~~%
%对Z初始化以后，给Z赋值，为每个节点的坐标
Z=zeros(NodeAmount);
for i=1:BeaconAmount
Z(1,i)=Sxy(2,i)
Z(2,i)=Sxy(3,i)
end
%信标节点赋值
for i=(BeaconAmount+1):NodeAmount
Z(1,i)=Y(1,i)
Z(2,i)=Y(2,i)
end
%未知节点赋值
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~第三步，进行基于误差自定位下的物体定位~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
X=centr(BorderLength,BeaconAmount,UNAmount,R,Sxy,Beacon,UN);
for i=1:ttime
error(1,i)=sqr((X(1,i)-a(1,i))^2+(X(2,i)-a(2,i))^2);
end
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~第四步，画出质点实际运动的过程和根据定位出来的运动过程~~~~~~~~~~~~~~~~~~~~~~%Node=[Z(1,1:NodeAmount),Z(2,1:NodeAmount)]%节点坐标
Target=(X(1,1:NodeAmount),X(2,1:NodeAmount))
plot(Z(1,1:ttime),Z(w,1:ttime),'r*',X(1,1:ttime),X(2,1:ttime,'k.')
xlim([0,(NodeAmount+ttime)]);
ylim([0,(NodeAmount+ttime)]);
title('* 红色实际路线 . 黑色估计路线')