Finding a root in matlab, not compiling - matlab

i have to do an assignment where i have to find the depth of how much the ball is under water depending on the water's density, then i have to plot it. i don't have matlab at home but i'm using octave and for some reason it's not compiling, and it's not saying any errors. here is my code
function my_script()
% weight function
function func = weight(x,p)
func = p *pi* 4- 3 * pi*x ^2 + pi* x ^3;
% make the secant method
function root = secant(func , p, x0, x1, tol, count)
while count ~= 0
y1 = func(x0,p);
y2 = func(x1, p);
k = (x1 - x0)/(y2 - y1);
R = y2 * k;
root = x1 - R;
if abs(func(root,p)) < tol
break;
end
x0 = x1;
x1 = root;
count = count - 1;
end
%create array for depth values
y_val = [];
%input the roots into the array with a loop
for p = 0:0.01:1
t = secant(#weight, p, 0, 1, 0.000001, 1000000);
disp (t);
y_val = [y_val t];
end
% create the x - axis
x_val = 0 : 0.01 : 1;
%plotting the graph
figure;
plot (x_val, y_val)
xlabel('Density');
ylabel('Depth ');
title('Floating Sphere vs Density ');

Another way of creating what you have written is to split it into three different functions.
You'd have the weight function, weight.m,
% weight function
function func = weight(x,p)
func = p *pi* 4- 3 * pi*x ^2 + pi* x ^3;
and the secant method function, secant.m,
% make the secant method
function root = secant(func , p, x0, x1, tol, count)
while count ~= 0
y1 = func(x0,p);
y2 = func(x1, p);
k = (x1 - x0)/(y2 - y1);
R = y2 * k;
root = x1 - R;
if abs(func(root,p)) < tol
break;
end
x0 = x1;
x1 = root;
count = count - 1;
end
%create array for depth values
y_val = [];
%input the roots into the array with a loop
for p = 0:0.01:1
t = secant(#weight, p, 0, 1, 0.000001, 1000000);
disp (t);
y_val = [y_val t];
end
% create the x - axis
x_val = 0 : 0.01 : 1;
%plotting the graph
figure;
plot (x_val, y_val)
xlabel('Density');
ylabel('Depth ');
title('Floating Sphere vs Density ');
Then you'd have my_script():
function my_script()
but it is an empty function, so the output is correct! You have to have all the commands for each function finished before you define the next function.
I have a feeling that you want to take out everything from the end of the while loop onwards from the secant function and put it into the main my_script() function.

Here's your code, Octave style. Observe that I temporarily commented the "disp(t);" statement to avoid a long listing of values. This way, the plot is plotted immediately. Hopefully this helps.
function my_script
%create array for depth values
y_val = [];
%input the roots into the array with a loop
for p = 0:0.01:1
t = secant(#weight, p, 0, 1, 0.000001, 1000000);
%disp (t);
y_val = [y_val t];
endfor
% create the x - axis
x_val = 0 : 0.01 : 1;
%plotting the graph
figure(1)
plot (x_val, y_val)
xlabel('Density');
ylabel('Depth ');
title('Floating Sphere vs Density ');
endfunction
% make the secant method
function root = secant(func , p, x0, x1, tol, count)
while count ~= 0
y1 = func(x0,p);
y2 = func(x1, p);
k = (x1 - x0)/(y2 - y1);
R = y2 * k;
root = x1 - R;
if abs(func(root,p)) < tol
break;
end
x0 = x1;
x1 = root;
count = count - 1;
endwhile
endfunction
% weight function
function func = weight(x,p)
func = p *pi* 4- 3 * pi*x ^2 + pi* x ^3;
endfunction

Related

Changing amplitude of fourier series in matlab

The code below currently plots the fourier series for a square wave for N terms. Is there any way I could change the range from [0;1] to [-1;1]?
% Assignment of variables
syms t
% Function variables
N = 5;
T0 = 1;
w0 = 2*pi/T0;
Imin = 0;
Imax = 0.5;
% Function
ft = 1;
% First term calculation
a0 = (1/T0)*int(ft, t, Imin, Imax);
y = a0;
% Calculation of n terms
for n = 1:N
an = (2/T0)*int(ft*cos(n*w0*t), t, Imin, Imax);
bn = (2/T0)*int(ft*sin(n*w0*t), t, Imin, Imax);
y = y + an*cos(n*w0*t) + bn*sin(n*w0*t);
end
fplot(y, [-4,4], "Black")
grid on
If you are talking about the figure scale, then ylim([-1 1])
1.- The following does what you asked for:
clear all;clc;close all
syms t
assume(t>0 & t<1)
% Function variables
N = 5;
T0 = 1;
w0 = 2*pi/T0;
Imin = 0;
Imax = 1;
% Function
h1=heaviside(t-.5)
h2=heaviside(t+.5)
ht=-2*((h1-h2)+.5)
% First term calculation
a0 = (1/T0)*int(ht, t, Imin, Imax);
y = a0;
% Calculation of n terms
for n = 1:N
an = (2/T0)*int(ht*cos(n*w0*t), t, Imin, Imax);
bn = (2/T0)*int(ht*sin(n*w0*t), t, Imin, Imax);
y = y + an*cos(n*w0*t) + bn*sin(n*w0*t);
end
fplot(y, [-4,4], "Black")
grid on
2.- You allocate a specific group of code lines headed with % Function to precisely define the function.
Yet you actually define the function with Imin and Imax.
It's good practice to constrain the function definition within the lines you intend for such purpose, not to scatter the function all over the place.

Steepest Descent using Armijo rule

I want to determine the Steepest descent of the Rosenbruck function using Armijo steplength where x = [-1.2, 1]' (the initial column vector).
The problem is, that the code has been running for a long time. I think there will be an infinite loop created here. But I could not understand where the problem was.
Could anyone help me?
n=input('enter the number of variables n ');
% Armijo stepsize rule parameters
x = [-1.2 1]';
s = 10;
m = 0;
sigma = .1;
beta = .5;
obj=func(x);
g=grad(x);
k_max = 10^5;
k=0; % k = # iterations
nf=1; % nf = # function eval.
x_new = zeros([],1) ; % empty vector which can be filled if length is not known ;
[X,Y]=meshgrid(-2:0.5:2);
fx = 100*(X.^2 - Y).^2 + (X-1).^2;
contour(X, Y, fx, 20)
while (norm(g)>10^(-3)) && (k<k_max)
d = -g./abs(g); % steepest descent direction
s = 1;
newobj = func(x + beta.^m*s*d);
m = m+1;
if obj > newobj - (sigma*beta.^m*s*g'*d)
t = beta^m *s;
x = x + t*d;
m_new = m;
newobj = func(x + t*d);
nf = nf+1;
else
m = m+1;
end
obj=newobj;
g=grad(x);
k = k + 1;
x_new = [x_new, x];
end
% Output x and k
x_new, k, nf
fprintf('Optimal Solution x = [%f, %f]\n', x(1), x(2))
plot(x_new)
function y = func(x)
y = 100*(x(1)^2 - x(2))^2 + (x(1)-1)^2;
end
function y = grad(x)
y(1) = 100*(2*(x(1)^2-x(2))*2*x(1)) + 2*(x(1)-1);
end

The camera calibration "Dual Absolute Quadric" cost function isn't converging

I tried to implement the cost function of the Dual Absolute Quadric in Matlab according to the following equation mentioned in this paper, with this data.
My problem is that the results didn't converge.
The code is down.
main code
%---------------------
% clear and close all
%---------------------
clearvars
close all
clc
%---------------------
% Data type long
%---------------------
format long g
%---------------------
% Read data
%---------------------
load('data.mat')
%---------------------------
% Display The Initial Guess
%---------------------------
disp('=======================================================')
disp('Initial Intrinsic parameters: ');
disp(A);
disp('xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')
%=========================================================================
DualAbsoluteQuadric = Optimize(A,#DAQ);
%---------------------
% Display The Results
%---------------------
disp('=======================================================')
disp('Dual Absoute Quadric cost function: ');
disp(DualAbsoluteQuadric);
disp('xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')
The optimization function used is:
function output = Optimize(A,func)
%------------------------------
options = optimoptions('lsqnonlin','Algorithm','levenberg-marquardt',...
'Display','iter','FunctionTolerance',1e-16,'Tolx',1e-16,...
'MaxFunctionEvaluations', 1000, 'MaxIterations',39,...
'OptimalityTolerance',1e-16);
%------------------------------
% NonLinear Optimization
%------------------------------
output_line = lsqnonlin(func,[A(1,1), A(1,3), A(2,2), A(2,3), A(1,2)],...
[],[],options);
%------------------------------------------------------------------------
output = Reshape(output_line);
The Dual Absolute Quadric Function:
function cost = DAQ(params)
Aj = [params(1) params(5) params(2) ;
0 params(3) params(4) ;
0 0 1 ];
Ai = [params(1) params(5) params(2) ;
0 params(3) params(4) ;
0 0 1 ];
% W^-1 (IAC Image of the Absolute Conic)
W_inv = Ai * Aj';
%----------------
%Find plane at infinity from MQM' ~ w (Dual Absolute Quadric)
Plane_at_infinity = PlaneAtInfinity(W_inv);
%Find H_Infty = [e21]F+e21*n'
Homography_at_infty = H_Infty(Plane_at_infinity);
%----------------
% Initialization
%----------------
global Fs;
% Initialize the cost as a vector
% (N-1 * N-2)/2: 9*8/2 = 36
vector_size = (size(Fs,3)-1)*(size(Fs,4)-2)/2;
cost = zeros(1, vector_size);
% Cost Function
k = 0;
loop_size = 3 * vector_size;
Second_Term = W_inv / norm(W_inv,'fro');
for i=1:3:loop_size
k = k+1;
First_Term = Homography_at_infty(:,i:i+2) * W_inv * ((Homography_at_infty(:,i:i+2))');
First_Term = First_Term / norm(First_Term, 'fro');
cost(k) = norm(First_Term - Second_Term,'fro');
end
end
Plane at infinity function:
function P_infty = PlaneAtInfinity(W_inv)
global PPM;
% Symbolic variables
X = sym('X', 'real');
Y = sym('Y', 'real');
Z = sym('Z', 'real');
L2 = sym('L2','real');
n = [X; Y; Z];
% DAQ
Q = [W_inv , (W_inv * n) ;
(n' * W_inv) , (n' * W_inv * n)];
% Get one only camera matrix (any)
M = PPM(:, :, 3);
% Autocalibration equation
m = M * Q * M';
% solve linear equations
solution = solve(m(1, 1) == (L2 * W_inv(1, 1)), ...
m(2, 2) == (L2 * W_inv(2, 2)), ...
m(3, 3) == (L2 * W_inv(3, 3)), ...
m(1, 3) == (L2 * W_inv(1, 3)));
P_infty = [double(solution.X(1)) double(solution.Y(1))...
double(solution.Z(1))]';
Homography at infinity function:
function H_Inf = H_Infty(planeInf)
global Fs;
k = 1;
% (3 x 3) x ((N-1)*(N-2) /2)
H_Inf = zeros(3,3*(size(Fs,3)-1)*(size(Fs,4)-2)/2);%(3*3)*36
for i = 2:size(Fs,3)
for j = i+1:size(Fs,4)
[~, ~, V] = svd(Fs(:,:,i,j)');
epip = V(:,end);
H_Inf(:,k:k+2) = epipole(Fs(:,:,i,j)) * Fs(:,:,i,j)+ epip * planeInf';
k = k+3;
end
end
end
Reshape function:
function output = Reshape(input)
%---------------------
% Reshape Intrinsics
%---------------------
% K = [a skew u0 ;
% 0 B v0 ;
% 0 0 1 ];
output = [input(1) input(5) input(2) ;
0 input(3) input(4) ;
0 0 1 ];
end
Epipole Function:
function epip = epipole(Fs)
% SVD Decompostition of (Fs)^T
[~,~,V] = svd(Fs');
% Get the epipole from the last vector of the SVD
epi = V(:,end);
% Reshape the Vector into Matrix
epip = [ 0 -epi(3) epi(2);
epi(3) 0 -epi(1);
-epi(2) epi(1) 0 ];
end
The plane at infinity has to be calculated as following:
function plane = computePlaneAtInfinity(P, K)
%Input
% P - Projection matrices
% K - Approximate Values of Intrinsics
%
%Output
% plane - coordinate of plane at infinity
% Compute the DIAC W^-1
W_invert = K * K';
% Construct Symbolic Variables to Solve for Plane at Infinity
% X,Y,Z is the coordinate of plane at infinity
% XX is the scale
X = sym('X', 'real');
Y = sym('Y', 'real');
Z = sym('Z', 'real');
XX = sym('XX', 'real');
% Define Normal to Plane at Infinity
N = [X; Y; Z];
% Equation of Dual Absolute Quadric (DAQ)
Q = [W_invert, (W_invert * N); (N' * W_invert), (N' * W_invert * N)];
% Select Any One Projection Matrix
M = P(:, :, 2);
% Left hand side of the equation
LHS = M * Q * M';
% Solve for [X, Y, Z] considering the System of Linear Equations
% We need 4 equations for 4 variables X,Y,Z,XX
S = solve(LHS(1, 1) == (XX * W_invert(1, 1)), ...
LHS(1, 2) == (XX * W_invert(1, 2)), ...
LHS(1, 3) == (XX * W_invert(1, 3)), ...
LHS(2, 2) == (XX * W_invert(2, 2)));
plane = [double(S.X(1)); double(S.Y(1)); double(S.Z(1))];
end

All my weights for gradient descent become 0 on feature expansion

I have 2 features which I expand to contain all possible combinations of the two features under order 6. When I do MATLAB's fminunc, it returns a weight vector where all elements are 0.
The dataset is here
clear all;
clc;
data = load("P2-data1.txt");
m = length(data);
para = 0; % regularization parameter
%% Augment Feature
y = data(:,3);
new_data = newfeature(data(:,1), data(:,2), 3);
[~, n] = size(new_data);
betas1 = zeros(n,1); % initial weights
options = optimset('GradObj', 'on', 'MaxIter', 400);
[beta_new, cost] = fminunc(#(t)(regucostfunction(t, new_data, y, para)), betas1, options);
fprintf('Cost at theta found by fminunc: %f\n', cost);
fprintf('theta: \n');
fprintf(' %f \n', beta_new); % get all 0 here
% Compute accuracy on our training set
p_new = predict(beta_new, new_data);
fprintf('Train Accuracy after feature augmentation: %f\n', mean(double(p_new == y)) * 100);
fprintf('\n');
%% the functions are defined below
function g = sigmoid(z) % running properly
g = zeros(size(z));
g=ones(size(z))./(ones(size(z))+exp(-z));
end
function [J,grad] = regucostfunction(theta,x,y,para) % CalculateCost(x1,betas1,y);
m = length(y); % number of training examples
J = 0;
grad = zeros(size(theta));
hyp = sigmoid(x*theta);
err = (hyp - y)';
grad = (1/m)*(err)*x;
sum = 0;
for k = 2:length(theta)
sum = sum+theta(k)^2;
end
J = (1/m)*((-y' * log(hyp) - (1 - y)' * log(1 - hyp)) + para*(sum) );
end
function p = predict(theta, X)
m = size(X, 1); % Number of training examples
p = zeros(m, 1);
index = find(sigmoid(theta'*X') >= 0.5);
p(index,1) = 1;
end
function out = newfeature(X1, X2, degree)
out = ones(size(X1(:,1)));
for i = 1:degree
for j = 0:i
out(:, end+1) = (X1.^(i-j)).*(X2.^j);
end
end
end
data contains 2 columns of rows followed by a third column of 0/1 values.
The functions used are: newfeature returns the expanded features and regucostfunction computes the cost. When I did the same approach with the default features, it worked and I think the problem here has to do with some coding issue.

`ode45` and tspan error Attempted to access

I'm using ode45 to solve second order differential equation. the time span is determined based on how many numbers in txt file, therefore, the time span is defined as follows
i = 1;
t(i) = 0;
dt = 0.1;
numel(theta_d)
while ( i < numel(theta_d) )
i = i + 1;
t(i) = t(i-1) + dt;
end
Now the time elements should not exceed the size of txt (i.e. numel(theta_d)). In main.m, I have
x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t, x] = ode45('ODESolver', t, x0, options);
and ODESolver.m header is
function dx = ODESolver(t, x)
If I run the code, I'm getting this error
Attempted to access theta_d(56); index out of bounds because numel(theta_d)=55.
Error in ODESolver (line 29)
theta_dDot = ( theta_d(i) - theta_dPrev ) / dt;
Why the ode45 is not being fixed with the time span?
Edit: this is the entire code
main.m
clear all
clc
global error theta_d dt;
error = 0;
theta_d = load('trajectory.txt');
i = 1;
t(i) = 0;
dt = 0.1;
numel(theta_d)
while ( i < numel(theta_d) )
i = i + 1;
t(i) = t(i-1) + dt;
end
x0 = [pi/4; 0];
options= odeset('Reltol',dt,'Stats','on');
[t, x] = ode45(#ODESolver, t, x0, options);
%e = x(:,1) - theta_d; % Error theta
plot(t, x(:,2), 'r', 'LineWidth', 2);
title('Tracking Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on
and ODESolver.m
function dx = ODESolver(t, x)
persistent i theta_dPrev
if isempty(i)
i = 1;
theta_dPrev = 0;
end
global error theta_d dt ;
dx = zeros(2,1);
%Parameters:
m = 0.5; % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1; % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81; % acceleration due to gravity m/s^2
% PID tuning
Kp = 5;
Kd = 1.9;
Ki = 0.02;
% theta_d first derivative
theta_dDot = ( theta_d(i) - theta_dPrev ) / dt;
theta_dPrev = theta_d(i);
% u: joint torque
u = Kp*(theta_d(i) - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_dDot - x(1));
dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));
i = i + 1;
end
and this is the error
Attempted to access theta_d(56); index out of bounds because numel(theta_d)=55.
Error in ODESolver (line 28)
theta_dDot = ( theta_d(i) - theta_dPrev ) / dt;
Error in ode45 (line 261)
f(:,2) = feval(odeFcn,t+hA(1),y+f*hB(:,1),odeArgs{:});
Error in main (line 21)
[t, x] = ode45(#ODESolver, t, x0, options);
The problem here is because you have data at discrete time points, but ode45 needs to be able to calculate the derivative at any time point in your time range. Once it solves the problem, it will interpolate the results back onto your desired time points. So it will calculate the derivative many times more than at just the time points you specified, thus your i counter will not work at all.
Since you have discrete data, the only way to proceed with ode45 is to interpolate theta_d to any time t. You have a list of values theta_d corresponding to times 0:dt:(dt*(numel(theta_d)-1)), so to interpolate to a particular time t, use interp1(0:dt:(dt*(numel(theta_d)-1)),theta_d,t), and I turned this into an anonymous function to give the interpolated value of theta_p at a given time t
Then your derivative function will look like
function dx = ODESolver(t, x,thetaI)
dx = zeros(2,1);
%Parameters:
m = 0.5; % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1; % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81; % acceleration due to gravity m/s^2
% PID tuning
Kp = 5;
Kd = 1.9;
Ki = 0.02;
% theta_d first derivative
dt=1e-4;
theta_dDot = (thetaI(t) - theta(I-dt)) / dt;
%// Note thetaI(t) is the interpolated theta_d values at time t
% u: joint torque
u = Kp*(thetaI(t) - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_dDot - x(1));
dx=[x(2); 1/I*(u - d*x(2) - m*g*L*sin(x(1)))];
end
and you will have to define thetaI=#(t) interp1(0:dt:(dt*(numel(theta_d)-1)),theta_d,t); before calling ode45 using [t, x] = ode45(#(t,x) ODESolver(t,x,thetaI, t, x0, options);.
I removed a few things from ODESolver and changed how the derivative was computed.
Note I can't test this, but it should get you on the way.