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. OK, I Understand
 
Top