Prepared

A reportby: _______________________
submitted in the
requirements of ENGR 6131

Project Report on
Simulation of Inverted
pendulum Control
 Table of contents

S. Topic Page
NO. No.

INTRODUCTION 2
STATE EQUATIONS AND OUTPUT EQUATIONS OF THE
1. 3
SYSTEM
2. TRANSFER FUNCTION - OPEN LOOP SYSTEM : 4
CONTROLLABLE, OBSERVABLE AND JORDAN CANONICAL
3. 5
FORMS
4. IMPULSE RESPONSE AND STEP RESPONSE 8
BODE PLOT AND ROOT-LOCUS OF THE UNCOMPENSATED
5. 9
SYSTEM
6. CONTROLLER DESIGN- CLASSICAL CONTROL THEORY 10

STEP RESPONSE, SQUARE RESPONSE, SINE WAVE

7. 14
RESPONSE FOR CLOSED LOOP SYSTEM

8. CONTROL INPUT SIGNALS 19

9. ROBUSTNESS OF THE SYSTEM 20
10. FULL STATE FEEDBACK CONTROL 22

STEP RESPONSE, SQUARE WAVE RESPONSE, SINE WAVE

11. 23
RESPONSE (FEEDBACK CONTROL)

12. FULL-ORDER DESIGN 24

13. REDUCED-ORDER OBSERVER DESIGN 26
14. OBSERVER-CONTROLLER TRANSFER FUNCTION 27

COMPARATIVE STUDY BETWEEN CLASSICAL CONTROL

15. 29
DESIGN AND MODERN CONTROL THEORY DESIGN

APPENDIX-A- MATLAB COMMANDS 29

INTRODUCTION:

1
The motive of this project is to analyze the linear model of Inverted
pendulum based on Modern Control Theory and Classical Control Theory and
a comparison is made based on the results obtained for given specifications.

The system model is described by following image.

The linearized model of the plant is given by

m1 x (t) + m l (t) m g (t) = F(t)

1 o 1

m1 lo x (t) + J
o
(t) (m1 lo + m2 lc) g (t) m1 g x(t)= 0

Where, F(T) is input signal and two output measurements are x and

Following table gives values for the parameters to be used

2
m1 0.213 kg
m2 1.785 Kg
lo 0.330 m
Jo 0.0246 Kg-m2
lc 0.071 m
g 9.81 m/s2

Since both the above equation are of order 2 and both the equation involve
variables x and , So the system described by these equations is a 4 th
order system and hence we need 4 state variables to define the state space
model.

1. STATE EQUATIONS AND OUTPUT EQUATIONS OF THE

SYSTEM
Assume following four state variables.

x1(t) = x(t), x 2(t) = x (t) , x 3 (t) = (t),

x4 (t) = (t)

The output variables are (t) = x3(t)

and x(t) = x1 (t)

By substituting the state variables and solving the input output equations,
following state equations and output equations are obtained .

State Equations:

