• API
• FAQ
• Tools
• Archive
SHARE
TWEET

Untitled

a guest Nov 17th, 2019 72 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. %****************MATLAB and its Applications in Engineering (MAE)*******
2. %**********************Helena Calatrava RodrÃ­guez***********************
3. %***************Problem set 3: MATLAB M-file Programming****************
4. %******************************STEP a) & b)*****************************
5.
6.
7.
8. %-------------debug flags
9. two_d = 0; %flag to study the two_d scenario
10. three_d = 1; %flag to study the three_d scenario
11. track = 0; %flag to study the drone movement
12. stepA = 0;
13. stepB = 1;
14.
15. %-------------init
16. close all;
17. N = 4;
18. delay = 0.1;
19.
20. %-------------In STEP B we introduce a gaussian noise component
21. deviation = 10; %10 meters deviation
22.
23.
24. %-----[1]--------MATRIX P: position of the N transmitters
25. %transmitters are located in a 5Km by 5Km area but between 2Km and 5Km in altitude
26. xTransmitter = rand(N,1)*5000;
27. yTransmitter = rand(N,1)*5000;
28. zTransmitter = rand(N,1)*[5000-2000]+2000;
29. %each column includes the (x, y, z) coordinates
30. P = [xTransmitter'; yTransmitter'; zTransmitter'];
31. %draw each transmitter position
32. for n=1:N
33.     row = P(:,n);
34.     if two_d
35.         plot(row(1), row(2), 'r*');
36.         hold on
37.     elseif three_d
38.         scatter3(row(1), row(2), row(3));
39.         hold on
40.     end
41. end
42.
43. %------[2]-------DRONE MOVEMENT
44. %the drone can't ever exit the volume (space of 1 km due to its trajectory)
45. %x and y go from 1 km to 4 km
46. %z goes from 0 to 2 km (no restriction because trajectory is horizontal)
47.
48. %x, y and z drone init positions
49. x = rand()*(4000-1000)+1000;
50. y = rand()*(4000-1000)+1000;
51. z = rand()*2000;
52. DronePos = [x, y, z];
53.
54. %angle random directions
55. angle1 = rand()*2*pi;
56. angle2 = angle1-pi/2;
57. angle3= angle1+pi;
58. angle4=angle2;
59.
60. %We will draw 10 cicles
61. % ___     ___     ___
62. %| 1 | 2 | 3 | 4 |...| 10|
63. %|   |___|   |___|   |___|
64. %total iterations = 21 (10 moves of 100 m + 11 moves of 1000 m)
65. iteTotal = 21;
66. distOneSec = 25; %25m/s*s = 25 meters
67. angleVect = [angle1 angle2 angle3 angle2];
68.
69. %Loops to compute the drone movement
70. for i = 1:iteTotal
71.     if mod(i,2) == 0
72.         coef = 100;
73.         ite = 4;
74.     else
75.         coef = 1000;
76.         ite = 40;
77.     end
78.     angle = angleVect(mod(i-1,4)+1);
79.     for j = 1:ite
80.         index = length(x);
81.         xCoord = distOneSec*cos(angle) + x(index);
82.         x = [x  xCoord];
83.         yCoord = distOneSec*sin(angle) + y(index);
84.         y = [y yCoord];
85.         if track
86.             plot(xCoord, yCoord, 'r*');
87.             hold on
88.         end
89.     end
90. end
91. if track
92.     axis square
93.     axis off
94. end
95.
96. %Matrix Drone: matrix with the Drone Positions
97. %Each row corresponds to the [x y z] coordinates in the moment t secs
98. Drone = [x' y' repmat(z, size(x,2), 1)];
99. Drone = Drone';
100.
101. %-----[3]--------Generation of Matrix D
102. %----------------Estimate the Drone movement
103. D = [];
104. drone_est = [];
105. T = size(Drone,2);
106. for t=1:T
107.     currentDistance = [];
108.     DroneCurrentPos = Drone(:,t);
109.
110.     if two_d
111.         plot(DroneCurrentPos(1), DroneCurrentPos(2), 'b*');
112.         hold on
113.     elseif three_d
114.         scatter3(DroneCurrentPos(1), DroneCurrentPos(2), DroneCurrentPos(3), 10, 2);
115.         hold on
116.     end
117.     pause(delay);
118.
119.     %We calculate the distance for every transmitter at every second
120.     for j = 1:N
121.         %use rand: Uniformly distributed pseudorandom numbers
122.
123.         if stepA
124.             noiseComponent = 0;
125.         elseif stepB
126.             noiseComponent = deviation*rand;
127.         end
128.         newDistance = norm(DroneCurrentPos-P(:,j)) + noiseComponent;
129.         currentDistance = [currentDistance newDistance];
130.     end
131.     D = [D; currentDistance];
132.
133.     n = 1:N;
134.     %anonymous function
135.     F = @(x,y,z) (x-P(1,n)).^2+(y-P(2,n)).^2+(z-P(3,n)).^2-D(t,n).^2;
136.     %we want only one variable to get rid of an error (double type error)
137.     F2 = @(u)F(u(1),u(2),u(3));
138.     %the first point to check will be [0 0 0]
139.     x0 = [0 0 0];
140.     if stepA
141.         %we use the fsolve function for stepA
142.         drone_est(:,t) = round(fsolve(F2 ,x0))';
143.     elseif stepB
144.         %we use the lsqnonlin function for stepB
145.         drone_est(:,t) = round(lsqnonlin(F2 ,x0))';
146.     end
147.     estim = drone_est(:,t);
148.     if two_d
149.         plot(estim(1), estim(2), 'm*');
150.         hold on
151.     elseif three_d
152.         scatter3(estim(1), estim(2), estim(3), 10, 1);
153.         hold on
154.     end
155.     pause(delay);
156. end
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.

Top