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