x 1 = x2

)[ ]
m1 lo g x 1(t ) m l J Jo
( ( )
2

x =
+ 1+ 2 c o 2 g x 3 ( t ) F (t)
2 m1 lo 2J 0 lo m1l m1 l o
o ( m1 l o )
2

x 3 = x4

3
 x 4 = x1(t) ( m1 g
2
m1 l o J o ) + x3(t) {[
g
m1 l o
2
m1 l o J o ( 1+
m2 l c J
)
o2 +
m1 l o m1 lo
1
lo ]} +F(t)

[ 1
+
Jo
m1 l o m1 l o ( m 1 l o2J o ) ]
Output Equations:

(t) = x (t)
3

x(t) = x1 (t)
By replacing the values of the constant parameter by the values from given
table

| | || |
0 1 0 0 x1 0
491.03 0 282.35 0 x 2 82.24
x = + [ F(t ) ]
0 0 0 1 x3 0
1,487.95 0 885.35 0 x 4 235

||
x1

y= | |
(t)
x (t) = |0 0 1 0 x2
1 0 0 0 x3 |
x4

A state space model for the system can be created in MATLAB by using the
command

sys_invpend = ss (A,B,C,D);

2. TRANSFER FUNCTION - OPEN LOOP SYSTEM :

For Linear time invariant systems the transfer function can be calculated as

4
 Y (S) 1
TF(S) = =C ( SI A ) B + D
U (S)

The transfer function of the system is in the form of transfer function matrix
because the system has two different outputs for the single input F(t).

These two transfer functions corresponding to two outputs will have the pole
location same but different zero locations.

The transfer function is obtained from MATLAB command H = tf (sys_invpend)

(S) 235 S 2+ 6,977

= 4
Tf_Theta =H(2) = U (S) S 394.3 S 21,4610

X ( S) 82.24 S 26,459
= 4
Tf_Pos = H(1)= U (S) S 394.3 S 214,610

3. CONTROLLABLE, OBSERVABLE AND JORDAN CANONICAL

FORMS:

3.a Controllability : To check the controllability of the system , controllability

matrix Cx is produced and is checked for the rank. If this matrix is full rank then
the system is said to be controllable.

Cx = |B AB A
n1
B|

In MATLAB , matrix Cx is calculated by using command, Cx= ctrb(sys_invpend);

5
 | |
0 82 0 25970
82 0 25970 0
Cx = 0 235 0 85688
235 0 85688 0

Rank = 4 = N (Controllable)

3.b Observability: To check the observability of the system , observability matrix

Ox is produced and is checked for the rank. If this matrix is full rank then the
system is observable

||
C
CA
2
Ox = CA

C A n1

In MATLAB , matrix Ox is calculated by using command, Ox= obsv(sys_invpend);

| |
0 0 1 0
1 0 0 0
0 0 0 1
0 1 0 0
Ox = 1488 0 885.6 0
491 0 282.4 0
0 1488 0 885.6
0 491 0 282.4

Rank = 4 =N ( Observable)

3.c Controllable Form : In order to find the matrices for controllable canonical
form, first the coefficients of the characteristic polynomial are calculated by using
the command poly(A)

a0 = -14611
a1 = 0

6
 a2 = -394
a3 = 0
a4 = 1

| | ||
0 1 0 0 0
0 0 1 0 0
Ac = 0 0 0 1 Bc = 0
14611 0 394 0 1

And Tc = CxCxc-1,
Cxc = |B c Ac B c Ac
n1
Bc|

Cc = C Tc..

Cc = |
6514.9 0 82.2 0
7136.8 0 235 0 | Dc =D =[0;0]

3.d Observable Form : The observable canonical form can be found by transposing
matrices in controllable canonical form

Ao = Ac'; Bo = Cc'; Co = Bc'; Do = D;

Therefore we get following results:

| |
0 0 0 14611
1 0 0 0
Ao = 0 1 0 394
0 0 1 0

Co = |0 0 0 1|

| |
6514.9 7136.8
0 0
Bo = 82.2 235 D0 = D =[0;0]
0 0

7
3.e Jordan Canonical Form :
For calculating the Jordan form we need modal matrix M and the eigen values of the matrix A so
as to create diagonal Jordan Matrix .
The Modal Matrix and the Jordan Matrix is calculated in MATLAB by using the command
[M,J]=eig(A)

| |
.0142 .0142 0.0887 i .0887 i
0.2932 0 .2932 0.5181 0.5181
M = 0.0461 0.0461 0.1436 i 00.1436 i
0.9548 0.9548 0.8385 0.8385

Now Jordan Matrices are obtained as

Aj= J Bj = inv(M)*B; Cj = C*M; Dj = D

| |
20.7 0 0 0
0 20.7 0 0
Aj = 0 0 5.84 i 0
0 0 o 5.84 i

| |
106.09
106.09
Bj = 19.33
19.33

Cj = |
0.0142 0.0142 0.0887 i 0.0887 i
0.0461 0.0461 0.1436 i 0.1436 i |

8
4. IMPULSE RESPONSE AND STEP RESPONSE

(a) Impulse Response

Assuming zero initial conditions the impulse response of the two output transfer
functions is calculated by using the MATLAB command impulse (sys_invpend) and
the result is shown in figure below. The output graph 1 is for Position x and the

output plot 2 is for the angle

9
 27
x 10 Impulse Response
2

1.5
To: Out(1)

0.5
Amplitude

0 27
x 10
0

-1
To: Out(2)

-2

-3

-4

-5
0 0.5 1 1.5 2 2.5 3
Time (seconds)

(b) Step Response: For step response , the MATLAB command used is step (sys_invpend)
Output plot 1 is for Position x and the output plot 2 is for the angle .

10
 25
x 10 Step Response
8

6
To: Out(1)

2
Amplitude

0 26
x 10
0

-0.5
To: Out(2)

-1

-1.5

-2

-2.5
0 0.5 1 1.5 2 2.5 3
Time (seconds)

11
5. BODE PLOT AND ROOT-LOCUS OF THE UNCOMPENSATED
SYSTEM

Standard MATLAB commands (discussed in Appendix) are used to find bode and root
locus plots for two output transfer functions

(a)Bode Plot for Position transfer function

(b)Root locus for position transfer function

Root Locus
15

10
Imaginary Axis (seconds-1)

-5

-10

-15
-25 -20 -15 -10 -5 0 5 10 15 20 25
Real Axis (seconds -1)

12
(c) Bode Plot for angle transfer function

Bode Diagram
150

100
Magnitude (dB)

50

-50
180

135
Phase (deg)

90

45

-45
-1 0 1 2
10 10 10 10
Frequency (rad/s)

(d)Root-Locus for angle transfer function

Root Locus
8

4
Im aginary Ax is (s ec onds
-1
)

-2

-4

-6

-8
-50 -40 -30 -20 -10 0 10 20 30 40 50
Real Axis (seconds -1)

13
 6. CONTROLLER DESIGN (USING CLASSICAL CONTROL
THEORY):

For designing a pid controller following table was considered while varying the
values of gains to get the desired output.

CL OVERSHOO SETTLING S-S

RISE TIME
RESPONSE T TIME ERROR

Kp Decrease Increase Small Change Decrease

Ki Decrease Increase Increase Eliminate

Kd Small Change Decrease Decrease No Change

Controller for Angle control:

To design the controller, following specifications were selected.

Damping ratio is 0.707.

Settling time less than 0.4 sec. So Time constant is 0.1sec.

The PID gains are achieved by an iterative process of changing individual parameter to tune
in the requirements for each output transfer function. There is stable . After an intensive search

14
looking for the best parameters to be applied to control the system, the following results were
found:

( S)
For U (S) , PID parameters are Kp = -900; Kd = -0.5; Ki = -50; The controller transfer

function obtained is: (-0.5 s^2 - 900 s 50)/ s.

The step response achieved with this control:

Step Response
1.8

1.6

1.4

1.2

1
Amplitude

0.8

0.6

0.4

0.2

0
0 0.02 0.04 0.06 0.08 0.1 0.12
Time (seconds)

Settling time 0.09 sec,

Wn= 62.86 rad.

Controller for Position Control:

X (S)
Controller for U (S) with PID parameters: Kp = 3000;Kd = 1.2; Ki = 10. The controller

transfer function obtained is: (1.2 s^2 + 3000 s + 10) /s.

15
 Step Response
1.8

1.6

1.4

1.2

1
Amplitude

0.8

0.6

0.4

0.2

0
0 0.02 0.04 0.06 0.08 0.1 0.12
Time (seconds)

Settling time 0.1 sec

Wn= 56.58 rad

No comments can be made about the damping ratio since the system behaviour does not
correspond to a 2nd order system

7. STEP RESPONSE, SQUARE RESPONSE, SINE WAVE

RESPONSE FOR CLOSED LOOP SYSTEM

( S)
Response for U (S) =the closed system transfer function is:

117.6 s^4 + 2.118e05 s^3 + 8275 s^2 - 6.282e06 s - 3.49e05

----------------------------------------------------------------

16
 s^5 + 117.6 s^4 + 2.114e05 s^3 + 8275 s^2 - 6.296e06 s - 3.49e05

Step Response:

Step Response
1.8

1.6

1.4

1.2

1
Amplitude

0.8

0.6

0.4

0.2

0
0 0.02 0.04 0.06 0.08 0.1 0.12
Time (seconds)

Square Wave Response

17
 18
x 10
4.5
Square wave response for angle control
4

3.5

2.5

1.5

0.5

0
0 1 2 3 4 5 6 7 8 9 10

Sinusoidal Wave Response

20
x 10
4.5
Sine Wave response for angle control
4

3.5

2.5

1.5

0.5

0
0 1 2 3 4 5 6 7 8 9 10

18
Response for TF2

98.8 s^4 + 2.47e05 s^3 - 6926 s^2 - 1.937e07 s - 6.458e04

X ( S)
U (S) = ---------------------------------------------------------------------

s^5 + 98.8 s^4 + 2.466e05 s^3 - 6926 s^2 - 1.939e07 s - 6.458e04

Step Response:

Step Response
1.8

1.6

1.4

1.2

1
Amplitude

0.8

0.6

0.4

0.2

0
0 0.02 0.04 0.06 0.08 0.1 0.12
Time (seconds)

Square Wave Response:

19
 31
x 10
5
Square wave response for position control
4.5

3.5

2.5

1.5

0.5

0
0 1 2 3 4 5 6 7 8 9 10

Sinusoidal response :

20
 34
x 10
12
Sine wave response for position control

10

0
0 1 2 3 4 5 6 7 8 9 10

The response of the system with the designed controller is controllable for step input but is
uncontrollable for square and sinusoidal inputs.

8. CONTROL INPUT SIGNALS

Input signal for Square Input

21
 1

0.9

0.8

0.7

0.6

0.5

0.4

0.3

0.2

0.1

0
0 1 2 3 4 5 6 7 8 9 10

Input signal for Sine Input

0.8

0.6

0.4

0.2

-0.2

-0.4

-0.6

-0.8

-1
0 1 2 3 4 5 6 7 8 9 10

9.ROBUSTNESS OF THE SYSTEM :

Systems robustness is tested by observing the root locus for closed loop system transfer
functions. A step disturbance is introduced to see the effect on the behaviour of closed loop
transfer functions.

22
 (S )

Robustness of transfer function :

Root Locus
6000

4000
Im aginary Ax is (s ec onds
)

2000
-1

-2000

-4000

-6000
-20000 -15000 -10000 -5000 0 5000
Real Axis (seconds -1)

Response to step disturbance:

23
 X ( S)
Robustness of transfer function U (S) ;

Root Locus
3000

2000
Imaginary Axis (seconds)

1000
-1

-1000

-2000

-3000
-9000 -8000 -7000 -6000 -5000 -4000 -3000 -2000 -1000 0 1000
-1
Real Axis (seconds )

Response to step disturbance:

12
x 10 Step Response
4

1
Amplitude

-1

-2

-3

-4
0 0.5 1 1.5 2 2.5 3 3.5
Time (seconds)

24
 From Root Locus it is evident that the stability region is very small for these two transfer
functions ,So if the gain is varied slightly the system will go to the instable state. Also, the step
disturbance input results in unstable system . Therefore, it can be said that the system under
control is not very robust with respect to changes in its parameters or disturbance input.

10. FULL STATE FEEDBACK CONTROL

The design of this state feedback controller is done with the similar transient specifications:

Damping ratio = 0.707

Settling time < 0.4 sec

In order to comply with the specifications, the system will be approximated to a second
order system with the following characteristic polynomial:

= 0.707;

wn= 1/ = 10; wn= 14.14

2 2
The characteristic polynomial will be approximated to: S +2 w nS +wn

S 2 +20 S +200

Poles at: -10 10i

The other poles will be placed 3-4 times farther from these conjugated poles, to force the
behavior to be as approximated as possible to a second order system. Given the fact that our
system is a fourth order system, it will have 4 poles and because of that the pole placement for
the system will be forced to be the following:

-10 10i, -30 and -40.

Poles = [-10+10i -10-10i -30 -40]; K = place (A, B, Poles);

And K= [-86.9858 -10.1242 -44.0342 -3.9260]

Given this State feedback matrix, the A matrix of the system will be changed in the
following manner: Af= A BK;

25
 | |
0 1 0 0
6663 833 3339 323
Af = 0 0 0 1
18954 2379 9463 923

11. STEP RESPONSE, SQUARE WAVE RESPONSE, SINE

WAVE RESPONSE FOR CLOSED LOOP SYSTEM WITH STATE
FEEDBACK CONTROL

Step Response
0.1

0
To: Out(1)

-0.1

-0.2
Amplitude

-0.3
0.4
Step response for Full State feedback control
0.2
To: Out(2)

-0.2

-0.4
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (seconds)

26
 Square response for Full State feedback control
0.5
Position
0.4 Theta

0.3

0.2

0.1

-0.1

-0.2

-0.3
0 1 2 3 4 5 6 7 8 9 10

Sine response for Full State feedback control

0.3
Position
Theta
0.2

0.1

-0.1

-0.2

-0.3

-0.4
0 1 2 3 4 5 6 7 8 9 10

So by comparing these results with section 7 ( i.e. response with pid

controller) it can be seen that the state feedback control gives better control
in comparison to the pid control design.

27
12 FULL-ORDER OBSERVER
The controller was calculated to provide a systems time constant () equal to 0.1 sec. In order to
provide an acceptable estimator for the system the required time constant of the observer to be
from 2 to 5 times faster; therefore choosing 5 times faster, the following parameters are used for
the observer:

= 0.02

= 0.707

wn= 70.72

Considering the parameters, the desired 2nd order characteristic polynomial for the system
2
will be: S +100 S+ 5000

Characteristic polynomial poles: -50 50i

Because the present system is a 4 th order system two other poles have to be assumed.
These poles will be placed over the real axes, 3-4 times farther than the conjugated pair from the
origin. The desired observers pole location: -50 50i, -150,-200. The command place is
used to provide the estimator gain matrix G.

Pole_of_obs=[-50+50i -50-50i -150 -200]; G = place(A',C',Pole_of_obs)';

Here, the det( SI A + GC) is equated to the polynomial formed from the desired poles location
to find the coefficients of G.

| |
247.5 38.6
9126.5 7843.7
G= 54.8 202.5
7323.1 9037.3

Then the full order observer state space equations are found out using the matrices:

Ae =A-G*C-B*K;

Be =G; Ce =-K; De=[0 0];

sys_obs_pend = ss(Ae,Be,Ce,De);

Full-Order Observer Step Response:

28
 Step Response
From: In(1) From: In(2)
60

40

20
Am plitude

-20

-40

-60
0 1 2 3 4 5 0 1 2 3 4 5
Time (seconds)

Output 1 is for positon and output 2 is for angle.

13. REDUCED-ORDER OBSERVER DESIGN

We need not to observe all the 4 states as we any measure two outputs as angle
and position and these are our known states x1 and x3 for reduced order observer.

Therefore by rearranging the state equations we get

A11 = [0 0;0 0];

A1e = [1 0;0 1];
Ae1 = [-491.03 -282.35; 1487.95 885.35];
Aee = [0 0; 0 0];
B1 =[0;0];
Be =[82.24;-235];
K1 =[-86.98 -44] Ke =[-10.12 -3.92];
Ge = [15 -5; 25 5];
Arce = Aee - (Ge*A1e) - (Be*Ke) + (Ge*B1*Ke);

Brce = Ae1 - (Ge*A11) + (Aee*Ge) - (Ge*A1e*Ge) - (Be*K1) + (Ge*B1*K1) - (Be*Ke*Ge) +

(Ge*B1*Ke*Ge);

29
Crce = -Ke;
Drce = -K1 - (Ke*Ge);
sys_red_obs = ss(Arce,Brce,Crce,Drce);

Step response reduced order observer response:

30
 Step Response
From: In(1) From: In(2)
350

300

250

200

150
Amplitude

100

50

-50

-100

-150
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (seconds)

14. OBSERVER-CONTROLLER TRANSFER FUNCTION

Closed loop system with full order Observer - Controller :

Ace = [A-B*K B*K];

zeros(size(A)) (A-G*C)];
Bce = [B*Nbar;
zeros(size(B))];
Cce = [C zeros(size(C))]; Dce = [0;0];
full_obs_cont = ss(Ace,Bce,Cce,Dce);
tf_full=tf(full_obs_cont);

