why my code is running so slowly in the while loop
5 views (last 30 days)
Show older comments
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 Comments
Answers (1)
VBBV
on 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 Comments
VBBV
on 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
See Also
Categories
Find more on Array Geometries and Analysis in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!