Вы находитесь на странице: 1из 39

Prepared

_______________________
A reportby:
submitted
in the

requirements of ENGR 6131

Project Report on
Simulation of Inverted
pendulum Control

Table of contents
S.
NO.

Topic

INTRODUCTION
1.
2.
3.
4.
5.
6.

Page
No.
2

STATE EQUATIONS AND OUTPUT EQUATIONS OF THE


SYSTEM
TRANSFER FUNCTION - OPEN LOOP SYSTEM :
CONTROLLABLE, OBSERVABLE AND JORDAN CANONICAL
FORMS
IMPULSE RESPONSE AND STEP RESPONSE
BODE PLOT AND ROOT-LOCUS OF THE UNCOMPENSATED
SYSTEM
CONTROLLER DESIGN- CLASSICAL CONTROL THEORY

3
4
5
8
9
10

7.

STEP RESPONSE, SQUARE RESPONSE, SINE WAVE


RESPONSE FOR CLOSED LOOP SYSTEM

14

8.

CONTROL INPUT SIGNALS

19

9.

ROBUSTNESS OF THE SYSTEM

20

10.

FULL STATE FEEDBACK CONTROL

22

11.

STEP RESPONSE, SQUARE WAVE RESPONSE, SINE WAVE


RESPONSE (FEEDBACK CONTROL)

23

12.

FULL-ORDER DESIGN

24

13.

REDUCED-ORDER OBSERVER DESIGN

26

14.

OBSERVER-CONTROLLER TRANSFER FUNCTION

27

15.

COMPARATIVE STUDY BETWEEN CLASSICAL CONTROL


DESIGN AND MODERN CONTROL THEORY DESIGN

29

APPENDIX-A- MATLAB COMMANDS

INTRODUCTION:
1

29

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
m1 lo

x (t) + J
o

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


1 o
1

(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
m2
lo
Jo
lc
g

0.213 kg
1.785 Kg
0.330 m
0.0246 Kg-m2
0.071 m
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 (t) ,

x 2(t) =

x 3 (t) =

(t),

(t)

x4 (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
x

= x2
=

m1 lo

m1 lo 2J 0

)[

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

= x4

x1(t)

Jo
1
+
m1 l o m1 l o ( m 1 l o2J o )

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
1
o2 +
m1 l o m1 lo
lo

]}

+F(t)

Output Equations:

(t) = x (t)
3
x(t) = x1 (t)

By replacing the values of the constant parameter by the values from given
table

| || |

1
0
0 x1
0
0 282.35 0 x 2 82.24
+
[ F(t ) ]
0
0
1 x3
0
0 885.35 0 x 4 235

0
491.03
x =
0
1,487.95

y=

| |
(t)
x (t)

||

x1
0 0 1 0 x2
1 0 0 0 x3
x4

A state space model for


command

the system can be created in MATLAB by using the

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

TF(S) =

Y (S)
1
=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
location same but different zero locations.

pole

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

Tf_Theta =H(2) =

(S)
235 S 2+ 6,977
= 4
U (S) S 394.3 S 21,4610
X ( S)
82.24 S 26,459
= 4
U (S) S 394.3 S 214,610

Tf_Pos = H(1)=

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

n1

B|

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

Cx =

0
82
0
25970
82
0
25970
0
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

Ox =

||
C
CA
2
CA

C A n1

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

Ox =

0
0
1
0
1
0
0
0
0
0
0
1
0
1
0
0
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

Ac =

0
0
0
14611

1 0
0 1
0 0
0 394

0
0
1
0

Bc =

||
0
0
0
1

And Tc = CxCxc-1,
Cxc =

|B

Ac B c

Ac

n1

Bc|

Cc = C Tc..
Cc =

3.d

6514.9 0 82.2 0
7136.8 0 235 0

Dc =D =[0;0]

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:

Ao =

Co =

Bo =

0
1
0
0

0
0
1
0

0 14611
0
0
0 394
1
0

|0 0 0 1|

6514.9 7136.8
0
0
82.2
235
0
0

D0 = D =[0;0]

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)

M =

.0142 .0142
0.0887 i .0887 i
0.2932 0 .2932 0.5181 0.5181
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

Aj =

Bj =

Cj =

Bj = inv(M)*B;

Cj = C*M;

20.7
0
0
0
0
20.7
0
0
0
0
5.84 i
0
0
0
o
5.84 i

| |
106.09
106.09
19.33
19.33

0.0142 0.0142 0.0887 i 0.0887 i


0.0461
0.0461 0.1436 i 0.1436 i

Dj = D

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
output plot 2 is for the angle

x and the