step(tf_full,5);grid

Transfer functions:

31
 X (S) 7154 S2 +561800
= 4
U (S) S 90 S3 +2800 S 2 +38000 s+ 240000

(S) 20440 S26,06900

= 4
U (S) S 90 S3 +2800 S 2 +38000 s+ 240000

Step Response for closed full observer- controller system:

The resulting Transfer function of the controller-observer adds 4 poles and 3 zeros to the plants
transfer function. If we try to obtain an equivalent controller from the classical point of view, it is
possible to say that this controller is 3 differentiators and 4 integrators.

It can be seen that the step response given by the system under control complies in a very
nice manner the original specifications proposed. The settling time is less than 1 second and the
damping ratio is around the 0.707 value.

Step Response
4

2
To: Out(1)

-1
Amplitude

-2
6

2
To: Out(2)

-2

-4

-6
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (seconds)

32
 It can be seen that the step response given by the system under control is responding as
per our design specifications. The settling time is less than 0.4 second and the damping ratio is
around the 0.707 value.

15. COMPARATIVE STUDY BETWEEN CLASSICAL CONTROL

DESIGN AND MODERN CONTROL THEORY DESIGN

In the project by applying these two different approaches, two different controllers
are designed. The inverter pendulum system is controlled by designing a controller
by adopting classical control techniques and Modern Control techniques.

