[\n ENTER SAMPLE SIZE:28428
\n ENTER TOLERANCE LIMIT:100
Undefined variable: xgetfile]1
So basically when I run this program that I have, downloaded from the internet, it always tells me that the variable xgetfile is undefined rather than prompting me to select a file with the data in it. The full code for said program is pasted down below. My question is how to remedy this issue and be able to input my data. The line that says xgetfile is pretty near the top so you don't have to do too much reading to get to it.
n=0;
tol_lim=0;
// ENTERING SAMPLE SIZE
while n<=5 | n==[] , n=input("\n ENTER SAMPLE SIZE:");
if (n<=5) printf("\n\n SAMPLE SIZE SHOULD BE GREATER THAN 5\n\n");end
if n==[] printf("\n\n SAMPLE SIZE CANNOT BE LEFT BLANK\n\n");end
end
//ENTERING TOLERANCE LIMIT
while tol_lim <= 0 | tol_lim==[], tol_lim=input("\n ENTER TOLERANCE LIMIT:");
if (tol_lim<=0) printf("\n TOLERANCE LIMIT SHOULD BE GREATER THAN 0\n\n");end
if tol_lim==[] printf("\n TOLERANCE LIMIT CANNOT BE LEFT BLANK\n\n");end
end
//INITIALIZING VARIABLES
F = zeros(n,3);
Y = zeros(n,1);
OY = zeros(n,1);
EY = zeros(n,1);
DOY = zeros(n,1);
Estimated_Y = zeros(n,1);
d = zeros(3,1);
THETA = zeros(3,1);
GN1=0;GN2=0;GN3=0;
sig=0;y=0;
sigma_hat_square=0; y_bar=0; dff=0; R_square=0;
U_t_hat=0; U_t_hat_square=0; U_t_minusone_hat=0; dd=0; DW=0;Covariance_Matrix=zeros(3,3);
f_obs=0; l_obs=0; r=0; D1=0; D2=0;
S1=0;S2=0;S3=0;D1=0;D2=0;r=0;sum_Y=0;Y_bar=0;Y_square=0;D_den=0;D=0;
AY=zeros(n,1);
OBS=zeros(n,1);
EST=zeros(n,1);
g=[];gh=[];
EXISTING_DATA='';
//CHOOSING INPUT EXCEL DATA FILE
gh=xgetfile();
while gh==[], gh=xgetfile('*.*',title='CHOOSE A FILE NAME');
if g==[] printf("FILE NAME CANNOT BE LEFT BLANK");end
end
Sheets=readxls(gh);
EXISTING_DATA=Sheets(1);
typeof(EXISTING_DATA);
printf("\n\n");
//DISPLAYING EXISTING DATA FROM EXCEL FILE
EXISTING_DATA
for i=2:(n+1),DOY(i-1,1)=EXISTING_DATA(i,2);end
while f_obs<=0 | f_obs==[] , f_obs=input("\n ENTER FIRST OBSERVATION NO:");
//if (f_obs<=0) printf("\n\n IT SHOULD BE GREATER THAN 0\n\n");end
//if f_obs==[] printf("\n\n IT CANNOT BE LEFT BLANK\n\n");end
end
while l_obs<f_obs | l_obs==[] , l_obs=input("\n ENTER LAST OBSERVATION NO:");
//if (l_obs<=f_obs) printf("\n\n IT SHOULD BE GREATER THAN FIRST OBSERVATION NO:\n\n");end
//if l_obs==[] printf("\n\n IT CANNOT BE LEFT BLANK\n\n");end
end
for i=1:n,OY(i,1)=log(DOY(i,1));end
r = ((l_obs - f_obs) + 1)/3;
for i=1:r, S1 = S1 + OY(i,1);end
for i=r+1:2*r, S2 = S2 + OY(i,1);end
for i=2*r+1:3*r, S3 = S3 + OY(i,1);end
D1 = S1 - S2;
D2 = S2 - S3;
A=0;B=0;C=0;
// CALCULATING INITIAL ESTIMATES OF A, B, C
C = (D2/D1)^(1/r);
B = ((1 - C)/C)* [(D1^3)/(D1-D2)^2];
A = (1/3)*(1/r)*[(S1 + S2 + S3) - (D1^2 + D1*D2 + D2^2)/(D1 - D2)];
Ini_A=A; Ini_B=B;Ini_C=C;
for i=1:n, F(i,1)=1;end
for i=1:n, F(i,2)=C^i;end
for i=1:n, F(i,3)=i*B*(C^(i-1));end
for i=1:n, EY(i,1)=A + B*(C^i);end
for i=1:n, Y(i,1) = OY(i,1) - EY(i,1);end
d = inv(F'*F)*F'*Y;
THETA(1,1) = A + d(1,1);
THETA(2,1) = B + d(2,1);
THETA(3,1) = C + d(3,1);
if abs(d(1,1)/A) < tol_lim & abs(d(2,1)/B) < tol_lim & abs(d(3,1)/C) < tol_lim
break;
end
for cnt=1:100
A = THETA(1,1);
B = THETA(2,1);
C = THETA(3,1);
for i=1:n, F(i,1)=1;end
for i=1:n, F(i,2)=C^i;end
for i=1:n, F(i,3)=i*B*(C^(i-1));end
for i=1:n, EY(i,1)=A + B*(C^i);end
for i=1:n, Y(i,1) = OY(i,1) - EY(i,1);end
d = inv(F'*F)*F'*Y;
THETA(1,1) = A + d(1,1);
THETA(2,1) = B + d(2,1);
THETA(3,1) = C + d(3,1);
if abs(d(1,1)/A) < tol_lim & abs(d(2,1)/B) < tol_lim & abs(d(3,1)/C) < tol_lim
break;
end
end
A=THETA(1,1);
B=THETA(2,1);
C=THETA(3,1);
for i=1:n,
GN1 = GN1 + (OY(i,1) - A - B*(C^i));
GN2 = GN2 + (OY(i,1) - A - B*(C^i))*(C^i);
GN3 = GN3 + (OY(i,1) - A - B*(C^i))*B*i*(C^(i-1));
end
p_GN1=GN1;
p_GN2=GN2;
p_GN3=GN3;
for i=1:n, EY(i,1) = A + B*(C^i);end
for i=1:n, Y(i,1) = OY(i,1) - EY(i,1);end
for i=1:n, sig =sig + Y(i,1)*Y(i,1);end
sigma_hat_square = sig/n;
for i=1:n, y = y + OY(i,1);end
y_bar = y/n;
for i=1:n, dff = dff + (OY(i,1) - y_bar)*(OY(i,1) - y_bar);end
R_square = 1 - (sig/dff);
for i=1:n,
F(i,1)=1;
F(i,2)=C^i;
F(i,3)=i*B*(C^(i-1));
end
//Showing Covariance Matrix
Covariance_Matrix = sigma_hat_square*inv(F'*F);
G = zeros(3,3);
G = inv(F'*F);
//Showing Standard Errors
std_err_A = sqrt((sigma_hat_square)*G(1,1));
std_err_B = sqrt((sigma_hat_square)*G(2,2));
std_err_C = sqrt((sigma_hat_square)*G(3,3));
for i=1:n,
U_t_hat_square = U_t_hat_square+ ((OY(i,1) - A - B*(C^i))^2);
end
for i=2:n,
U_t_hat = OY(i,1) - A - B*(C^i);
U_t_minusone_hat = OY(i-1,1) - A - B*(C^(i-1));
dd = dd + (U_t_hat - U_t_minusone_hat)*(U_t_hat - U_t_minusone_hat);
end
DW = dd/U_t_hat_square;
for i=1:n, EY(i,1) = A + B*(C^i);end
for i=1:n, Y(i,1) = OY(i,1) - EY(i,1);end
for i=1:n,sum_Y = sum_Y + OY(i,1);end
Y_bar = sum_Y /n;
for i=1:n, Y_square = Y_square + Y(i,1)*Y(i,1);end
for i=1:n, D_den = D_den + (OY(i,1) - Y_bar)*(OY(i,1) - Y_bar);end
D = Y_square/D_den;
printf("\n\n\nREPORT SHOWING RESULTS\n");
printf("----------------------\n\n\n");
printf("Sample Size = %d Tolerance Limit=%f\n\n",n,tol_lim);
printf("PARAMETER INITIAL ESTIMATES FINAL ESTIMATES STD. ERRORS DW ");
printf("\n------- ----------------- ---------------- ------------ ---- \n");
printf("\nA %f %f %f %f", Ini_A,A, std_err_A, DW);
printf("\nB %f %f %f ", Ini_B,B, std_err_B);
printf("\nC %f %f %f ", Ini_C,C, std_err_C);
printf("\n\n\n ");
printf("No. of Iterations: = %d\n\n",cnt+1);
printf("GN1 = %.7f\n\n", p_GN1);
printf("GN2 = %.7f\n\n", p_GN2);
printf("GN3 = %.7f\n\n", p_GN3);
printf("Sigma_Hat_Square= %f \t\t R_Square= %f\t D=%f",sigma_hat_square, R_square,D);
printf("\n\n COVARIANCE MATRIX \n");
printf("-------------------------\n");
printf("%f\t\t%f\t\t%f\n",Covariance_Matrix(1,1), Covariance_Matrix(1,2), Covariance_Matrix(1,3));
printf("%f\t\t%f\t\t%f\n",Covariance_Matrix(2,1), Covariance_Matrix(2,2), Covariance_Matrix(2,3));
printf("%f\t\t%f\t\t%f",Covariance_Matrix(3,1), Covariance_Matrix(3,2), Covariance_Matrix(3,3));
printf("\n\n Residuals\n\n ");
printf("Y = %f\n",Y);
x=input("\n\n Exit Program??...Press 1 to exit or enter to save");
if (x==1)
exit();
end
g=x_dialog(['enter file name:']);
u=mopen(g,'w');
mfprintf(u,"REPORT SHOWING RESULTS\n");
mfprintf(u,"----------------------\n\n\n");
mfprintf(u,"Sample Size = %d Tolerance Limit = %f\n\n\n",n,tol_lim);
mfprintf(u,"PARAMETER INITIAL ESTIMATES FINAL ESTIMATES STD. ERRORS DW ");
mfprintf(u,"\n------- ----------------- ---------------- ------------ ------ \n");
mfprintf(u,"\nA %f %f %f %f", Ini_A,A, std_err_A, DW);
mfprintf(u,"\nB %f %f %f ", Ini_B,B, std_err_B);
mfprintf(u,"\nC %f %f %f ", Ini_C,C, std_err_C);
mfprintf(u,"\n\n\n ");
mfprintf(u,"No. of Iterations: = %d\n\n",cnt+1);
mfprintf(u,"GN1 = %.7f\n\n", p_GN1);
mfprintf(u, "GN2 = %.7f\n\n", p_GN2);
mfprintf(u, "GN3 = %.7f\n\n", p_GN3);
mfprintf(u, "Sigma_Hat_Square= %f \t\t R_Square= %f\t D=%f",sigma_hat_square, R_square,D);
mfprintf(u,"\n\n COVARIANCE MATRIX \n");
mfprintf(u,"-------------------------\n");
mfprintf(u,"%f\t\t%f\t\t%f\n",Covariance_Matrix(1,1), Covariance_Matrix(1,2), Covariance_Matrix(1,3));
mfprintf(u,"%f\t\t%f\t\t%f\n",Covariance_Matrix(2,1), Covariance_Matrix(2,2), Covariance_Matrix(2,3));
mfprintf(u,"%f\t\t%f\t\t%f",Covariance_Matrix(3,1), Covariance_Matrix(3,2), Covariance_Matrix(3,3));
mfprintf(u,"\n\n SHOWING RESIDUALS\n\n ");
mfprintf(u, "Y = %f\n",Y);
mclose(u);
t=[1:1:n]';
Estimated_Y=A + B*(C^t);
for i=1:n, OBS(i,1)=OY(i,1);end
for i=1:n,EST(i,1)=Estimated_Y(i,1);end
plot2d(t,[OBS,EST],[2,3],leg="Observed#Estimated",nax=[1,n,1,n]);
legends(['t';'(Year)'],[1,1],opt="lr")
legends(['Y';'(Dependent Variable)'],[1,1],opt="ul")
xtitle("GOMPERTZ GROWTH CURVE");
end_prog=input("\n\n Continue??..PRESS 1 TO CONTINUE.....PRESS 2 TO EXIT");
if (end_prog==1)
exec("C:\SCILAB\Gompertz.sce");
end
if (end_prog==2)
printf("CLOSING PROGRAM........");
exit;
end
Your Scilab program seems to be written for a very old version since xgetfile disappeared with version 5.2 (year 2001). Just replace the line with
gh=uigetfile('*.*','CHOOSE A FILE NAME');
and this part of your program will work.
Related
I am getting an error when I run this code for disc waves. The code is attached.
The Error is in line 137 and 292. Please help in resolving issue.
function waves
% WAVES Wave equation in one and two space dimensions.
% The two-dimensional domains include a pi-by-pi square, a unit disc,
% a three-quarter circular sector and the L-shaped union of three squares.
% The eigenfunctions of the square are sin(m*x)*sin(n*y). With polar
% coordinates, the eigenfunctions of the disc and the sector involve Bessel
% functions. The eigenfunctions of the L-shaped domain also involve
% Bessel functions and are computed by the MATLAB function membranetx.m.
% 2-D eigenvalues and eigenfunctions
m = 11; % Determines number of grid points
speed = 1;
bvals = [1; 0; 0; 0; 0];
t = 0;
while bvals(5) == 0
% Initialize figure
shg
clf reset
set(gcf,'doublebuffer','on','menubar','none','tag','', ...
'numbertitle','off','name','Waves','colormap',hot(64));
for k= 1:5
b(k) = uicontrol('style','toggle','value',bvals(k), ...
'units','normal','position',[.15*k .01 .14 .05]);
end
set(b(1),'style','pop','string', ...
{'1-d','square','disc','sector'})
set(b(2),'string','modes/wave')
set(b(3),'string','slower')
set(b(4),'string','faster')
set(b(5),'string','close')
if bvals(3)==1
speed = speed/sqrt(2);
set(b(3),'value',0);
end
if bvals(4)==1
speed = speed*sqrt(2);
set(b(4),'value',0);
end
bvals = cell2mat(get(b,'value'));
region = bvals(1);
modes = bvals(2)==0;
if region == 1
% 1-D
x = (0:4*m)/(4*m)*pi;
orange = [1 1/3 0];
gray = get(gcf,'color');
if modes
% 1-D modes
for k = 1:4
subplot(2,2,k)
h(k) = plot(x,zeros(size(x)));
axis([0 pi -3/2 3/2])
set(h(k),'color',orange,'linewidth',3)
set(gca,'color',gray','xtick',[],'ytick',[])
end
delta = 0.005*speed;
bvs = bvals;
while all(bvs == bvals)
t = t + delta;
for k = 1:4
u = sin(k*t)*sin(k*x);
set(h(k),'ydata',u)
end
drawnow
bvs = cell2mat(get(b,'value'));
end
else
% 1-D wave
h = plot(x,zeros(size(x)));
axis([0 pi -9/4 9/4])
set(h,'color',orange,'linewidth',3)
set(gca,'color',gray','xtick',[],'ytick',[])
delta = 0.005*speed;
a = 1./(1:4);
bvs = bvals;
while all(bvs == bvals)
t = t + delta;
u = zeros(size(x));
for k = 1:4
u = u + a(k)*sin(k*t)*sin(k*x);
end
set(h,'ydata',u)
drawnow
bvs = cell2mat(get(b,'value'));
end
end
elseif region <= 5
switch region
case 2
% Square
x = (0:2*m)/(2*m)*pi;
y = x';
lambda = zeros(4,1);
V = cell(4,1);
k = 0;
for i = 1:2
for j = 1:2
k = k+1;
lambda(k) = i^2 + j^2;
V{k} = sin(i*y)*sin(j*x);
end
end
ax = [0 pi 0 pi -1.75 1.75];
case 3
% Disc, mu = zeros of J_0(r) and J_1(r)
mu = [bjzeros(0,2) bjzeros(1,2)];
[r,theta] = meshgrid((0:m)/m,(-m:m)/m*pi);
x = r.*cos(theta);
y = r.*sin(theta);
V = cell(4,1);
k = 0;
for j = 0:1
for i = 1:2
k = k+1;
if j == 0
V{k} = besselj(0,mu(k)*r);
else
V{k} = besselj(j,mu(k)*r).*sin(j*theta);
end
V{k} = V{k}/max(max(abs(V{k})));
end
end
lambda = mu.^2;
ax = [-1 1 -1 1 -1.75 1.75];
case 4
% Circular sector , mu = zeros of J_(2/3)(r) and J_(4/3)(r)
mu = [bjzeros(2/3,2) bjzeros(4/3,2)];
[r,theta] = meshgrid((0:m)/m,(3/4)*(0:2*m)/m*pi);
x = r.*cos(theta+pi);
y = r.*sin(theta+pi);
V = cell(4,1);
k = 0;
for j = 1:2
for i = 1:2
k = k+1;
alpha = 2*j/3;
V{k} = besselj(alpha,mu(k)*r).*sin(alpha*theta);
V{k} = V{k}/max(max(abs(V{k})));
end
end
lambda = mu.^2;
ax = [-1 1 -1 1 -1.75 1.75];
case 5\
% L-membrane
x = (-m:m)/m;
y = x';
lambda = zeros(4,1);
V = cell(4,1);
for k = 1:4
[L lambda(k)] = membranetx(k,m,9,9);
L(m+2:2*m+1,m+2:2*m+1) = NaN;
V{k} = rot90(L,-1);
end
ax = [-1 1 -1 1 -1.75 1.75];
end
if modes
% 2-D modes
p = [.02 .52 .02 .52];
q = [.52 .52 .02 .02];
for k = 1:4
axes('position',[p(k) q(k) .46 .46]);
h(k) = surf(x,y,zeros(size(V{k})));
axis(ax)
axis off
view(225,30);
caxis([-1.5 1]);
end
delta = .08*speed;
mu = sqrt(lambda(:));
bvs = bvals;
while all(bvs == bvals)
t = t + delta;
for k = 1:4
U = 1.5*sin(mu(k)*t)*V{k};
set(h(k),'zdata',U)
set(h(k),'cdata',U)
end
drawnow
bvs = cell2mat(get(b,'value'));
end
else
% 2-D wave
h = surf(x,y,zeros(size(V{1})));
axis(ax);
axis off
view(225,30);
caxis([-1.5 1]);
delta = .02*speed;
mu = sqrt(lambda(:));
a = 1.25./(1:4);
bvs = bvals;
while all(bvs == bvals)
t = t + delta;
U = zeros(size(V{1}));
for k = 1:4
U = U + a(k)*sin(mu(k)*t)*V{k};
end
set(h,'zdata',U)
set(h,'cdata',U)
drawnow
bvs = cell2mat(get(b,'value'));
end
end
elseif region == 6
figure
bizcard
set(b(1),'value',1)
end
% Retain uicontrol values
bvals = cell2mat(get(b,'value'));
end
close
% -------------------------------
function z = bjzeros(n,k)
% BJZEROS Zeros of the Bessel function.
% z = bjzeros(n,k) is the first k zeros of besselj(n,x)
% delta must be chosen so that the linear search can take
% steps as large as possible without skipping any zeros.
% delta is approx bjzero(0,2)-bjzero(0,1)
delta = .99*pi;
Jsubn = inline('besselj(n,x)''x','n');
a = n+1;
fa = besselj(n,a);
z = zeros(1,k);
j = 0;
while j < k
b = a + delta;
fb = besselj(n,b);
if sign(fb) ~= sign(fa)
j = j+1;
z(j) = fzerotx(Jsubn,[a b],n);
end
a = b;
fa = fb;
end
I need help with a dynamic optimization problem that consist in a consumed energy optimization of a UAV with this optimal control problem.
enter image description here
enter image description here
My code is this
Ecuations:
Parameters
tf
#Velocidad de rotores rad/s
#Las condiciones iniciales permiten igualar la acción de la gravedad
#Se tomo 4000rad/s como la velocidad maxima de los rotores
w1 = 912.32, >=0, <=3000
w2 = 912.32, >=0, <=3000
w3 = 912.32, >=0, <=3000
w4 = 912.32, >=0, <=3000
t1 = 0, >=0
t2 = 0, >=0
t3 = 0, >=0
t4 = 0, >=0
Constants
!----------------COEFICIENTES DEL MODELO-----------------!
#Gravedad
g = 9.81 !m/s^2
pi = 3.14159265359
#Motor Coefficients
J = 4.1904e-5 !kg*m^2
kt = 0.0104e-3 !N*m/A
kv = 96.342 !rad/s/volt
Dv = 0.2e-3 !N*m*s/rad
R = 0.2 !Ohms
#Battery parameters
Q = 1.55 !Ah
Rint = 0.02 !Ohms
E0 = 1.24 !volt
K = 2.92e-3 !volt
A = 0.156
B =2.35
#Quadrotor parameters
l = 0.175 !m
m = 1.3 !kg
Ix = 0.081 !kg*m^2
Iy = 0.081 !kg*m^2
Iz = 0.142 !kg*m^2
kb = 3.8305e-6 !N/rad/s
ktau = 2.2518e-8 !(N*m)/rad/s
#Parametrizacion del polinomio
a1 = -1.72e-5
a2 = 1.95e-5
a3 = -6.98e-6
a4 = 4.09e-7
b1 = 0.014
b2 = -0.0157
b3 = 5.656e-3
b4 = -3.908e-4
c1 = -0.8796
c2 = 0.3385
c3 = 0.2890
c4 = 0.1626
Variables
!------------------CONDICONES INICIALES------------------!
x = 0
xp = 0
y = 0
yp = 0
z = 0
zp = 0
pitch = 0, >=-pi/2, <=pi/2 !theta - restricciones
pitchp = 0
roll = 0, >=-pi/2, <=pi/2 !phi - restricciones
rollp = 0
yaw = 0 !psi
yawp = 0%, >=-200/180, <=200/180
#Función objetivo
of = 0 !condición inicial de la función objetivo
Intermediates
#Motor 1
aw1 = a1*w1^2 + b1*w1 + c1
bw1 = a2*w1^2 + b2*w1 + c2
cw1 = a3*w1^2 + b3*w1 + c3
dw1 = a4*w1^2 + b4*w1 + c4
#Motor 2
aw2 = a1*w2^2 + b1*w2 + c1
bw2 = a2*w2^2 + b2*w2 + c2
cw2 = a3*w2^2 + b3*w2 + c3
dw2 = a4*w2^2 + b4*w2 + c4
#Motor 3
aw3 = a1*w3^2 + b1*w3 + c1
bw3 = a2*w3^2 + b2*w3 + c2
cw3 = a3*w3^2 + b3*w3 + c3
dw3 = a4*w3^2 + b4*w3 + c4
#Motor 4
aw4 = a1*w4^2 + b1*w4 + c1
bw4 = a2*w4^2 + b2*w4 + c2
cw4 = a3*w4^2 + b3*w4 + c3
dw4 = a4*w4^2 + b4*w4 + c4
#frj(wj(t),Tj(t))
fr1=aw1*t1^3 + bw1*t1^2 + cw1*t1 + dw1
fr2=aw2*t2^3 + bw2*t2^2 + cw2*t2 + dw2
fr3=aw3*t3^3 + bw3*t3^2 + cw3*t3 + dw3
fr4=aw4*t4^3 + bw4*t4^2 + cw4*t4 + dw4
!---------------------CONTROL INPUTS---------------------!
T = kb * (w1^2 + w2^2 + w3^2 + w4^2)
u1 = kb * (w2^2 - w4^2)
u2 = kb * (w3^2 - w1^2)
u3 = ktau * (w1^2 - w2^2 + w3^2 - w4^2)
wline = w1 - w2 + w3 - w4
!-------------------ENERGIA POR ROTOR--------------------!
Ec1 = ((J*$w1 + ktau*w1^2 + Dv*w1)/fr1)*w1
Ec2 = ((J*$w2 + ktau*w2^2 + Dv*w2)/fr2)*w2
Ec3 = ((J*$w3 + ktau*w3^2 + Dv*w3)/fr3)*w3
Ec4 = ((J*$w4 + ktau*w4^2 + Dv*w4)/fr4)*w4
Ectotal = Ec1 + Ec2 + Ec3 + Ec4
Equations
!---------------MINIMIZAR FUNCIÓN OBJETIVO---------------!
minimize tf * of
!-----------------RELACION DE VARIABLES------------------!
xp = $x
yp = $y
zp = $z
pitchp = $pitch
rollp = $roll
yawp = $yaw
!-----------------CONDICONES DE FRONTERA-----------------!
#Condiciones finales del modelo
tf * x = 4
tf * y = 5
tf * z = 6
tf * xp = 0
tf * yp = 0
tf * zp = 0
tf * roll = 0
tf * pitch = 0
tf * yaw = 0
!-----------------TORQUE DE LOS MOTORES------------------!
t1 = J*$w1 + ktau*w1^2 + Dv*w1
t2 = J*$w2 + ktau*w2^2 + Dv*w2
t3 = J*$w3 + ktau*w3^2 + Dv*w3
t4 = J*$w4 + ktau*w4^2 + Dv*w4
!------------------------SUJETO A------------------------!
#Modelo aerodinámico del UAV
m*$xp = (cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw))*T
m*$yp = (cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw))*T
m*$zp = (cos(roll)*cos(pitch))*T-m*g
Ix*$rollp = ((Iy - Iz)*pitchp*yawp + J*pitchp*wline + l*u1)
Iy*$pitchp = ((Iz - Ix)*rollp*yawp - J*rollp*wline + l*u2)
Iz*$yawp = ((Ix - Iy)*rollp*pitchp + u3)
!--------------------FUNCIÓN OBJETIVO--------------------!
$of = Ectotal
MATLAB:
clear all; close all; clc
server = 'http://127.0.0.1';
app = 'traj_optima';
addpath('C:/Program Files/MATLAB/apm_matlab_v0.7.2/apm')
apm(server,app,'clear all');
apm_load(server,app,'ecuaciones_mod.apm');
csv_load(server,app,'tiempo2.csv');
apm_option(server,app,'apm.max_iter',200);
apm_option(server,app,'nlc.nodes',3);
apm_option(server,app,'apm.rtol',1);
apm_option(server,app,'apm.otol',1);
apm_option(server,app,'nlc.solver',3);
apm_option(server,app,'nlc.imode',6);
apm_option(server,app,'nlc.mv_type',1);
costo=1e-5;%1e-5
%VARIABLES CONTROLADAS
%Velocidades angulares
apm_info(server,app,'MV','w1');
apm_option(server,app,'w1.status',1);
apm_info(server,app,'MV','w2');
apm_option(server,app,'w2.status',1);
apm_info(server,app,'MV','w3');
apm_option(server,app,'w3.status',1);
apm_info(server,app,'MV','w4');
apm_option(server,app,'w4.status',1);
% Torques
apm_info(server,app,'MV','t1');
apm_option(server,app,'t1.status',1);
apm_info(server,app,'MV','t2');
apm_option(server,app,'t2.status',1);
apm_info(server,app,'MV','t3');
apm_option(server,app,'t3.status',1);
apm_info(server,app,'MV','t4');
apm_option(server,app,'t4.status',1);
%Salida
output = apm(server,app,'solve');
disp(output)
y = apm_sol(server,app);
z = y.x;
tiempo2.csv
time,tf
0,0
0.001,0
0.2,0
0.4,0
0.6,0
0.8,0
1,0
1.2,0
1.4,0
1.6,0
1.8,0
2,0
2.2,0
2.4,0
2.6,0
2.8,0
3,0
3.2,0
3.4,0
3.6,0
3.8,0
4,0
4.2,0
4.4,0
4.6,0
4.8,0
5,0
5.2,0
5.4,0
5.6,0
5.8,0
6,0
6.2,0
6.4,0
6.6,0
6.8,0
7,0
7.2,0
7.4,0
7.6,0
7.8,0
8,0
8.2,0
8.4,0
8.6,0
8.8,0
9,0
9.2,0
9.4,0
9.6,0
9.8,0
10,1
Finally the answer obtained is:
enter image description here
I need help with this local infeasibility problem, please.
The infeasible solution is caused by the terminal constraints:
tf * z = 4
tf * z = 5
tf * z = 6
When tf=0, the constraints are evaluated to 0=4, 0=5, 0=6 and the solver reports that these can not be satisfied by the solver. Instead, you can pose the constraints as:
tf * (x-4) = 0
tf * (y-5) = 0
tf * (z-6) = 0
That way, the constraint is valid when tf=0 and when tf=1 at the final time. A potential better way yet is to convert the terminal constraints to objective terms with f=1000 such as:
minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
minimize f*tf*(xp^2 + yp^2 + zp^2)
minimize f*tf*(roll^2 + pitch^2 + yaw^2)
That way, the optimizer won't report an infeasible solution if it can't reach the terminal constraints as discussed in the pendulum problem. I made a few other modifications to your model and script to achieve a successful solution. Here is a summary:
Converted terminal constraints to objective function (soft constraints)
Parameters t1-t4 should be variables
Fixed degree of freedom issue by making w1-w4 variables and w1p-w4p variables. w1-w4 are differential states.
Added constraints to w1p-w4p between -10 and 10 to help the solver converge
Added initialization step to simulate the model before optimizing. There are more details on initialization strategies in this paper: Safdarnejad, S.M., Hedengren, J.D., Lewis, N.R., Haseltine, E., Initialization Strategies for Optimization of Dynamic Systems, Computers and Chemical Engineering, 2015, Vol. 78, pp. 39-50, DOI: 10.1016/j.compchemeng.2015.04.016
Model
Parameters
tf
w1p = 0 > -10 < 10
w2p = 0 > -10 < 10
w3p = 0 > -10 < 10
w4p = 0 > -10 < 10
Constants
!----------------COEFICIENTES DEL MODELO-----------------!
#Gravedad
g = 9.81 !m/s^2
pi = 3.14159265359
#Motor Coefficients
J = 4.1904e-5 !kg*m^2
kt = 0.0104e-3 !N*m/A
kv = 96.342 !rad/s/volt
Dv = 0.2e-3 !N*m*s/rad
R = 0.2 !Ohms
#Battery parameters
Q = 1.55 !Ah
Rint = 0.02 !Ohms
E0 = 1.24 !volt
K = 2.92e-3 !volt
A = 0.156
B =2.35
#Quadrotor parameters
l = 0.175 !m
m = 1.3 !kg
Ix = 0.081 !kg*m^2
Iy = 0.081 !kg*m^2
Iz = 0.142 !kg*m^2
kb = 3.8305e-6 !N/rad/s
ktau = 2.2518e-8 !(N*m)/rad/s
#Parametrizacion del polinomio
a1 = -1.72e-5
a2 = 1.95e-5
a3 = -6.98e-6
a4 = 4.09e-7
b1 = 0.014
b2 = -0.0157
b3 = 5.656e-3
b4 = -3.908e-4
c1 = -0.8796
c2 = 0.3385
c3 = 0.2890
c4 = 0.1626
Variables
!------------------CONDICONES INICIALES------------------!
x = 0
xp = 0
y = 0
yp = 0
z = 0
zp = 0
pitch = 0, >=-pi/2, <=pi/2 !theta - restricciones
pitchp = 0
roll = 0, >=-pi/2, <=pi/2 !phi - restricciones
rollp = 0
yaw = 0 !psi
yawp = 0 %, >=-200/180, <=200/180
#Velocidad de rotores rad/s
#Las condiciones iniciales permiten igualar la acción de la gravedad
#Se tomo 4000rad/s como la velocidad maxima de los rotores
w1 = 912.32, >=0, <=3000
w2 = 912.32, >=0, <=3000
w3 = 912.32, >=0, <=3000
w4 = 912.32, >=0, <=3000
t1 = 0, >=0
t2 = 0, >=0
t3 = 0, >=0
t4 = 0, >=0
#Función objetivo
of = 0 !condición inicial de la función objetivo
Intermediates
#Motor 1
aw1 = a1*w1^2 + b1*w1 + c1
bw1 = a2*w1^2 + b2*w1 + c2
cw1 = a3*w1^2 + b3*w1 + c3
dw1 = a4*w1^2 + b4*w1 + c4
#Motor 2
aw2 = a1*w2^2 + b1*w2 + c1
bw2 = a2*w2^2 + b2*w2 + c2
cw2 = a3*w2^2 + b3*w2 + c3
dw2 = a4*w2^2 + b4*w2 + c4
#Motor 3
aw3 = a1*w3^2 + b1*w3 + c1
bw3 = a2*w3^2 + b2*w3 + c2
cw3 = a3*w3^2 + b3*w3 + c3
dw3 = a4*w3^2 + b4*w3 + c4
#Motor 4
aw4 = a1*w4^2 + b1*w4 + c1
bw4 = a2*w4^2 + b2*w4 + c2
cw4 = a3*w4^2 + b3*w4 + c3
dw4 = a4*w4^2 + b4*w4 + c4
#frj(wj(t),Tj(t))
fr1=aw1*t1^3 + bw1*t1^2 + cw1*t1 + dw1
fr2=aw2*t2^3 + bw2*t2^2 + cw2*t2 + dw2
fr3=aw3*t3^3 + bw3*t3^2 + cw3*t3 + dw3
fr4=aw4*t4^3 + bw4*t4^2 + cw4*t4 + dw4
!---------------------CONTROL INPUTS---------------------!
T = kb * (w1^2 + w2^2 + w3^2 + w4^2)
u1 = kb * (w2^2 - w4^2)
u2 = kb * (w3^2 - w1^2)
u3 = ktau * (w1^2 - w2^2 + w3^2 - w4^2)
wline = w1 - w2 + w3 - w4
!-------------------ENERGIA POR ROTOR--------------------!
Ec1 = ((J*$w1 + ktau*w1^2 + Dv*w1)/fr1)*w1
Ec2 = ((J*$w2 + ktau*w2^2 + Dv*w2)/fr2)*w2
Ec3 = ((J*$w3 + ktau*w3^2 + Dv*w3)/fr3)*w3
Ec4 = ((J*$w4 + ktau*w4^2 + Dv*w4)/fr4)*w4
Ectotal = Ec1 + Ec2 + Ec3 + Ec4
! scaling factor for terminal constraint
f = 1000
Equations
!---------------MINIMIZAR FUNCIÓN OBJETIVO---------------!
minimize tf * of
!-----------------RELACION DE VARIABLES------------------!
xp = $x
yp = $y
zp = $z
pitchp = $pitch
rollp = $roll
yawp = $yaw
w1p = $w1
w2p = $w2
w3p = $w3
w4p = $w4
!-----------------CONDICONES DE FRONTERA-----------------!
#Condiciones finales del modelo
#tf * (x-4) = 0
#tf * (y-5) = 0
#tf * (z-6) = 0
#tf * xp = 0
#tf * yp = 0
#tf * zp = 0
#tf * roll = 0
#tf * pitch = 0
#tf * yaw = 0
minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
minimize f*tf*(xp^2 + yp^2 + zp^2)
minimize f*tf*(roll^2 + pitch^2 + yaw^2)
!-----------------TORQUE DE LOS MOTORES------------------!
t1 = J*w1p + ktau*w1^2 + Dv*w1
t2 = J*w2p + ktau*w2^2 + Dv*w2
t3 = J*w3p + ktau*w3^2 + Dv*w3
t4 = J*w4p + ktau*w4^2 + Dv*w4
!------------------------SUJETO A------------------------!
#Modelo aerodinámico del UAV
m*$xp = (cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw))*T
m*$yp = (cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw))*T
m*$zp = (cos(roll)*cos(pitch))*T-m*g
Ix*$rollp = ((Iy - Iz)*pitchp*yawp + J*pitchp*wline + l*u1)
Iy*$pitchp = ((Iz - Ix)*rollp*yawp - J*rollp*wline + l*u2)
Iz*$yawp = ((Ix - Iy)*rollp*pitchp + u3)
!--------------------FUNCIÓN OBJETIVO--------------------!
$of = Ectotal
MATLAB Script
clear all; close all; clc
server = 'http://byu.apmonitor.com';
app = 'traj_optima';
addpath('apm')
apm(server,app,'clear all');
apm_load(server,app,'ecuaciones_mod.apm');
csv_load(server,app,'tiempo2.csv');
apm_option(server,app,'apm.max_iter',1000);
apm_option(server,app,'apm.nodes',3);
apm_option(server,app,'apm.rtol',1e-6);
apm_option(server,app,'apm.otol',1e-6);
apm_option(server,app,'apm.solver',3);
apm_option(server,app,'apm.imode',6);
apm_option(server,app,'apm.mv_type',1);
costo=1e-5;%1e-5
%VARIABLES CONTROLADAS
%Velocidades angulares
apm_info(server,app,'MV','w1p');
apm_option(server,app,'w1p.status',1);
apm_info(server,app,'MV','w2p');
apm_option(server,app,'w2p.status',1);
apm_info(server,app,'MV','w3p');
apm_option(server,app,'w3p.status',1);
apm_info(server,app,'MV','w4p');
apm_option(server,app,'w4p.status',1);
%Salida
disp('')
disp('------------- Initialize ----------------')
apm_option(server,app,'apm.coldstart',1);
output = apm(server,app,'solve');
disp(output)
disp('')
disp('-------------- Optimize -----------------')
apm_option(server,app,'apm.time_shift',0);
apm_option(server,app,'apm.coldstart',0);
output = apm(server,app,'solve');
disp(output)
y = apm_sol(server,app);
z = y.x;
This gives a successful solution but the terminal constraints are not met. The solver optimizes the use of w1p-w4p to minimize the objective but there is no solution that makes it to the terminal constraints.
The solution was found.
The final value of the objective function is 50477.4537378181
---------------------------------------------------
Solver : IPOPT (v3.12)
Solution time : 3.06940000000759 sec
Objective : 50477.4537378181
Successful solution
---------------------------------------------------
As a next step, I recommend that you increase the number of time points or allow the final time to change to meet the terminal constraints. You may also want to consider switching to Python Gekko that uses the same underlying engine as APM MATLAB. In this case, the modeling language is fully integrated with Python.
correlation = zeros(length(s1), 1);
sizeNum = 0;
for i = 1 : length(s1) - windowSize - delta
s1Dat = s1(i : i + windowSize);
s2Dat = s2(i + delta : i + delta + windowSize);
if length(find(isnan(s1Dat))) == 0 && length(find(isnan(s2Dat))) == 0
if(var(s1Dat) ~= 0 || var(s2Dat) ~= 0)
sizeNum = sizeNum + 1;
correlation(i) = abs(corr(s1Dat, s2Dat)) ^ 2;
end
end
end
What's happening here:
Run through every values in s1. For every value, get a slice for s1
till s1 + windowSize.
Do the same for s2, only get the slice after an intermediate delta.
If there are no NaN's in any of the two slices and they aren't flat,
then get the correlaton between them and add that to the
correlation matrix.
This is not an answer, I am trying to understand what is being asked.
Take some data:
N = 1e4;
s1 = cumsum(randn(N, 1)); s2 = cumsum(randn(N, 1));
s1(randi(N, 50, 1)) = NaN; s2(randi(N, 50, 1)) = NaN;
windowSize = 200; delta = 100;
Compute correlations:
tic
corr_s = zeros(N - windowSize - delta, 1);
for i = 1:(N - windowSize - delta)
s1Dat = s1(i:(i + windowSize));
s2Dat = s2((i + delta):(i + delta + windowSize));
corr_s(i) = corr(s1Dat, s2Dat);
end
inds = isnan(corr_s);
corr_s(inds) = 0;
corr_s = corr_s .^ 2; % square of correlation coefficient??? Why?
sizeNum = sum(~inds);
toc
This is what you want to do, right? A moving window correlation function? This is a very interesting question indeed …
I'm using matlab to implement a multilayer neural network. In the code I represent
the value of each node AS netValue{k}
the weight between layer k and k + 1 AS weight{k}
etc.
Since these data is three-dimensional, I have to use cell to hold a 2-D matrix to enable matrix multiply.
So it becomes really really slow to train the model, which I expect to have resulted from the usage of cell.
Can anyone tell me how to accelerate this code? Thanks
clc;
close all;
clear all;
input = [-2 : 0.4 : 2;-2:0.4:2];
ican = 4;
depth = 4; % total layer - 1, by convension
[featureNum , sampleNum] = size(input);
levelNum(1) = featureNum;
levelNum(2) = 5;
levelNum(3) = 5;
levelNum(4) = 5;
levelNum(5) = 2;
weight = cell(0);
for k = 1 : depth
weight{k} = rand(levelNum(k+1), levelNum(k)) - 2 * rand(levelNum(k+1) , levelNum(k));
threshold{k} = rand(levelNum(k+1) , 1) - 2 * rand(levelNum(k+1) , 1);
end
runCount = 0;
sumMSE = 1; % init MSE
minError = 1e-5;
afa = 0.1; % step of "gradient ascendence"
% training loop
while(runCount < 100000 & sumMSE > minError)
sumMSE = 0; % sum of MSE
for i = 1 : sampleNum % sample loop
netValue{1} = input(:,i);
for k = 2 : depth
netValue{k} = weight{k-1} * netValue{k-1} + threshold{k-1}; %calculate each layer
netValue{k} = 1 ./ (1 + exp(-netValue{k})); %apply logistic function
end
netValue{depth+1} = weight{depth} * netValue{depth} + threshold{depth}; %output layer
e = 1 + sin((pi / 4) * ican * netValue{1}) - netValue{depth + 1}; %calc error
assistS{depth} = diag(ones(size(netValue{depth+1})));
s{depth} = -2 * assistS{depth} * e;
for k = depth - 1 : -1 : 1
assistS{k} = diag((1-netValue{k+1}).*netValue{k+1});
s{k} = assistS{k} * weight{k+1}' * s{k+1};
end
for k = 1 : depth
weight{k} = weight{k} - afa * s{k} * netValue{k}';
threshold{k} = threshold{k} - afa * s{k};
end
sumMSE = sumMSE + e' * e;
end
sumMSE = sqrt(sumMSE) / sampleNum;
runCount = runCount + 1;
end
x = [-2 : 0.1 : 2;-2:0.1:2];
y = zeros(size(x));
z = 1 + sin((pi / 4) * ican .* x);
% test
for i = 1 : length(x)
netValue{1} = x(:,i);
for k = 2 : depth
netValue{k} = weight{k-1} * netValue{k-1} + threshold{k-1};
netValue{k} = 1 ./ ( 1 + exp(-netValue{k}));
end
y(:, i) = weight{depth} * netValue{depth} + threshold{depth};
end
plot(x(1,:) , y(1,:) , 'r');
hold on;
plot(x(1,:) , z(1,:) , 'g');
hold off;
Have you used the profiler to find out what functions are actually slowing down your code? It shows what lines take the most time to execute.
I have this program that finds the vanishing point for a given set of images. Is there a way to find the distance from the camera and the vanishing point?
Also once the vanishing point is found out, I manually need to find the X and Y coordinates using the tool provided in matlab. How can i code a snippet that writes all the X and Y coordinates into a text or excel file?
Also is there a better and simpler way to find the vanishing point in matlab?
Matlab Calling Function to find Vanishing Point:
clear all; close all;
dname = 'Height';
files = dir(dname);
files(1) = [];
files(1) = [];
for i=1:size(files, 1)
original = imread(fullfile(dname, files(i).name));
original = imresize(original,0.35);
im = im2double(rgb2gray(original));
[row, col] = findVanishingPoint(im);
imshow(original);hold;plot(col,row,'rx');
saveas(gcf,strcat('Height_Result',num2str(i)),'jpg');
close
end
The findVanishingPoint function:
function [row, col] = findVanishingPoint(im)
DEBUG = 0;
IM = fft2(im);
ROWS = size(IM,1); COLS = size(IM,2);
PERIOD = 2^floor(log2(COLS)-5)+2;
SIZE = floor(10*PERIOD/pi);
SIGMA = SIZE/9;
NORIENT = 72;
E = 8;
[C, S] = createGaborBank(SIZE, PERIOD, SIGMA, NORIENT, ROWS, COLS, E);
D = ones(ROWS, COLS);
AMAX = ifftshift(real(ifft2(C{1}.*IM)).^2+real(ifft2(S{1}.*IM))).^2;
for n=2:NORIENT
A = ifftshift(real(ifft2(C{n}.*IM)).^2+real(ifft2(S{n}.*IM))).^2;
D(find(A > AMAX)) = n;
AMAX = max(A, AMAX);
if (DEBUG==1)
colormap('hot');subplot(131);imagesc(real(A));subplot(132);imagesc(real(AMAX));colorbar;
subplot(133);imagesc(D);
pause
end
end
if (DEBUG==2)
figure('DoubleBuffer','on');
end
T = mean(AMAX(:))-3*std(AMAX(:));
VOTE = zeros(ROWS, COLS);
for row=round(1+SIZE/2):round(ROWS-SIZE/2)
for col=round(1+SIZE/2):round(COLS-SIZE/2)
if (AMAX(row,col) > T)
indices = lineBresenham(ROWS, COLS, col, row, D(row, col)*pi/NORIENT-pi/2);
VOTE(indices) = VOTE(indices)+AMAX(row,col);
end
end
if (DEBUG==2)
colormap('hot');imagesc(VOTE);pause;
end
end
if (DEBUG==2)
close
end
M=1;
[b index] = sort(-VOTE(:));
col = floor((index(1:M)-1) / ROWS)+1;
row = mod(index(1:M)-1, ROWS)+1;
col = round(mean(col));
row = round(mean(row));
The creatGaborBank function:
function [C, S] = createGaborBank(SIZE, PERIOD, SIGMA, NORIENT, ROWS, COLS, E)
if (length(NORIENT)==1)
orientations=[1:NORIENT];
else
orientations = NORIENT;
NORIENT = max(orientations);
end
for n=orientations
[C{n}, S{n}] = gabormask(SIZE, SIGMA, PERIOD, n*pi/NORIENT);
C{n} = fft2(padWithZeros(C{n}, ROWS, COLS));
S{n} = fft2(padWithZeros(S{n}, ROWS, COLS));
end
The gabormask function:
function [cmask, smask] = gabormask(Size, sigma, period, orient, E)
if nargin < 5; E = 8; end;
if nargin < 4; orient = 0; end;
if nargin < 3; period = []; end;
if nargin < 2; sigma = []; end;
if nargin < 1; Size = []; end;
if isempty(period) & isempty(sigma); sigma = 5; end;
if isempty(period); period = sigma*2*sqrt(2); end;
if isempty(sigma); sigma = period/(2*sqrt(2)); end;
if isempty(Size); Size = 2*round(2.575*sigma) + 1; end;
if length(Size) == 1
sx = Size-1; sy = sx;
elseif all(size(Size) == [1 2])
sy = Size(1)-1; sx = Size(2)-1;
else
error('Size must be scalar or 1-by-2 vector');
end;
hy = sy/2; hx = sx/2;
[x, y] = meshgrid(-hx:sx-hx, -hy:sy-hy);
omega = 2*pi/period;
cs = omega * cos(orient);
sn = omega * sin(orient);
k = -1/(E*sigma*sigma);
g = exp(k * (E*x.*x + y.*y));
xp = x * cs + y * sn;
cx = cos(xp);
cmask = g .* cx;
sx = sin(xp);
smask = g .* sx;
cmask = cmask - mean(cmask(:));
cmask = cmask/sum(abs(cmask(:)));
smask = smask - mean(smask(:));
smask = smask/sum(abs(smask(:)));
The padWithZeros function:
function out = padWithZeros(in, ROWS, COLS)
out = padarray(in,[floor((ROWS-size(in,1))/2) floor((COLS-size(in,2))/2)],0,'both');
if size(out,1) == ROWS-1
out = padarray(out,[1 0],0,'pre');
end
if size(out,2) == COLS-1
out = padarray(out,[0 1],0,'pre');
end
The findHorizonEdge function:
function row = findHorizon(im)
DEBUG = 2;
ROWS = size(im,1); COLS = size(im,2);
e = edge(im,'sobel', [], 'horizontal');
dd = sum(e, 2);
N=3;
row = 1;
M = 0;
for i=1+N:length(dd)-N
m = sum(dd(i-N:i+N));
if (m > M)
M = m;
row = i;
end
end
imshow(e);pause
The findHorizon function:
function row = findHorizon(im)
DEBUG = 2;
IM = fft2(im);
ROWS = size(IM,1); COLS = size(IM,2);
PERIOD = 2^floor(log2(COLS)-5)+2;
SIZE = floor(10*PERIOD/pi);
SIGMA = SIZE/9;
NORIENT = 72;
E = 16;
orientations = [NORIENT/2-10:NORIENT/2+10];
[C, S] = createGaborBank(SIZE, PERIOD, SIGMA, orientations, ROWS, COLS, E);
ASUM = zeros(ROWS, COLS);
for n=orientations
A = ifftshift(real(ifft2(C{n}.*IM)).^2+real(ifft2(S{n}.*IM))).^2;
ASUM = ASUM + A;
if (DEBUG==1)
colormap('hot');subplot(131);imagesc(real(A));subplot(132);imagesc(real(AMAX));colorbar;
pause
end
end
ASUM(1:round(1+SIZE/2), :)=0; ASUM(end-round(SIZE/2):end, :)=0;
ASUM(:,end-round(SIZE/2):end)=0; ASUM(:, 1:1+round(SIZE/2))=0;
dd = sum(ASUM, 2);
[temp, row] = sort(-dd);
row = round(mean(row(1:10)));
if (DEBUG == 2)
imagesc(ASUM);hold on;line([1:COLS],repmat(row,COLS));
pause
end
The lineImage function:
function v = lineimage(x0, y0, angle, s)
if (abs(tan(angle)) > 1e015)
a(1,:) = repmat(x0,s(1),1)';
a(2,:) = [1:s(1)];
elseif (abs(tan(angle)) < 1e-015)
a(2,:) = repmat(y0,s(2),1)';
a(1,:) = [1:s(2)];
else
k = tan(angle);
hiX = round((1-(s(1)-y0+1)+k*x0)/k);
loX = round((s(1)-(s(1)-y0+1)+k*x0)/k);
temp = max(loX, hiX);
loX = max(min(loX, hiX), 1);
hiX = min(s(2),temp);
a(1,:) = [loX:hiX];
a(2,:) = max(1, floor(s(1)-(k*a(1,:)+(s(1)-y0+1)-k*x0)));
end
v = (a(1,:)-1).*s(1)+a(2,:);
The lineVector function:
function [abscissa, ordinate] = linevector(x0, y0, angle, s)
if (rad2deg(angle) == 90)
abscissa = repmat(x0,s(1),1);
ordinate = [1:s(1)];
else
k = tan(angle);
hiX = round((1-(s(1)-y0+1)+k*x0)/k);
loX = round((s(1)-(s(1)-y0+1)+k*x0)/k);
temp = max(loX, hiX);
loX = max(min(loX, hiX), 1);
hiX = min(s(2),temp);
abscissa = [loX:hiX];
ordinate = k*abscissa+((s(1)-y0+1)-k*x0);
end
The lineBresenham function:
function [i] = lineBresenham(H,W,Sx,Sy,angle)
k = tan(angle);
if (angle == pi || angle == 0)
Ex = W;
Ey = Sy;
Sx = 1;
elseif (angle == pi/2)
Ey = 1;
i = (Sx-1)*H+[Ey:Sy];
return;
elseif k>0 & k < (Sy-1)/(W-Sx)
Ex = W;
Ey = round(Sy-tan(angle)*(Ex-Sx));
elseif k < 0 & abs(k) < (Sy-1)/(Sx-1)
Ex = 1;
Ey = round(Sy-tan(angle)*(Ex-Sx));
else
Ey = 1;
Ex = round((Sy-1)/tan(angle)+Sx);
end
Dx = Ex - Sx;
Dy = Ey - Sy;
iCoords=1;
if(abs(Dy) <= abs(Dx))
if(Ex >= Sx)
D = 2*Dy + Dx;
IncH = 2*Dy;
IncD = 2*(Dy + Dx);
X = Sx;
Y = Sy;
i(iCoords) = (Sx-1)*H+Sy;
iCoords = iCoords + 1;
while(X < Ex)
if(D >= 0)
D = D + IncH;
X = X + 1;
else
D = D + IncD;
X = X + 1;
Y = Y - 1;
end
i(iCoords) = (X-1)*H+Y;
iCoords = iCoords + 1;
end
else
D = -2*Dy + Dx;
IncH = -2*Dy;
IncD = 2*(-Dy + Dx);
X = Sx;
Y = Sy;
i(iCoords) = (Sx-1)*H+Sy;
iCoords = iCoords + 1;
while(X > Ex)
if(D <= 0)
D = D + IncH;
X = X - 1;
else
D = D + IncD;
X = X - 1;
Y = Y - 1;
end
i(iCoords) = (X-1)*H+Y;
iCoords = iCoords + 1;
end
end
else
Tmp = Ex;
Ex = Ey;
Ey = Tmp;
Tmp = Sx;
Sx = Sy;
Sy = Tmp;
Dx = Ex - Sx;
Dy = Ey - Sy;
if(Ex >= Sx)
D = 2*Dy + Dx;
IncH = 2*Dy;
IncD = 2*(Dy + Dx);
X = Sx;
Y = Sy;
i(iCoords) = (Sy-1)*H+Sx;
iCoords = iCoords + 1;
while(X < Ex)
if(D >= 0)
D = D + IncH;
X = X + 1;
else
D = D + IncD;
X = X + 1;
Y = Y - 1;
end
i(iCoords) = (Y-1)*H+X;
iCoords = iCoords + 1;
end
else
D = -2*Dy + Dx;
IncH = -2*Dy;
IncD = 2*(-Dy + Dx);
X = Sx;
Y = Sy;
i(iCoords) = (Sy-1)*H+Sx;
iCoords = iCoords + 1;
while(X > Ex)
if(D <= 0)
D = D + IncH;
X = X - 1;
else
D = D + IncD;
X = X - 1;
Y = Y - 1;
end
i(iCoords) = (Y-1)*H+X;
iCoords = iCoords + 1;
end
end
end
The vanishing point is at infinity hence the distance to the camera is of no use.
Use xlswrite or dlmwrite to write into excel or text file respectively.