Digital Control Example: Inverted Pendulum using State-Space method

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

where

 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

• Settling time for x and phi less than 5 seconds
• Rise time for x of less than 1 second
• Overshoot of phi less than 0.35 rad (20 deg)
• Steady-state error of x and phi less than 2%

## Discrete state-space

The first thing to do here is to convert the above continuous state-space equations to discrete state-space. To do this, we are going to use the MATLAB function called c2d. To use this c2d, we need to specify three arguments: state-space system, sampling time (Ts in sec/sample), and the 'method'. You should already be familiar with how to construct a state-space system from A, B, C, and D matrices. The sampling time should be smaller than 1/(30*BW) sec, where BW is the closed-loop bandwidth frequency. The method we will use is the zero-order hold ('zoh').

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

## Controllability and Observability

The next step is to check the controllability and the observability of the system. For the system to be completely state controllable, the controllability matrix

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.

## Control design via pole placement

The schematic of a full-state feedback system is shown below.

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).

## Reference input

Unlike other design methods, the full-state feedback system does not compare the output to the reference; instead, it compares all states multiplied by the control matrix (K*x) to the reference (see the schematic shown above). Thus, we should not expect to see the output equal to the input. To obtain the desired output, we need to scale the reference input so that the output equals the reference. This can be easily done by introducing a feedforward scaling factor called Nbar. The basic schematic with Nbar is shown below.

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.

## Observer design

The above response satisfies all design requirements; however, it was found assuming all states are measurable. This assumption may not be valid for all systems. In this section, we develop a technique for estimating the states of a plant from the information that is available concerning the plant. The system that estimates the states of another system is called an observer. Thus, in this section we will design a full-order state observer to estimate those states that are not measurable. For further explanation on how an observer works, please consult your control textbook.

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.

Digital Control Examples
Cruise Control | Motor Speed | Motor Position | Bus Suspension | Inverted Pendulum | Pitch Controller | Ball & Beam

Inverted Pendulum Examples
Modeling | PID | Root Locus | Frequency Response | State Space | Digital Control | Simulink

Tutorials
MATLAB Basics | MATLAB Modeling | PID Control | Root Locus | Frequency Response | State Space | Digital Control | Simulink Basics | Simulink Modeling | Examples