The Classical control approach is inefficient and test designers experience in

reaching to a control solution for the stated problem. It is basically a hit and trial
approach where the gain of the controller is changed and the response of the
system is monitored till we reach to particular gain values that meet the
specifications and result in a stable system. So it takes lot of time to reach an
expected value and then too the accuracy is not guaranteed.

The modern control theory provides an easy-fast and efficient way of solving a
control problem on the other hand.

We can make a unstable system stable by placing the poles at the desired locations
in modern approach. As we have seen from the results above , it is possible to
place the poles almost perfectly at the desired pole locations without the time
consuming iterations. This is done easily by understanding the mathematical model
of the system.

There are few disadvantage of the modern control theory, to name a few: one is that
it needs a good understanding of the mathematical model of the system to solve
the problem of control system in an efficient way.

Also we get valuable information on the control problems from the notions of
Controllability and Observability that provides the knowledge about the system
behaviour prior to designing a controller or observer. But there is no such notions
available in Classical control Theory.

33
APPENDIX: A- MATLAB COMMANDS

%%%%%%%%%%%State Space Equations%%%%%%%%%%%

Var_A= [0 1 0 0;-491.03 0 -282.35 0; 0 0 0 1; 1487.95 0 885.35 0];
Var_B= [0; 82.24; 0;-235];
Var_C= [1 0 0 0; 0 0 1 0];
Var_D= [0; 0];
sys_invpend= ss(Var_A,Var_B,Var_C,Var_D);

