Scribd
Upload
87 views

Matlab Pendulum

The document defines symbolic variables to model a cart-pole system and derive its equations of motion using Lagrangian mechanics. It defines the system configuration using generalized coordinates, calculates the kinetic and potential energies, derives the Lagrangian, and applies the Euler-Lagrange equations of motion to obtain symbolic expressions for the accelerations of the cart and pole. It then linearizes the system around the origin to obtain state-space matrices for control design and simulation.

Uploaded by

Samuel
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as XLS, PDF, TXT or read online on Scribd
87 views

Matlab Pendulum

The document defines symbolic variables to model a cart-pole system and derive its equations of motion using Lagrangian mechanics. It defines the system configuration using generalized coordinates, calculates the kinetic and potential energies, derives the Lagrangian, and applies the Euler-Lagrange equations of motion to obtain symbolic expressions for the accelerations of the cart and pole. It then linearizes the system around the origin to obtain state-space matrices for control design and simulation.

Uploaded by

Samuel
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as XLS, PDF, TXT or read online on Scribd
You are on page 1/ 41

pkg load symbolic

clc
clear all

% define symbolic variables


syms t; % time
syms u; % control input
syms q1f(t); % generalized koordinate which describes the cart position (symbolic function)
syms q2f(t); % generalized koordinate which describes the pole angle (symbolic function)
syms g l m1 m2 d1; % constants: gravitation, lenth of pole, mass of cart, mass of pendulum, viscous friction of th

% mass matrices
M1=m1*eye(3); M1(3,3)=0; % Moment of inertia = 0 -> cart doesn't turn
M2=m2*eye(3); M2(3,3)=0; % Moment of inertia around cg = 0 -> concentrated mass at the end of the pole

% pose of the cart


y1=[ q1f;...
0;...
0 ];

% pose of the pole


y2=[ q1f-l*sin(q2f); ...
l*cos(q2f); ...
q2f ];

% derive poses
y1d=diff(y1,t);
y2d=diff(y2,t);

% calculate energies
T=0.5*(y1d.'*M1*y1d + y2d.'*M2*y2d);
gvec=[0; g; 0].';
U=gvec*M1*y1+gvec*M2*y2;

% calculate Lagrangian
L=T-U;
L=simplify(L);

% L_q:
q=[q1f; q2f];
L_q=jacobian(L,q);
L_q=simplify(L_q);
% L_qd:
qd=diff(q,t);
L_qd=jacobian(L,qd);
L_qd=simplify(L_qd);

% L_qd_t:
L_qd_t=diff(L_qd,t);
L_qd_t=simplify(L_qd_t);

% Euler-Lagrange formalism (note the external viscous friction force)


lhs=L_qd_t - L_q - [ u - d1 * diff(q1f,t) 0];
lhs=simplify(lhs)

% rename variables
syms q1dd q2dd q1d q2d q1 q2
lhs=subs(lhs,diff(diff(q1f,t),t),q1dd);
lhs=subs(lhs,diff(diff(q2f,t),t),q2dd);
lhs=subs(lhs,diff(q1f,t),q1d);
lhs=subs(lhs,diff(q2f,t),q2d);
lhs=subs(lhs,q1f(t),q1);
lhs=subs(lhs,q2f(t),q2);

% solve for the highest derrivatives


eq=lhs== 0;
Sol=solve(eq,q1dd,q2dd);
q1dd=simplify(Sol.q1dd);
p1ddCode=char(q1dd)
q2dd=simplify(Sol.q2dd);
p2ddCode=char(q2dd)

% state space representation: statevector = [x, x_dot, theta, theta_dot]


statevector=[q1; q1d; q2; q2d];
f=[q1d; q1dd; q2d; q2dd];

%compute the jacobians


A=jacobian(f,statevector);
B=jacobian(f,u);

%evaluate jacobians at fixpoint = [0 0 0 0]


A_=subs(A,[q1,q1d, q2,q2d],[0,0,0,0])
B_=subs(B,[q1,q1d, q2,q2d],[0,0,0,0])
ulum, viscous friction of the cart

he end of the pole


function xd = evaluateOde(t, x, m1, m2, l, g, d1, K)

% setpoints
xs = [-1; 0; 0; 0];
if(t>5)
xs = [1; 0; 0; 0];
end

% open loop
if(t>8)
%if(t>0)
u = 0;
else

