Example: State-space design for the inverted pendulum

The state equations for this problem are:

The design criteria for this system with the cart receiving a 0.2 m step input are as follows:

• Settling time for x and theta of less than 5 seconds.
• Rise time for x of less than 1 second.
• Overshoot of theta less than 20 degrees (0.35 radians).

As you may have noticed if you went through some of the other inverted pendulum examples, the design criteria for this example are different. In the other examples we were dealing with an impulse and not a step input. Also, we were only concerned with the pendulums angle and disregarded the cart's position in the design of the controller. However, for an inverted pendulum it is unrealistic to consider just the single output system. Using state-space methods it is relatively simple to work with a multi-output system, so in this example we will design a controller with both the pendulum angle and the cart position in mind.

To see how this problem was originally set up, consult the inverted pendulum modeling page.

This problem can be solved using full state feedback. The schematic of this type of control system is shown below:

If you are interested in running an animation of this example based on the control techniques used in the state-space tutorial please go to the Inverted Pendulum Animation Page after completing this tutorial.

### Open-loop poles

In this problem R represents the commanded step input to the cart. The 4 states represent the position and velocity of the cart and the angle and angular velocity of the pendulum. The output y contains both the position of the cart and the angle of the pendulum. We want to design a controller so that when an step input is given to the system, the pendulum should be displaced, but eventually return to zero (i.e. the vertical) and the cart should move to its new commanded position. To view the system's open-loop response please refer to the inverted pendulum modeling page.

The first step in designing this type of controller is to determine the open-loop poles of the system. Enter the following lines of code into a m-file (or a '.m' file located in the same directory as MATLAB):

```     M = 0.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
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];

pend = ss(A,B,C,D);
p = eig(A)```
The MATLAB command window should output the following text as a result:
```     p =
0
-0.1428
5.5651
-5.6041
```
As you can see, there is one right-half-plane pole at 5.5651. This should confirm your intuition that the system is unstable in open loop.

### LQR design

The next step in the design process is to assume that we have full-state feedback (i.e. that we can measure all four states), and find the vector K which determines the feedback control law. This can be done in a number of ways. If you know the desired closed-loop poles, you can use the place or acker command. Another option is to use the lqr function; this will give you the optimal controller (under certain assumptions; consult your textbook for more details). The lqr function allows you to choose two parameters, R and Q, which will balance the relative importance of the input and state in the cost function that you are trying to optimize. The simplest case is to assume R=1, and Q=C'*C. You may notice that we are using both outputs (the pendulum's angle and the cart's position). Essentially, the lqr method allows for the control of both outputs. In this case, it is pretty easy to do. The controller can be tuned by changing the nonzero elements in the Q matrix to get a desirable response. To find the structure of Q, enter the following into the MATLAB command window:

```C'*C
```
You should see the following in the command window:
```     ans =

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 input weighting R will remain at 1. Now that we know what the Q matrix should look like we can experiment to find the K matrix that will give us a good controller. We will go ahead and find the K matrix and plot the response all in one step so that changes can be made in the control and be seen automatically in the response. Enter the following text into your m-file:
```x = 1;
y = 1;
Q = [x 0 0 0;
0 0 0 0;
0 0 y 0;
0 0 0 0];
R = 1;
K = lqr(A,B,Q,R)
Ac = [(A-B*K)];
Bc = [B];
Cc = [C];
Dc = [D];
sys_cl=ss(Ac,Bc,Cc,Dc);

T=0:0.01:5;
U=0.2*ones(size(T));
[Y,T,X]=lsim(sys_cl,U,T);
plot(T,Y)
legend('Cart','Pendulum')
```
You should get the following value for K and a response plot:
```
K =

-1.0000   -1.6567   18.6854    3.4594
```

The curve in green represents the pendulum's angle in radians, and the curve in blue represents the cart's position in meters. As you can see, this plot is not satisfactory. The pendulum and cart's overshoot appear fine, but their settling times need improvement and the cart's rise time needs to go down. As I'm sure you have noticed the cart is not near the desired location but has in fact moved in the other direction. This error will be dealt with in the next section and right now we will focus on the settling and rise times. Go back to your m-file and change the x and y variab to see if you can get a better response. You will find that increasing x makes the settling and rise times go down, and lowers the angle the pendulum moves. Using x=5000 and y=100, the following value of K and step response were found:

```
K =