%%%%%%%%%%%Transfer function Open Loop%%%%%%%%%%%

H = tf(sys_invpend);
Tf_Theta =H(2);
Tf_Pos =H(1);

%%%%%%%%%%%Var_Canonical forms%%%%%%%%%%%

%%%%%%%%%%%Var_Contrability test%%%%%%%%%%%
Var_Cx = ctrb(sys_invpend);
rank(Var_Cx);

%%%%%%%%%%%Observability test%%%%%%%%%%%
Ox = obsv(sys_invpend);
rank(Ox);

%%%%%%%%%%%Contrallable canonical form%%%%%%%%%%%

[P] = poly(Var_A);
Var_Ac = [0 1 0 0; 0 0 1 0;0 0 0 1;14611 0 394 0];
Var_Bc = [0;0;0;1];
Var_Cxc = [Var_Bc Var_Ac*Var_Bc Var_Ac*Var_Ac*Var_Bc
Var_Ac*Var_Ac*Var_Ac*Var_Bc];
Tc = Var_Cx*(inv(Var_Cxc));
Var_Cc = Var_C*Tc;
Var_Dc = Var_D;

%%%%%%%%%%%Observable canonical form%%%%%%%%%%%

Var_Ao = Var_Ac';
Var_Bo = Var_Cc';
Var_Co = Var_Bc';
Var_Do = Var_D;