if ( x(3) > (-pi/5) ) && ( x(3) < ( +pi/5 ) )


% lqr control u = k*(xs-x)
u = K * (xs - [x(1);x(2);x(3);x(4)]); % stupid octave necessary notation...
else
% energy based swing-up control
E=2/3*m2*l^2*x(4)^2 - m2*g*l*(cos(x(3))-1);
u=0.2*m1*E*x(4)*cos(x(3));
end

end

% differential equation
xd(1) = x(2);
xd(2) = (-d1*x(2) + g*m2*sin(2*x(3))/2 - l*m2*x(4)^2*sin(x(3)) + u)/(m2*sin(x(3))^2 + m1);
xd(3) = x(4);
xd(4) = (g*(m1 + m2)*sin(x(3)) - (d1*x(2) + l*m2*x(4)^2*sin(x(3)) - u)*cos(x(3)))/(l*(m2*sin(x(3))^2 + m1));

end
(l*(m2*sin(x(3))^2 + m1));
function drawPendulum(t,x,m1,m2,g,l)
s = x(:,1);
phi = x(:,3);
Epot = m2 * g * l * cos(phi);

% dimensions
W = 1*sqrt(m1/5); % cart width
H = .5*sqrt(m1/5); % cart height
mr = .3*sqrt(m2); % mass radius

% positions
px = s - l*sin(phi);
py = H/2 + l*cos(phi);

% create new figure and


figure('position', [50 500 500 400])
plot([-10 10],[0 0],'w','LineWidth',2)
hold on

% plot the cart


h1=rectangle('Position',[s(1)-W/2,0,W,H],'Curvature',.1,'FaceColor',[1 0.1 0.1],'EdgeColor',[1 1 1]);

% plot the pole


h2=plot([s(1) px(1)],[0 py(1)],'w','LineWidth',2);
h3=rectangle('Position',[px(1)-mr/2,py(1)-mr/2,mr,mr],'Curvature',1,'FaceColor',[.3 0.3 1],'EdgeColor',[1 1
h4=text(0.95, 1.5, ['phi: ', num2str(1)]);
h5=text(0.95, 1.7, ['Epot: ', num2str(1)]);
set(h4,'color','w', 'fontsize', 14);
set(h5,'color','w', 'fontsize', 14);
xlim([-2 2]);
ylim([-2 2]);
set(gca,'Color','k','XColor','w','YColor','w')
set(gcf,'Color','k')

tic

% animation in a for loop


for k=1:length(t)

% update pole and cart position


set(h1, 'position',[s(k)-W/2,0,W,H]);
set(h2, 'XData',[s(k) px(k)], 'YData', [H/2 py(k)]);
set(h3, 'position', [px(k)-mr/2,py(k)-mr/2,mr,mr]);
set(h4, 'string',['phi: ', num2str(phi(k))]);
set(h5, 'string',['Epot: ', num2str(Epot(k))]);
drawnow();

% strop the animation when q is pressed


if (kbhit (1) == 'q')
break
endif

% meassure the time and create a fixed time loop


t2=toc;
while t2 < t(k)
t2 = toc;
endwhile
t3(k) = t2;

end

%figure
%plot(diff(t3));
%legend('plot time');
%close all
EdgeColor',[1 1 1]);

[.3 0.3 1],'EdgeColor',[1 1 1]);


pkg load symbolic
clc
clear all

% define symbolic variables


syms t; % time
syms u; % control input
syms q1f(t); % generalized koordinate which describes the cart position (symbolic function)
syms q2f(t); % generalized koordinate which describes the pole angle (symbolic function)
syms g l m1 m2 d1; % constants: gravitation, lenth of pole, mass of cart, mass of pendulum, visco

% mass matrices
M1=m1*eye(3); M1(3,3)=0; % Moment of inertia = 0 -> cart doesn't turn
M2=m2*eye(3); M2(3,3)=0; % Moment of inertia around cg = 0 -> concentrated mass at the end of th

% pose of the cart


y1=[ q1f;...
0;...
0 ];

% pose of the pole


y2=[ q1f-l*sin(q2f); ...
l*cos(q2f); ...
q2f ];

% derive poses
y1d=diff(y1,t);
y2d=diff(y2,t);

% calculate energies
T=0.5*(y1d.'*M1*y1d + y2d.'*M2*y2d);
gvec=[0; g; 0].';
U=gvec*M1*y1+gvec*M2*y2;

% calculate Lagrangian
L=T-U;
L=simplify(L);

% L_q:
q=[q1f; q2f];
L_q=jacobian(L,q);
L_q=simplify(L_q);

% L_qd:
qd=diff(q,t);
L_qd=jacobian(L,qd);
L_qd=simplify(L_qd);
% L_qd_t:
L_qd_t=diff(L_qd,t);
L_qd_t=simplify(L_qd_t);

% Euler-Lagrange formalism (note the external viscous friction force)


lhs=L_qd_t - L_q - [ u - d1 * diff(q1f,t) 0];
lhs=simplify(lhs)

% rename variables
syms q1dd q2dd q1d q2d q1 q2
lhs=subs(lhs,diff(diff(q1f,t),t),q1dd);
lhs=subs(lhs,diff(diff(q2f,t),t),q2dd);
lhs=subs(lhs,diff(q1f,t),q1d);
lhs=subs(lhs,diff(q2f,t),q2d);
lhs=subs(lhs,q1f(t),q1);
lhs=subs(lhs,q2f(t),q2);

% solve for the highest derrivatives


eq=lhs== 0;
Sol=solve(eq,q1dd,q2dd);
q1dd=simplify(Sol.q1dd);
p1ddCode=char(q1dd)
q2dd=simplify(Sol.q2dd);
p2ddCode=char(q2dd)

% state space representation: statevector = [x, x_dot, theta, theta_dot]


statevector=[q1; q1d; q2; q2d];
f=[q1d; q1dd; q2d; q2dd];

% compute the jacobians


A=jacobian(f,statevector);
B=jacobian(f,u);

% evaluate jacobians at fixpoint = [0 0 0 0]


A_=subs(A,[q1,q1d, q2,q2d],[0,0,0,0])
B_=subs(B,[q1,q1d, q2,q2d],[0,0,0,0])
(symbolic function)
ymbolic function)
ass of pendulum, viscous friction of the cart

