Discrete state-space
Controllability and Observability
Control design via pole placement
Reference input
Observer design
In this digital control version of the inverted pendulum problem, we will use the state-space method to design the digital controller. If you refer to the Inverted Pendulum Modeling page, the state-space equations were derived as
| M | mass of the cart | 0.5 kg |
| m | mass of the pendulum | 0.5 kg |
| b | friction of the cart | 0.1 N/m/sec |
| l | length to pendulum center of mass | 0.3 m |
| I | inertia of the pendulum | 0.006 kg*m^2 |
| u | step force applied to the cart | |
| x | cart position coordinate | |
| phi | pendulum angle from vertical | |
Outputs are the cart displacement (x in meters) and the pendulum deflection angle (phi in radians).
The design requirements are
Assuming that the closed-loop bandwidth frequencies are around 1 rad/sec for both the cart and the pendulum, let the sampling time be 1/100 sec/sample. Now we are ready to use c2d. Enter the following commands to an m-file.
M = .5;
m = 0.2;
b = 0.1;
i = 0.006;
g = 9.8;
l = 0.3;
p = i*(M+m)+M*m*l^2; %denominator for the A and B matrices
A = [0 1 0 0;
0 -(i+m*l^2)*b/p (m^2*g*l^2)/p 0;
0 0 0 1;
0 -(m*l*b)/p m*g*l*(M+m)/p 0];
B = [ 0;
(i+m*l^2)/p;
0;
m*l/p];
C = [1 0 0 0;
0 0 1 0];
D = [0;
0];
Ts = 1/100;
pend = ss(A,B,C,D);
pend_d = c2d(pend,Ts,'zoh')
Running this m-file in the MATLAB command window gives you the following four matrices.
a =
x1 x2 x3 x4
x1 1 0.0099909 0.00013359 4.4532e-07
x2 0 0.99818 0.026717 0.00013359
x3 0 -2.2719e-05 1.0016 0.010005
x4 0 -0.0045437 0.31192 1.0016
b =
u1
x1 9.0859e-05
x2 0.018167
x3 0.00022719
x4 0.045437
c =
x1 x2 x3 x4
y1 1 0 0 0
y2 0 0 1 0
d =
u1
y1 0
y2 0
Sampling time: 0.01
Discrete-time system.
Now we have obtained the discrete state-space model of the form
must have the rank of n. The rank of the matrix is the number of independent rows (or columns). In the same token, for the system to be completely state observable, the observability matrix
must also have the rank of n.
Since our controllability matrix and observability matrix are '4x4', the rank of both matrices must be 4. The function rank can give you the rank of each matrix. In an new m-file, enter the following commands and run it in the command window.
F = [1.0000 0.0100 0.0001 0.0000;
0 0.9982 0.0267 0.0001;
0 0.0000 1.0016 0.0100;
0 -0.0045 0.3119 1.0016];
G = [0.0001
0.0182;
0.0002;
0.0454];
H = [1 0 0 0;
0 0 1 0];
J = [0;
0];
Ts = 1/100;
pend_d = ss(F,G,H,J,Ts);
co = ctrb(pend_d);
ob = obsv(pend_d);
Controllability = rank(co)
Observability = rank(ob)
In the command window, you should see
Controllability =
4
Observability =
4
This proves that our discrete system is both completely state controllable and completely state observable.
The next step is to assume that all four states are measurable, and find the control matrix (K). If you refer to the continuous Inverted Pendulum: State-Space page, the Linear Quadratic Regulator (LQR) method was used to find the control matrix (K). In this digital version, we will use the same LQR method. This method allows you to find the optimal control matrix that results in some balance between system errors and control effort. Please consult your control textbook for details. To use this LQR method, we need to find three parameters: Performance index matrix (R), state-cost matrix (Q), and weighting factors. For simplicity, we will choose the performance index matrix equal 1 (R=1), and the state-cost matrix (Q) equals H' x H. The weighting factors will be chosen by trial and error. The state-cost matrix (Q) has the following structure
Q =
1 0 0 0
0 0 0 0
0 0 1 0
0 0 0 0
The element in the 1,1 position will be used to weight the cart's position and the element in the 3,3 position will be used to weight the pendulum's angle. The weighting factors for the cart's position and the pendulum's angle will be chosen individually.
Now we are ready to find the control matrix (K) and see the response of the system. Enter the following commands to an new m-file and run it in the MATLAB command window.
T = 0:0.01:5;
U = 0.2*ones(size(T));
F = [1.0000 0.0100 0.0001 0.0000;
0 0.9982 0.0267 0.0001;
0 0.0000 1.0016 0.0100;
0 -0.0045 0.3119 1.0016];
G = [0.0001;
0.0182;
0.0002;
0.0454];
H = [1 0 0 0;
0 0 1 0];
J = [0;
0];
x = 1; %weighting factor for the cart position
y = 1; %weighting factor for the pendulum angle
Q = [x 0 0 0;
0 0 0 0;
0 0 y 0;
0 0 0 0];
R = 1;
K = dlqr(F,G,Q,R)
Ts = 1/100;
sys_cl = ss(F-G*K,G,H,J,Ts);
[Y,T,X] = lsim(sys_cl,U);
stairs(T,Y)
legend('Cart (x)','Pendulum (phi)')
You should see the following step response.
The curve in green represents the pendulum's angle, in radians, and the curve in blue represents the cart's position in meters. The pendulum's and cart's overshoot appear fine, but their settling times need improvement, and the cart's rise time needs to be decreased. Also the cart has, in fact, moved in the opposite direction. For now, we will concentrate on improving the settling times and the rise times, and fix the steady-state error later.
Let's increase the weighting factors (x and y) and see if both the settling and rise times decrease. Go back to your m-file and change the x and y to x=5000 and y=100. Running this m-file in the command window gives you the following new step response.
From this plot, we see that all design requirements are satisfied except the steady-state error of the cart position (x). We can easily correct this by introducing a feedforward scaling factor (Nbar).
Unfortunately, we can not use our user-defined function rscale to find Nbar because it was defined for continuous-time single-output systems. But certainly we can find it by trial and error. After several trials, Nbar equals -61.55 provided a satisfactory response. Try the following m-file and obtain the step response shown below.
T = 0:0.01:5;
U = 0.2*ones(size(T));
F = [1.0000 0.0100 0.0001 0.0000;
0 0.9982 0.0267 0.0001;
0 0.0000 1.0016 0.0100;
0 -0.0045 0.3119 1.0016];
G = [0.0001;
0.0182;
0.0002;
0.0454];
H = [1 0 0 0;
0 0 1 0];
J = [0;
0];
x = 5000; %weighting factor for the cart position
y = 100; %weighting factor for the pendulum angle
Q = [x 0 0 0;
0 0 0 0;
0 0 y 0;
0 0 0 0];
R = 1;
K = dlqr(F,G,Q,R)
Nbar = -61.55;
Ts = 1/100;
sys_cl = ss(F-G*K,G*Nbar,H,J,Ts);
[Y,T,X] = lsim(sys_cl,U);
stairs(T,Y)
legend('Cart (x)','Pendulum (phi)')
Notice that the steady-state error of the cart's position have been eliminated. Now we have designed a system that satisfies all design requirements.
A basic schematic of the plant-observer system is shown below.
To design the observer, first, we need to find the L matrix. To find the L matrix, we need to find the poles of the system without the observer (the poles of F-G*K). Copy the following commands to a new m-file and run it in the MATLAB command window.
F = [1.0000 0.0100 0.0001 0.0000;
0 0.9982 0.0267 0.0001;
0 0.0000 1.0016 0.0100;
0 -0.0045 0.3119 1.0016];
G = [0.0001;
0.0182;
0.0002;
0.0454];
H = [1 0 0 0;
0 0 1 0];
J = [0;
0];
x = 5000; %weighting factor for the cart position
y = 100; %weighting factor for the pendulum angle
Q = [x 0 0 0;
0 0 0 0;
0 0 y 0;
0 0 0 0];
R = 1;
K = dlqr(F,G,Q,R);
poles = eig (F-G*K)
In the command window, you should see
poles =
0.9156 + 0.0729i
0.9156 - 0.0729i
0.9535 + 0.0079i
0.9535 - 0.0079i
We want to place observer poles so that the observer dynamics are a lot faster than the system without the observer. Let's place the observer poles far left of above poles, say, at [-0.3 -0.31 -0.32 -0.33]. These poles can be changed later, if necessary. We will use the MATLAB function place to find the L matrix. Enter the following commands to an new m-file and run it.
F = [1.0000 0.0100 0.0001 0.0000;
0 0.9982 0.0267 0.0001;
0 0.0000 1.0016 0.0100;
0 -0.0045 0.3119 1.0016];
H = [1 0 0 0;
0 0 1 0];
P = [-0.3 -0.31 -0.32 -0.33];
L = place (F',H',P)'
You should see the following L matrix in the command window.
L =
2.6310 -0.0105
172.8146 -1.3468
-0.0129 2.6304
-2.2954 173.2787
Now we will obtain the overall system response including the observer. Once again, create a new m-file and copy the following code.
T = 0:0.01:5;
U = 0.2*ones(size(T));
F = [1.0000 0.0100 0.0001 0.0000;
0 0.9982 0.0267 0.0001;
0 0.0000 1.0016 0.0100;
0 -0.0045 0.3119 1.0016];
G = [0.0001;
0.0182;
0.0002;
0.0454];
H = [1 0 0 0;
0 0 1 0];
J = [0;
0];
x = 5000; %weighting factor for the cart position
y = 100; %weighting factor for the pendulum angle
Q = [x 0 0 0;
0 0 0 0;
0 0 y 0;
0 0 0 0];
R = 1;
K = dlqr(F,G,Q,R)
Nbar = -61.55;
L = [2.6310 -0.0105;
172.8146 -1.3468;
-0.0129 2.6304;
-2.2954 173.2787];
Fce = [F-G*K G*K;
zeros(size(F)) (F-L*H)];
Gce = [G*Nbar;
zeros(size(G))];
Hce = [H zeros(size(H))];
Jce = [0;0];
Ts = 1/100;
sys_cl = ss(Fce,Gce,Hce,Jce,Ts);
[Y,T,X] = lsim(sys_cl,U);
stairs(T,Y)
legend('cart (x)','pendulum (phi)')
After running this m-file, you should get the following step response.
As you noticed, this response is identical to the as previous one since observer knows the plant exactly and has the same initial condition.