%%%%%%%%%%%Jordan canonical form%%%%%%%%%%%

[M,J] = eig(Var_A);
Var_Aj = J;
Var_Bj = inv(M)*Var_B;
Var_Cj = Var_C*M;
Var_Dj = Var_D;

%%%%%%%%%%%Open loop responses%%%%%%%%%%%

figure

34
impulse(sys_invpend);grid
figure
step(sys_invpend);grid

%%%%%%%%%%%Var_Bode plot and root locus%%%%%%%%%%%

figure
bode(Tf_Pos);
figure
rlocus(Tf_Pos);
figure
bode(Tf_Theta);
figure
rlocus(Tf_Theta);

%%%%%%%%%%%Signal Generation%%%%%%%%%%%
[Var_Asq,tsq] = gensig('square',2,10,0.01);
[Var_Asn,tsn] = gensig('sin',2,10,0.01);

%%%%%%%%%%%Angular control%%%%%%%%%%%

Controller_Kp = -900;
Controller_Kd = -0.5;
Controller_Ki = -50;
Var_Contrl1=tf([Controller_Kd Controller_Kp Controller_Ki],[1 0]);
Var_CL_1 = Var_Contrl1* Tf_Theta;
pid_theta= feedback(Var_CL_1,1);

%%%%%%%%%%%Step response%%%%%%%%%%%
figure
step(pid_theta);grid