d mass at the end of the pole


Gjn
function drawcartpend(y,m,M,L)
x = y(1);
th = y(3);

% kinematics
% x = 3; % cart position
% th = 3*pi/2; % pendulum angle

% dimensions
% L = 2; % pendulum length
W = 1*sqrt(M/5); % cart width
H = .5*sqrt(M/5); % cart height
wr = .2; % wheel radius
mr = .3*sqrt(m); % mass radius

% positions
% y = wr/2; % cart vertical position
y = wr/2+H/2; % cart vertical position
w1x = x-.9*W/2;
w1y = 0;
w2x = x+.9*W/2-wr;
w2y = 0;

px = x + L*sin(th);
py = y - L*cos(th);

plot([-10 10],[0 0],'w','LineWidth',2)


hold on
rectangle('Position',[x-W/2,y-H/2,W,H],'Curvature',.1,'FaceColor',[1 0.1 0.1],'EdgeColor',[1 1 1])
rectangle('Position',[w1x,w1y,wr,wr],'Curvature',1,'FaceColor',[1 1 1],'EdgeColor',[1 1 1])
rectangle('Position',[w2x,w2y,wr,wr],'Curvature',1,'FaceColor',[1 1 1],'EdgeColor',[1 1 1])

plot([x px],[y py],'w','LineWidth',2)

rectangle('Position',[px-mr/2,py-mr/2,mr,mr],'Curvature',1,'FaceColor',[.3 0.3 1],'EdgeColor',[1 1 1])

% set(gca,'YTick',[])
% set(gca,'XTick',[])
xlim([-5 5]);
ylim([-2 2.5]);
set(gca,'Color','k','XColor','w','YColor','w')
set(gcf,'Position',[10 900 800 400])
set(gcf,'Color','k')
set(gcf,'InvertHardcopy','off')

% box off
drawnow
hold off
0.1],'EdgeColor',[1 1 1])
geColor',[1 1 1])
geColor',[1 1 1])

3 0.3 1],'EdgeColor',[1 1 1])


function drawcartpend(y,m,M,L)
x = y(1);
th = y(3);

% kinematics
% x = 3; % cart position
% th = 3*pi/2; % pendulum angle

% dimensions
% L = 2; % pendulum length
W = 1*sqrt(M/5); % cart width
H = .5*sqrt(M/5); % cart height
wr = .2; % wheel radius
mr = .3*sqrt(m); % mass radius