-70.7107  -37.8345  105.5298   20.9238
```

You may have noted that if you increased x and y even higher, you could improve the response even more. The reason this plot was chosen was because it satisfied the design requirements while keeping x and y as small as possible. In this problem, x and y have been used to describe the relative weight of the tracking error in the cart's position and pendulum's angle versus the control effort. The higher x and y are, the more control effort is used, but the smaller the tracking error. The system response has a settling time under 2 seconds.

Now we want to get rid of the steady-state error. In contrast to the other design methods, where we feedback the output and compare it to the reference input to compute an error, with a full-state feedback controller we are feeding back all the states. We need to compute what the steady-state value of the states should be, multiply that by the chosen gain K, and use a new value as our reference for computing the input. This can be done by adding a constant gain Nbar after the reference. The schematic below shows this relationship:

Nbar can be found using the user-defined function rscale (copy it to the directory that your m-file is in). Delete the lsim line and copy the following to your m-file and run it to view the step response with Nbar added.

```Cn = [1 0 0 0];
sys = ss(A,B,Cn,0);
Nbar = rscale(sys,K)
sys_cl = ss(Ac,Bc*Nbar,Cc,Dc);
[Y,T,X] = lsim(sys_cl,U,T);
plot(T,Y)
legend('Cart','Pendulum')
```

A different C had to be used because the rscale function will not work for multiple outputs. However, the Nbar found is correct, as you can see from the output below:

```
Nbar =

-70.7107
```

Now, the steady-state error is within our limits, the rise and settling times are met and the pendulum's overshoot is within range of the design criteria.

### Observer design

This response is good, but was found assuming full-state feedback, which most likely will not be a valid assumption. To compensate for this, we will next design a full-order estimator to estimate those states that are not measured. A schematic of this kind of system is shown below, without Nbar:

To begin, we must first find the controller poles. To do this copy the following code to the end of your m-file:

```p = eig(Ac)
```
If you changed the weighting factors x and y above to x=5000 and y=100, you should see the following poles in the MATLAB command window:

```p =

-8.4910 + 7.9283i
-8.4910 - 7.9283i
-4.7592 + 0.8309i
-4.7592 - 0.8309i
```
We want to design estimator poles that are about 4-10 times as fast as slowest pole, say at -40. We will use the place command in MATLAB to find the L vector (note that acker would also work). Remember that the place command cannot have all the desired poles at the same location. Delete from the lsim command on and enter the following text to the end of your m-file to find the L matrix:

```P = [-40 -41 -42 -43];
L = place(A',C',P)'
```
We are using both outputs (the angle of the pendulum and the position of the cart) to design the observer. The system is not observable using only the angle of the pendulum as output; you can check this in MATLAB by computing rank(obsv(A,C(2,:))). This should make sense to you: if you can only measure the angle of the pendulum, you cannot determine what the position of the cart will be.

You should see the following in the MATLAB window:

```L =

1.0e+03 *

0.0826   -0.0010
1.6992   -0.0402
-0.0014    0.0832
-0.0762    1.7604

```
Now we will combine the control-law design with the estimator design to get the compensator. The response should be similar to the one from the control-law design. To set up the compensator copy the following code to the end of your m-file:

```Ace = [A-B*K             B*K;
zeros(size(A)) (A-L*C)];
Bce = [       B*Nbar;
zeros(size(B))];
Cce = [Cc zeros(size(Cc))];
Dce = [0;0];
est_cl = ss(Ace,Bce,Cce,Dce);
T = 0:0.01:5;
U = 0.2*ones(size(T));
[Y,T,X] = lsim(est_cl,U,T);
plot(T,Y)
legend('Cart','Pendulum')
```
After running this m-file, you should output the following step response simulation plot:

This response is about the same as before. All of the design requirements have been met with the minimum amount of control effort, so no more iteration is needed.

As you can see, it is much easier to control multi-input or multi-output systems with the state space method than with any other of the methods.

If you are interested in running an animation of the inverted pendulum example based on the control techniques used in this tutorial please go to the Inverted Pendulum Animation Page.

State Space 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 | Animation

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