AN APPLICATION OF ADAPTIVE CONTROL THEORY
TO THE WORKBENCH CONTROL SYSTEM
by
David Minh Truong
B.S.E.E, University of Colorado at Denver, 1990
A thesis submitted to the
Faculty of the Graduate School of the
University of Colorado at Denver
in partial fulfillment
of the requirements for the degree of
Master of Science
Electrical Engineering
1995
This thesis for the Master of Science
degree by
David Minh Truong
has been approved
by
Miloje S. Radenkovic
Hamid Z. Fardi
okmr
Date
Truong, David Minh (M.S., Electrical Engineering)
An Application of Adaptive Control Theory to the Workbench Control System
Thesis directed by Associate Professor Miloje S. Radenkovic
ABSTRACT
While many adaptive control studies have been reported, relatively few of them can be
applied to real physical systems. In hoping to narrow the gap between the Adaptive
Control Theory and its application, this thesis shows a development of an adaptive
control algorithm that can be implemented on a computer to control the reel on the
Workbench to follow a desired trajectory. The thesis presents a derivation of a
mathematical model for the Workbench and selections of a control method and an
estimation scheme for a real-time self-tuning regulator. All necessary steps in the
design process are considered to assure the validity and stability of the algorithm
before it can be applied to the real system. The performance of the algorithm is
evaluated based on the responses of the Workbench to different desired trajectories
and its ability to adapt to any sudden changes in the system dynamic.
This abstract accurately represents the content of the candidates thesis. I recommend
its publication.
Signed
Miloje S. Radenkovic
111
CONTENTS
Chapter Page
1. Introduction........................................ 1
1.1 Objective........................................ 1
2. Workbench Model Derivation..........................3
2.1 System Description............................... 3
2.2 Derivation of Workbench Model.................... 4
2.2.1 Movement of the Reel........................4
2.2.2 Bar Motion.................................. 5
2.2.3 Motor Dynamics.............................. 6
2.2.4 Motor-Bar System............................ 7
3. Adaptive Control Schemes............................ 11
3.1 Introduction..................................... 11
3.2 Self-Tuning Regulator (STR)...................... 13
3.3 Self-Tuning Predictor............................ 16
4. Estimation Schemes.................................. 20
4.1 Introduction..................................... 20
4.2 Least Square (LS) Algorithm...................... 20
4.3 Recursive Least Square (RLS)..................... 24
5. Stability of Self-Tuning Regulator..................29
6. Workbench Controller Calculation....................40
IV
6.1 Verification of the Model
44
7. Discussion and Conclusion..........................56
Appendix A............................................ 81
References............................................ 104
v
FIGURES
Figure Page
2.1 Sketch of The Workbench 3
2.2 Forces Acting on the Reel 5
2.3 Model of the Motor 6
2.4 The Motor and Bar System 8
3.1 Parallel Model Reference Adaptive Systems 12
3.2 Series-Parallel Model Reference Adaptive Systems 12
3.3 Series Model Reference Adaptive Systems 13
3.4 Block Diagram of a Parameter Adaptive Control Systems 13
3.5 Block Diagram of the Self-Tuning Regulator 15
4.1 Structure of Parameters Estimator 21
6.1-6.10 Simulations 46-55
7.1-7.23 Workbench Performance 58-80
vi
ACKNOWLEDGMENTS
This study would not have been possible without the support of many people to whom
I will always be grateful.
I wish to express my gratitude to my advisor Professor Mijole S. Radenkovic for his
guidance during the preparation of this thesis. I would also like to thank Professor Jan
T. Bialasiewicz and Professor Hamid Z. Fardi for serving on my committee.
Finally, this thesis is dedicated to my mother for her devotion to the well being of her
children.
vu
1. Introduction
In the real world most of the industrial processes are nonlinear, and their dynamics
are often not known in advance. The steps in designing a controller for these
processes using conventional system control design methods are sometimes
complicated. It requires some knowledge of the dynamic of the process we want to
design a controller for, and we have to translate this knowledge to meet the
specification of the desired response. Moreover, when the controller is obtained, its
performance is optimal only if there is no change in the process dynamic. But such an
assumption is not practical in physical systems. Because of these difficulties, Adaptive
Control Systems have been a topic for many intensive researches for almost half a
century in hoping to find a better way to design a controller with more optimal
response. Despite significant achievements in this field, its application has been
restricted until early 1980's due to the development of personal computer and its lower
cost.
1.1 Objective
The objective of this thesis is to research and develop a combination of parameter
estimation and control scheme such that it can be implemented on a computer to
control a Balance Control Workbench whose parameters are not known. The thesis is
organized as follows: The mathematical model of the Workbench is derived in chapter
2, so we can limit the research to a certain class of controllers that might be applicable
1
to the Workbench. Following that is a presentation on adaptive control methods in
chapter 3 and parameters estimation schemes in chapter 4. Finally, an estimation and
control algorithm is developed in chapter 6 using results obtained in chapters 2, 3, and
4.
2
2. Workbench Model Derivation
2.1 System Description
Figure 2.1 Sketch of The Workbench.
3
The Balance Control Workbench as shown in figure 2.1 on the previous page consists
of an I/O card (a PCL711 data acquisition board), an analog to digital (A/D)
converter and a digital to analog converter, a D.C motor, a copper reel, a bar with a
resistive coating on top to monitor the position of the reel on the bar, and a
potentiometer to measure the angular velocity of the bar. The bar's angular velocity is
controlled by a D.C motor. The control algorithm is implemented on a 486
microprocessor based computer. The communication between the Workbench and the
computer is handled by the I/O card. The sampling time is fixed at 0.055 seconds.
The problem is to find an optimal control strategy for a reel following a prescribed
reference position along the bar which is mounted on a pivot. In order to find such a
control algorithm, the dynamic of the model of the Workbench should be derived first
so that the order and the delay of the system can be known.
2.2 Derivation of Workbench Model
2.2.1 Movement of the Reel
First we look at the movement of the reel on the bar and we assume the following:
a) Only pure rolling which gives cor = x
b) The moment of inertia of the reel is J = mr2
2
c) No rolling friction
The equation of motion is found from the forces acting on the reel (see figure 2.2):
the gravitational force mg sin a, gives rise to a translatory motion of the mass center
given by the force component mx, and a rolling motion as given by the torque
A
Figure 2.2 Forces Acting on The Reel.
Zr = J03.
The torque is caused by the contact force fr between the reel and the bar, such that
xr = frr. By adding forces we find
mg sin a = mx------. (2.1)
r
If we assume a small angular deflection of the bar, then sin a = a. By entering the
assumptions a) and b), the above equation can be simplified to
x = ^ga. (2.2)
The centrifugal force acting on the reel due to the rotational motion of the bar is
neglected.
2.2.2 Bar Motion
The equation of motion for the bar is found by looking at moments around the bar
axis, and assuming:
d) No joint friction
5
The equation becomes
(Jb+mx2)a = mgx + x ,
(2.3)
where Jb is the moment of inertia of the bar and X is an external control torque. The
force due to the simultaneous motion of the bar and the reel is not included.
2.2.3 Motor Dynamics
The control torque is produced by a D.C. motor. Let us look at the behavior of
the motor and transmission system. We assume the following:
e) The electrical time constant can be neglected.
f) No elasticity of the string (nylon trace).
By use of voltage balance in the motor we get the following relation
where Ka is the motor EM-constant, and the electrical time constant is neglected.
u = Ri + EMF = Ri + Kac
(2-4)
R
U
+
EMF
Figure 2.3 Model of The Motor.
6
Assume that the motor torque, Mm is proportional to the current i as
i= Mm.
K. m
(2.5)
Replacing equation (2.5) into equation (2.4) we can then write
m ' '
(2.6)
where Kx is the torque constant. By use of moment balance we get the following
equation for the motor
= J Cl)m + T.
(2.7)
where Jm is the moment of inertia of the motor, and xm is the motor load. By
combining the equations (2.6) and (2.7) we get
K
J (Dm + = 'Hu Kd))
771 771 ' '
(2.8)
2.2.4 Motor-Bar System
The motor is connected to the bar by a string as shown in figure 2.4 with
transmission ratio n. The velocities and torque at the motor and the bar axis are then
related by:
7
Figure 2.4 The Motor and Bar System.
n a = com (2.9)
t = (2.10)
with friction and elasticity in the transmission not modeled. By entering the motor
equation (2.8) into the bar equation (2.3), we then obtain the nonlinear model
n2KK nK,
(Jb +mx2 +n2JJa = mgx-f-La+*
(2.11)
Dividing both sides of equation (2.11) by (Jb +mx2 +n2Jm) would yield
a =
mgx
n KaKx
-a+-
nKr
(Jb+mx +n~Jm) R(Jb+mx +n Jm) R(Jb+mx~ +n~Jm)
u. (2.12)
A linear expression for a can be obtained by assuming that the term mx2 is very small
compared to the other terms in the denominator of the above equation, and it can be
omitted. And, because all the parameters (except g) of the coefficients in equation
(2.12) are not known, equation (2.12) can be written as
8
a = a1x+a2 a+bu,
(2.13)
where are arbitrary. In the frequency (S) domain equation (2.13) and
equation (2.2) can be transformed to
s2 a = axx+a2sa+bu
and
a =s2x = ans2x
2 g
(2.14)
(2.15)
respectively. To express the mathematical model of the Workbench in terms of the
reel position (x), its derivatives and the Workbench's input, we replace a in equation
(2.14) by equation (2.15). The result yields
a0s4x = axx+a2a0s3x+bu, (2.16)
in which x can be derived as
x = -^^-s3x+s4x+bu. (2.17)
flj ax
Again, the variables (a^a^a^b) are arbitrary. It would be more convenient for later
reference if we rename the coefficients on the right hand side of equation (2.17) and
write it in the form
x = a3s3x+a4sAx -b0u,
(2.18)
9
where a3 = a%a^, and a4 =. With equation (2.18) as the mathematical model of
ai fli
the Workbench in the continuous time domain, we see that the order and the delay of
the Workbench are 4 and 1 respectively. Therefore we can assume the dynamic model
of the Workbench in the discrete time domain as follows
Y(t) = a?(t -1)+a2Y(t 2) + a3Y(t 3) + aj(t 4)+b0U (f -1). (2.19)
For convenience in later reference, equation (2.19) should be written in a short form
A(z)Y(t) = B(z)U(t-1) (2.20)
with Y(t) is the output of the Workbench,
U(f) is the input to the Workbench,
A(z) = [1+OjZ+a2z2 + a2z3 + a4z4], (2.21)
and Â£(*)=[&]. (2.22)
10
3. Adaptive Control Schemes
3.1 Introduction
So far we have the dynamic model of the Workbench with unknown parameters
aj, and b0 in equation (2.19). The only information we have from the above
derivation is the order of A(z-1) and polynomials and the delay of the system.
This is the situation where adaptive control system would have the best application.
"An adaptive system measures a certain index of performance (IP) using the inputs,
states and the outputs of the adjustable system. From the comparison of the measured
index of performance (IP) values and a set of given ones, the adaptation mechanism
modifies the parameters of the adjustable system or generates an auxiliary input in
order to maintain the index of performance (IP) values close to the set of given ones."
[1 pg 354].
There are many types and structures of adaptive control systems. The
configuration of an adaptive control system depends on the criterion and identification
scheme. In general, most of adaptive control systems fall in the configurations shown
on the following pages. The most popular structure is the Parallel configuration,
which is called "output error method." This method is well used in the self-tuning
regulator.
11
1) Parallel Model Reference Adaptive System (MRAS!
Reference
Model
*
Error
Uc
Adjustable __ N
^ System
Adaptation
Mechanism
Figure 3.1 Parallel Model Reference Adaptive Systems.
2) Series-parallel MRAS
Uc
Reference
Model(p)
JL Error
O
Reference
171 Model(s)
U
Adjustable
System
*
Adaptation
Mechanism
Figure 3.2 Series-parallel Model Reference Adaptive Systems.
12
3) Series MRAS
Uc
Error
6&
Reference
Model(s)
u
Adjustable __
System '
Adaptation
'Mechanism
Figure 3.3 Series Model Reference Adaptive System.
3.2 The Self-Tuning Regulator (STR)
A self-tuning regulator by its nature is one type of adaptive control system. The
block diagram of a self-tuning regulator is shown in figure 3.4.
Figure 3.4 Block Diagram of a Parameter Adaptive Control System.
13
The regulator can be thought of as composed of three parts: a recursive parameter
estimator, a design calculator, and a regulator with adjustable parameters. The basic
idea of the Self-Tuning Regulator is based on the certainty equivalent control. The
major assumptions in this method are that:
- The system is minimum phase, or the zeros of system's transfer function are
inside the unit circle.
- The time delay is known and
- A bound can be given to the order of the system [2],
Since there are many different ways to do control design and to estimate
parameters, there are many varieties of self-tuning regulators such as Self-tuning
Based on Pole Zero Assignment, Implicit Self-time Regulator and Explicit Self-tune
Regulator, but here we only present the self-tuning based on model reference method.
The principle of Self-tuning based on model reference can be illustrated in figure
3.5, in which the process is modeled as
A(z)Y(t) = zdB(z)U(t) (3.1)
where A(z) = [l Oj a],
B(z) = [b0 fcj bm],
and zd = q~d
The algorithm can be described as follows: Given a desired response specified by
the polynomials Bm(z) and A^Cz); find the polynomials G(z)JR(z) and C(z) using the
control law
14
R(z)U(t)=C(z)Uc(t)-G(z)Y(t)
(3.2)
so that
Y\t) _ Y(t)
Uc(f) Uc(t)
(3-3)
Expressing equation (3.3) in terms of R(z),G(z),C(z)J3m(z) and AJj), we can show
that
C(z)B(z) BJz)
A(z)R(z)+zd B(z)G(z) AJz) '
Figure 3.5 Block Diagram of the Self-Tuning Regulator.
If we let R{z)= B(z)F(z) and eliminate B(z) in equation (3.4), it would become
15
(3.5)
C(z) BJz)
A(z)F(z)+zdG(z) Am(z)
The polynomials of the regulator G(z)yR(z) and C(z) can be found by equating the
numerators and the denominators on both sides of equation (3.5) as shown below
C(z) = Bm(z), (3.6)
A(z)F(z) + zdG(z) = Am(z), (3.7)
where C(z) = 1 + cxz +---1-c,z;
G(z) = g0 + g1z+---+gK_1z'1 n=max(l-d + l,n), (3.8)
F(z)=l+f1z+--+.
3.3 The Self-Tuning Predictor
Another form of self-tune regulator is the Self-Tune Predictor which has the same
structure as the self-tuning based on model reference except that the polynomials
Bm(z) and Am(z) in figure 3.5 are not known, instead a series of setpoints (a reference
B (V)
trajectory Uc(t) or Y*(f)) are known in advance, and -1. This implies that
Anfe)
Y* (t) is Uc(t+ d) delayed by d instances of time later.
In this case the calculation for the transfer function of the system (the process plus
the regulator) in figure 3.5 would yield
16
Y(t) ... z'C(z)i(i)
(/,(<) A(z)R(z)+zdB(z)G(z) ' ' J
Again, let i?(z)= B(z)F(z) and eliminate 5(z) in equation (3.9), the transfer function of
the whole system becomes
m=_____________________
UJt) A(z)F(z)+zdG(z) '
B (z)
Also from figure 3.5 with = 1, we have Uc(t)=z~dY*(t). We substitute it into
An(z)
equation (3.10) and solve for
C{z)Y\t) = zdG{z)Y(t)+A{z)F{z)Y{t) (3.11)
which defines the estimation of the process's current output at time t. In the above
expression, Y*(t) is expressed in term of the process's output 7(f) only. If we replace
7(f) in the second term on the right hand side of equation (3.11) by
(3.12)
A(z)
from the process's model, the estimation of the process's output now can be expressed
in terms of process's output and input as shown in the following expression
C(z)Y\t) = zdG(z)Y(t)+zdB(z)F(z)U(t). (3.13)
17
In order for the process's output Y(t) to track the desired trajectory Y*(t), we
have to predict the future output of the process based on the information on F(-) and
U (-) available at time t and use the predicted output to calculate the current control
signal U(t) that would produce an output Y(t + d) as close to Y*(t + d) as possible.
Let us define the prediction of the process's future output at d instant of time ahead to
be Y(t+d) and assume that
Y(t+d) = Uc(t+d)
which implies
Y\t) = Y(t).
(3.14)
(3.15)
The substitution of equation (3.15) into equation (3.13) will change equation (3.13)
to
C(z)Y (t)= zdG(z)Y(t)+zdB(z)F(z)U(t). (3.16)
And by multiplying both sides of equation (3.16) by z~d, the predictor's output can be
expressed in the form
C(z)Y (t + d)= G(z)Y(t)+B(z)F(z)U(t) (3.17)
which defines the prediction of the process's future output in terms of process's
outputs and inputs at time t.
18
The criterion for Y(t) to follow Y*(t) is to minimize the steady state loss function
J{t) = E[Y{t)-Y\t)f. (3.18)
If we subtract both sides of equation (3.17) by C(z)Y*(t+d) as shown in the
expression below
C(z)[Y(t+d)-Y\t+d)]=G(z)Y(t)+B(z)F(zMt)-C(,z)Y\t + d), (3.19)
the left hand side of equation (3.19) becomes zero by the assumption made in equation
(3.15) that Y (t + d) is equal to Y*(t+d). Therefore its right hand side can be
rewritten as
G(z)Y(t)+B(z)F(z)U(t)-C(z)Y\t + d)= 0. (3.20)
Equation (3.20) defines the control law for calculating the control signal U(t) such
that the process's output can achieve the objective defined in equation (3.18).
19
4. Estimation Scheme
4.1 Introduction
In adaptive control, parameters of a process are usually not known. So if we want
to control the process we must have a way to estimate the process parameters
characterized by A(z-1) and B(z~l) or the parameters of the regulator characterized
by C(z~1),F(z~l) and G(z_1) in order to calculate for the control signal. In an on-
line adaptive control system as shown in figure 4.1 on the following page, while
identifying the process parameters, the parameters of a regulator should be changed
continuously to compensate for any changes in process parameters. For this reason, it
is necessary to have an estimation scheme that can update the parameters recursively
and automatically while the system is run by a controller.
4.2 Least Square (LS) Algorithm
Many estimation schemes have been considered and simulated in the experiment,
but only Recursive Least Square method is mentioned in this thesis because of its
simplicity and possible application. The Recursive Least Square method can be
implemented in a program that has very limited space and running time. This method
also has a desirable property that the estimates converge to the true parameter values
as the number of observations increases toward infinity.
20
figure 4.1 Structure of parameters estimator.
The Recursive Least Square method is the result of an extended derivation from
the Least Square method, whose principle was developed at the end of the eighteenth
century by Gauss to determine the orbits of the planets. According to its principle the
unknown parameters of a mathematical model should be chosen in such a way that the
sum of the squares of the differences between the actually observed and the computed
values, multiplied by numbers that measure the degree of precision, is a minimum.
Least Squares (LS) method can be applied to a large variety of problems. It's
particularly simple for a mathmatical model that can be written in the form
Ym(t) = chY{t-l) + -+anY(t-n)+b0U(t-l) + -+bmU(t-m-l) (4.1)
where Ym(t) is the output of the model; al,a2,--,ari are unknown parameters; and the
pair {(Y(i),U(i),i = 1,2,3,,!)} are obtained from experiment The problem is to
determine the parameters a, in such a way that the output Ym (t) computed from the
21
model in equation (4.1) agree as closely as possible with the measured variables Y(t)
collected from the experiment In order to determine the parameters a; in equation
(4.1), we introduce a model
Y(t) = a1(t)Y(t-l)+- + an(t)Y(t-n) + b0(t)U(t-l)+-+bm(t)U(t-m-l) (4.2)
where Y(t) is the prediction of F(f) using informations on Y(i) and U(i) of the real
process up to time (f-1), and a;(f) are the estimates of a; in equation (4.1) at time t
and they will become close to the values of a; as we try to minimize the loss function
(4.3)
f=l
The derivation of the Least Square estimate algorithm would be simplified if equation
(4.2) can be written in a short form as
Y(t) = Q(t)T(t) (4.4)
where 0(f) = ft (t), (t);b0 (t), , bm (t)f
and <|>(f) = [Y{t-1), , Y(t n); U(t -1), , U(t m)f.
A
Rewrite equation (4.3) in term of 0(f) and <3>(f), it becomes
22
(4.5)
AO = () e()r 1>(n -1)]2.
77=1
Taking partial derivative of equation (4.5) with respect to 0(0, the Least Square
algorithm can be derived as below:
^= JjÂ£i:K. () - -1)]2
(4.6)
3/(7)
30
= 2 Â£ [Ym (n) eT m(n -!)][-(n -1)]
71=1
(4.7)
3/(0
30
^ = -2^ Y(n)&(n -1) +2Â£Â§r (0*( l)*(n -1)
71=1
71=1
(4.8)
Because 6T (t)$(n -1) is scalar, can be written as
30
3/(Q
30
= -2^ Y(n)&Qt -1) +2^ (n l)$r (n -1)0(0.
77=1
77=1
(4.9)
To find 0(0 that minimizes equation (4.5), we set the right side of equation (4.9)
equal to zero as below
-2^ Y(ri)&(ri -1) +2^ 4>(n l)0>r (n -1)0(0 = 0,
(4.10)
77=1
77=1
and then solve for
23
(4.11)
0(0 = [X(fi-w (n -1)]-12 4>(n l)7(n).
11=1
If we define
r
i,(0"1=2(n-i)*r(-i)*
(4.12)
then the parameter estimates can be found using
e(t) = -PW2>(-i)W-
t
(4.13)
4.3 Recursive Least Square (RLS)
Equations (4.12) and (4.13) constitute a Least Square algorithm to update the
parameter estimates. But the procedure of taking the inverse of P~l(t) is
complicated, time consuming and not suitable for an on-line estimation. The
Recursive Least Square algorithm therefore would be an alternative in performing
parameters estimation. This algorithm is the result of further derivation of Least
Square algorithm as shown below.
From equation (4.12) we have
(4.14)
which can be written in the form
(4.15)
24
Now subtracting equation (4.12) from equation (4.15) we have
Pit +1)'1 Pity1 = $(0$r (0+2$(*- l)$r (n-1)>X$(B-l)$r (n-1). (4.16)
n=l n=l
The last two terms on the right hand side of equation (4.16) are cancelled out, and
equation (4.16) becomes
P(t + \yl-P{t)-1 =$(f)$r(r), (4.17)
which is equivalent to
= P{t + \y1-<$>{t)
From Least Square algorithm, equation (4.13) can be written at time (t +1) as
f+1
6(t +1) = P(t +1)Â£ <&(n -1 )Y(n). (4.19)
n=1
Multiplying both sides of equation (4.19) by P(t+1)"1 we have
r+1
P(t +1)"19(r +1) = Â£ *(n l)T(n) (4.20)*
n=1
Similarly, we multiply both sides of equation (4.12) by Pity1 and obtain
25
(4.21)
p(0_1e(0=X*(-i)r().
n=l
By subtracting equation (4.21) from equation (4.20), we have
P(t +l)"1 Â§(t +1) P(f)"10(0 = $(t)Y(t +1), (4.22)
which is equivalent to
P(r+l)-10(r+l) = (4.23)
Rewrite equation (4.23) with P~l(t) replaced by equation (4.18), then solve for
Q(t +1) as shown in the derivation below
P(t+1)_1 Â§(f+1) = $(t)Y(t+1)+[P(t+1)"1 O(0*r (f)]Â§(f) (4.24)
Pit+iy1 Q(t+1) = **
P(t+1)_10(t+1) = P(t+1)"10(0++1) - (00(01 (4-26)**
Multiplying both sides of equation (4.26) by P(f +1) would yield the final expression
0(f+l) = 0(O + P{t+1) (Ot Y(t+1) - (00(01, (4-27)
26
for estimating the regulator's parameters. And from equation (4.17) by using Matrix
Inverse Lemma, the estimator gain becomes
P(j + l)=[P(0-
P(Ofr(f)tt(OrP(Q
l+fc(f)rP(f)(0 J
(4.28)
Equations (4.27) and (4.28) constitute the Recursive Least Square algorithm. This
algorithm is used to estimate the next values of process parameters based on their
previous values and the error between the real output and the predicted output.
Pit) is the gain matrix which determines the rate of change of the estimates. For
fast convergence, the initial values of P(t) should be in the power of 3 or higher in
some cases. When the parameter estimates converge, the value of P(t) matrix gets
small and the rate of adaptation becomes slow. To maintain the fast convergence rate,
the estimation algorithm needs to be revitalized by resetting the values of P(t) at the
time a significant change in parameter values occurs.
Other versions of Recursive Least Square algorithm are
and
P{t+\) = P{t)~
P{t)~~{t){tf P{t)~~
X+(t)T P(0$(f)
P(r+l)=^[P(r)-
P(03>(0
X+(OrP(t)
(4.29)
(4.30)
in which X is introduced as a weighting factor on past measurements, and where
0 < X < 1. In equations (4.29) and (4.30), the gain of the estimate depends on the rate
27
in which past measurements are discounted. For A, = 1, the gain matrix P(t) will
decrease monotonically to zero as time increases; and if A < 1, P(t) will not go to
zero. It has been found that a value of X between .95 and .99 works well in most
cases.
In servo problems such as the one in this experiment, the major excitation comes
from the changes in the command signal. Such changes may be irregular, and it has
been found that there may be bursts in the process output if P(t +1) in equation (4.30)
has X less than one. The present of bursts can be understood as follow. The negative
term in equation (4.30) represents the reduction in parameter uncertainty due to the
last measurement. When there are no changes in the setpoints, the vector P(t)<3>(t)
will be zero. There will not be any changes in the parameter estimate and the negative
term in the right hand side of equation (4.30) will be zero. Equation (4.30) then
reduced to P(t + l)=-^-P(t) and the matrix P(t) will increases exponentially if X < 1.
If there is no change for a long time, the matrix P(t) may become very large. A
change in a command signal may then lead to large changes in parameter estimates and
in the process output [3]. There are many ways to eliminate bursts. One possibility is
to stop the updating of the matrix P(t) when the signal on the prediction error is
smaller than a given value. Another possibility is to subtract a term like aP(t)2 from
the right hand side of equation (4.30) to ensure that the matrix P(t) stays bounded, or
to choose the forgetting factor, so that a function of P(-) (like trP) is constant.
28
5. Stability of Self-Tuning Regulator
In this section the stability of Self-Tuning Regulator is presented. Let us consider
the discrete time system
A(q~l)Y(t + \)=B(q~1)U(t)+C(q~1)w(t+1), t>l (5.1)
where {F(i)}, {U(t)} and {w(r)} are system output, input and disturbance sequences
respectively and q~x represents the unit delay operator. The matrix polynomials
A{q~x), B(q~l) and C(q~l) are given by
A (q~l)= 1 + a^q~l + + a^q'*, IV
B(q~x)= b0 + bxq~x + +bsq~ttB, nB >0 (5.2)
C{q~l)=l + c1q-l + --- + cncq~c , nc>0.
It is assumed that the upper bounds of the orders rtA, nB, nc are known. Regarding
the system model (5.1), we introduce the following standard assumptions.
(Al) {co(0} isf a white noise sequence satisfying
sup E{(n(t+l)2+fl} < ka < a.s.
t
for some p. > 0 and
29
a.s.
(5.3)
1 N
lim T (ti(t)(Â£{t)T= R> 0
N "
(A2) Cfc*)"1 + C(ea)r / > 0, \fX e [0,2*].
(A3) B(z~l) is stable.
The problem considered in this section is to stabilize system (5.1) by designing an
adaptive controller so that the following functional criterion is minimized:
(5.4)
where Y*(t) is a given reference signal. We will assume that the sequence {Y*(t)} is
bounded. The following adaptive control schemes will be analyzed.
Assuming that the high-frequency gain matrix b0 is known and non-degenerate, the
control law U(t) is defined by
C/(r) = fe"1 [y* (r+1) 0(r )r 0>(r)]
(5.5)
where 0(f) is the estimate of
=l---anAbi
obtained by the following Least Square algorithm:
(5.6)
30
0(r+1) = 0(0 +p(t)&(t)[Y(t+1) - T {t+l)f. t > 1
(5.7)
p{t)= p(t-1}
i+(t)Tp(t)(t)
p(0)=p0I,p0>0,
(t)T =[Y(t)r--J(t-nA+l);U(t-l),---Mt-nB);&(t),---Mt-nc
where
G>(t) = Y(t)-b0U(t-l)-Q(t)T&(t-l), t> 1, 6(0 = 0, t
According to [4], the control law is given by
u{t)=b01 (O'1 [r* (f+1)+Â£0 (t)m mT 0(0],
where bo (t) is chosen by the designer as
b0(t)
ifboitf >
W (0 =
b0(t) +
1
[log r. (t 1)]
1/2
[logr^(r-l)]
otherwise
1/2
Here
^(0 = X||
j=1
In this algorithm we use for 0(0 a finite arbitrary initial value 0(0)
of generality we also assume that 7(0 = 0, U(t) = 0, and co(0= 0 for t
(5.8)
+ 1)] (5.9)
(5.10)
(5.11)
(5.12)
(5.13)
. Without loss
<1.
31
For a discrete time signal x(f)jc(i)<= 91, we will use the quantity
r,(0=5>(*)2. (5.14)
7=1
Also, throughout this section we assume that all constants C; and Kf,i = 1,2,3,..., are
non-negative. Note that the system model (5.1) can be written in the form
Y(t +1) = 0o 0>(0+b0U(t) + q[C(q_1) l][co(f) 6(0]+0)(r+1) (5.15)
where 0O, **
substituting the corresponding control law (5.5) or (5.11) into (5.15), we derive**
F(r+1) F* (r+1) co(r+1) = -z{t)+q[C(q_1 y1 ][co(0 o>(0] (5.16)
where
z(r)=-0(r)rm 0(O = 0(O-0o (5.17)
For future reference we cite the following lemma (3,4)
Lemma 1:
If assumptions (Al) (A2) hold, the RLS algorithm provide
(i)
0(r+l)-0o
a.s.
(5.18)
32
where ^^(r) is the minimal eigenvalue of p(t) 1.
(ii) |)(ffl(0-CD(0)
f=l
OH) SQlogr4W a.s. (5.20)
r=l t + Pw
where r^t) is defined by (5.13) and
P(fc=(0rJp(f- 1W). (5.21)
Proof. The first two statements are proved in [5], while the proof of (5.20) is given in
[4]-
By analyzing possible bursting phenomena, we will show in the next two sections
that the following statements are valid for the RLS algorithm.
(i) rz(r)<0(r1_E+2e), 0<Â£0e/2, 0<Â£<|j/(2+|i) a.s. (5.22)
where rz(t) is given by equation (5.14) when x(t) = z(t), z(t) is defined by (23), Â£0 is
an arbitrarily small number and |i is defined by assumption (Al).
(ii) ^(70-+l);-y*(i+l)-m(i+l))2 <0(r1_e+2e)
i=i
(5.23)
a.s.
(5.24)
(iii) lim Y [F(i +1) F (i +1)]2 = R
where R is given by (5.3)
(iv) ||4>(r)||2<0(r1-e+2e) a.s. (5.25)
(V) IF sup-^2[F(t)2 + 17(f)2] < a.s. N~> N ^ (5.26)
In order to establish the convergence properties of this algorithm, we need the
following simple lemma
Lemma 2:
Let assumptions (A1)-(A2) hold. Then for sufficiently large t
(i) r^(t)
(ii) lim inf rJt)/t >C6>0 a.s. (5.28)
(iii) P(f) < 0[rz(f-l) + ft-E] a.s. (5-29)
where rz(t) and e are defined in (5.22), and r^(t) and (3(f) are given by (5.13) and
(5.21) respectively.
Proof: The proof of this lemma is given in [6, M. Radenkovic and A. Michel].
34
The properties of RLS are formulated in the next theorem, whose proof is based on
the evaluation of the possible bursts of rz(t).
Theorem 1:
Let assumptions (A1)-(A3) hold. Then RLS provides the validity of the statements
(5.22) through (5.26).
Proof. Statement (iii) of Lemma 2 implies that P(r) can be of order Q[^(r-1)] or
0(r1_E). This means that rz(t l) can be larger or smaller than r1-E, i.e. bursts of
rz(t -1 )/r1-E are possible. In this sense let us define sequences xk and ck, k > 1, as
A
\=xl
so that
^(r-l)
rz(t -1)> t1_E for t e Tk = [afc,Tfc+1) a.s. (5.32)
where Â£ is given in (5.22). If r2(0)> 1, then we set Tj =0 and Oj =1 in (5.30). For
the sequences %k and ck we will analyze three cases.
Case 1. Tt and Gk are finite for all finite k.
Case 2. There exists a finite k0 so that t*o < and c*o = .
Case 3. There exists a finite /q so that Gki < and xfci+1 =
Case 2 is trivial and from (5.31) it follows that rz(t)< r1-E (a.s.) for all t > xft(. From
assumption (Al) which implies co(f)2 = 0(t1-E), we see that rz(t) is actually the order
35
of G)(02. In this case the upper bound on rz{t) is stronger than the one established in
[4].
Let us consider Case 1. Since rz{t-1)< r1-E \ft e Qk, from (5.31) and (5.32) it is
obvious that in this case we need to evaluate rz(i) only for teTk. From (5.29) and
(5.32) we can derive that for sufficiently large k
l + P^Ejmaxt^r-l);*1-6]^^-!) VreT^ a.s. (5.33)
where Â£j is a small positive number satisfying
0<Â£a
^ e0(l-e)
C3(le+e0)
(5.34)
Here e0 is an arbitrarily small number, 0 < Â£0 < e/2, while Â£ is defined in (5.22) and C3
is the constant from (5.20). Such choice of Â£: will be clear from the derivations that
follow. Substituting (5.33) into (5.20), we obtain
Since
Â£ Z(t)
f *r7{t)-rz{t-1)
hr&-1) h r*(t-D
N 'ito
J
rz(N)
rzyk-1)
(5.36)
we derive from (5.35)
36
T (N)
!og5----- <Â£iC3log^(iV), N eTk a.s. (5.37)
Wt-!)
By using (5.27) and (5.32), we can obtain for N e Tk
logr^(N)< log{2Cn max[rz(N -1 );N]} < log rz(N l)1/a_E)+C18 . (5.38)
Substituting (5.38) into (5.37) yields
(l-Y^)logr2(iV)
or
rz(N)
Let us now evaluate rz(ck -1). From (5.29) and (5.31) it follows that
l+P(aJt-l)$e2(afc-l)1"e, 0
for sufficiently large k. Then from (5.20) we can derive
z(c* -l)2 <[1 + P(g* -1)]C3 logr*(cfc -1) < e2C3(a* -l)1"6logr,(Q* -1). (5.42)
Since from (5.31) rz(ak -2)<(ck -1)1_E, relation (5.27) yields
log^(afc -1)< log(afc 1)+C21 a.s. (5.43)
37
and therefore by (5.42) we obtain for k sufficiently large
z(afc -1 )2 < C22 (ak l)1_e log(afc -1) a.s.
or
^ (o* -1) = rx (ofc 2)+z(ck -1)2 < (ok l)1"* + C22 (o* -1)1"6 log(Gfc
- Q3(aJt l)1_elog(crJt -1)
After substituting (4.12) into (5.40) we conclude that
rz(N)< C24(a* -l)^-Â£-^[log(afc _i)]M, N e Tk a.s.
Since for sufficiently large k
[log(ak-l)fm'c'CiC3)<(ck-ir\ 0 < e0 < s/2
we obtain from (5.46)
rz(N)< -1)1"*-, NsTk a.s.
(5.44)
1)
(5.45)
(5.46)
(5.47)
(5.48)
Here we have used the fact that by (5.34) (1-Â£)/(1-Â£-Â£jC3)< l-Â£ + Â£0, where, as
stated with (5.34), Â£0 is an arbitrarily small constant, 0< Â£0 < Â£/2. Finally we see that
(5.31) and (5.48) constitute statement (5.22).
Case 3, i.e. when in (5.30) there exists a kx so that aki
impossible. Specifically, from (5.48) it follows that rz(t) < (a^ -1) < o (a.s.) for
t>Gki, which contradicts the fact that from (5.32) rz(t)> r1_e for all t > aki.
From the previous analysis it is obvious that regarding rz(t) two possibilities exist:
(i) rz(t-\)/t1~E exhibits bursts throughout the adaptation process or
(ii) Bursts do not exist and rz(t) 1. In this case
our upper bound for rz(t) is stronger than that presented in [4].
Statements (5.23) and (5.24) of the theorem follow from (5.16), (5.19) and (5.22).
Thus the theorem is proved.
6. Workbench Controller Calculation
The developments in chapters 2, 3, 4 now allow us to combine all the results
together and formulate an estimation and control algorithm for controlling the
Workbench using self-tuning predictor method. From the system derivation, we have
the mathematical model for the Workbench as
Y(t) = aj(t -1) + a2Y(t 2) + a^it 3) + aj(t -4)+b0U(t-l). (6.1)
By equating the above equation, it's not difficult to see that:
- The delay of the Workbench is d = 1,
- The order of polynomial A(z) is n = 4
- The order of polynomial B(z) is m = 0;
-And C(z) = 1.
In this case we have
F(z) = /0 = 1
and
G(z)=[g0+ gxz + g2z2 + g3z3 ]
according to expressions in (3.8). From equation (3.5) with
(6-2)
(6.3)
40
we have
Bm(z) =l
An(z)
C(z) = F{z)A(z)+z1G(z). (6.4)
In terms of a; and g{, i = 1,2,3,- , equation (6.4) can be written as
1 = 1[1+Ojz+a2z+a3z + a4z]+z[g0 + &z+g2z2 + g3z3 ], (6.5)
and by equating the coefficients of terms that have the same power we have
8o--^8i--^82--(h^3=-a4- (6-^)
In our case, at (i = 1, 2, 3, 4) are not known, and if we replace them by a{ obtained
from the Recursive Least Square algorithm, expressions in (6.6) become
So ^lSl ^282 ~ ^3S3 ~ ^4 (6.7)
From equation (3.17), the predictor can be written in the form
Y(t+l\t) = g0Y(t)+glY(t-l) + g2Y(t-2)+g3Y(t-3) + (bf)0U(t-l), (6.8)
and by replacing gt by a{, b0 by bQ and /0 = 1 as shown in equation (6.2) we have
41
Y(t+l\t) = a1Y(t) + a2Y(t-l) + a3Y(t-2) + a4Y(t-3)+b0U(t-l).
(6.9)
If we set Y{t\t-l) in equation (6.9) equal to 7*(? + l), and solve for U(t), the
control law for the Workbench can be calculated using
U(t) = i[Y*(t+l)-a1Y(t)-a2Y(t-l)-a3Y(t-2)-aJ(t-3)] . (6.10)
h
Because the input to the Workbench is bounded, the value of U (t) should be trimmed
to Ub(t) before being applied to the motor, and the output of the Workbench should
be estimated with the bounded Ub(t) using
Y(t\t-l) = a1Y(t-l) + a2Y(t-2)+aiY(t-3) + aAY(t-4)+b0Ub(t-l). (6.11)
The error between the Workbench's real output and estimated output would be
e{t) = Y{t)-Y{t\t-1), (6.12)
or in terms of (?) and 0(t), it can be written as
e(t) = Y(t)-$(t)TQ(t). (6.13)
This error is used in equations (4.27) to calculate the next parameter estimates.
42
Summing up all the works we have so far, the Workbench estimation and control
algorithm can be described in the following steps:
1. Read the position of the reel Y(t).
2. Estimate the position of the reel using equation (6.11)
Y(t\ t-l) = a1Y(t-l) + a2Y(t-2)+ ^(r 3) + aAY{t 4)+b0Ub (t -1)
with bounded control signal Ub(t-Y).
3. Compute the error between the real output and its estimate
e(t) = Y(t)-Y(t).
4. Estimate next parameters using equations (4.27) and (4.28)
Â§(t +1) = 0(f) + Pit+1)$(*M*)
pmmitfpjt)
l+OKrfPWOa)J
5. Calculate the control signal U{t) using equation (6.10)
U(t) = i[F* (t+1) o1F(r) aj{t -1) aj{t 2) ZAY{t 3)]
h
6. Trim the control signal U(t) to Ub (t) and send it to the Workbench.
7. Repeat stepl through step 7.
This algorithm is written in C language and the program is listed in Appendix A.
43
6.1 Verification of the model
Before the dynamic model of the Workbench and the control algorithm can be
implemented to perform an on-line estimation on the real process, they should be
verified by simulation. The simulation was performed in MatLab using off-line
estimation, and it consisted of three steps as below.
1. Data Collection:
The information on U(t) and Y(t), which are respectively the real input and
output of the Workbench, are collected by program written in C language while the
Workbench was run by other control scheme.
2. System Identification:
Using information on U (t) and Y(t) collected in step 1, the polynomials A(z) and
B(z) of the Workbench's dynamic model
A(z)Y(t) = B(z)U(t-1), (6.14)
where A(z) = axz + a2z2 + a3z3 + a4z4,
and B(z) = b0z,
are identified using Least Square Estimation method. The results of system
identification of the Workbench are shown in figures 6.1 and 6.2.
3. Validation of the Control Scheme:
This step is performed with the assumptions that
44
- The polynomials A(z) and B(z) found in step 2 are the real parameters of the
Workbench and they need to be estimated.
- And the output Y{t) from equation (6.15) which is written in the form
Y{t) =a?{t-\)+a?(t-2)+a?(t-3)+a?{t-4)+b0U{t). (6.15)
represents the real output of the Workbench, and it needs to be regulated.
The Recursive Least Square method is used to estimate polynomials A(z) and
A
B(z). Then the calculations of regulator polynomials C{z),F{z) and G(z) are based
on A(z) and B{z) and the control signal U (t) is calculated using the control scheme
illustrated in figure 3.5. In step 3, Y(t) and U (r) are not from collected data, but they
A A
are calculated using the estimated parameters A(z) and B(z) obtained in this step.
The convergence of parameter estimates and the boundedness of the control signal
U (t) shown in the simulated results on the following pages show that the estimation
and control algorithm is working. The system output is stable with unbounded input
U (t) as shown in figures 6.3 and 6.4. The control signal U (t) (figure 6.5) is finite
although it is larger than the limits of Ub (t). The estimated polynomials A{z) and B(z)
(figure 6.6) converged to A(z) and B(z) obtained in step 2 and the output Y(t) has a
good tracking on both sinusoidal trajectory and triangular trajectory Y* (t).
The problem is that the control signal U (t) obtained from equation (6.10) is too
large to apply to the Workbench, therefore a second simulation is performed with
U (f) found in equation (6.10) trimmed within the limits of the input of the Workbench
which are +/- 2.5 volts. The results as shown in figure 6.8 through figure 6.10 assure
that the algorithm is also working and stable at least in simulation.
45
0 100 200 300 400 500 600 700 800 900
Figure 6.1. Result of system identification. Real Output curve is ploted from raw data.
Model Output: P( 0 =a, F(r -1) +a2l'(f 2) +af(t 3) +akY(t 4) +b0U{t).
46
Parameter estimates
Figure 6.2. Result of parameters identification of:
Y{t)=a,Y{t-1) +a2Y(t 2) +a3f(r 3) +aj(t -4 )+b0U(t)
47
Validation of the Control Scheme.
Figure 6.3. Simulation of controller performance with sinusoidal reference input.
48
Validation of Control Scheme.
Figure 6.4. Simulation of controller performance with triangular reference input.
Real Output: Y(t) =axY{t-1) +a2F(f-2) +aliY(t-3) +aAY(t-4)+b0U(t).
Predictor Output: F(r) =atY(t-1) +d2Y(t-2) +aj(t-3) +aj(t-4)+bQU(t).
49
Validation of Control Scheme.
50
2
Validation of Control Scheme.
Figure 6.6. Estimated parameters of the regulator.
51
Validation of Control Scheme.
52
Validation of Control Scheme.
Figure 6.8. Simlated output of the WB
with bounded control signal Ub(t) and triangular reference trajectory.
53
Validation of Control Scheme.
Figure 6.9. Simulated output of the WB
with bounded control signal Ub(t) and sinusoidal reference trajectory.
54
Validation of Control Scheme.
55
7. Discussion and Conclusion
The performance (as shown in figure 7.1 through figure 7.10) of the parameter
adaptive controller developed in this thesis shows a good tracking of the reel on a
triangular reference signal and a sinusoidal reference signal. The error between the
real output and the reference signal is less than 15 percent as shown in the plots.
However the actual error is better than it shows in the plots due to the fact that when
the computer collects data for plotting, the communication between the Workbench
and the computer is interrupted as the computer writes data to a disket, therefore the
rate of sampling the reel's position is reduced and so does the accuracy of the regulator
in tracking the reference trajectory. Figure 7.11 through figure 7.13 show that the
regulator has an ability to adapt to changes as we tried to disturb the dynamics of the
Workbench by putting more weight on one side of the balance. After a while of
adjusting itself, the Workbench comes back to its stable state. The parameter
estimates converge to some values and have a very slow rate of drifting throughout
the estimation. However the performance of the regulator is dependent on the initial
values of the parameter estimates when the system is switched to the parameter
adaptive controller. But a good initial condition is very difficult to obtain due to the
lack of software to generate a rich informative input that has enough frequencies to
cover all the poles of the Workbench. Another factor that needs to be improved is the
reset of the gain matrix P{t) to revitalize the estimation algorithm. What makes the
reset of P(t) difficult is that we cannot monitor all of its 5x5 matrix elements. It's
56
because the controller algorithm was written in two different programs. One is the
Bench Controller (BC) which is in C language, and it can be modified for a different
control scheme. The other is the Bench Controller Monitor (BCM), and it is used to
graphically monitor the output variables defined in the Bench Controller (BC) file.
This BCM file is provided in executable file only (binary code file), and it is impossible
to modify this file without the source code. Therefore the new control scheme must
be written in terms of the variables that have already been defined in the BCM file, and
these variables are not enough to name all the parameters, the states, and the elements
of the gain matrix P{t) of the control scheme developed in this thesis.
57
0.3
Figure 7.1 Workbenchs response to a desired triangular trajectory.
Error between desired response and real response.
58
59
Figure 7.3 Workbench's response to a desired triangular trajectory.
Error between desired response and real response.
60
Figure 7.4 Workbench's response to a desired triangular trajectory.
Error between desired response and real response.
61
0.4
Figure 7.5 Workbench's response to a desired triangular trajectory.
Error between desired response and real response.
62
Figure 7.6 Workbench's response to a desired triangular trajectory.
Error between desired response and real response.
63
Figure 7.7 Workbench's response to a desired triangular trajectory.
Error between desired response and real response.
64
Figure 7.8 Workbench's response to a desired triangular trajectory.
65
Figure 7.9 Workbench's response to a desired triangular trajectory.
Error between desired response and real response.
66
0.2
Figure 7.10 Workbench's response to a desired triangular trajectory.
Error between desired response and real response.
67
0.2
Figure 7.11 Workbenchs response to a desired sinusoidal trajectory.
Error between desired response and real response.
68
0.2
Figure 7.12 Workbench's response to a desired sinusoidal trajectory.
Error between desired response and real response.
69
0.2
Figure 7.13 Workbenchs response to a desired sinusoidal trajectory.
Error between desired response and real response.
70
0.2
Figure 7.14 Workbench's response to a desired sinusoidal trajectory.
Error between desired response and real response.
71
0.2
Figure 7.15 Workbench's response to a desired sinusoidal trajectory.
0.1
0
-0.1
-0.2
0 50 100 150 200 250 300 350 400
0.02
0.01
0
-0.01
-0.02
0 50 100 150 200 250 300 350 400
Error between desired response and real response.
72
Figure 7.16 Workbench's response to a desired sinusoidal trajectory.
Error between desired response and real response.
73
Error between desired response and real response.
74
Figure 7.18 Workbench's response to a desired sinusoidal trajectory.
Error between desired response and real response.
75
0.2
Figure 7.19 Workbench's response to a desired sinusoidal trajectory.
Error between desired response and real response.
76
0.2
Figure 7.20 Workbench's response to a desired sinusoidal trajectory.
Error between desired response and real response.
77
Figure 7.21 Workbenchs response to disturbance.
The error between the work bench output and the reference signal
78
Figure 7.22 Workbench's response to disturbance.
The error between the work bench output and the reference signal
79
Figure 7.23 Workbench's response to disturbance.
The error between the work bench output and the reference signal
80
APPENDIX A.
The following is the C language program that controls the Workbench.
#include
#include
#include
#include "tsr.h"
#include "reg.h"
void estimation(void);
#define supplyvolts 5 /* Supply voltage */
#define rodlength .51 /* Length of rod in metres */
#define TachoConstant 1.2 /* vs/rad */
#define maxout 2.5 /* Output voltage clipping limit */
#define DITHER 1 /* Apply dither to the output voltage */
#define REFDIM 182 /* Dimension of reference arrays */
#define LoadedMessage "VnBalanceController loaded"
^define ExitMessage "\nBalanceController unloaded"
#defme TakenMessage "\nBalanceController has already been loaded"
#define PowerMessage "\nWaming! your Balance Control Workbench has n<
powered up yet"
#define NoioMessage "NnWaming! unable to locate i/o hardware"
#define NpresMessage "NnBalanceController not present"
#define Freeze 1 /* regmode choises */
#define TestSubl 2
#define TestSub2 3
#define TestSub3 4
#define TestSub4 5
#define pcl711 1 /* Use PCL-711 type card */
81
int reffunc[ REFDIM ], refdim = REFDIM, maxrefdim = REFDIM,
timecc, autoref=0, regmode=0, options[5];
float neutral, yl=0, y2=0, /* Measured inputs *1
zpl=0, zp2=0, zp3=0, zp4=4, /* State vectors */
xpl=0, xp2=0, xp3=0, xp4=0, /* estimate parameters al-a4 */
xel=0, xe2=0, xe3=0, xe4=0, /* reserve */
zel=0, ze2=0, ze3=0, ze4=0,
epl=l, ep2=l, /* Innovation process states */
ypl=0, yp2=0,
ypll=0, yp22=0,
kll = 1, kl2 = 0,
k21 = 20, E to II o o o
k31 = 20, k32 = .06,
k41 = 0, k42 = .6,
gl= 30, g2=20, g3 =15, g4=2.5;
all= =0, al2=l, al3=0, al4=0,
a21= =0, a22=0, a23=6.53, a24=0,
a31= =0, a32=0, a33=0, a34=l,
a41= =0, a42=0, a43=0, o' 1 II
/* Feedback vector : */
/* A matrix */
dl 1=1, dl2=0, dl3=0, dl4=0, /* D matrix */
d21=0, d22=0, d23=0, d24=l,
bl 1, b21, b31, b41=4.4,
/* B matrix */
ul=0, dither=0.05, ts=0.054936, /* Misc. other globals */
xref=0, vref=0, firef, fivref,
ylderiv, params[5];
#define estdim 5
#define lamdal 1.00
#define collect 15
82
float R2, error, bc_output, bc_input, YRY;
float RlY[estdim], YtRlfestdim], yhat;
float Vl[estdim] = { 0, 0, 0, 0, 80 }; /* VI is the parameter estimation vector */
float Y1 l[estdim] = { 0, 0, 0, 0, 0}; /* Y1 is the measurement vector */
float Rl[estdim][estdim] = {{ le6, 0, 0, 0, 0 }, /* R1 is the gain matrix used in */
{ 0, le6, 0, 0, 0 }, /* the estimation algorithm. */
{ 0, 0, le6, 0, 0 },
{ 0, 0, 0, le6, 0 },
{ 0, 0, 0, 0, le6 }};
int i, j, k, countl = 0;
static void NormalOperation( void )
{
static int ditherpol=0;
static float oldyl;
countl += 1; xe3 = countl;
if (countl > 10000) countl = collect;
oldyl=yl;
read_inputs();
estimationO; /* Compute derivatives of yl for comparison purposes */
ylderiv = (yl oldyl) / ts;
kalmanfilterO;
genref();
ul = (xref (V1[0]*Y11[0]+V1[1]*Y11[1]+V1[2]*Y11[2]+V1[3]*Y11[3]))/V1[4];
ul = LIMIT(ul, -maxout, maxout);
ul = ditherpol ? ul + dither : ul dither;
ditherpol = ditherpol;
write_output(ul );
}
void estimationO
{
Yl 1[4] =ul;
/* ypl is the next step process output yhat(k+l) */
/* Add dither */
83
ypl =
V1[0]*Y11[0]+V1[1]*Y11[1]+V1[2]*Y11[2]+V1[3]*Y11[3]+V1[4]*Y11[4];
error = yl ypl; xe2 = R1[0][0]; xe4 = error;
if (ypl > .25 ) ypl = .25;
else if (ypl < -.25) ypl = -.25;
YRY = lamdal;
for (i = 0; i < estdim; i++)
{ RlY[i] = 0;
YtRl[i] = 0;
for (j = 0; j < estdim; j++)
{
RlY[i] += Rl[i][j]*Yl l[j]; /* this 5x1 matrix is needed to cal R */
}
YtRl[i] = RlY[i]; /* this 1x5 matrix is needed to cal R which is the gain matrix */
YRY += Yl l[i]*Rl Y[i]; /* denominator of rh terms of R*/
}
for (i = 0; i < estdim; i++)
{R2 = 0;
for (j = 0; j < estdim; j++)
{
Rl[i][j] = (Rl[i][j] R1 Y[i]*YtRl [j]/YRY); /*/lamdal;*/
R2 += Rl[i][j]*Yll[j];
}
VI [i] += R2* error;
}
Yl 1 [3] = Yl 1 [2]; Yl 1 [2] = Yl 1 [1]; Yl 1 [1] = Yl 1 [0]; Yl 1 [0] = yl;
/* Yl 1[4] = ul; */
xpl = VI [0]; xp2 = VI [1]; xp3 = VI [2]; xp4 = VI [3]; xel = VI [4];
}
static void kalmanfilter( void ) /* Run the kalmanfilter one iteration */
{
/* Compute predicted state (Aperiori estimate )
assuming:
[fi] = [I + AT], and
[delta] =BT
84
Note that only a few elements in A are used
*/
zpl = zel + (all*zel + al2*ze2-+ al3*ze3 + al4*ze4 + bll*ul) ts;
zp2 = ze2 + (a21*zel + a22*ze2 + a23*ze3 + a24*ze4 + b21*ul) ts;
zp3 = ze3 + (a31*zel + a32*ze2 + a33*ze3 + a34*ze4 + b31*ul) ts;
zp4 = ze4 + (a41*zel + a42*ze2 + a43*ze3 + a44*ze4 + b41*ul) ts;
ypll = dll*zpl;
yp22 = d24*zp4;
/* Corrected state ( Aposteriori estimate ) */
epl =yl ypll;
ep2 = y2 yp22;
zel =zpl +kll*epl +kl2*ep2;
ze2 = zp2 + k21*epl + k22*ep2;
ze3 = zp3 + k31*epl + k32*ep2;
ze4 = zp4 + k41 *epl + k42*ep2;
/* Stay within reasonable limits */
zel = LIMIT( zel, -.25, .25 );
ze2 = LIMIT( ze2, -.5, .5);
ze3 = LIMIT( ze3, -1, 1 );
ze4 = LIMIT( ze4, -2,2);
static void freeze( void) {
ul=0;
write_output( ul);
static void testsubl( void ) { /* Run kalmanfilter, but apply a constant
voltage to the
motor */
read_inputs();
ul=params[0];
kalmanfilterO;
ul=params[0];
write_output( ul);
85
static void testsub2( void) { /* Run kalmanfilter, but apply a random
voltage to the
motor */
read_inputs();
u 1 =(float)(rand()-16384)/8192;
kalmanfilterO;
write_output( ul);
}
static void testsub3( void) { /* Run kalmanfilter, but apply a constant
voltage that is
reversed at random
intervals to the
motor */
static int swapcc;
read_inputsO;
kalmanfilterO;
timecc++;
if (timecc >= swapcc) {
ul = ul > 0 ? -params[l] : params[l];
swapcc += rand0/1000/options[0];
write_output( ul);
}
/* Run filter one iteration */
static void testsub4( void) {
voltage to output */
static int swapcc;
read_inputsO;
ul = 0;
if (options[l])
~options[l];
else
ul = params[2];
write_output( ul);
kalmanfilterO;
}
/* Wait predefined interval, then apply
a predefined
/* Run filter one iteration */
86
void read_inputs( void )
{ /* Read and filtrate the process measurements position and tacho */
#ifdefpcl711
neutral =get711(0);
yl = get711(1)/ supplyvolts rodlength rodlength / 2;
y2 = get711(2)* TachoConstant;
#endif
#ifdef dt2801
#ifdef Orgtass
yl = get2801( 0, 0 ) /10.5 rodlength rodlength / 2;
y2 = get2801( 3,1 )* TachoConstant;
#else
neutral = get2801( 1, 0 );
yl = get2801( 1, 1) / supplyvolts rodlength rodlength / 2;
y2 = get2801( 3, 2) TachoConstant;
#endif
#endif
#ifdef dt28001
int i, ip, it, maxt, mint, maxp, minp;
maxt = -32000;
mint = 32000;
maxp = -32000;
minp = 32000;
ip = 0;
it= 0;
for(j = 1; j < 6; ++j) {
i = read2801(0, 1 );
if (i > maxp) maxp=i;
if( i < minp ) minp=i;
ip = ip + i;
i = read2801( 3,2);
if (i > maxt) maxt=i;
87
if (i < mint) mint=i;
it = it + i;
}
yl = 1.628e-3*(ip-maxp-minp);
y2 = 2.03e-4*(it-maxt-mint);
#endif
}
void write_output( float utemp )
{
/* Note : motor amplifier has internal gain=2 due to the fullbridgecoupling
voltages below .5 volts will disable the motor
*/
#ifdef pc!711
put711( LIMIT((float)utemp/2+neutral,.5,5) );
#endif
#ifdefdt2801
#ifdef Orgtass
put2801( LIMIT((float)utemp3-2.5,2.5));
#else
put2801( LIMIT((float)utemp/2+neutral,.5,5));
#endif
#endif
}
void genref( void )
{
/* Generate reference vector */
static unsigned indx=0, reftime=l;
int inext;
if (autoref) {
inext = indx < refdim-1 ? indx+1 : 0;
xref = .00001 reflimc[ indx ];
++reftime;
if( reftime > autoref) {
reftime = 1;
++indx;
}
88
if (indx > refdim-1 ) indx = 0;
}
switch ( options[0]) {
case 0:
vref = LIMIT((xref-zel)/2,-.3,.3);
firef = LIMIT((vref-ze2),-. 1,. 1);
fivref = UMIT((firef-ze3),-. 1,. 1);
break;
case 1:
vref = LIMIT((xref-zel)/3,-.3,.3);
firef = fivref = 0;
break;
case 2:
if ( autoref)
vref = (.00001 refliinc[ inext ] xref) / autoref / ts ;
else
vref = 0;
vref = LIMIT(vref,-.3,.3);
firef = fivref = 0;
break;
case 3:
vref = firef = fivref = 0;
}
}
void initio( void ){ /* Initialize the I/O hardware */
#ifdef dt2801
#ifdef Orgtass
if (cinitO = 0 )
put2801( 0.);
gl=20 g2=13, g3=18, g4=8, /* Feedback vector : */
dither = .2;
neutral = 2.5;
k21= 1.1;
#else
if ( cinitO = 0) {
neutral = get2801( 1,0);
put2801( neutral);
89
}
else
neutral = -500;
#endif
#endif
#ifdefpcl711
neutral = get711(0);
put711( neutral);
#endif
}
/* Timer interrupt entry point */
static void interrupt far BalanceControllerO
{
(*oldtimer)0; /* execute old timer vector */
newstackO; /* swap stack */
switch( regmode) { /* Select a controller routine */
case Freeze:
freezeO; break;
caseTestSubl :
testsubl(); break;
case TestSub2:
testsub20; break;
case TestSub3 :
testsub30; break;
case TestSub4 :
testsub40; break;
default:
NormalOperationO;
}
-H-timecc; /* Increment the tic counter, timecc is used to syncronize
the fetching of parameters */
oldstackO; /* Go back to interrupted program's stack */
}
90
#ifdefdt2801
D T 2 8 0 1 Interface routines
#define adinn 12
#define adout 8
float get2801( int gain, int channel) {
switch ( gain ) {
case 0:
return (float)(read2801( 0 channel )-2048)*20./4096;
case 1:
return (float)(read2801( 1, channel )-2048)*10./4096;
case 2:
return (float)(read2801( 2, channel )-2048)*5./4096;
case 3:
return (float)(read2801( 3 channel )-2048)*2.5/4096;
}
}
int cinit( void) { /* Initialize the dt2801, signal if it is not there */
int count;
/* Issue a stop */
outportb( 0x2ed, 15 ); delayO;
/* Read status byte */
count = 0;
while ( !(inportb( 0x2ed ) & 0x05) && ++count < 30000)
9
if ( count >= 30000) return -1;
inportb( 0x2ec);
/* Card should now be ready for a command,
check! */
count = 0;
91
while ((inportb( 0x2ed) & 0x06) != 0x04 && -H-count < 30000 )
if ( count >= 30000) return -2;
/* Clear data regs */
if (inportb( 0x2ed ) & 0x05 ) inportb( 0x2ec );
/* Issue a RESET (BYTE) */
outportb( 0x2ed, 0 ); delayO;
/* Clear data regs */
if (inportb( 0x2ed ) & 0x05 ) inportb( 0x2ec );
return 0;
void delay( void ) {
inti;
for (i=0; i<50; ++i) ;
int cinw( void ) {
int high, low;
while ( !(inportb( 0x2ed) & 0x05));
low = inportb( 0x2ec );
while ( !(inportb( 0x2ed ) & 0x05) );
high = inportb( 0x2ec);
return ( high 8 ) + low;
/* read data register (WORD) */
/* Loop until data ready */
/* get low byte */
/* Loop until data ready */
/* get high byte */
void cutb( int byte ) { /* Write data register (BYTE) */
while (inportb( 0x2ed ) & 0x02 ); /* Loop until data ready */
outportb( 0x2ec, byte);
void cutw( int word ) { /* Write data register (WORD) */
while (inportb( 0x2ed ) & 0x02 ); /* Loop until data ready */
92
outportb( 0x2ec, word); /* output low byte */
while (inportb( 0x2ed) & 0x02); /* Loop until data ready */
outportb( 0x2ec, word8 ); /* output high byte */
void ckom( int comm ) { /* Issue a command (BYTE) */
while ((inportb( 0x2ed) & 0x06) != 0x04)
; /* Loop until ready */
outportb( 0x2ed, comm); /* output high byte */
int read2801( int gain, int channel) {
int count,value;
count=0;
do { /* redo if illegal value, but max 10 times */
outportb( 0x2ed, 15);
ckom( adinn );
cutb( gain );
cutb( channel);
value = cinwO;
} while ( ( value <= 0 || value >= 4096) && ++count < 10 );
return value;
void put2801( float value )
{
int j;
j = value 204.8 + 2048;
if (j > 4095 ) j=4095;
if(j <0 )j=0;
ckom( adout);
cutb( 0 );
cutw(j);
#endif
93
~~
~~ |