%%%%%%%%%%%Square wave response%%%%%%%%%%%

[y,t]=lsim(pid_theta,Var_Asq,tsq);
figure
plot(t,y); grid
legend('Square wave response for angle control')

%%%%%%%%%%%Sine wave response%%%%%%%%%%%

[y,t]=lsim(pid_theta,Var_Asn,tsn);
figure
plot(t,y); grid
legend('Sine Wave response for angle control')

%%%%%%%%%%%Position control%%%%%%%%%%%

Controller_Kd = 1.2;
Controller_Kp =3000;
Controller_Ki = 10;
Var_Contrl2=tf([Controller_Kd Controller_Kp Controller_Ki],[1 0]);
Var_CL_2= Tf_Pos * Var_Contrl2;
pid_pos= feedback(Var_CL_2,1);

%%%%%%%%%%%Step response%%%%%%%%%%%
figure

35
step(pid_pos);grid

%%%%%%%%%%%Square wave response%%%%%%%%%%%

[y,t]=lsim(pid_pos,Var_Asq,tsq);
figure
plot(t,y);grid
legend('Square response for position control')

%%%%%%%%%%%%%%%%%%%Sine wave response%%%%%%%%%%%%%%%%%%

[y,t]=lsim(pid_pos,Var_Asn,tsn);
figure
plot(t,y);grid
legend('Sine response for position control')

%%%%%%%%%%%%%%%%%%Input control Signals%%%%%%%%%%%%%%%%%%

plot(t,Var_Asq)
plot(t,Var_Asn)

