-
Notifications
You must be signed in to change notification settings - Fork 0
/
CT_Simulation_R3.m
90 lines (80 loc) · 3.35 KB
/
CT_Simulation_R3.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
%Shane Rozen-Levy
%EE105 Final Project
%CT Simulation in R3
%
%This project involved recreating much of the work done in our professor's
%paper on a distributed algorithm for sensor localization (U. A. Khan, S.
%Kar, and J. M. F. Moura, “Linear theory for self-localization: Convexity,
%barycentric coordinates, and Cayley-Menger determinants,” IEEE Access,
%vol. 3, pp. 1326–1339, Aug. 2015).
%The first part of the project was a report where we went through most of
%the math to veryify the method. The second part of the project was 4
%different MATLAB simulations. The first simulation was an implimentation
%of the DT algorithm in R2. The second simulation was an implimentation of
%the DT algorithm in R3. The third simulation was an implimentation of the
%CT algorithm in R2. The fourth and final simulation was an implimentation
%of the CT algorithm in R3.
%
%The algorithm works by having a small number of points that know where
%they are and a large number of points that do not know where they are. The
%points can talk to other points up to a certain distance away and find
%their distance. Each point uses the distance between each neighbor and
%assumes that the neighbors know where they are and updates its posistion.
%The algorithm iterates this process and given a certain criteria on the
%points, the algorithm converges to the true location of the points.
%
%This is the code for the CT simulation in R2
%parameters for the simulaton
m=3;
numNodes=10;
r=18;
kappa=[0 0 0; 10 0 0; 5 10 0; 15/3 10/3 10];
%generate the points and A and B
[p,A,B]=generatePoints(kappa,m,numNodes,r);
%check the spectral radius of A and ensure it is less than 1 (if it is not
%the system will not converge
max(abs(eig(A)))
%create the time vector
t=0:0.1:20;
%use the CT state space function to simulate the response of the CT system
sys=ss(A-eye(numNodes),B,eye(numNodes),[]);
%initial conditions
x=abs(10*rand(numNodes, 1));
y=abs(10*rand(numNodes, 1));
z=abs(10*rand(numNodes, 1));
%use lsim to simulte the system response
%repmat repeats the anchors for the step response
x=lsim(sys,repmat(kappa(:,1),1,length(t)),t,x);
y=lsim(sys,repmat(kappa(:,2),1,length(t)),t,y);
z=lsim(sys,repmat(kappa(:,3),1,length(t)),t,z);
x=x';
y=y';
z=z';
figure(40); clf
%plot the anchors
plot3([kappa(:,1);kappa(1,1)],[kappa(:,2);kappa(1,2)],[kappa(:,3);kappa(1,3)],'^','Color','g','Linewidth',2)
hold on
%plot the true location of the nodes
plot3(p(:,1),p(:,2),p(:,3),'o','Color','b','Linewidth',3)
for i=1:numNodes
plot3(x(i,:),y(i,:),z(i,:),'--x','Color','b')
end
title(['Location of Points Over Time r=',num2str(r),' numNodes=',num2str(numNodes),' m=',num2str(m)])
legend('Anchors','True Location','Estimate Location')
rmsError=zeros(1,length(t));
largestError=zeros(1,length(t));
%calculate the rms of the error and the largest error between the
%estimated error and the actual error.
for i=1:length(t)
error=((p(:,1)-x(:,i)).^2+(p(:,2)-y(:,i)).^2+(p(:,3)-z(:,i)).^2).^(1/2);
rmsError(i)=rms(error);
largestError(i)=max(abs(error));
end
figure (41); clf%plot the errors on the same graph
plot(t,rmsError,'linewidth',2)
hold on
plot(t,largestError,'linewidth',2)
title(['Error Over Time r=',num2str(r),' numNodes=',num2str(numNodes),' m=',num2str(m)])
legend('RMS Error','Largest Error')
ylabel('Error')
xlabel('Time')