% positions
% y = wr/2; % cart vertical position
y = wr/2+H/2; % cart vertical position
w1x = x-.9*W/2;
w1y = 0;
w2x = x+.9*W/2-wr;
w2y = 0;

px = x + L*sin(th);
py = y - L*cos(th);

plot([-10 10],[0 0],'k','LineWidth',2)


hold on
rectangle('Position',[x-W/2,y-H/2,W,H],'Curvature',.1,'FaceColor',[1 0.1 0.1])
rectangle('Position',[w1x,w1y,wr,wr],'Curvature',1,'FaceColor',[0 0 0])
rectangle('Position',[w2x,w2y,wr,wr],'Curvature',1,'FaceColor',[0 0 0])

plot([x px],[y py],'k','LineWidth',2)

rectangle('Position',[px-mr/2,py-mr/2,mr,mr],'Curvature',1,'FaceColor',[.1 0.1 1])

% set(gca,'YTick',[])
% set(gca,'XTick',[])
xlim([-5 5]);
ylim([-2 2.5]);
set(gcf,'Position',[100 550 1000 400])
% box off
drawnow
hold off
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = -1; % pendulum up (s=1)

% y = [x; dx; theta; dtheta];


A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];

B = [0; 1/M; 0; s*1/(M*L)];

C = [1 0 0 0];

D = zeros(size(C,1),size(B,2));

%% Augment system with disturbances and noise


Vd = .1*eye(4); % disturbance covariance
Vn = 1; % noise covariance

BF = [B Vd 0*B]; % augment inputs to include disturbance and noise

sysC = ss(A,BF,C,[0 0 0 0 0 Vn]); % build big state space system... with single output

sysFullOutput = ss(A,BF,eye(4),zeros(4,size(BF,2))); % system with full state output, disturbance,

%% Build Kalman filter


[L,P,E] = lqe(A,Vd,C,Vd,Vn); % design Kalman filter
Kf = (lqr(A',C',Vd,Vn))'; % alternatively, possible to design using "LQR" code

sysKF = ss(A-L*C,[B L],eye(4),0*[B L]); % Kalman filter estimator

%% Estimate linearized system in "down" position (Gantry crane)


dt = .01;
t = dt:dt:50;
uDIST = randn(4,size(t,2));
uNOISE = randn(size(t));
u = 0*t;
u(100:120) = 100; % impulse
u(1500:1520) = -100; % impulse

uAUG = [u; Vd*Vd*uDIST; uNOISE];

[y,t] = lsim(sysC,uAUG,t);
[xtrue,t] = lsim(sysFullOutput,uAUG,t);

[x,t] = lsim(sysKF,[u; y'],t);

plot(t,xtrue,'-',t,x,'--','LineWidth',2)

figure
plot(t,y)
hold on
plot(t,xtrue(:,1),'r')
plot(t,x(:,1),'k--')
h single output

l state output, disturbance, no noise


clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = 1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];
B = [0; 1/M; 0; s*1/(M*L)];
eig(A)

C = eye(4);
sys = ss(A,B,C,0*B);

%%

tspan = 0:.001:10;
if(s==-1)
y0 = [0; 0; 0; 1.5];
[yL,t,xL] = initial(sys,y0,tspan);
[t,yNL] = ode45(@(t,y)cartpend(y,m,M,L,g,d,0),tspan,y0);
elseif(s==1)
y0 = [0; 0; pi+.0001; 0];
[yL,t,xL] = initial(sys,y0-[0; 0; pi; 0],tspan);
[t,yNL] = ode45(@(t,y)cartpend(y,m,M,L,g,d,0),tspan,y0);
else
end

figure
% plot(t,yL);
plot(t,yL+ones(10001,1)*[0; 0; pi; 0]');
hold on
plot(t,yNL);

figure
for k=1:100:length(t)
drawcartpend_bw(yNL(k,:),m,M,L);
end
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = 1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];
B = [0; 1/M; 0; s*1/(M*L)];
eig(A)

C = eye(4);
sys = ss(A,B,C,0*B);

%%

tspan = 0:.001:10;
if(s==-1)
y0 = [0; 0; 0; 1.5];
[yL,t,xL] = initial(sys,y0,tspan);
[t,yNL] = ode45(@(t,y)cartpend(y,m,M,L,g,d,0),tspan,y0);
elseif(s==1)
y0 = [0; 0; pi+.0001; 0];
[yL,t,xL] = initial(sys,y0-[0; 0; pi; 0],tspan);
[t,yNL] = ode45(@(t,y)cartpend(y,m,M,L,g,d,0),tspan,y0);
else
end

figure
% plot(t,yL);
plot(t,yL+ones(10001,1)*[0; 0; pi; 0]');
hold on
plot(t,yNL);

figure
for k=1:100:length(t)
drawcartpend_bw(yNL(k,:),m,M,L);
end
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = 1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];

B = [0; 1/M; 0; s*1/(M*L)];


eig(A)

Q = [1 0 0 0;
0 1 0 0;
0 0 10 0;
0 0 0 100];
R = .0001;

%%
det(ctrb(A,B))

%%
K = lqr(A,B,Q,R);

tspan = 0:.001:10;
if(s==-1)
y0 = [0; 0; 0; 0];
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[4; 0; 0; 0])),tspan,y0);
elseif(s==1)
y0 = [-3; 0; pi+.1; 0];
% % [t,y] = ode45(@(t,y)((A-B*K)*(y-[0; 0; pi; 0])),tspan,y0);
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[1; 0; pi; 0])),tspan,y0);
else
end

