Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- %****************MATLAB and its Applications in Engineering (MAE)*******
- %**********************Helena Calatrava Rodríguez***********************
- %***************Problem set 3: MATLAB M-file Programming****************
- %******************************STEP a) & b)*****************************
- %-------------debug flags
- two_d = 0; %flag to study the two_d scenario
- three_d = 1; %flag to study the three_d scenario
- track = 0; %flag to study the drone movement
- stepA = 0;
- stepB = 1;
- %-------------init
- close all;
- N = 4;
- delay = 0.1;
- %-------------In STEP B we introduce a gaussian noise component
- deviation = 10; %10 meters deviation
- %-----[1]--------MATRIX P: position of the N transmitters
- %transmitters are located in a 5Km by 5Km area but between 2Km and 5Km in altitude
- xTransmitter = rand(N,1)*5000;
- yTransmitter = rand(N,1)*5000;
- zTransmitter = rand(N,1)*[5000-2000]+2000;
- %each column includes the (x, y, z) coordinates
- P = [xTransmitter'; yTransmitter'; zTransmitter'];
- %draw each transmitter position
- for n=1:N
- row = P(:,n);
- if two_d
- plot(row(1), row(2), 'r*');
- hold on
- elseif three_d
- scatter3(row(1), row(2), row(3));
- hold on
- end
- end
- %------[2]-------DRONE MOVEMENT
- %the drone can't ever exit the volume (space of 1 km due to its trajectory)
- %x and y go from 1 km to 4 km
- %z goes from 0 to 2 km (no restriction because trajectory is horizontal)
- %x, y and z drone init positions
- x = rand()*(4000-1000)+1000;
- y = rand()*(4000-1000)+1000;
- z = rand()*2000;
- DronePos = [x, y, z];
- %angle random directions
- angle1 = rand()*2*pi;
- angle2 = angle1-pi/2;
- angle3= angle1+pi;
- angle4=angle2;
- %We will draw 10 cicles
- % ___ ___ ___
- %| 1 | 2 | 3 | 4 |...| 10|
- %| |___| |___| |___|
- %total iterations = 21 (10 moves of 100 m + 11 moves of 1000 m)
- iteTotal = 21;
- distOneSec = 25; %25m/s*s = 25 meters
- angleVect = [angle1 angle2 angle3 angle2];
- %Loops to compute the drone movement
- for i = 1:iteTotal
- if mod(i,2) == 0
- coef = 100;
- ite = 4;
- else
- coef = 1000;
- ite = 40;
- end
- angle = angleVect(mod(i-1,4)+1);
- for j = 1:ite
- index = length(x);
- xCoord = distOneSec*cos(angle) + x(index);
- x = [x xCoord];
- yCoord = distOneSec*sin(angle) + y(index);
- y = [y yCoord];
- if track
- plot(xCoord, yCoord, 'r*');
- hold on
- end
- end
- end
- if track
- axis square
- axis off
- end
- %Matrix Drone: matrix with the Drone Positions
- %Each row corresponds to the [x y z] coordinates in the moment t secs
- Drone = [x' y' repmat(z, size(x,2), 1)];
- Drone = Drone';
- %-----[3]--------Generation of Matrix D
- %----------------Estimate the Drone movement
- D = [];
- drone_est = [];
- T = size(Drone,2);
- for t=1:T
- currentDistance = [];
- DroneCurrentPos = Drone(:,t);
- if two_d
- plot(DroneCurrentPos(1), DroneCurrentPos(2), 'b*');
- hold on
- elseif three_d
- scatter3(DroneCurrentPos(1), DroneCurrentPos(2), DroneCurrentPos(3), 10, 2);
- hold on
- end
- pause(delay);
- %We calculate the distance for every transmitter at every second
- for j = 1:N
- %use rand: Uniformly distributed pseudorandom numbers
- if stepA
- noiseComponent = 0;
- elseif stepB
- noiseComponent = deviation*rand;
- end
- newDistance = norm(DroneCurrentPos-P(:,j)) + noiseComponent;
- currentDistance = [currentDistance newDistance];
- end
- D = [D; currentDistance];
- n = 1:N;
- %anonymous function
- F = @(x,y,z) (x-P(1,n)).^2+(y-P(2,n)).^2+(z-P(3,n)).^2-D(t,n).^2;
- %we want only one variable to get rid of an error (double type error)
- F2 = @(u)F(u(1),u(2),u(3));
- %the first point to check will be [0 0 0]
- x0 = [0 0 0];
- if stepA
- %we use the fsolve function for stepA
- drone_est(:,t) = round(fsolve(F2 ,x0))';
- elseif stepB
- %we use the lsqnonlin function for stepB
- drone_est(:,t) = round(lsqnonlin(F2 ,x0))';
- end
- estim = drone_est(:,t);
- if two_d
- plot(estim(1), estim(2), 'm*');
- hold on
- elseif three_d
- scatter3(estim(1), estim(2), estim(3), 10, 1);
- hold on
- end
- pause(delay);
- end
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement