I wish to simulate the output of a certain gear system I have. How the gear system looks isn't particularly important to the problem, I managed to get the differential equation needed from the mechanical system.
Here is the code I have
% parameters
N2 = 90;
N1 = 36;
Jn1 = 0.5;
Jn2 = 0.8;
J2 = 2;
D = 8;
K = 5;
J = (N2/N1)^2 * Jn1 + Jn2 + J2;
% define the system
sys = ss([0 1; -K/J -D/J], [0; N2/(N1*J)], [1 0], 0);
% initial state: (position, velocity) [rad; rad/s]
x0 = [0; 0];
% define the time span
t = linspace(0, 15, 10000)';
% define the input step
T1 = zeros(length(t), 1);
T1(t>=0) = 1;
% compute the system step response at once
theta1 = lsim(sys, T1, t, x0);
% compute the system response as aggregate of the forced and unforced
% temporal evolutions
theta2 = lsim(sys, T1, t, [0; 0]) + initial(sys, x0, t);
% plot results
figure('color', 'white');
hold on;
yyaxis left;
plot(t, T1, '-.', 'linewidth', 2);
ylabel('[N]');
yyaxis right;
plot(t, theta1, 'linewidth', 3);
plot(t, theta2, 'k--');
xlabel('t [s]');
ylabel('[rad]');
grid minor;
legend({'$T_1$', '$\theta_1$', '$\theta_2$'}, 'Interpreter', 'latex',...
'location', 'southeast');
hold off;
This should work in generating a graph that shows the positions, my outputs, for a Heaviside/step input. My question is, how would I do this for a sine wave input. I figure I should have sin(w*t) instead of (t>=0), where w is my pulse frequency. Still, I can't seem to make this work. Any help would be really appreciated! :)
Here is the solution to my problem :)
function x = integrate(t, u, x0)
% parameters
N2 = 90;
N1 = 36;
Jn1 = 0.5;
Jn2 = 0.8;
J2 = 2;
D = 8;
K = 5;
J = (N2/N1)^2 * Jn1 + Jn2 + J2;
% integrate the differential equation
[t, x] = ode23(#fun, t, x0);
% plot results
figure('color', 'white');
% plot position
yyaxis left;
plot(t, x(:, 1));
ylabel('$x$ [rad]', 'Interpreter', 'latex');
% plot velocity
yyaxis right;
plot(t, x(:, 2));
ylabel('$\dot{x}$ [rad/s]', 'Interpreter', 'latex');
grid minor;
xlabel('$t$ [s]', 'Interpreter', 'latex');
function g = fun(t, x)
g = zeros(2, 1);
g(1) = x(2);
g(2) = (-K/J)*x(1) + (-D/J)*x(2) + (N2/(N1*J)*u(t));
end
end
Now we can use an anonymous function for example:
t = linspace(0, 120, 10000)';
x0 = [0.1; 0];
x = integrate(t, #(t)(sin(1.5*t)), x0);
Test Run
These are the output results I currently get on MATLAB R2019b. As Luis' comment has suggested I have also declared a sinusoid as T1 to serve as the input. Currently not sure if this result is the expected output.
Code Snippet:
t = linspace(0, 15, 10000)';
f = 0.1;
phi = 0;
T1 = sin(2*pi*f*t + phi);
f → Frequency of sinusoidal input (0.1Hz in this example).
phi → Phase offset of sinusoidal input/initial phase (0 in this example).
t → Time vector dictating the samples of the sinusoid.
0 → Start time (0 seconds in this example).
15 → End time (15 seconds in this example).
10000 → Number of samples between the start time (0s) and end time (15s).
Implementation in Script:
% parameters
N2 = 90;
N1 = 36;
Jn1 = 0.5;
Jn2 = 0.8;
J2 = 2;
D = 8;
K = 5;
J = (N2/N1)^2 * Jn1 + Jn2 + J2;
% define the system
sys = ss([0 1; -K/J -D/J], [0; N2/(N1*J)], [1 0], 0);
% initial state: (position, velocity) [rad; rad/s]
x0 = [0; 0];
% define the time span
t = linspace(0, 15, 10000)';
% define the input step
T1 = zeros(length(t), 1);
T1(t>=0) = 1;
f = 0.1; %Sinusoid frequency = 0.1Hz%
phi = 0; %Phase = 0%
T1 = sin(2*pi*f*t + phi);
% compute the system step response at once
theta1 = lsim(sys, T1, t, x0);
% compute the system response as aggregate of the forced and unforced
% temporal evolutions
theta2 = lsim(sys, T1, t, [0; 0]) + initial(sys, x0, t);
% plot results
figure('color', 'white');
hold on;
yyaxis left;
plot(t, T1, '-.', 'linewidth', 2);
ylabel('[N]');
yyaxis right;
plot(t, theta1, 'linewidth', 3);
plot(t, theta2, 'k--');
xlabel('t [s]');
ylabel('[rad]');
grid minor;
legend({'$T_1$', '$\theta_1$', '$\theta_2$'}, 'Interpreter', 'latex',...
'location', 'southeast');
hold off;
Related
So I want to create an MPC controller for my seesaw-cart system. All the "grunt work" (getting equations of motion, state-space representation etc.) has been done, so I went into coding into MATLAB.
Here's the following:
clc; clear all; close all;
yalmip('clear')
%% Parameters
a = 0.116553; % height of center mass of the pendulum, [m]
b = 0; % position of the weight B on the vertical rod (not taken into consideration)
c = 0.180047; % height of the center of mass of the cart, [m]
mA = 4.839; % mass of the pendulum, [kg]
mB = 0; % not taken into consideration
mC = 1; % mass of the cart, [kg]
g = 9.81; % gravity factor, [m/s^2]
kappa = 0.1; % coefficient of the viscous damping in the rotational joint
J = 0.68; % moment of inertia of the pendulum, [kgm^2]
Ke = 0.077; % motor constant of the EM force, [Vs^-1]
Kt = 0.077; % proportional moment motor constant, [NmA^-1]
Ra = 2.6; % electrical resistance, [ohm]
p = 1/3.71; % motor gearbox ratio
r = 7.7*10^(-3); % effective radius of the motor shaft, [m]
%% Defining the continuous system
A = [0 1 0 0;
(a*mA*g+b*mB*g)/(J+mB*b^2), -kappa/(J+mB*b^2), -mC*g/(J+mB*b^2), -Ke*Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2);
0 0 0 1;
(a*mA*g+b*mB*g)/(J+mB*b^2)-g, -kappa*c/(J+mB*b^2), -c*mC*g/(J+mB*b^2), -(J+mB*b^2+mC*c^2)*Ke*Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2)];
B = [0;
Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2);
0;
(J+mB*b^2+mC*c^2)*Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2)];
C = eye(4); % check
D = 0;
sysC = ss(A, B, C, D);
%% Defining the discrete system
Ts = 10e-3; % sample time
sysD = c2d(sysC,Ts); % discrete time
Ad = sysD.A;
Bd = sysD.B;
%% Formulation of the MPC problem
[nx, nu] = size(B);
Q = eye(nx);
R = eye(nu);
N = 1000;
% Input constraints, maximum and minimum voltage
umin = -6;
umax = 6;
% Still have to find this
xmin = [-deg2rad(10); -deg2rad(50); -0.5; -10];
xmax = [deg2rad(10); deg2rad(50); 0.5; 10];
uVar = sdpvar(repmat(nu,1,N),ones(1,N));
xVar = sdpvar(repmat(nx,1,N+1),ones(1,N+1));
constraints = [];
objective = 0;
ops = sdpsettings('verbose',0,'solver','quadprog');
for k = 1:N
objective = objective + xVar{k}'*Q*xVar{k} + uVar{k}*R*uVar{k};
constraints = [constraints, xVar{k+1} == Ad*xVar{k} + Bd*uVar{k}];
constraints = [constraints , umin <= uVar{k} <= umax , xmin <= xVar{k+1} <= xmax];
end
controller = optimizer(constraints, objective,ops,xVar{1},[uVar{:}]);
%% Simulation of the linearized model
nSim = 1000;
x0 = [0; 0; 0; 0.05]; % initial values, check
time = 0:Ts:nSim*Ts;
x = zeros(nx,nSim+1);
u = zeros(nu,nSim);
x(:,1) = x0;
for i = 2:nSim+1
disp(strcat(['Sim iter: ', num2str(i-1)]));
uOpt = controller{x(:,i-1)};
u(:,i-1) = uOpt(:,1);
x(:,i) = Ad*x(:,i-1) + Bd*u(:,i-1);
end
%% Plot
figure;
subplot(4,1,1);
plot(time,x(1,:), 'LineWidth', 2, 'Color', 'red'); grid; ylabel('{\theta} (rad)'); title('States');
subplot(4,1,2);
plot(time,x(2,:), 'LineWidth', 2, 'Color', [0.6350, 0.0780, 0.1840]); grid; ylabel('$\dot{\theta}$ (rad/s)', 'Interpreter', 'latex');
subplot(4,1,3);
plot(time,x(3,:), 'LineWidth', 2, 'Color', [0.4940, 0.1840, 0.5560]); grid; ylabel('s (m)');
subplot(4,1,4);
plot(time,x(4,:), 'LineWidth', 2, 'Color',[0, 0.7, 0.9]); grid; ylabel('$\dot{s}$ (m/s)', 'Interpreter', 'latex'); xlabel('t (s)');
figure;
stairs(time(1:end-1),u, 'LineWidth', 2, 'Color',[1, 0.647, 0]); grid; ylabel('Ua (V)'); xlabel('t (s)'); title('Input');
So I was wondering for any suggestions on improvements. What can I do to make my regulator more robust?
Here are my outputs for this particular code:
Note: I'm using YALMIP for the optimization part of the MPC.
You lose feasibility after a few iterations,
sol =
struct with fields:
yalmipversion: '20210331'
matlabversion: '9.9.0.1524771 (R2020b) Update 2'
yalmiptime: 0.1159
solvertime: 0.2331
info: 'Infeasible problem (QUADPROG))'
problem: 1
K>> i
i =
4
Your MPC controller is thus badly tuned (too short horizon being the obvious start)
Also, your definition of u is weird as it has length 4 instead of length N (thus meaning you have two trailing variables in u which never are used, leading to them having value Nan when you look at them)
There are numerous MPC-examples in the tutorials which you should be able to use directly
https://yalmip.github.io/example/standardmpc/
YALMIP-specific question are better asked at
https://github.com/yalmip/YALMIP/discussions
https://groups.google.com/g/yalmip
I have a code that creates the correct xy plot for elastic pendulum with spring. I would like to show an animation of the elastic spring pendulum on an xy plot as the system marches forward in time. How can this be done?
Here is my simulation code:
clc
clear all;
%Define parameters
global M K L g;
M = 1;
K = 25.6;
L = 1;
g = 9.8;
% define initial values for theta, thetad, del, deld
theta_0 = 0;
thetad_0 = .5;
del_0 = 1;
deld_0 = 0;
initialValues = [theta_0, thetad_0, del_0, deld_0];
% Set a timespan
t_initial = 0;
t_final = 36;
dt = .1;
N = (t_final - t_initial)/dt;
timeSpan = linspace(t_final, t_initial, N);
% Run ode45 to get z (theta, thetad, del, deld)
[t, z] = ode45(#OdeFunHndlSpngPdlmSym, timeSpan, initialValues);
% initialize empty column vectors for theta, thetad, del, deld
M_loop = zeros(N, 1);
L_loop = zeros(N, 1);
theta = zeros(N, 1);
thetad = zeros(N, 1);
del = zeros(N, 1);
deld = zeros(N, 1);
T = zeros(N, 1);
x = zeros(N, 1);
y = zeros(N, 1);
% Assign values for variables (theta, thetad, del, deld)
for i = 1:N
M_loop(i) = M;
L_loop(i) = L;
theta(i) = z(i, 1);
thetad(i) = z(i, 2);
del(i) = z(i, 3);
deld(i) = z(i, 4);
T(i) = (M*(thetad(i)^2*(L + del(i))^2 + deld(i)^2))/2;
V(i) = (K*del(i)^2)/2 + M*g*(L - cos(theta(i))*(L + del(i)));
E(i) = T(i) + V(i);
x(i) = (L + del(i))*sin(theta(i));
y(i) = -(L + del(i))*cos(theta(i));
end
figure(1)
plot(x, y,'r');
title('XY Plot');
xlabel('x position');
ylabel('y position');
Here is my function code:
function dz = OdeFunHndlSpngPdlmSym(~, z)
% Define Global Parameters
global M K L g
% Take output from SymDevFElSpringPdlm.m file for fy1 and fy2 and
% substitute into z2 and z4 respectively
%fy1=thetadd=z(2)= -(M*g*sin(z1)*(L + z3) + M*z2*z4*(2*L + 2*z3))/(M*(L + z3)^2)
%fy2=deldd=z(4)=((M*(2*L + 2*z3)*z2^2)/2 - K*z3 + M*g*cos(z1))/M
% return column vector [thetad; thetadd; deld; deldd]
dz = [z(2);
-(M*g*sin(z(1))*(L + z(3)) + M*z(2)*z(4)*(2*L + 2*z(3)))/(M*(L + z(3))^2);
z(4);
((M*(2*L + 2*z(3))*z(2)^2)/2 - K*z(3) + M*g*cos(z(1)))/M];
You can "simulate" animation in a plot with a continous updating FOR loop and assignig graphic object to variables.
Something like (I assume only to use x,y as arrays function of time array t)
%In order to block the axis and preventing continuous zoom, choose proper axes limit
x_lim = 100; %these values depends on you, these are examples
y_lim = 100;
axis equal
axis([-x_lim x_lim -y_lim y_lim]); %now x and y axes are fixed from -100 to 100
ax = gca;
for i=1:length(t)
if i > 1
delete(P);
delete(L);
end
P = plot(x(i),y(i)); %draw the point
hold on
L = quiver(0,0,x(i),y(i)); %draw a vector from 0,0 to the point
hold on
%other drawings
drawnow
pause(0.1) %pause in seconds needed to simulate animaton.
end
"Hold on" instruction after every plot instruction.
This is only a basic animation, of course.
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 solved a differential equation and the solutions of that are the complex values in the time domain. I have to transform it to the frequency domain with FFT. I have used FFT in MATLAB, but the answers are not correct. How can I choose my interval of frequency?
The time domain is between -10 and 60 and the number of steps is 1000.
function r = fur()
clc
clear
format long
a = -10; % tmin
b = 60; % tmax
m = 1000; % Number of steps
t = zeros(1, m);
y = zeros(1, m);
t(1) = a; % Boundary condition
y(1) = 0; % Boundary condition
t0 = 0.01;
h = (b-a)/m;
for j=1:m
T = t(j); Y = y(j);
k1 = h*Fun(T, Y);
k2 = h*Fun(T + h/2, Y + k1/2);
k3 = h*Fun(T + h/2, Y + k2/2);
k4 = h*Fun(T + h, Y + k3);
y(j+1) = Y + (k1 + 2*k2 + 2*k3 + k4)/6;
t(j+1) = a + h*(j);
end
% real_y = real(y);
% imag_y = imag(y);
y;
%% Fast Fourier transformation for P(W)
NFFT = length(y);
fs = 2*pi/h;
X = fftshift(fft(y, NFFT));
fVals = (0:NFFT-1)*fs/NFFT;
figure(1)
plot(fVals, abs(X), '-b');
title('Fast Fourier transform');
xlabel('Frequency (THz)')
ylabel('p(w)');
hold on
%% Fast Fourier transformation for E(W)
NFFT = length(y);
fs = 2*pi/h;
Z = fftshift(fft(Et(t, t0), NFFT));
fVals = (0:NFFT-1)*fs/NFFT;
figure (2)
plot(fVals, abs(Z), '-r');
title('Fast Fourier Transform');
xlabel('Frequency (THz)')
ylabel('E(w)');
hold on
%% Linear susceptibility
f = X./Z;
f_imag = imag(f);
f_real = real(f);
fVal = (0:NFFT-1)*fs/NFFT;
figure(3)
plot(fVal, f_real, '-r');
title('total part of susceptibility');
xlabel('Frequency (THz)')
ylabel('Kappa(w)');
hold on
figure(4);
plot(fVals, f_imag, '-r');
title('imaginary part of susceptibility');
xlabel('Frequency (THz)')
ylabel('kappa(w)');
hold on
And this is the Fun.m file:
function F = Fun(t, y)
format long
E_g = 1.52; % Binding energy for GaAs
gamma = 0.1*E_g;
t0 = 0.01; % This is a damping operator
k = 1;
F = -i*((((k^2)-i*gamma)*y)-Et(t, t0)); % F = -i*((-i*gamma)*y-1/2);;
And this is the Et.m file:
function e = Et(t, t0)
format long
t0 = 0.1;
e = (1/2)*(exp(-(t/t0).^2));
end
I'm working on a small simulation in matlab. For this purposes I want to create an animation of the simulated object (inverted pendulum). Unfortunatly the axes keep rescaling.
I tried everything. There are similar questions, but I just can't get it to work. The best i got is the code below. Where I get both axes at the same time, the scaled from -5 to 5 and those scaled by matlab.
%init visualisation
visualisation = figure();
axis([-5 5 -5 5]);
xlim([-5 5]);
ylim([-5 5]);
ax_hand = axes;
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
figure(visualisation);
plot(x0(3), 0, 'ro', 'LineWidth', 3);
%plotting pendulum
l = 2;
%plot(sin(x0(1))*l, cos(x0(1))*l, 'b*' , 'LineWidth', 2);
% regulator
end;
Here's one approach:
Do the first plot (possibly empty, doesn't matter). Get a handle to it, say h.
Set axis limits and include the statement axis manual fo freeze them.
Update plot (in a loop) via the 'XData' and 'YData' poperties of h.
Example:
h = plot(NaN, NaN, 'o'); %// empty plot
axis([0 10 0 5])
axis manual %// this line freezes the axes
for n = 1:10
x = 1:n;
y = sqrt(x);
set(h, 'XData', x, 'YData', y)
pause(.2)
end
Example with two plots:
h1 = plot(NaN, NaN, 'bo'); %// empty plot
hold on
h2 = plot(NaN, NaN, 'r*'); %// empty plot
axis([0 10 0 5])
axis manual %// this line freezes the axes
for n = 1:10
x1 = 1:n;
y1 = sqrt(x1);
set(h1, 'XData', x1, 'YData', y1)
x2 = 4.5;
y2 = n/2-.5;
set(h2, 'XData', x2, 'YData', y2)
pause(.2)
end