for k=1:100:length(t)
drawcartpend_bw(y(k,:),m,M,L);
end
1
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = -1; % pendulum up (s=1)

% y = [x; dx; theta; dtheta];


A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];

B = [0; 1/M; 0; s*1/(M*L)];

C = [1 0 0 0];

D = zeros(size(C,1),size(B,2));

%% Augment system with disturbances and noise


Vd = .1*eye(4); % disturbance covariance
Vn = 1; % noise covariance

BF = [B Vd 0*B]; % augment inputs to include disturbance and noise

sysC = ss(A,BF,C,[0 0 0 0 0 Vn]); % build big state space system... with single output

sysFullOutput = ss(A,BF,eye(4),zeros(4,size(BF,2))); % system with full state output, disturbance,

%% Build Kalman filter


[L,P,E] = lqe(A,Vd,C,Vd,Vn); % design Kalman filter
Kf = (lqr(A',C',Vd,Vn))'; % alternatively, possible to design using "LQR" code

sysKF = ss(A-L*C,[B L],eye(4),0*[B L]); % Kalman filter estimator


%% Estimate linearized system in "down" position (Gantry crane)
dt = .01;
t = dt:dt:50;

uDIST = randn(4,size(t,2));
uNOISE = randn(size(t));
u = 0*t;
u(100:120) = 100; % impulse
u(1500:1520) = -100; % impulse

uAUG = [u; Vd*Vd*uDIST; uNOISE];

[y,t] = lsim(sysC,uAUG,t);
[xtrue,t] = lsim(sysFullOutput,uAUG,t);

[x,t] = lsim(sysKF,[u; y'],t);

plot(t,xtrue,'-',t,x,'--','LineWidth',2)

figure
plot(t,y)
hold on
plot(t,xtrue(:,1),'r')
plot(t,x(:,1),'k--')
h single output

l state output, disturbance, no noise


2
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = 1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];
B = [0; 1/M; 0; s*1/(M*L)];
eig(A)

C = eye(4);
sys = ss(A,B,C,0*B);

%%

tspan = 0:.001:10;
if(s==-1)
y0 = [0; 0; 0; 1.5];
[yL,t,xL] = initial(sys,y0,tspan);
[t,yNL] = ode45(@(t,y)cartpend(y,m,M,L,g,d,0),tspan,y0);
elseif(s==1)
y0 = [0; 0; pi+.0001; 0];
[yL,t,xL] = initial(sys,y0-[0; 0; pi; 0],tspan);
[t,yNL] = ode45(@(t,y)cartpend(y,m,M,L,g,d,0),tspan,y0);
else
end

figure
% plot(t,yL);
plot(t,yL+ones(10001,1)*[0; 0; pi; 0]');
hold on
plot(t,yNL);

figure
for k=1:100:length(t)
drawcartpend_bw(yNL(k,:),m,M,L);
end
3
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = 1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];
B = [0; 1/M; 0; s*1/(M*L)];
eig(A)

C = eye(4);
sys = ss(A,B,C,0*B);

%%

tspan = 0:.001:10;
if(s==-1)
y0 = [0; 0; 0; 1.5];
[yL,t,xL] = initial(sys,y0,tspan);
[t,yNL] = ode45(@(t,y)cartpend(y,m,M,L,g,d,0),tspan,y0);
elseif(s==1)
y0 = [0; 0; pi+.0001; 0];
[yL,t,xL] = initial(sys,y0-[0; 0; pi; 0],tspan);
[t,yNL] = ode45(@(t,y)cartpend(y,m,M,L,g,d,0),tspan,y0);
else
end

figure
% plot(t,yL);
plot(t,yL+ones(10001,1)*[0; 0; pi; 0]');
hold on
plot(t,yNL);
figure
for k=1:100:length(t)
drawcartpend_bw(yNL(k,:),m,M,L);
end
4
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = 1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];

B = [0; 1/M; 0; s*1/(M*L)];


eig(A)

Q = [1 0 0 0;
0 1 0 0;
0 0 10 0;
0 0 0 100];
R = .0001;

%%
det(ctrb(A,B))

%%
K = lqr(A,B,Q,R);

tspan = 0:.001:10;
if(s==-1)
y0 = [0; 0; 0; 0];
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[4; 0; 0; 0])),tspan,y0);
elseif(s==1)
y0 = [-3; 0; pi+.1; 0];
% % [t,y] = ode45(@(t,y)((A-B*K)*(y-[0; 0; pi; 0])),tspan,y0);
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[1; 0; pi; 0])),tspan,y0);
else