27

Impulse Response

x 10

To: Out(1)

1.5
1

Amplitude

0.5
27
0
x 10
0

To: Out(2)

-1
-2
-3
-4
-5

0.5

1.5

2.5

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

Step Response

x 10

To: Out(1)

6
4

Amplitude

2
26
0
x 10
0

To: Out(2)

-0.5
-1
-1.5
-2
-2.5
0

0.5

1.5
Time (seconds)

11

2.5

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

Imaginary Axis (seconds-1)

10

-5

-10

-15
-25

-20

-15

-10

-5

Real Axis (seconds -1)

12

10

15

20

25

(c) Bode Plot for angle transfer function


Bode Diagram

Magnitude (dB)

150
100
50
0
-50
180

Phase (deg)

135
90
45
0
-45

-1

10

10

10

10

Frequency (rad/s)

(d)Root-Locus for angle transfer function

Root Locus

-1
Im aginary Ax is (s ec onds
)

6
4
2
0
-2
-4
-6
-8
-50

-40

-30

-20

-10

10

Real Axis (seconds -1)

13

20

30

40

50

6. CONTROLLER
THEORY):

DESIGN

(USING

CLASSICAL

CONTROL

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

RISE TIME

OVERSHOO
T

SETTLING
TIME

S-S
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:
For

( S)
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

Amplitude

1.2
1
0.8
0.6
0.4
0.2
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:
Controller for

X (S)
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

Amplitude

0.8

0.6

0.4

0.2

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,


RESPONSE FOR CLOSED LOOP SYSTEM

Response for

( S)
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

SINE

WAVE

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

Amplitude

1.2
1
0.8
0.6
0.4
0.2
0

0.02

0.04

0.06

0.08

Time (seconds)

Square Wave Response

17

0.1

0.12

18

4.5

x 10

Square wave response for angle control

4
3.5
3
2.5
2
1.5
1
0.5
0

10

Sinusoidal Wave Response


20

4.5

x 10

Sine Wave response for angle control


4
3.5
3
2.5
2
1.5
1
0.5
0

18

10

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

Amplitude

1.2
1
0.8
0.6
0.4
0.2
0

0.02

0.04

0.06

0.08

Time (seconds)

Square Wave Response:


19

0.1

0.12

31

x 10

Square wave response for position control

4.5
4
3.5
3
2.5
2
1.5
1
0.5
0

Sinusoidal response :

20

10

34

12

x 10

Sine wave response for position control


10

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

10

Input signal for Sine Input


1
0.8
0.6
0.4
0.2
0
-0.2
-0.4
-0.6
-0.8
-1

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

-1
Im aginary Ax is (s ec onds
)

4000

2000

-2000

-4000

-6000
-20000

-15000

-10000

-5000

Real Axis (seconds -1)

Response to step disturbance:

23

5000

Robustness of transfer function

X ( S)
U (S) ;

Root Locus

3000

1000

-1

Imaginary Axis (seconds)

2000

-1000

-2000

-3000
-9000

-8000

-7000

-6000

-5000

-4000

-3000

-2000

-1000

-1

Real Axis (seconds )

Response to step disturbance:

12

Step Response

x 10

3
2

Amplitude

1
0
-1
-2
-3
-4

0.5

1.5

2.5

Time (seconds)

24

3.5

1000

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

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
0
0
0
1
18954 2379 9463 923

Af =

11.
STEP RESPONSE, SQUARE WAVE RESPONSE, SINE
WAVE RESPONSE FOR CLOSED LOOP SYSTEM WITH STATE
FEEDBACK CONTROL

Step Response

0.1

To: Out(1)

0
-0.1

Amplitude

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

0.2
0
-0.2
-0.4

0.5

1.5

2.5
Time (seconds)

26

3.5

4.5

Square response for Full State feedback control

0.5

Position
Theta

0.4
0.3
0.2
0.1
0
-0.1
-0.2
-0.3

10

Sine response for Full State feedback control

0.3

Position
Theta

0.2
0.1
0
-0.1
-0.2
-0.3
-0.4

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

S +100 S+ 5000

will be:

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.

G=

247.5
38.6
9126.5 7843.7
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)

60

From: In(2)

40

Am plitude

20

-20

-40

-60

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)

350

From: In(2)

300

250

200

Amplitude

150

100

50

-50

-100

-150

0.5

1.5

2.5

3.5

4.5

5 0

0.5

1.5

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

3.5

4.5

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

To: Out(1)

Amplitude

-1

-2
6

To: Out(2)

-2

-4

-6

0.5

1.5

2.5
Time (seconds)

32

3.5

4.5

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