%%%%%%%%%%%%%%%%%%Robustness of the system %%%%%%%%%%%%%%%%%%

figure
rlocus(pid_pos);
figure
rlocus(pid_theta);

Var_CL_distr1 = 1 + Var_CL_1;
Var_Dist1 = Tf_Theta / Var_CL_distr1;
figure
step(Var_Dist1)

Var_CL_distr2 = 1 + Var_CL_2;
Var_Dist2 = Tf_Pos / Var_CL_distr2;
figure
step(Var_Dist2)
%%%%%%%%%%%%%%%%%%State feedback control %%%%%%%%%%%%%%%%%%
Poles = [-10+10i -10-10i -30 -40];
K = place(Var_A,Var_B,Poles);
Var_Af = Var_A-Var_B*K;
sys_fdbk = ss(Var_Af,Var_B,Var_C,Var_D);
Tf_fdbk =tf(sys_fdbk);

%%%%%%%%%%%%%%%%%%Step , Square and Sine wave response%%%%%%%%%%%%%%%%%%

figure
step(Tf_fdbk,5);grid
legend('Step response for State feedback control');

[y,t,x]=lsim(sys_fdbk,Var_Asq,tsq);
figure
plot(t,y);grid
title('Square response for State feedback control')
legend('Pos','Theta');

[y,t,x]=lsim(sys_fdbk,Var_Asn,tsn);
figure

36
plot(t,y);grid
title('Sine response for State feedback control')
legend('Pos','Theta');

%%%%%%%%%%%%%%%%%%Full order and reduced order observer%%%%%%%%%%%%%%%%%%

Pole_of_obs=[-50+50i -50-50i -150 -200];

G = place(Var_A',Var_C',Pole_of_obs)';
Var_Ae =Var_A-G*Var_C-Var_B*K;
Var_Be =G;
Var_Ce =-K;
Var_De=[0 0];
sys_obs_pend = ss(Var_Ae,Var_Be,Var_Ce,Var_De);
tf_sys_obs = tf(sys_obs_pend);

figure
step(sys_obs_pend,5);grid

Var_Ace = [Var_A-Var_B*K Var_B*K zeros(size(Var_A)) (Var_A-G*Var_C)];

Var_Bce = [Var_B zeros(size(Var_B))];
Var_Cce = [Var_C zeros(size(Var_C))];
Var_Dce = [0;0];
sys_full_obs = ss(Var_Ace,Var_Bce,Var_Cce,Var_Dce);
tf_sys_full_obs = tf(sys_full_obs);

figure
step(sys_full_obs,5);grid

[y,t,x]=lsim(sys_full_obs,Var_Asn,tsn);
figure
plot(t,y);grid
title('Sine response for Var_Closed loop Full order Observer -
Var_Controller')
legend('Pos','Theta');

% Reduced order observer

Ge = [15 -5; 25 5];% reduced order estimator gain matrix
Var_A11 = [0 0;0 0];
Var_A1e = [1 0;0 1];
Var_Ae1 = [-491.03 -282.35; 1487.95 885.35];
Var_Aee = [0 0; 0 0];
Var_B1 =[0;0]; Var_Be =[82.24;-235];
K1 =[K(1) K(3)]; Ke =[K(2) K(4)];
Var_Arce = Var_Aee - (Ge*Var_A1e) - (Var_Be*Ke) + (Ge*Var_B1*Ke);
Var_Brce = Var_Ae1 - (Ge*Var_A11) + (Var_Aee*Ge) - (Ge*Var_A1e*Ge) -
(Var_Be*K1) + (Ge*Var_B1*K1) - (Var_Be*Ke*Ge) + (Ge*Var_B1*Ke*Ge);
Var_Crce = -Ke;
Var_Drce = -K1 - (Ke*Ge);
red_obs_pend = ss(Var_Arce,Var_Brce,Var_Crce,Var_Drce);
tf_red_obs_pend = tf(red_obs_pend);

figure
step(red_obs_pend,5);grid

37
38