Here is Matlab code for a continous time linearized model of a cart pole.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% state = [cart-position
% pendulum-angle
% cart-velocity
% pendulum-angular-velocity]
% xdot = A*x + B*u
% y = C*x + D*u
A = [ 0 0 1 0
0 0 0 1
0 0 0 0
0 10 0 0 ]
B = [ 0
0
1
1 ]
% we combine cart-position and pendulum-angle to keep this a SISO system.
C = [ 1 1 0 0 ]
D = [ 0 ]
sys1 = ss( A, B, C, D )
pzmap( sys1 )
[mypoles,myzeros] = pzmap( sys1 )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
This results in:
mypoles =
3.1623
-3.1623
0
0
myzeros =
-2.2361
2.2361
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Now let's design an LQR controller. Use simple choices for Q and R, which are definitely not the best choices.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Qc = eye(4)
Rc = eye(1)
[kc,sc,ec] = lqr(A,B,Qc,Rc)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
This results in:
kc =
-1.0000 33.0796 -2.3698 10.5036
sc =
2.3698 -10.5036 2.3080 -3.3080
-10.5036 174.5317 -21.5835 54.6631
2.3080 -21.5835 4.4190 -6.7888
-3.3080 54.6631 -6.7888 17.2925
ec =
-3.6999
-2.7061
-0.8639 + 0.5024i
-0.8639 - 0.5024i
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate the Cart
sys3 = ss( A - B*kc, B, eye(4), D )
x0 = [ 1 0 0 0 ]'
tt = 0:0.01:6;
uu = zeros( size(tt) );
[y,t] = lsim(sys3,uu,tt,x0);
f = -kc*y';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
plot(y(:,1))
title('cart position')
xlabel('centiseconds')
ylabel('m')
plot(y(:,2))
title('pendulum angle')
xlabel('centiseconds')
ylabel('radians')
plot(y(:,3))
title('cart velocity')
xlabel('centiseconds')
ylabel('m/s')
plot(y(:,4))
title('pendulum angular velocity')
xlabel('centiseconds')
ylabel('r/s')
plot(f)
title('force on cart')
xlabel('centiseconds')
ylabel('arbitrary units')
Let's assess robustness by using the same gains Kc, but changing parameters in A and B.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test robustness of LQR gains by increasing mgl/I
A = [ 0 0 1 0
0 0 0 1
0 0 0 0
0 10 0 0 ]
newplot;
hold on;
for v = 10:1.0:23
A(4,2) = v;
sys4 = ss( A-B*kc, B, C, D );
pzmap( sys4 );
end
hold off;
title('Pole-Zero Map: A(4,2) = 10:1.0:23')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
We can increase mgl/I by a factor of 2.3.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test robustness of LQR gains by decreasing mgl/I
A = [ 0 0 1 0
0 0 0 1
0 0 0 0
0 10 0 0 ]
newplot;
hold on;
for v = 10:-1.0:1
A(4,2) = v;
sys4 = ss( A-B*kc, B, C, D );
pzmap( sys4 );
end
hold off;
title('Pole-Zero Map: A(4,2) = 10:-1.0:1')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
We can decrease mgl/I by a factor of 10, at least.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test robustness of LQR gains by decreasing the cart mass
A = [ 0 0 1 0
0 0 0 1
0 0 0 0
0 10 0 0 ]
B = [ 0
0
1
1 ]
newplot;
hold on;
for v = 1:-0.1:0.5
B(3,1) = v;
B(4,1) = v;
sys4 = ss( A-B*kc, B, C, D );
pzmap( sys4 );
end
hold off;
title('Pole-Zero Map: B = 1:-0.1:0.5')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Looks like we can decrease mass by about a factor of 2.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test robustness of LQR gains by increasing the cart mass
B = [ 0
0
1
1 ]
newplot;
hold on;
for v = 1:1.0:10
B(3,1) = v;
B(4,1) = v;
sys4 = ss( A-B*kc, B, C, D );
pzmap( sys4 );
end
hold off;
title('Pole-Zero Map: B = 1:1.0:10')
% zoom in on origin
axis([-5 1 -1 1])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Looks like we remain stable with a factor of 10 increase in mass.
Zoom in on origin.
Now let's design an observer. Again, use simple choices for Q and R, which are definitely not the best choices.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now design observer
Cf = [ 1 0 0 0
0 1 0 0 ]
Qf = eye(4)
Rf = eye(2)
% You can use the duality of LQR and Kalman Filter design:
[kft,sf,ef] = lqr(A',Cf',Qf,Rf)
% This produces the transpose of the Kalman gain: kft = kf'
% Or you can use the overly complicated Matlab Kalman design routine:
G = eye(4)
D = zeros(2,1)
H = zeros(2,4)
sys6 = ss(A,[B G],Cf,[D H]);
[kest,kf,sf] = kalman(sys6,Qf,Rf)
% To crudely try to move the poles to the right, I will scale Q and R
s = 1
[kft,sf,ef] = lqr(A',Cf',s*Qf,Rf/s)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
This results in
s = 1
kft =
1.7321 -0.0000 1.0000 -0.0000
-0.0000 6.4109 -0.0000 20.0499
sf =
1.7321 -0.0000 1.0000 -0.0000
-0.0000 6.4109 -0.0000 20.0499
1.0000 -0.0000 1.7321 -0.0000
-0.0000 20.0499 -0.0000 64.4288
ef =
-0.8660 + 0.5000i
-0.8660 - 0.5000i
-3.6799
-2.7310
s = 10
kft =
10.9545 -0.0000 10.0000 -0.0000
-0.0000 12.1772 0.0000 24.1421
sf =
1.0954 -0.0000 1.0000 -0.0000
-0.0000 1.2177 0.0000 2.4142
1.0000 0.0000 10.9545 -0.0000
-0.0000 2.4142 -0.0000 17.2212
ef =
-9.9494
-1.0051
-10.8770
-1.3002
s = 1000000
ef =
-999999.999999500
-1000000.000009500
-1.000000000
-1.000000000
Note that in the above changing the ratio of Qf and Rf did not move all the poles to the left.
Now combine the state estimator and controller.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now see if workable combination exists
AA = [ A -B*kc
kf*Cf A-B*kc-kf*Cf ]
BB = [ B
B ]
CC = [ 1 1 0 0 0 0 0 0 ]
DD = [ 0 ]
sys2 = ss( AA, BB, CC, DD )
pzmap( sys2 )
[p,z] = pzmap( sys2 )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
This results in the following pole/zero map
Now we are going to plot the same thing at the same scale of some future plots.
Okay, now we introduce modeling error.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This is all setup
% Model dynamics
Am = A;
Bm = B;
AA = [ A -B*kc
kf*Cf Am-Bm*kc-kf*Cf ]
BB = [ B
Bm ]
CC = [ 1 1 0 0 0 0 0 0 ]
DD = [ 0 ]
sys3 = ss( AA, BB, CC, DD )
[p,z] = pzmap( sys3 )
pzmap( sys3 )
axis([-7 3 -5 5])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Let's actually do a sweep of different pendulum dynamics in both directions.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% cart2.jpg
% reset true dynamics
A = [ 0 0 1 0
0 0 0 1
0 0 0 0
0 10 0 0 ]
B = [ 0
0
1
1 ]
newplot;
hold on;
for v = 10:0.1:12
A(4,2) = v;
AA = [ A -B*kc
kf*Cf Am-Bm*kc-kf*Cf ];
BB = [ B
Bm ];
sys3 = ss( AA, BB, CC, DD );
pzmap( sys3 );
end
hold off;
title('Pole-Zero Map: A(4,2) = 10:0.1:12')
axis([-7 3 -5 5])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
So increasing mgl/I drives the system unstable.
Now for decreasing mgl/I.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% cart3.jpg
% reset true dynamics
A = [ 0 0 1 0
0 0 0 1
0 0 0 0
0 10 0 0 ]
B = [ 0
0
1
1 ]
newplot;
hold on;
for v = 10:-0.5:1
A(4,2) = v;
AA = [ A -B*kc
kf*Cf Am-Bm*kc-kf*Cf ];
BB = [ B
Bm ];
sys3 = ss( AA, BB, CC, DD );
pzmap( sys3 );
end
hold off;
title('Pole-Zero Map: A(4,2) = 10:-0.5:1')
axis([-7 3 -5 5])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
This drove poles to the origin, but not unstable.
Now let's play with the mass of the cart.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% cart4.jpg
% reset true dynamics
A = [ 0 0 1 0
0 0 0 1
0 0 0 0
0 10 0 0 ]
B = [ 0
0
1
1 ]
newplot;
hold on;
for v = 1:-0.01:0.85
B(3,1) = v;
B(4,1) = v;
AA = [ A -B*kc
kf*Cf Am-Bm*kc-kf*Cf ];
BB = [ B
Bm ];
sys3 = ss( AA, BB, CC, DD );
pzmap( sys3 );
end
hold off;
title('Pole-Zero Map: B = 1:-0.01:0.85')
axis([-7 3 -5 5])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Didn't take much decrease of the mass of the cart (1 to 0.85) to cause the system to go unstable.
Let's try increasing the mass of the cart.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% cart5.jpg
% reset true dynamics
A = [ 0 0 1 0
0 0 0 1
0 0 0 0
0 10 0 0 ]
B = [ 0
0
1
1 ]
newplot;
hold on;
for v = 1:0.1:2
B(3,1) = v;
B(4,1) = v;
AA = [ A -B*kc
kf*Cf Am-Bm*kc-kf*Cf ];
BB = [ B
Bm ];
sys3 = ss( AA, BB, CC, DD );
pzmap( sys3 );
% [p,z] = pzmap( sys3 );
% p
end
hold off;
title('Pole-Zero Map: B = 1:0.1:2')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Doubling the mass of the cart drives the system unstable.