I have difficulties simulating an object discribed by the following state space equations in simulink:
The right hand side of the state space equation is described by the funcion below.
function dxdt = RHS( t, x, F)
% parameters
b = 1.22; % cart friction coeffitient
c = 0.0027; %pendulum friction coeffitient
g = 9.81; % gravity
M = 0.548+0.022*2; % cart weight
m = 0.031*2; %pendulum masses
I = 0.046;%0.02*0.025/12+0.02*0.12^2+0.011*0.42^2; % moment of inertia
l = 0.1313;
% x(1) = theta
% x(2) = theta_dot
% x(3) = x
% x(4) = x_dot
dxdt = [x(2);
(-(M+m)*c*x(2)-(M+m)*g*l*sin(x(1))-m^2*l^2*x(2)^2*sin(x(1))*cos(x(1))+m*l*b*x(4)*cos(x(1))-m*l*cos(x(1))*F)/(I*(m+M)+m*M*l^2+m^2*l^2*sin(x(1))^2);
x(4);
(F - b*x(4) + l*m*x(2)^2*sin(x(1)) + (l*m*cos(x(1))*(c*x(2)*(M + m) + g*l*sin(x(1))*(M + m) + F*l*m*cos(x(1)) + l^2*m^2*x(2)^2*cos(x(1))*sin(x(1)) - b*l*m*x(4)*cos(x(1))))/(I*(M + m) + l^2*m^2*sin(x(1))^2 + M*l^2*m))/(M + m)];
end
The coresponding rk4 function with a simple visualisation is shown below.
function [wi, ti] = rk4 ( RHS, t0, x0, tf, N )
%RK4 approximate the solution of the initial value problem
%
% x'(t) = RHS( t, x ), x(t0) = x0
%
% using the classical fourth-order Runge-Kutta method - this
% routine will work for a system of first-order equations as
% well as for a single equation
%
% calling sequences:
% [wi, ti] = rk4 ( RHS, t0, x0, tf, N )
% rk4 ( RHS, t0, x0, tf, N )
%
% inputs:
% RHS string containing name of m-file defining the
% right-hand side of the differential equation; the
% m-file must take two inputs - first, the value of
% the independent variable; second, the value of the
% dependent variable
% t0 initial value of the independent variable
% x0 initial value of the dependent variable(s)
% if solving a system of equations, this should be a
% row vector containing all initial values
% tf final value of the independent variable
% N number of uniformly sized time steps to be taken to
% advance the solution from t = t0 to t = tf
%
% output:
% wi vector / matrix containing values of the approximate
% solution to the differential equation
% ti vector containing the values of the independent
% variable at which an approximate solution has been
% obtained
%
% x(1) = theta
% x(2) = theta_dot
% x(3) = x
% x(4) = x_dot
t0 = 0; tf = 5; x0 = [pi/2; 0; 0; 0]; N = 400;
neqn = length ( x0 );
ti = linspace ( t0, tf, N+1 );
wi = [ zeros( neqn, N+1 ) ];
wi(1:neqn, 1) = x0';
h = ( tf - t0 ) / N;
% force
u = 0.0;
%init visualisation
h_cart = plot(NaN, NaN, 'Marker', 'square', 'color', 'red', 'LineWidth', 6);
hold on
h_pend = plot(NaN, NaN, 'bo', 'LineWidth', 3);
axis([-5 5 -5 5]);
axis manual;
xlim([-5 5]);
ylim([-5 5]);
for i = 1:N
k1 = h * feval ( 'RHS', t0, x0, u );
k2 = h * feval ( 'RHS', t0 + (h/2), x0 + (k1/2), u);
k3 = h * feval ( 'RHS', t0 + h/2, x0 + k2/2, u);
k4 = h * feval ( 'RHS', t0 + h, x0 + k3, u);
x0 = x0 + ( k1 + 2*k2 + 2*k3 + k4 ) / 6;
t0 = t0 + h;
% model output
wi(1:neqn,i+1) = x0';
% model visualisation
%plotting cart
l = 2;
set(h_cart, 'XData', x0(3), 'YData', 0, 'LineWidth', 5);
%plotting pendulum
%hold on;
set(h_pend, 'XData', sin(x0(1))*l+x0(3), 'YData', -cos(x0(1))*l, 'LineWidth', 2);
%hold off;
% regulator
pause(0.02);
end;
figure;
plot(ti, wi);
legend('theta', 'theta`', 'x', 'x`');
This gives realistic looking results for a pendulum on a cart.
Now to the problem.
I wanted to recreate the exact same equations in simulink. I thought it is going to be as easy as creating the following simulink model.
where I fill the fcn blocks with the second and fourth equation from the RHS file. Like this.
(-(M+m)*c*u(2)-(M+m)*g*l*sin(u(1))-m^2*l^2*u(2)^2*sin(u(1))*cos(u(1))+m*l*b*u(3)*cos(u(1))-m*l*cos(u(1))*u(4))/(I*(m+M)+m*M*l^2+m^2*l^2*sin(u(1))^2)
(u(5) - b*u(4) + l*m*u(2)^2*sin(u(1)) + (l*m*cos(u(1))*(c*u(2)*(M + m) + g*l*sin(u(1))*(M + m) + u(5)*l*m*cos(u(1)) + l^2*m^2*u(2)^2*cos(u(1))*sin(u(1)) - b*l*m*u(4)*cos(u(1))))/(I*(M + m) + l^2*m^2*sin(u(1))^2 + M*l^2*m))/(M + m)
The problem is this doesn't give the correct results from above, but the one below
Does anybody know what I do incorrectly?
Edit:After #am304 comment I decided to add the following information. I changed the setting for the simulink solver to use the fixed-step rk4 solver, so as to get the same results. The second integrator3 from the model above has been initialized to pi/2.
Edit2: If somebody wants to check out the simulink model for themselves click on the link to download the file.
Edit3: As you can read in the answer below the problem was trivial. You can download the correct model here
I looked through your Simulink model and it seems you may have mixed up the two functions you were using. You used the theta_dd function where you meant to put x_dd and vice versa.
In your model, you also force x_d to be set to a constant value 0. I assume you actually meant to set the initial condition to 0, which you can see is done via the Integrator block. x_d (as an input to f) should be your state vector which is also an output of your integrators. This is just a consequence of what you define x_d to be, the integral of x_dd. This is how RK4 works as well; you use an initial state vector first and then use the predicted state vector to drive the next RK4 step.
The resulting output from the scope (i've outputted your whole state vector here) is as follows and looks like what you expect:
I do not think I should link externally to the simulink file so if you would like a copy of the file you can open a chat and ask for it. Otherwise the picture above should be sufficient enough to help you reproduce the same results.
Related
I have stumbled upon this matlab code that solves this ODE
y'''(t) + a y(t) = -b y''(t) + u(t)
but I am confused by the ode_system function definition, specifically by the y(2) y(3) part. I would greatly appreciate if someone can shed some light
y(2) y(3) part in the ode_system function confuses me and how it contributes to overaal solution
% Define the parameters a and b
a = 1;
b = 2;
% Define the time horizon [0,1]
time_horizon = [0, 1];
% Define the initial conditions for y, y', and y''
initials = [0; 0; 0];
% Define the function handle for the input function u(t)
%sin(t) is a common example of a time-varying function.
% You can change the definition of u to any other function of time,
% such as a constant, a step function, or a more complex function, depending on your needs
u = #(t) sin(t);
% Define the function handle for the system of ODEs
odefunction = #(t, y) ode_system(t, y, a, b, u);
% Solve the ODEs using ode45
[t, y] = ode45(odefunction, time_horizon, initials);
% Plot the solution
plot(t, y(:,1), '-', 'LineWidth', 2);
xlabel('t');
ylabel('y');
function dydt = ode_system(t, y, a, b, u)
%Define the system of ODEs
dydt = [y(2); y(3); -b*y(3) + u(t) - a*y(1)];
end
This is more of a maths question than a Matlab one.
We would like to rewrite our ODE equation so that there is a single time derivative on the left-hand side and no derivatives on the right.
Currently we have:
y'''(t)+ay(t)=-by''(t)+u(t)
By letting z = y' and x = z' (= y''), we can rewrite this as:
x'(t)+a y(t)=-b x(t)+u(t)
So now we have 3 equations in the form:
y' = z
z' = x
x' = -b * x + u - a *y
We can also think of this as a vector equation where v = (y, z, x).
The right-hand side would then be,
v(1)' = v(2)
v(2)' = v(3)
v(3)' = -b * v(3) + u - a * v(1)
which is what you have in the question.
The exact solution of the damped harmonic oscillator
$$x'' + 2\gamma x' + \omega^2 x = 0, \quad x(0)=x_0, \quad x'(0)=-\gamma x_0$$
with $0 < \gamma < \omega$ is
$$x(t)= x_0 e^{-\gamma t} \cos(\beta t) \quad \text{where} \quad \beta:=\sqrt{\omega^2 - \gamma^2}$$
Notice that this second order ODE can be written as a first order system by making the substitutions:
$x' = y$ and,
$y' = -2\gamma y - \omega^2 x$
I want to solve the system using the method:
$$\dfrac{ x_{n+1} - x_{n-1} }{2h} = y_n \quad \quad \dfrac{y_{n+1} - y_{n-1}}{2h} = -2\gamma y_n - \omega^2 x_n.$$
which is an explicit midpoint rule. This is the code that I constructed for the problem, but it is not giving me the correct result. My plot has no harmonic behavior as I would anticipate.
function resonance
omega = 1; % resonant frequency = sqrt(k/m)
a = 0.2; % drag coeficient per unit mass
b = 0.1; % driving amplitude per unit mass
omega0 = 1.2; % driving frequency
tBegin = 0; % time begin
tEnd = 80; % time end
x0 = 0.2; % initial position
v0 = 0.8; % initial velocity
a = omega^2; % calculate a coeficient from resonant frequency
% Use Runge-Kutta 45 integrator to solve the ODE
[t,w] = ode45(#derivatives, [tBegin tEnd], [x0 v0]);
x = w(:,1); % extract positions from first column of w matrix
v = w(:,2); % extract velocities from second column of w matrix
plot(t,x);
title('Damped, Driven Harmonic Oscillator');
ylabel('position (m)');
xlabel('time (s)');
% Function defining derivatives dx/dt and dv/dt
% uses the parameters a, b, A, omega0 in main program but changeth them not
function derivs = derivatives(tf,wf)
xf = wf(1); % wf(1) stores x
vf = wf(2); % wf(2) stores v
dxdt = vf; % set dx/dt = velocity
dvdt = xf + 2 * b * vf + a * tf; % set dv/dt = acceleration
derivs = [dxdt; dvdt]; % return the derivatives
end
end
Also, my apologies about the formatting. I am use to math stackexchange, and the LaTeX style formatting doesn't seem to be applicable here and I do not know how to put my math in the math environment.
You missed a sign, it should be
dvdt = - ( xf + 2 * b * vf + a * tf ); % set dv/dt = acceleration
However, the whole expression is at odds with the previously stated equation,
x'' + 2*b*x' * a*x = 0
should result in
dvdt = - ( 2*b*vf + a*xf ); % set dv/dt = acceleration
But then again you have defined a twice, so change w2=omega^2 to get
dvdt = - ( 2*b*vf + w2*xf + a ); % set dv/dt = acceleration
I updated the question to clarify it more. Here is a graph:
For the curve in the attached photo, I hope to draw the curve. I have its equation and it is after simplification will be like this one
% Eq-2
(b*Y* cos(v) + c - k*X*sin(v))^2 + ...
sqrt(k*X*(cos(v) + 1.0) + b*Y*sin(v))^2) - d = 0.0
Where:
v = atan((2.0*Y)/X) + c
and b, c, d and k are constants.
from the attached graph,
The curve is identified in two points:
p1 # (x=0)
p2 # (y=0)
I a new on coding so accept my apologize if my question is not clear.
Thanks
So, after your edit, it is a bit more clear what you want.
I insist that your equation needs work -- the original equation (before your edit) simplified to what I have below. The curve for that looks like your plot, except the X and Y intercepts are at different locations, and funky stuff happens near X = 0 because you have numerical problems with the tangent (you might want to reformulate the problem).
But, after checking your equation, the following code should be helpful:
function solve_for_F()
% graininess of alpha
N = 100;
% Find solutions for all alphae
X = zeros(1,N);
options = optimset('Display', 'off');
alpha = linspace(0, pi/2, N);
x0 = linspace(6, 0, N);
for ii = 1:numel(alpha)
X(ii) = fzero(#(x)F(x, alpha(ii)), x0(ii), options);
end
% Convert and make an X-Y plot
Y = X .* tan(alpha);
plot(X, Y,...
'linewidth', 2,...
'color', [1 0.65 0]);
end
function fval = F(X, alpha)
Y = X*tan(alpha);
% Please, SIMPLIFY in the future
A = 1247745517111813/562949953421312;
B = 4243112111277797/4503599627370496;
V = atan2(2*Y,X) + A;
eq2 = sqrt( (5/33*( Y*sin(V) + X/2*(cos(V) + 1) ))^2 + ...
(5/33*( Y*cos(V) - X/2* sin(V) ))^2 ) - B;
fval = eq2;
end
Results:
So, I was having fun with this (thanks for that)!
Different question, different answer.
The solution below first searches for the constants causing the X and Y intercepts you were looking for (p1 and p2). For those constants that best fit the problem, it makes a plot, taking into account numerical issues.
In fact, you don't need eq. 1, because that's true always for any curve -- it's just there to confuse you, and problematic to use.
So, here it is:
function C = solve_for_F()
% Points of interest
px = 6;
py = 4.2;
% Wrapper function; search for those constants
% causing the correct X,Y intercepts (at px, py)
G = #(C) abs(F( 0, px, C)) + ... % X intercept at px
abs(F(py, 0, C)); % Y intercept at py
% Initial estimate, based on your original equation
C0 = [5/33
1247745517111813/562949953421312
4243112111277797/4503599627370496
5/66];
% Minimize the error in G by optimizing those constants
C = fminsearch(G, C0);
% Plot the solutions
plot_XY(px, py, C);
end
function plot_XY(xmax,ymax, C)
% graininess of X
N = 100;
% Find solutions for all alphae
Y = zeros(1,N);
X = linspace(0, xmax, N);
y0 = linspace(ymax, 0, N);
options = optimset('Display', 'off',...,...
'TolX' , 1e-10);
% Solve the nonlinear equation for each X
for ii = 1:numel(X)
% Wrapper function for fzero()
fcn1 = #(y)F(y, X(ii), C);
% fzero() is probably the fastest and most intuitive
% solver for this problem
[Y(ii),~,flag] = fzero(fcn1, y0(ii), options);
% However, it uses an algorithm that easily diverges
% when the function slope is large. For those cases,
% solve with fminsearch()
if flag ~= 1
% In this case, the minimum of the absolute value
% is searched for (which should be zero)
fcn2 = #(y) abs(fcn1(y));
Y(ii) = fminsearch(fcn2, y0(ii), options);
end
end
% Now plot the X,Y solutions
plot(X, Y,...
'linewidth', 2,...
'color', [1 0.65 0]);
xlabel('X'), ylabel('Y')
axis([0 xmax+.1 0 ymax+.1])
end
function fval = F(Y, X, C)
% Unpack constants
b = C(1); d = C(3);
c = C(2); k = C(4);
% pre-work
V = atan2(2*Y, X) + c;
% Eq. 2
fval = sqrt( (b*Y*sin(V) + k*X*(cos(V) + 1))^2 + ...
(b*Y*cos(V) - k*X* sin(V) )^2 ) - d;
end
I am trying to implement the finite difference method in matlab. I did some calculations and I got that y(i) is a function of y(i-1) and y(i+1), when I know y(1) and y(n+1). However, I don't know how I can implement this so the values of y are updated the right way. I tried using 2 fors, but it's not going to work that way.
EDIT
This is the script and the result isn't right
n = 10;
m = n+1;
h = 1/m;
x = 0:h:1;
y = zeros(m+1,1);
y(1) = 4;
y(m+1) = 6;
s = y;
for i=2:m
y(i) = y(i-1)*(-1+(-2)*h)+h*h*x(i)*exp(2*x(i));
end
for i=m:-1:2
y(i) = (y(i) + (y(i+1)*(2*h-1)))/(3*h*h-2);
end
The equation is:
y''(x) - 4y'(x) + 3y(x) = x * e ^ (2x),
y(0) = 4,
y(1) = 6
Thanks.
Consider the following code. The central differential quotient is discretized.
% Second order diff. equ.
% y'' - 4*y' + 3*y = x*exp(2*x)
% (y(i+1)-2*y(i)+y(i-1))/h^2-4*(y(i+1)-y(i-1))/(2*h) + 3*y(i) = x(i)*exp(2*x(i));
The solution region is specified.
x = (0:0.01:1)'; % Solution region
h = min(diff(x)); % distance
As said in my comment, using this method, all points have to be solved simultaneously. Therefore, above numerical approximation of the equation is transformed in a linear system of euqations.
% System of equations
% Matrix of coefficients
A = zeros(length(x));
A(1,1) = 1; % known solu for first point
A(end,end) = 1; % known solu for last point
% y(i) y'' y
A(2:end-1,2:end-1) = A(2:end-1,2:end-1)+diag(repmat(-2/h^2+3,[length(x)-2 1]));
% y(i-1) y'' -4*y'
A(1:end-1,1:end-1) = A(1:end-1,1:end-1)+diag(repmat(1/h^2+4/(2*h),[length(x)-2 1]),-1);
% y(i+1) y'' -4*y'
A(2:end,2:end) = A(2:end,2:end)+diag(repmat(1/h^2-4/(2*h),[length(x)-2 1]),+1);
With the rhs of the differential equation. Note that the known values are calculated by 1 in the matrix and the actual value in the solution vector.
Y = x.*exp(2*x);
Y(1) = 4; % known solu for first point
Y(end) = 6; % known solu for last point
y = A\Y;
Having an equation to approximate the first order derivative (see above) you can verify the solution. (note, ddx2 is an own function)
f1 = ddx2(x,y); % first derivative (own function)
f2 = ddx2(x,f1); % second derivative (own function)
figure;
plot(x,y);
saveas(gcf,'solu1','png');
figure;
plot(x,f2-4*f1+3*y,x,x.*exp(2*x),'ko');
ylim([0 10]);
legend('lhs','rhs','Location','nw');
saveas(gcf,'solu2','png');
I hope the solution shown below is correct.
I am trying to write a function that implements Newton's method in two dimensions and whilst I have done this, I have to now adjust my script so that the input parameters of my function must be f(x) in a column vector, the Jacobian matrix of f(x), the initial guess x0 and the tolerance where the function f(x) and its Jacobian matrix are in separate .m files.
As an example of a script I wrote that implements Newton's method, I have:
n=0; %initialize iteration counter
eps=1; %initialize error
x=[1;1]; %set starting value
%Computation loop
while eps>1e-10&n<100
g=[x(1)^2+x(2)^3-1;x(1)^4-x(2)^4+x(1)*x(2)]; %g(x)
eps=abs(g(1))+abs(g(2)); %error
Jg=[2*x(1),3*x(2)^2;4*x(1)^3+x(2),-4*x(2)^3+x(1)]; %Jacobian
y=x-Jg\g; %iterate
x=y; %update x
n=n+1; %counter+1
end
n,x,eps %display end values
So with this script, I had implemented the function and the Jacobian matrix into the actual script and I am struggling to work out how I can actually create a script with the input parameters required.
Thanks!
If you don't mind, I'd like to restructure your code so that it is more dynamic and more user friendly to read.
Let's start with some preliminaries. If you want to make your script truly dynamic, then I would recommend that you use the Symbolic Math Toolbox. This way, you can use MATLAB to tackle derivatives of functions for you. You first need to use the syms command, followed by any variable you want. This tells MATLAB that you are now going to treat this variable as "symbolic" (i.e. not a constant). Let's start with some basics:
syms x;
y = 2*x^2 + 6*x + 3;
dy = diff(y); % Derivative with respect to x. Should give 4*x + 6;
out = subs(y, 3); % The subs command will substitute all x's in y with the value 3
% This should give 2*(3^2) + 6*3 + 3 = 39
Because this is 2D, we're going to need 2D functions... so let's define x and y as variables. The way you call the subs command will be slightly different:
syms x, y; % Two variables now
z = 2*x*y^2 + 6*y + x;
dzx = diff(z, 'x'); % Differentiate with respect to x - Should give 2*y^2 + 1
dzy = diff(z, 'y'); % Differentiate with respect to y - Should give 4*x*y + 6
out = subs(z, {x, y}, [2, 3]); % For z, with variables x,y, substitute x = 2, y = 3
% Should give 56
One more thing... we can place equations into vectors or matrices and use subs to simultaneously substitute all values of x and y into each equation.
syms x, y;
z1 = 3*x + 6*y + 3;
z2 = 3*y + 4*y + 4;
f = [z1; z2];
out = subs(f, {x,y}, [2, 3]); % Produces a 2 x 1 vector with [27; 25]
We can do the same thing for matrices, but for brevity I won't show you how to do that. I will defer to the code and you can see it then.
Now that we have that established, let's tackle your code one piece at a time to truly make this dynamic. Your function requires the initial guess x0, the function f(x) as a column vector, the Jacobian matrix as a 2 x 2 matrix and the tolerance tol.
Before you run your script, you will need to generate your parameters:
syms x y; % Make x,y symbolic
f1 = x^2 + y^3 - 1; % Make your two equations (from your example)
f2 = x^4 - y^4 + x*y;
f = [f1; f2]; % f(x) vector
% Jacobian matrix
J = [diff(f1, 'x') diff(f1, 'y'); diff(f2, 'x') diff(f2, 'y')];
% Initial vector
x0 = [1; 1];
% Tolerance:
tol = 1e-10;
Now, make your script into a function:
% To run in MATLAB, do:
% [n, xout, tol] = Jacobian2D(f, J, x0, tol);
% disp('n = '); disp(n); disp('x = '); disp(xout); disp('tol = '); disp(tol);
function [n, xout, tol] = Jacobian2D(f, J, x0, tol)
% Just to be sure...
syms x, y;
% Initialize error
ep = 1; % Note: eps is a reserved keyword in MATLAB
% Initialize counter
n = 0;
% For the beginning of the loop
% Must transpose into a row vector as this is required by subs
xout = x0';
% Computation loop
while ep > tol && n < 100
g = subs(f, {x,y}, xout); %g(x)
ep = abs(g(1)) + abs(g(2)); %error
Jg = subs(J, {x,y}, xout); %Jacobian
yout = xout - Jg\g; %iterate
xout = yout; %update x
n = n + 1; %counter+1
end
% Transpose and convert back to number representation
xout = double(xout');
I should probably tell you that when you're doing computation using the Symbolic Math Toolbox, the data type of the numbers as you're calculating them are a sym object. You probably want to convert these back into real numbers and so you can use double to cast them back. However, if you leave them in the sym format, it displays your numbers as neat fractions if that's what you're looking for. Cast to double if you want the decimal point representation.
Now when you run this function, it should give you what you're looking for. I have not tested this code, but I'm pretty sure this will work.
Happy to answer any more questions you may have. Hope this helps.
Cheers!