Simulink simulation acting in general different from the same simulation using rk4
I am having difficulty simulating an object that is described by the following spatial space equations in simulink:
The right side of the spatial equation of state is described 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
Below is the corresponding rk4 function with a simple visualization.
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 results for a cart pendulum.
Now to the problem. I wanted to recreate the same equations in simulink. I thought it would be as easy as creating the following simulink model.
where I fill fcn blocks with the second and fourth equations 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 it doesn't give correct results from above but below
Does anyone know what I am doing wrong?
Edit: Following @ am304's comment, I decided to add the following information. I changed the setting for the simulink solution to use a fixed step rk4 solver to get the same results. The second integrator 3 from the above model was initialized with pi / 2.
Edit2: If anyone wants to test the simulink model for themselves, click 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
source to share
I looked at your Simulink model and it looks like you might have confused the two functions you were using. You used the theta_dd function where you wanted to put x_dd and vice versa.
In your model, you also force the value x_d
to a constant value of 0. I am assuming that you actually wanted to set the initial condition to 0, which you can see done with the Integrator block. x_d
(as input to f
) should be your state vector, which is also the output of your integrators. It is simply a consequence of the fact that you define x_d, the integral of x_dd. This is how RK4 works; you use the initial state vector first and then use the predicted state vector to control the next RK4 hop.
The resulting out-of-scope result (I dumped your entire state vector here) is as follows and looks like what you expect:
I don't think I should link externally to the simulink file, so if you need a copy of the file, you can open the chat and ask for it. If not, the image above should be enough to help you reproduce the same results.
source to share