why my code is running so slowly in the while loop
5 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
Hi, im new at Matlab and im trying to implement a guidance for Uav but my while loop running so slowly.
Any suggestions for fix this trouble ?
clc;
clear all;
close all;
r = 1250;
O = [2500 2500]
Px = 2500; Py = 1000;
P = [ Px Py ];
Va = 100;
To = 4;
q22 = 0.1
psi = -0.4;
delta = 10;
K =0.5;
Rmin = 75
K2 = 35;
B = 0 : 0.001 : 2*pi;
c = (r* (cos(B)))+O(1);
d = (r* (sin(B)))+O(2);
N = (r * (cos(B)))+O(1);
M = (r * (sin(B)))+O(2);
% Wa = [0 300]; %
% Wb = [0 300]; %
Vx = Va * cos( psi );
Vy = Va * sin( psi );
thetaU = atan2(( Px - O(2)),( Py - O(1)));
d = abs(sqrt (( O(1) - Py )^2 + ( O(2) - Px )^2) - r)
xt = ((r * (cos ( thetaU+delta ))) + O(1));
yt = ((r * (sin ( thetaU+delta ))) + O(2));
Vd = Va*sin(psi-thetaU);
q11 = sqrt(abs(To / (To-d)));
psiD = atan2(( yt -Py ),( xt - Px ));
u =- ((d*q11) + sqrt(2*q11+q22^2)*Vd);
while d > 0
t = 0.05;
d = abs(sqrt (( O(1) - Py )^2 + ( O(2) - Px )^2) - r)
thetaU = atan2(( Px - O(2)),( Py - O(1)));
xt = ((r * (cos ( thetaU+delta ))) + O(1));
yt = ((r * (sin ( thetaU+delta ))) + O(2));
Vd = Va*sin(psi-thetaU);
q11 = sqrt(abs(To / (To-d)));
psiD = atan2(( yt -Py ),( xt - Px ));
u =- ((d*q11) + sqrt(2*q11+q22^2)*Vd);
psi = psiD ;
if(abs(u) > (Va^2)/Rmin)
if (u > 0)
u = (Va^2)/Rmin;
else
u = -(Va^2)/Rmin;
end
end
Vy = Va * sin ( psi ) + u * t;
Vx = Va * cos(psi) + u * t;
Px = Px + Vx *t;
Py = Py + Vy *t;
t = t + 0.1;
hold on;
plot (Px ,Py ,"r--.",O(1) , O(2) ,'b-o');
plot (c,d,'r')
plot(N,M,'k')
title("2D Carrot Chase Trajectory")
xlabel("Position");
ylabel("Position"),
end
0 Kommentare
Antworten (1)
VBBV
am 20 Aug. 2022
clc;
clear all;
close all;
r = 1250;
O = [2500 2500]
Px = 2500; Py = 1000;
P = [ Px Py ];
Va = 100;
To = 4;
q22 = 0.1
psi = -0.4;
delta = 10;
K =0.5;
Rmin = 75
K2 = 35;
B = 0 : 0.001 : 2*pi;
c = (r* (cos(B)))+O(1);
d = (r* (sin(B)))+O(2);
N = (r * (cos(B)))+O(1);
M = (r * (sin(B)))+O(2);
% Wa = [0 300]; %
% Wb = [0 300]; %
Vx = Va * cos( psi );
Vy = Va * sin( psi );
thetaU = atan2(( Px - O(2)),( Py - O(1)));
d = abs(sqrt (( O(1) - Py )^2 + ( O(2) - Px )^2) - r)
xt = ((r * (cos ( thetaU+delta ))) + O(1));
yt = ((r * (sin ( thetaU+delta ))) + O(2));
Vd = Va*sin(psi-thetaU);
q11 = sqrt(abs(To / (To-d)));
psiD = atan2(( yt -Py ),( xt - Px ));
u =- ((d*q11) + sqrt(2*q11+q22^2)*Vd);
i = 1
t = 0.05;
while d > 0
thetaU = atan2(( Px - O(2)),( Py - O(1)));
xt = ((r * (cos ( thetaU+delta ))) + O(1));
yt = ((r * (sin ( thetaU+delta ))) + O(2));
Vd = Va*sin(psi-thetaU);
q11 = sqrt(abs(To / (To-d)));
psiD = atan2(( yt -Py ),( xt - Px ));
u =- ((d*q11) + sqrt(2*q11+q22^2)*Vd);
psi = psiD ;
if(abs(u) > (Va^2)/Rmin)
if (u > 0)
u = (Va^2)/Rmin;
else
u = -(Va^2)/Rmin;
end
end
Vy = Va * sin ( psi ) + u * t;
Vx = Va * cos(psi) + u * t;
Pxx(i) = Px + Vx *t;
Pyy(i) = Py + Vy *t;
t = t + 0.1;
d = d-1; % use one counter to check the while conditon
i = i+1;
end
plot (Pxx ,Pyy ,"r--.",O(1) , O(2) ,'b-o');
% subplot(211)
% plot (c,d,'or')
% subplot(212)
% plot(N,M,'+k')
title("2D Carrot Chase Trajectory")
xlabel("Position");
ylabel("Position"),
6 Kommentare
VBBV
am 23 Aug. 2022
This is done to save computed values in a new array during iteration in while loop.
clc;
clear all;
close all;
r = 1250;
O = [2500 2500]
Px = 2500; Py = 1000;
P = [ Px Py ];
Va = 200;
alpha = 1;
k = 1;
Rmin = 75;
psi = -0.4;
delta = 0.1;
Vx = Va * cos( psi );
Vy = Va * sin( psi );
K =0.5;
B = 0 : 0.001 : 2*pi;%plot of circle
c = (r* (cos(B)))+O(1);
d = (r* (sin(B)))+O(2);
N = (r * (cos(B)))+O(1);
M = (r * (sin(B)))+O(2);
d = abs(sqrt (( O(2) - Py )^2 + ( O(1) - Px )^2));
theta = atan2(Py-O(2),Px-O(1));
xt = ((r * (cos ( theta+delta ))) + O(1));
yt = ((r * (sin ( theta+delta ))) + O(2));
psiD = atan2(( yt -Py ),( xt - Px ));
if d > 2*r
psid = theta-pi+asin(r/d)
psic = psid+ (Va*sin(psi-theta) /(alpha*d))
else
psid = (theta-(pi/2) - (pi/3)*(((d-r)/r)^k) );
psic = psid-(Va*sin(psi-theta)/(alpha*d))- (k*Va*pi*(d^(k-1))) / (3*(r^k)*alpha);
end
u = K*(psic-psi)*Va;
i = 1;
t = 0.05;
d = abs(sqrt (( O(2) - Py )^2 + ( O(1) - Px )^2) - r);
while abs (d) > 0
theta = atan2(Py-O(2),Px-O(1))
xt = ((r * (cos ( theta+delta ))) + O(1));
yt = ((r * (sin ( theta+delta ))) + O(2));
psiD = atan2(( yt -Py ),( xt - Px ));
if d > 2*r
psid = theta-pi+asin(r/d)
psic = psid+ (Va*sin(psi-theta) /(alpha*d))
else
psid = (theta-(pi/2) - (pi/3)*(((d-r)/r)^k) );
psic = psid-(Va*sin(psi-theta)/(alpha*d))- (k*Va*pi*(d^(k-1))) / (3*(r^k)*alpha);
end
psi = psiD;
if(abs(u) > (Va^2)/Rmin)
if (u > 0)
u = (Va^2)/Rmin;
else
u = -(Va^2)/Rmin;
end
end
Vy = Va * sin ( psi ) + u * t;
Vx = Va * cos(psi) + u * t;
Px = Px + Vx *t; %
Py = Py + Vy *t;
t = t + 0.01;
d = d-1;
i = i+1;
hold on;
plot (Px ,Py ,"ro",O(1) , O(2) ,'b-o');
title("2D Carrot Chase Trajectory")
xlabel("Position");
ylabel("Position"),
end
Siehe auch
Kategorien
Mehr zu Array Geometries and Analysis finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!