end

for k=1:100:length(t)
drawcartpend_bw(y(k,:),m,M,L);
end
5
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

tspan = 0:.1:10;
y0 = [0; 0; pi; .5];
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,0),tspan,y0);

for k=1:length(t)
drawcartpend_bw(y(k,:),m,M,L);
end

% function dy = pendcart(y,m,M,L,g,d,u)
6
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = 1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];

B = [0; 1/M; 0; s*1/(M*L)];


eig(A)

rank(ctrb(A,B)) % is it controllable

%% Pole placement

% p is a vector of desired eigenvalues


% p = [-.01; -.02; -.03; -.04]; % not enough
p = [-.3; -.4; -.5; -.6]; % just barely
p = [-1; -1.1; -1.2; -1.3]; % good
p = [-2; -2.1; -2.2; -2.3]; % aggressive
p = [-3; -3.1; -3.2; -3.3]; % aggressive
% p = [-3.5; -3.6; -3.7; -3.8]; % breaks
K = place(A,B,p);
% K = lqr(A,B,Q,R);

tspan = 0:.001:10;
if(s==-1)
y0 = [0; 0; 0; 0];
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[4; 0; 0; 0])),tspan,y0);
elseif(s==1)
y0 = [-3; 0; pi+.1; 0];
% [t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[1; 0; pi; 0])),tspan,y0);
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[1; 0; pi; 0])),tspan,y0);
else
end

for k=1:100:length(t)
drawcartpend_bw(y(k,:),m,M,L);
end
7
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = -1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];

B = [0; 1/M; 0; s*1/(M*L)];

C = [1 0 0 0]; % only observable if x measured... because x can't be


% reconstructed
% C = [0 1 0 0];
obsv(A,C)
det(obsv(A,C))

%% Which measurements are best if we omit "x"


A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];
B = [0; 1/M; 0; s*1/(M*L)];
A = A(2:end,2:end);
B = B(2:end);
C = [1 0 0];
% C = [0 1 0];
% C = [0 0 1];
obsv(A,C)

D = zeros(size(C,1),size(B,2));
sys = ss(A,B,C,D);
det(gram(sys,'o'))
8
clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = 1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];

B = [0; 1/M; 0; s*1/(M*L)];


eig(A)

Q = [1 0 0 0;
0 1 0 0;
0 0 10 0;
0 0 0 100];
R = .0001;

%%
det(ctrb(A,B))

%%
K = lqr(A,B,Q,R);

tspan = 0:.001:10;
if(s==-1)
y0 = [0; 0; 0; 0];
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[4; 0; 0; 0])),tspan,y0);
elseif(s==1)
y0 = [-3; 0; pi+.1; 0];
% % [t,y] = ode45(@(t,y)((A-B*K)*(y-[0; 0; pi; 0])),tspan,y0);
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[1; 0; pi; 0])),tspan,y0);
else

end

for k=1:100:length(t)
drawcartpend_bw(y(k,:),m,M,L);
end

% function dy = pendcart(y,m,M,L,g,d,u)

You might also like