Professional Documents
Culture Documents
Controlling An Inverted Pendulum Using S PDF
Controlling An Inverted Pendulum Using S PDF
Tsegazeab Shishaye
[ID:2012420012]
Abstract
In this paper an inverted pendulum is presented using state space modeling method. And full state feedback controller is
developed using pole placement and LQR (Linear Quadratic Regulation) methods. After that, tracking problem is
addressed by designing a steady state error controller. Then, considering the reality conditions, first assuming by some of
the state variables are measurable, a reduced state observer is designed and then for a worst scenario a full order state
observer is designed. Finally, the state feedback controller and the state observer are summed up to give a pre-
compensator and an overall steady state error controller is added to that new system. All mathematical modeling are
presented clearly and simulations together with their analysis were done using MATLAB software. For clear view on
what is going on with the control method and the system, an animation GUI is also presented.
Introduction
As a typical control system, the control of an inverted pendulum is excellent in testing and evaluating different control
methods. Its popularity derives in part from the fact that it is unstable without control, that is, the pendulum will simply
fall over if the cart isn't moved to balance it. Additionally, the dynamics of the system are nonlinear. The objective of the
control system is to balance the inverted pendulum by applying a force to the cart that the pendulum is attached to. A real-
world example that relates directly to this inverted pendulum system is the attitude control of a booster rocket at takeoff
but The fundamental principles within this control system can be found in many industrial applications, such as stability
control of walking robots, vibration control of launching platform for shuttles etc
Problem formulation
Given parameters:
- Linearizing the non-linear equation of motion of the system around θ=0 and
- Finding the associated state space model of the system
System modeling
Since the analysis (state space model) and control design techniques that I will employ in this problem apply only to linear
systems, these equations need to be linearized. Specifically, I will linearize the equations about the vertically upward
equilibrium position, θ = 0, and will assume that the system stays within a small neighborhood of this equilibrium.
Linearization
Here, instead of using the linearization method (Taylor method for small perturbations), I will use simple approximation
around θ = 0. This assumption is reasonably valid since under control it is desired that the pendulum not deviate more
than 20 degrees (0.35 radians) from the vertically upward position.
= , And
= 1, (According to the above diagram will lie in the first quadrant. so, the Cosine function is positive)
Also dropping all the non-linear components ( ̇ = 0) the above equations will become:
( + ) ̈+ ̇+ ̈= …………….. (3)
( + ) ̈− θ = −m ẍ …………….. (4)
Solving for ̈ and ̈ independently from equation (3) and equation (4) respectively gives,
̈= ̈− ̇+ …………………. (5)
̈= ̈+ …………………………. (6)
( )
̈= + ̇−
( ) ( ) ( )
( )
̈= ( )
− ( )
̇+ ( )
State-space model
̇ =
( + )
̇ = + −
( + )+ ( + ) ( + )+
̇ =
− ( + ) +
̇ = − +
( + )+ ( + )+ ( + )+
̇ = +
= +
So, substituting the above equations in to the matrices properly will give the following state space representation.
0 1 0 0 0
⎡ ⎤ ⎡ ⎤
̇ ⎢ ( )
0 0 ⎥ ⎢− ⎥
̇ ⎢ ( ) ( ) ⎥ ⎢ ( ) ⎥
̇= =⎢ ⎥ +⎢ ⎥ ……………. (7)
̇ 0 0 0 1 0
̇ ⎢ ⎥ ⎢ ⎥
⎢ ( ) ⎥ ⎢ ⎥
⎣ ( )
0 0 − ( ) ⎦ ⎣ ( ) ⎦
0 0 1 0 0
= = + ………………………. (8)
1 0 0 0 0
In MATLAB,
N.B.
Examining the results, especially the numerators, in the transfer functions of each output solved from the state space
model, there exists some terms with very small coefficients. These terms should actually be zero, they only show up due
to numerical round-off errors that accumulate in the conversion algorithms that MATLAB employs. This can be checked
by solving the transfer functions of the cart and the pendulum independently from the motion equations using Laplace
transform by assuming zero initial conditions.
#Tsak-2
- Simulating the dynamic behavior of the system under impulse force and step force
- Analyzing the stability, controllability and observability condition of the system
System analysis
Examining on how the system responds to a 1-Nsec impulsive force applied to the cart, I found the following result.
60
To: x
40
20
Amplitude
0
0
-100
To: theta
-200
-300
-400
-500
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time (sec)
From the plot, the response is unsatisfactory. Both outputs never settle, the angle of the pendulum goes to several
hundred radians in a clockwise direction though it should be less than 0.35 rad. And the cart goes to the right infinitely.
So, this system is unstable in an open loop condition when there is a small impulsive force applied to the cart.
2. Open-loop step response of the system
Here also, it can be seen from the outputs that the system is unstable under 1-Newton step input applied to the cart. The
outputs are found by using the command of MATLAB which can be employed to simulate the response of LTI
models to arbitrary inputs.
The plot, fig.2, shows that the responses of the system to a step force are unstable.
30
20
10
-10
-20
-30
-40
-50
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
To check stability means to analyze whether the open-loop system (without any control) is stable. That has partly done by
the above simulations under the impulse and step forces. But as per the definition, the eigenvalues of the system state
matrix, A, can determine the stability. That is equivalent to finding the poles of the transfer function of the system. The
eigenvalues of the A matrix are the values of s where ( − ) = . A system is stable if all its poles have lied in the
left-half of the s-plane.
≫ poles = eig(A)
≫ Poles =
7.1430
-7.2220
-0.1000
Or it can be shown by pole-zero mapping of the system as below using MATLAB command
≫ ( )
0.8
0.6
0.4
0.2
Imaginary Axis
-0.2
-0.4
-0.6
-0.8
-1
-8 -6 -4 -2 0 2 4 6 8
Real Axis
As can be seen from the output, there is one pole on the right-half plane at 7.1430. This confirms the intuition that the
system is unstable in open loop.
A system is controllable if there exists a control input ( ) that transfers any state of the system to zero in finite time. It
can be shown that an LTI system is controllable if and only if its controllability matrix, CO, has full rank. i.e. if
( ) = , where n is the number of states.
So, the necessary and sufficient condition for controllability of the system is:
( )= [ … ]=
Using the following MATLAB command, the controllability matrix of the system is:
≫RankCO = rank ( ( , ))
RankCO= 4
So, since the controllability matrix has full rank, the system is controllable!
Conceptually, a system is observable if the initial state, x(t_0), can be determined from the system output, y(t), over some
finite time t0 < t < tf.
Since this system is linearized to be LTI system, the system is observable if and only if the Observability matrix, OB, has
full rank (i.e. if ( ) = , where n is the number of states).
⎡ ⎤
⎢ ⎥
( )= ⎢ ⎥=
⎢ ⋮ ⎥
⎣ ⎦
≫ = ( ( , ))
RankOB=
So, since the observability matrix has full rank, the system is observable!
#Task-3
- Designing a state feedback controller to stabilize the system by improving performance of the system using pole
placement and linear quadratic regulator(LQR) methods
- If all the states are not measurable, designing a faster stable full-order state observer for state feedback controller.
- If only , are measurable, designing a reduced-order state observer for state feedback controller.
- To make design more challenging, applying a step input to the cart and yet achieving the following design
requirements,
o Settling time for and of less than 5 seconds.
o Rise time for of less than 0.5 seconds.
o Overshoot of less than 20 degrees (0.35 radians).
Design procedure:
So, it has been checked before that the system is controllable. i.e. the pair ( , ) is controllable.
Since, the state of the system is to be to be feedback as an input, the controller dynamics will be:
= −
̇ = + ( − )=( − ) +
This method depends on the performance criteria, such as rise time, settling time, and overshoot used in the design.
Settling time, , for both outputs should be less than 5 seconds. And
%Overshoot, %OS, of the angle of the pendulum should be less than 20 (0.35 radians).
1. Using time domain specifications to locate dominant poles – roots of + + = . This is done by
using the following formulas and finding the dominant poles at – ±
(% / )
ζ=
(% / )
=ω 1−
= ζω
β= ζ
2. Then placing rest of poles so they are “much faster” than the dominant second order behavior.
Typically, keeping the same damped frequency ω and then moving the real part to make them faster
than the real part of the dominant poles so that the transient response of the real poles of the system will
decay exponentially to insignificance at the settling time generated by the second order pair.
While taking care of moving the poles too far to the left because it takes a lot of control effect (needs
large actuating signal)
#procedure – 1
ζ=0.456,
=1.78rad/sec,
β= ζ = 62.870,
σ=ζ = -0.811,
= 1.584
Test – 2
Step Response using pole placement-2nd test
0.05 0.02
K2 = [-21.0855 -3.2251 -2.0191 -1.8811]
Test - 3
-0.1 -0.01
K3 = [-40.2010 -5.9361 -6.7842 -4.8694]
Note:
faster when the real poles go farther to the left from the real
cart position (m)
-0.01 0
part of the dominant poles. A more faster response can be
found by moving the real poles deeper in to the left half -0.02 -2
-0.04 -6
0 1 2 3 4 5 6 7 8 9 10
2- Using Linear Quadratic Regulator (LQR)
This approach is to place the pole locations so that the closed-loop system optimizes the cost function given by
= [ ( ) ( )+ ( ) ( )]
Where:
Therefore, LQR selects closed-loop poles that balance between state errors and control effort.
>> Q = C'*C;
-3
>> Q(1,1) = 10; Step Response with LQR Control x 10
0.02 5
>> Q(3,3) = 100
>> R = 1
>> K = lqr(A,B,Q,R)
>> Ac = A-B*K; %control matrix
pendulum angle (radians)
-0.02 -5
0 1 2 3 4 5 6 7 8 9 10
Note:
- The reason this weighting was chosen is because it just satisfies the transient design requirements. Increasing the
magnitude of more would make the tracking error smaller, but would require greater control force. More
control effort generally corresponds to greater cost
cost (more energy, larger actuator, etc.). More improved responses
can be seen clearly by varying the weights tetha and x in the animation GUI that will be attached with this document.
- The above LQR design method has brought a good stability to the system and fulfilled the design requirements in
a good manner. But, if the reference input is different from
fro zero, ( )= ≠ , the systems performance will be
degraded. So, for best quality of performance, an asymptotic tracking of a step reference input must be designed.
- So far, both controller design methods, the pole placement and LQR design, helped to pick the gain vector K so
that the dynamics of the system to have nice properties – more importantly to stabilize A.
- The question remains as to how well the designed controller in both methods allows to track a reference
command?
This deals with performance issue of the system rather than just stability.
̇ = +
= −
( )≈ ( ) →∞
= − , where – is a feedforward gain used to scale the closed loop transfer function.
̇ =( − ) +
( )
= ( −( − )) = ( )
( )
= ( ) = −( ( − )) )
In this specific problem, before calculating , the = should be modified to a new one =
[ ] because it is redefined that = , which means the reference input is only applied to the position of the
cart.
Therefore, having K from the previous LQR design and scaling the reference input with following result, the system
responses are plotted below.
= −( ( − ) )
In MATLAB,
<0.5 sec. And also, it can be clearly seen that the system
cart position (m)
0.2 0.05
only.
0 -0.05
-0.1 -0.1
0 1 2 3 4 5 6 7 8 9 10
Designing a reduced order state observer
If and are measurable, then a reduced order state estimator can be designed to estimate ̇ and ̇ .
̇= +
̇ = + +
̇ = + +
- − =
- =
- All eigenvalues of F have to have negative real parts and are different from the eigenvalues of A.
So, to estimate the states ̇ = ̇ and ̇ = ̇ , the reduced dimension observer can be designed as the following:
Design Procedures:
1. Choosing an arbitrary 2 2 matrix so that all eigenvalues have negative real parts and are different from those of
.
F =
1.0e+003 *
-0.0632 0.0011
-1.0497 0.0016
And checking its eig(F)= -30.8206 +10.2537i and -30.8206 -10.2537i, both eigenvalues of F have negative real
values and all are different from those of eig(A)= 0,7.1430, -7.2220, -0.1000.
=
1.0 + 004 ∗
−6.4715 0.0600
−0.2790 0.0034
= (− , , − ∗ )
T =
1.0e+004 *
= =
p=
1.0e+004 *
0 0 0.0001 0
0.0001 0 0 0
-0.0035 0.0002 0.0094 -0.0068
-0.1986 0.0084 6.4223 -0.3785
=
1.0e+003 *
-0.0000 0.0010 0 0
2.0405 -0.0009 0.0019 -0.0000
0.0010 0 0 0
0.0624 -0.0005 0.0000 -0.0000
ℎ= =
1.0e+003 *
-0.1315
-7.2163
̇= + +ℎ
=
All the coefficient matrices are calculated above.
̇= ̇− ̇= + +ℎ − ( + )
̇ =
−0.0000 0.0010 0 0
2.0405 −0.0009 0.0019 −0.0000
= 1. + 003 ∗
0.0010 0 0 0
0.0624 −0.0005 0.0000 −0.0000
−0.0632 0.0011
̇ = 1.0 + 003 ∗
−1.0497 0.0016
Designing full order state observer (closed loop state estimator)
So far, it was assumed that there is full or partial access to the state ( ) when the controllers in each method are
designed. But in reality, most often, all of this information is not available.
To address this issue, a replica of the dynamic system that provides an “estimate” of the system states based on the
measured output of the system should be developed – the so called a state observer or state estimator.
Here, a full order state observer will be developed assuming as if all the states can’t be measured taking in to
consideration that there might be lack of sensors.
Design procedures:
So, it has been checked initially that this system is observable. i.e the pair ( , ) is observable.
̇ = + +
= + + ( − )
=( − ) + +
( )= −
̇ = ̇− ̇
=[ + ]−[ + + ( − )]
=( − )( − )
̇ =( − )
̇ =( − ) Where = = −
This equation governs the estimator error. If all eigenvalues of ( − ) can be can be assigned in the negative half plane,
then all entries of the error will approach to zero at faster rates. Thus, there is no need to compute the initial state of the
original state equation.
To select a common guideline is to make the estimator poles 4-10 times faster than the slowest controller pole. Making
the estimator poles too fast can be problematic if the measurement is corrupted by noise or there are errors in the sensor
measurement in general.
Controller poles from the above system with error tracking controller are:
>>contr_poles=eig(Ac)
>>contr_poles =
-7.6280 + 3.4727i
-7.6280 - 3.4727i
-3.1744 + 2.1467i
-3.1744 - 2.1467i
The slowest poles have real parts at -3.1744, so the estimator poles can be placed at -30. Since the closed-loop estimator
dynamics are determined by a matrix ( − ) that has a similar form to the matrix that determines the dynamics of the
state-feedback system ( − ).
Therefore, the same commands, that were used to find the state feedback gain , can be used to find the estimator gain .
1.0e+003 *
-0.0006 0.0632
-0.0018 1.0497
0.0626 -0.0008
0.9732 -0.0341
Final goal is to build the compensator (state feedback controller + full state observer).
Dynamic output feedback compensator is the combination of the regulator (state feedback controller developed using the
LQR method) and full state estimator using
=−
̇ =( − ) + +
̇ =( − − ) +
̇ = +
=−
Where
= − − =
( )= ( )− ( )
So, equations that can describe the overall closed loop dynamics will be
̇ = + = − +
̇ = + = + ( − )= + ( − )
=− + +
In short, the overall closed loop final state space model is:
̇ −
= +
̇ −
=[ ]
Where
= − − =
By making an equivalent transformation to the above state space model, it can be represented as the following equivalent
state equation. And this equivalent state equation governs the whole controller-estimator configuration.
̇ −
= + Where = = −
̇ −
=[ ]
0 0
-0.02 -5
0 1 2 3 4 5 6 7 8 9 10
Note:
The overall design requirements are satisfied and the system has a relatively good performance. But it can be seen that the
response graph for the inverted pendulum lucks a slight smoothness. That means there is something which creates that
vibration. i.e. there is a tracking problem of the output to the reference input. The next task will solve this problem.
#Final-Task
- Redefining = ( )
- Designing no steady state error tracking controller for the new SISO system to eliminate the steady state
error
Having the final controller-observer state space model, if the reference input is a constant different from zero, ( ) = ≠
, and if it is applied only on the cart’s position, it can be taken that = so that the system will become a SISO system.
So, the matrix = should be modified to a new one =[ ] when calculating a steady state
error tracking controller that can be implemented by adding a feedforward gain to scale the reference input.
This has the same approach as I used in the LQR controller, but slightly different in calculating the scaling factor since
it uses the overall controller-estimator configuration closed loop matrices.
So, taking the equivalent state equation of the new system (controller + observer),
̇ −
= + Where = = −
̇ −
=[ ]
= − , where – is a feedforward gain used to scale the closed loop transfer function.
= −( ( )) )
−
Where = , = and =[ ] in which =[ ] because the
−
step reference command is to be applied only to the cart’s position ( = ).
Then the final inverted pendulum system controlled by designing a controller, a full state observer and
adding a no steady state error tracker can be described by the following state space model.
̇ −
= +
̇ −
=[ ]
And the system response graph can be seen below. Step Response with compensator and no steady state error traker
0.4 0.15
0 -0.05
Explanation:
Conclusion
Modeling of an inverted pendulum shows that the system is unstable without a controller. Results of applying state
feedback controllers show that the system can be stabilized. While both pole placement and LQR controller methods are
cumbersome because of selection of constants of controller though they can brought a good result if done systematically
with some guide lines.
When a DC reference input is applied to the cart, the system has failed slightly to track the input and has given a stable
output with some oscillations – unsatisfactory steady state performances. To eliminate this, a no steady state error tracking
controller is designed and has brought good results as can be seen on the graphs in each steps.
And since in reality all the states can’t be measured, a full-order state estimator (observer) is designed. Finally, the state
feedback controller is summed with a full-order state observer to give a pre-compensator and then a steady state error
tracking mechanism is also added to the whole new system. All those have brought a really nice controlled inverted
pendulum system with good performance.
References
[1] Chi-Tsong Chen, “Linear system theory and design, 3rd edition” , 1999 Oxford University press
[2] Chi-Tsong Chen, “Analog and digital control system design – transfer function, state space and algebraic methods”
[3] B.Wayne Bequette, “Process control – Modeling, Design and simulation”, Rensselaer Polytechnic Institute
[5] P. Kumar, O.N. Mehrotra, J. Mahto, “Controller design of inverted pendulum using pole placement and LQR”, ISSN:
2319 - 1163
[7] Andrew K. Stimac, “Standup and Stabilization of the Inverted Pendulum”, MIT, June 1999
[8] Stormy Attaway, “MATLAB, Practical Introduction to Programming and Problem Solving Second Edition”,
department of Mechanical engineering Boston university, 2012
[9] http://ocw.mit.edu/courses/aeronautics-and-astronautics/16-30-feedback-control-systems-fall-2010/lecture-notes/
[10] http://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum§ion=ControlStateSpace
Appendix A: Matlab Scripts and Function Files
File-1: InvertedPendulum.m
- This file has all the steps of developing the state feedback controller, full order state observer and the steady state
error controller. Its outputs are shown in graphs launched automatically when you run it.
%{
====================================================================
File:InvertedPendulum.m
Project: Controlling an inverted pendulum
Method: state space model and state feedback
%--------System Modeling-------%
% Defining variables
clc;
clear;
M=0.5;
m=0.5;
b=0.1;
l=.3;
I=.006;
g=9.8;
d=I*(M+m)+M*m*l^2;
% State-space model
disp('====================================')
disp('state space model of the system is:')
disp('====================================')
system=ss(A,B,C,D)
disp('=======================================')
disp('The transfer function of the system is:')
disp('=======================================')
inputs = {'u'};
outputs = {'x'; 'tetha'};
G=tf(system)
set(G,'InputName',inputs)
set(G,'OutputName',outputs)
%--------System Analysis-------%
inputs = {'u'};
outputs = {'x'; 'theta'};
set(G,'InputName',inputs)
set(G,'OutputName',outputs)
figure(1);clf;subplot(221)
t=0:0.01:1;
impulse(G,t);
grid;
title('Open-Loop Impulse Response of the system')
subplot(222)
t = 0:0.05:10;
u = ones(size(t));
[y,t] = lsim(G,u,t);
plot(t,y)
title('Open-Loop Step Response of the system')
axis([0 2 -50 50])
legend('x','theta')
grid;
disp('===========================')
disp('Poles of the system are:')
disp('===========================')
Poles=eig(A)
subplot(2,2,3:4)
pzmap(system);
title('Pole-zero mapping of inverted pendulum')
% Checking controllability
disp('=========================')
disp('checking controllability:')
disp('=========================')
CO=ctrb(system);
Rank_CO=rank(CO)
unco=length(A)-rank(CO)
if(unco==0)
disp('system is controllable!')
else
disp(['number of uncontrollable states are:',unco])
end;
% Checking Obserevability
disp('=======================')
disp('Checking observability:')
disp('=======================')
OB=obsv(system);
Rank_OB=rank(OB)
unobsv=length(A)-rank(OB)
if(unobsv==0)
disp('system is observable!')
else
disp(['number of unobservable states are:',unobsv])
end;
disp('================================================')
disp('desirable poles and the respective gain vectors:')
disp('================================================')
t = 0:0.01:10;
r =0.2*ones(size(t));
subplot(312)
[y,t,x]=lsim(system_c2,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pend-angle(rad)')
title('Step Response using pole placement-2nd test')
grid
disp('===============================================')
disp('The weight matrices Q and R and the vector K:')
disp('===============================================')
Q = C'*C;
Q(1,1) = 80; %increasing the weight on the pendulum's angle(tetha)
Q(3,3) = 400 % increasing the weight on the cart's position(x)
R = 1
K = lqr(A,B,Q,R) %state-feedback control gain matrix
Ac = A-B*K; %constrol matrix
system_c = ss(Ac,B,C,D); %the controlled system state space model
t = 0:0.01:10;
r =0.2*ones(size(t));
figure(3);clf
[y,t,x]=lsim(system_c,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response using LQR')
grid
t = 0:0.01:10;
r =0.2*ones(size(t));
figure(4);clf
[y,t,x]=lsim(sys_lqr_et,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response using LQR and steady state error controller')
grid
obsr_poles = [-30 -31 -32 -33] % 10 times faster than the controller
L = place(A',C',obsr_poles)' % placing observer poles
sys_co_ob = ss(Aco,Bco,Cco,Dco);
t = 0:0.01:10;
r = 0.2*ones(size(t));
figure(5);clf
[y,t,x]=lsim(sys_co_ob,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response using compensator (controller + observer)')
grid
t = 0:0.01:10;
r = 0.2*ones(size(t));
figure(6);clf
[y,t,x]=lsim(sys_cm_et,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response using compensator with steady state error controller')
grid
File-2: inv_pend_ani.m
- This file has all the necessary codes (functions) for the GUI animation of the Inverted Pendulum system control
using a compensator and a steady state error controller.
%{
***************************************************************************
This function is intended for use in the inv_pend_ani GUI file. It contains
the code necessary to run the GUI's simulations and controls based on the
designed compensator with steady state error controller.
**************************************************************************
%}
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @inv_pend_ani_OpeningFcn, ...
'gui_OutputFcn', @inv_pend_ani_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Outputs from this function are returned to the command line.
function varargout = inv_pend_ani_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
A = [0 1 0 0;
51.58 0 0 0.5263;
0 0 0 1;
-7.737 0 0 -0.1789];
B = [0; -5.263; 0; 1.789];
C = [0 0 1 0;1 0 0 0];
D = [0;0];
Cn = [0 0 1 0]; %C matrix when Y=x, or step command only applied to x
% Get the weighing factors from the editable text fields of the GUI
% x=for pendulum angle, y=for cart position
x=str2num(get(handles.xtext,'string'));
y=str2num(get(handles.ytext,'String'));
Q=[x 0 0 0;
0 0 0 0;
0 0 y 0;
0 0 0 0];
R = 1;
%Finding the state feedback gain K matrix with the lqr command
%Modifing Ccl because reference step command applied to x only ==> y=x
Ccln=[Cn zeros(size(Cn))];
%there should be some modification in the input signal for good tracking performance. that means...
%there should be extra gain used to scale the closed loop transfer function to make the steady state step
error=0.
Nbarval = get(handles.reference,'Value');
if Nbarval == 0
Nbar = 1;
set(handles.Run,'UserData',Nbar);
stepaxis=stepval/1000;
elseif Nbarval == 1
%calculating the scale factor for the overall closed-loop system
Nbar=-inv(Ccln*(Acl\Bcl))
set(handles.Run,'UserData',Nbar);
end
%Get the value of the step input from the step slider
stepval=get(handles.stepslider,'Value');
T=0:0.1:6;
U=stepval*ones(size(T));
Bclt = [B*Nbar;zeros(size(B))];
[Y,X]=lsim(Acl,Bclt,Ccl,Dcl,U,T);
cartpos=X(:,3);
pendangl=X(:,1);
cart_length=0.3;
cl2=cart_length/2;
ltime=length(cartpos);
cartl=cartpos-cl2;
cartr=cartpos+cl2;
pendang=-pendangl;
pendl=0.6;
pendx=pendl*sin(pendang)+cartpos;
pendy=pendl*cos(pendang)+0.03;
axes(handles.axes1)
plot(T(1),cartpos(1), 'r', 'EraseMode', 'none')
plot(T(1),pendangl(1), 'b', 'EraseMode', 'none')
axes(handles.axes1)
if stepval > 0
axis([0 6 -stepval/2 stepval*2])
elseif stepval < 0
axis([0 6 stepval*2 -stepval/2])
else
axis([0 6 -0.5 0.5])
end
hold on
axes(handles.axes2)
cla
L = plot([cartpos(1) pendx(1)], [0.03 pendy(1)], 'b', 'EraseMode', ...
'xor','LineWidth',[7]);
hold on
J = plot([cartl(1) cartr(1)], [0 0], 'r', 'EraseMode', ...
'xor','LineWidth',[20]);
manual=get(handles.manualbox,'Value');
for i = 2:ltime-1,
if manual == 1
pause
end
axes(handles.axes1)
plot([T(i),T(i+1)],[cartpos(i),cartpos(i+1)], 'r', 'EraseMode', 'none')
plot([T(i),T(i+1)],[pendangl(i),pendangl(i+1)], 'b', 'EraseMode', 'none')
end
axes(handles.axes1)
hold on
legend('Pendulum Angle (rad.)','Cart Position (cm.)')
guidata(hObject, handles);
axes(handles.axes2)
cartpos=0;
cart_length=0.3;
cl2=cart_length/2;
cartl=cartpos-cl2;
cartr=cartpos+cl2;
pendang=0;
pendl=0.6;
pendx=pendl*sin(pendang)+cartpos;
pendy=pendl*cos(pendang)+0.03;
cla
K = plot([cartpos(1) pendx(1)], [0.03 pendy(1)], 'b', 'EraseMode', ...
'xor','LineWidth',[7]);
hold on
J = plot([cartl(1) cartr(1)], [0 0], 'r', 'EraseMode', ...
'xor','LineWidth',[20]);
guidata(hObject, handles);