Related
Consider the following problem:
I am now in the third part of this question. I wrote the vectorial loop equations (q=teta2, x=teta3 and y=teta4):
fval(1,1) = r2*cos(q)+r3*cos(x)-r4*cos(y)-r1;
fval(2,1) = r2*sin(q)+r3*sin(x)-r4*sin(y);
I have these 2 functions, and all variables except x and y are given. I found the roots with help of this video.
Now I need to plot graphs of q versus x and q versus y when q is at [0,2pi] with delta q of 2.5 degree. What should I do to plot the graphs?
Below is my attempt so far:
function [fval,jac] = lorenzSystem(X)
%Define variables
x = X(1);
y = X(2);
q = pi/2;
r2 = 15
r3 = 50
r4 = 45
r1 = 40
%Define f(x)
fval(1,1)=r2*cos(q)+r3*cos(x)-r4*cos(y)-r1;
fval(2,1)=r2*sin(q)+r3*sin(x)-r4*sin(y);
%Define Jacobian
jac = [-r3*sin(X(1)), r4*sin(X(2));
r3*cos(X(1)), -r4*cos(X(2))];
%% Multivariate NR
%Initial conditions:
X0 = [0.5;1];
maxIter = 50;
tolX = 1e-6;
X = X0;
Xold = X0;
for i = 1:maxIter
[f,j] = lorenzSystem(X);
X = X - inv(j)*f;
err(:,i) = abs(X-Xold);
Xold = X;
if (err(:,i)<tolX)
break;
end
end
Please take a look at my solution below, and study how it differs from your own.
function [th2,th3,th4] = q65270276()
[th2,th3,th4] = lorenzSystem();
hF = figure(); hAx = axes(hF);
plot(hAx, deg2rad(th2), deg2rad(th3), deg2rad(th2), deg2rad(th4));
xlabel(hAx, '\theta_2')
xticks(hAx, 0:pi/3:2*pi);
xticklabels(hAx, {'$0$','$\frac{\pi}{3}$','$\frac{2\pi}{3}$','$\pi$','$\frac{4\pi}{3}$','$\frac{5\pi}{3}$','$2\pi$'});
hAx.TickLabelInterpreter = 'latex';
yticks(hAx, 0:pi/6:pi);
yticklabels(hAx, {'$0$','$\frac{\pi}{6}$','$\frac{\pi}{3}$','$\frac{\pi}{2}$','$\frac{2\pi}{3}$','$\frac{5\pi}{6}$','$\pi$'});
set(hAx, 'XLim', [0 2*pi], 'YLim', [0 pi], 'FontSize', 16);
grid(hAx, 'on');
legend(hAx, '\theta_3', '\theta_4')
end
function [th2,th3,th4] = lorenzSystem()
th2 = (0:2.5:360).';
[th3,th4] = deal(zeros(size(th2)));
% Define geometry:
r1 = 40;
r2 = 15;
r3 = 50;
r4 = 45;
% Define the residual:
res = #(q,X)[r2*cosd(q)+r3*cosd(X(1))-r4*cosd(X(2))-r1; ... Δx=0
r2*sind(q)+r3*sind(X(1))-r4*sind(X(2))]; % Δy=0
% Define the Jacobian:
J = #(X)[-r3*sind(X(1)), r4*sind(X(2));
r3*cosd(X(1)), -r4*cosd(X(2))];
X0 = [acosd((45^2-25^2-50^2)/(-2*25*50)); 180-acosd((50^2-25^2-45^2)/(-2*25*45))]; % Accurate guess
maxIter = 500;
tolX = 1e-6;
for idx = 1:numel(th2)
X = X0;
Xold = X0;
err = zeros(maxIter, 1); % Preallocation
for it = 1:maxIter
% Update the guess
f = res( th2(idx), Xold );
X = Xold - J(Xold) \ f;
% X = X - pinv(J(X)) * res( q(idx), X ); % May help when J(X) is close to singular
% Determine convergence
err(it) = (X-Xold).' * (X-Xold);
if err(it) < tolX
break
end
% Update history
Xold = X;
end
% Unpack and store θ₃, θ₄
th3(idx) = X(1);
th4(idx) = X(2);
% Update X0 for faster convergence of the next case:
X0 = X;
end
end
Several notes:
All computations are performed in degrees.
The specific plotting code I used is less interesting, what matters is that I defined all θ₂ in advance, then looped over them to find θ₃ and θ₄ (without recursion, as was done in your own implementation).
The initial guess (actually, analytical solution) for the very first case (θ₂=0) can be found by solving the problem manually (i.e. "on paper") using the law of cosines. The solver also works for other guesses, but you might need to increase maxIter. Also, for certain guesses (e.g. X(1)==X(2)), the Jacobian is ill-conditioned, in which case you can use pinv.
If my computation is correct, this is the result:
function [x1] = tutorial1(x0,nMax,tol)
% Calculate the root of the function f(x) = x^3 - 3^x + 1
% using the Newton Method of root-finding.
% Inputs:
% - x0 initial guess
% - nMax number of iterations
% - tol solution accuracy tolerance
% Output:
% - x1 converged root of the equation
% Initialisation of the values x0, nMax, to
x0 = 1.5;
nMax = 15;
tol = 1e-4;
% for loop acting continously for 15 iterations
for i = 1:nMax
fx0= (x0).^3-3.^(x0)+1;
differentialx0=3.*(x0.^2) - 3.^x0.*log(3);
%function f(x0)respectively
x1 = x0 -fx0./differentialx0;
if abs(x1-x0)<tol
break
end
x0 = x1;
fprintf('Iteration = %d, x0 = %.4f, x1 = %.4f, fx1 = %.4f\n',i,x0,x1);
end
% Sample output code for monitoring (this should be included in your loop structure.
return
You are getting the right value actually. With this kind of method, you are numerically solving an equation. So you will converge to the solution with the precision you want (if it works) set by the tolerance criteria.
Execute the code below. It is the same with more digits for the number printing, and a more strict tolerance, and a print of the error (and I added fx1 computation to print it too).
x0 = 1.5;
nMax = 15;
tol = 1e-10; %1e-4
% for loop acting continously for 15 iterations
for i = 1:nMax
fx0= (x0).^3-3.^(x0)+1;
differentialx0=3.*(x0.^2) - 3.^x0.*log(3);
%function f(x0)respectively
x1 = x0 -fx0./differentialx0;
fx1= (x0).^3-3.^(x0)+1;
error = abs(x1-x0) ;
if error<tol
break
end
x0 = x1;
fprintf('Iteration = %i, x0 = %.6f, x1 = %.6f, fx1 = %.6f, error = %.6f \n',i,x0,x1,fx1,error);
end
% Sample output code for monitoring (this should be included in your loop structure.
You will get this output in the command :
Iteration = 1, x0 = 2.288476, x1 = 2.288476, fx1 = -0.821152, error = 0.788476
Iteration = 2, x0 = 1.994137, x1 = 1.994137, fx1 = 0.628953, error = 0.294339
Iteration = 3, x0 = 2.000009, x1 = 2.000009, fx1 = -0.012367, error = 0.005873
Iteration = 4, x0 = 2.000000, x1 = 2.000000, fx1 = 0.000020, error = 0.000009
It gives a solution closer to 2 (yet if you add more digits to fprintf, you will see that it is not a perfect 2 but something like 2.000000000001).
Keep in mind that, if the algorithm converges, it will give you a numerical solution close to theoretical solution with an error which depends on the tolerance you provide
I am trying to plot a state-space diagram, as well as a time-history diagram, of a dynamical system. There's a catch, though. The state-space is divided into two halves by a plane located at x1 = 0. The state-space axes are x1, x2, x3. The x1 = 0 plane is parallel to the x2/x3 plane. The state-space above the x1 = 0 plane is described by the ODEs in eqx3, whereas the state-space below the x1 = 0 plane is described by the ODEs in eqx4.
So, there is a discontinuity on the plane x1 = 0. I have a vague understanding that an event function (function [value,isterminal,direction] = myEventsFcn(t,y)) should be used, but I do not know what values to give to "value", "isterminal", and "direction".
In my code below, I have an initial condition for eqx3, and another initial condition for eqx4. The initial condition for eqx3 is in the upper half of the state-space (x1 > 0). The orbit then hits the x1 = 0 plane and there is a discontinuity, and the eqx4 trajectory begins on this plane, but from a different point from where eqx3 ended.
Can this be done? How do I put it into a code? Do I stop the integration when the orbit reaches the plane x1 = 0?
eta = 0.05;
omega = 25;
tspan = [0,50];
initcond = [2, 3, 4]
[t,x] = ode45(#(t,x) eqx3(t,x,eta, omega), tspan, initcond);
initcond1 = [0, 1, 1]
[t,y] = ode45(#(t,y) eqx4(t,y,eta, omega), tspan, initcond1);
plot3(x(:,1), x(:,2), x(:,3),y(:,1), y(:,2), y(:,3))
xlabel('x1')
ylabel('x2')
zlabel('x3')
%subplot(222)
%plot(t, x(:,1), t,x(:,2),t,x(:,3),'--');
%xlabel('t')
function xdot = eqx3(t,x,eta,omega)
xdot = zeros(3,1);
xdot(1) = -(2*eta*omega + 1)*x(1) + x(2) - 1;
xdot(2) = -(2*eta*omega + (omega^2))*x(1) + x(3) + 2;
xdot(3) = -(omega^2)*x(1) + x(2) - 1;
end
function ydot = eqx4(t,y,eta,omega)
ydot = zeros(3,1);
ydot(1) = -(2*eta*omega + 1)*y(1) + y(2) + 1;
ydot(2) = -(2*eta*omega + (omega^2))*y(1) + y(3) - 2;
ydot(3) = -(omega^2)*y(1) + y(2) + 1;
end
function [value,isterminal,direction] = myEventsFcn(t,y)
value = 0
isterminal = 1
direction = 1
end
Without events, using a close-by smooth system
First, as the difference between the systems is the addition or subtraction of a constant vector it is easy to find an approximate smooth version of the system using some sigmoid function like tanh.
eta = 0.05;
omega = 25;
t0=0; tf=4;
initcond = [2, 3, 4];
opt = odeset('AbsTol',1e-11,'RelTol',1e-13);
opt = odeset(opt,'MaxStep',0.005); % in Matlab: opt = odeset('Refine',10);
[t,x] = ode45(#(t,x) eqx34(t,x,eta, omega), [t0, tf], initcond, opt);
clf;
subplot(121);
plot3(x(:,1), x(:,2), x(:,3));
xlabel('x1');
ylabel('x2');
zlabel('x3');
subplot(122);
plot(t, 10.*x(:,1), t,x(:,2),':',t,x(:,3),'--');
xlabel('t');
function xdot = eqx34(t,x,eta,omega)
S = tanh(1e6*x(1));
xdot = zeros(3,1);
xdot(1) = -(2*eta*omega + 1)*x(1) + x(2) - S;
xdot(2) = -(2*eta*omega + (omega^2))*x(1) + x(3) + 2*S;
xdot(3) = -(omega^2)*x(1) + x(2) - S;
end
resulting in the plots
As is visible, after t=1.2 the solution is essentially constant with x(1)=0 and the other coordinates close to zero.
With events
If you want to use the event mechanism, make ODE and event function dependent on a sign parameter S denoting the phase and the direction of the zero crossing.
eta = 0.05;
omega = 25;
t0=0; tf=4;
initcond = [2, 3, 4];
opt = odeset('AbsTol',1e-10,'RelTol',1e-12,'InitialStep',1e-6);
opt = odeset(opt,'MaxStep',0.005); % in Matlab: opt = odeset('Refine',10);
T = [t0]; TE = [];
X = [initcond]; XE = [];
S = 1; % sign of x(1)
while t0<tf
opt = odeset(opt,'Events', #(t,x)myEventsFcn(t,x,S));
[t,x,te,xe,ie] = ode45(#(t,x) eqx34(t,x,eta, omega, S), [t0, tf], initcond, opt);
T = [ T; t(2:end) ]; TE = [ TE; te ];
X = [ X; x(2:end,:)]; XE = [ XE; xe ];
t0 = t(end);
initcond = x(end,:);
S = -S % alternatively = 1-2*(initcond(1)<0);
end
disp(TE); disp(XE);
subplot(121);
hold on;
plot3(X(:,1), X(:,2), X(:,3),'b-');
plot3(XE(:,1), XE(:,2), XE(:,3),'or');
hold off;
xlabel('x1');
ylabel('x2');
zlabel('x3');
subplot(122);
plot(T, 10.*X(:,1), T,X(:,2),':',T,X(:,3),'--');
xlabel('t');
function xdot = eqx34(t,x,eta,omega,S)
xdot = zeros(3,1);
xdot(1) = -(2*eta*omega + 1)*x(1) + x(2) - S;
xdot(2) = -(2*eta*omega + (omega^2))*x(1) + x(3) + 2*S;
xdot(3) = -(omega^2)*x(1) + x(2) - S;
end
function [value,isterminal,direction] = myEventsFcn(t,x,S)
value = x(1)+2e-4*S;
isterminal = 1;
direction = -S;
end
The solution enters a sliding mode for 1.36 < t < 1.43 where (theoretically) x(1)=0 and the vector field points to the other phase from both sides. The theoretical solution is to take a linear combination that sets the first component to zero, so that the resulting direction is tangential to the separating surface. In the first variant above the sigmoid achieves something like this automatically. Using events one could add additional event functions that test the vector field for these conditions, and when they cease to persist.
A quick solution is to thicken the boundary surface, that is, test for x(1)+epsilon*S==0 so that the solution has to cross the boundary surface before triggering the event. In sliding mode, it will be immediately pushed back, giving a ping-pong or zigzag motion. epsilon has to be small to not perturb the solution too much, however not too small to avoid the triggering of too many events. With epsilon=2e-4 octave gives the following solution in a close-up to the sliding interval
The octave solver, and in some way also Matlab, will not trigger a terminal event if it happens in the first integration step. For that reason the InitialStep option was set to a rather small value, it should be about 0.01*epsilon. The full solution looks now similar to the one obtained in the first variant
I have to fit the data of a given equation to other data in the blue line shown in picture attached.
The initial values for r and c are 0.004 and 40 respectively. The result values for r and c will be 0.0068 and 54.23 respectively. The equation are provided below.
I am not getting the fit graph. I don't know where I am making a mistake. Red circles are I think my fitting result. Here is my working on Matlab.
Function file:
function y = FOM2(t,r,c)
t=(0:0.01:2);
q=zeros(1,length(t));
for i=1:length(t)
if (t(i) < 1)
q(i) = 600;
else
q(i) = 0;
end
y= q*(r+(1/c));
end
Error value function:
function error=RCvalues(x,time,data)
r = x(1); c = x(2);
test = FOM2(time,r,c);
d= data - test;
error = sum(d.^2);
Fitting file:
Data = load ('Paw.txt'); %Load measurement data
Time = load ('time.txt');
Data=data';
Time=ime';
opt = optimset('MaxFunEvals',10000,'TolFun',1e-6);
x = fminsearch(#(x) RCvalues(x,Time,Data),[0.004 40],opt);
r = x(1);
c = x(2);
yy= FOM2(Time,r,c);
fprintf('Final values:\n');
fprintf('r %7.3f\n',x(1));
fprintf('c %7.3f\n',x(2));
plot(Time,Data)
hold on
plot(Time,yy,'o');
Here are the equations,
totaltime is 2 sec
dPalv/dt=q/c
Paw= r*q+palv
I have to use Paw equation but don't know how.
I am trying to simulate the rotation dynamics of a system. I am testing my code to verify that it's working using simulation, but I never recovered the parameters I pass to the model. In other words, I can't re-estimate the parameters I chose for the model.
I am using MATLAB for that and specifically ode45. Here is my code:
% Load the input-output data
[torque outputs] = DataLogs2();
u = torque;
% using the simulation data
Ixx = 1.00;
Iyy = 2.00;
Izz = 3.00;
x0 = [0; 0; 0];
Ts = .02;
t = 0:Ts:Ts * ( length(u) - 1 );
[ T, x ] = ode45( #(t,x) rotationDyn( t, x, u(1+floor(t/Ts),:), Ixx, Iyy, Izz), t, x0 );
w = x';
N = length(w);
q = 1; % a counter for the A and B matrices
% The Algorithm
for k=1:1:N
w_telda = [ 0 -w(3, k) w(2,k); ...
w(3,k) 0 -w(1,k); ...
-w(2,k) w(1,k) 0 ];
if k == N % to handle the problem of the last iteration
w_dash(:,k) = (-w(:,k))/Ts;
else
w_dash(:,k) = (w(:,k+1)-w(:,k))/Ts;
end
a = kron( w_dash(:,k)', eye(3) ) + kron( w(:,k)', w_telda );
A(q:q+2,:) = a; % a 3N*9 matrix
B(q:q+2,:) = u(k,:)'; % a 3N*1 matrix % u(:,k)
q = q + 3;
end
% Forcing J to be diagonal. This is the case when we consider our quadcopter as two thin uniform
% rods crossed at the origin with a point mass (motor) at the end of each.
A_new = [A(:, 1) A(:, 5) A(:, 9)];
vec_J_diag = A_new\B;
J_diag = diag([vec_J_diag(1), vec_J_diag(2), vec_J_diag(3)])
eigenvalues_J_diag = eig(J_diag)
error = norm(A_new*vec_J_diag - B)
where my dynamic model is defined as:
function [dw, y] = rotationDyn(t, w, tau, Ixx, Iyy, Izz, varargin)
% The output equation
y = [w(1); w(2); w(3)];
% State equation
% dw = (I^-1)*( tau - cross(w, I*w) );
dw = [Ixx^-1 * tau(1) - ((Izz-Iyy)/Ixx)*w(2)*w(3);
Iyy^-1 * tau(2) - ((Ixx-Izz)/Iyy)*w(1)*w(3);
Izz^-1 * tau(3) - ((Iyy-Ixx)/Izz)*w(1)*w(2)];
end
Practically, what this code should do, is to calculate the eigenvalues of the inertia matrix, J, i.e. to recover Ixx, Iyy, and Izz that I passed to the model at the very begining (1, 2 and 3), but all what I get is wrong results.
Is the problem with using ode45?
Well the problem wasn't in the ode45 instruction, the problem is that in system identification one can create an n-1 samples-signal from an n samples-signal, thus the loop has to end at N-1 in the above code.