2007:086
MASTER'S THESIS
UAV Stabilized Platform
Martin Ernesto Orejas
Luleå University of Technology
Master Thesis, Continuation Courses
Space Science and Technology
Department of Space Science, Kiruna
2007:086 - ISSN: 1653-0187 - ISRN: LTU-PB-EX--07/086--SE
UAV Stabilized Platform
Ing. Orejas, Martín Ernesto
Thesis Supervisors: Ing. Hromcik, Martin, Ph.D; Ing. Harlin, Gösta
Master Diploma Thesis for
Erasmus Mundus programme SpaceMaster
Department of Space Science
Kiruna Space Campus
Department of Control Engineering
Faculty of Electrical Engineering
Czech Technical University
Prague –May 2007
Abstract
Abstract
This work develops a performance analysis for a particular distribution of inertial sensors as
they are assumed to be implemented on a prototype stabilized platform for a new UAV aircraft
(under development at VTUL PVO and AVEKO now). The device to be stabilized is a 2-axis
gimbal –
inner and outer gimbal axis- that will have an attached array of optical and electric
sensors at one of its axis. Gyroscopes are planned to be used to sense the angular rates of this
highly non-linear and coupled system. The whole set of equations for the complete system is
introduced with a specific focus on the inner gimbal dynamics. The use of different observers to
filter the data obtained by the sensors is analyzed along with the effectiveness of various types
of controllers. The inner gimbal system is modeled and several simulations are performed to
study the effect of different practical contraints -actuator saturation, sensor bandwidth, sampling
frequency, etc., in addition to the performance for different disturbance frequencies and
controller parameters.
Key words: LOS stabilization, gyroscopes, disturbance rejection.
i
Table of Contents
Table of Contents
1. Introduction ............................................................................................1
1.1. Introduction to stabilized platforms .............................................................................1
1.2. Objetives.......................................................................................................................3
1.3. Thesis Layout ...............................................................................................................3
2. Gyros, motors and gimbal geometry ......................................................5
2.1. General characteristics of gyroscopes ..........................................................................5
2.1.1.
Gyro specifications .....................................................................................5
2.1.2.
Gyro modeling ............................................................................................6
2.2. Brush type DC motors..................................................................................................7
2.2.1.
Motor modeling ..........................................................................................9
2.3. Gimbal..........................................................................................................................9
3. Dynamics and kinematics.....................................................................11
3.1. Basic coordinate frame transformations.....................................................................11
3.2. Eul
e
r
’
smome
n
te
qua
t
i
on
s..........................................................................................14
3.3. Inner gimbal dynamics ...............................................................................................14
3.4. Outer gimbal dynamics ..............................................................................................19
3.5. Augmented inner gimbal dynamics –sensor dynamics .............................................22
4. Observers and controllers .....................................................................25
4.1. Observer for inner gimbal ..........................................................................................25
4.1.1.
First order approximation .........................................................................25
4.1.2.
Extended Kalman filter .............................................................................35
4.2. Controller for the inner gimbal...................................................................................41
4.2.1.
Constraints for the controller ....................................................................45
ii
Table of Contents
4.2.2.
Alternative controller for the inner gimbal ...............................................53
5. Conclusions ..........................................................................................62
6.1. General conclusions ...................................................................................................62
6.2. Future work ................................................................................................................63
6. References ............................................................................................64
iii
Figures List
Figures List
Fig. 1.1. Descriptive picture of LOS system................................................................................2
Fig. 2.1. Gyro circuit and block diagram.....................................................................................6
Fig. 2.2. 3D representation of the motor......................................................................................8
Fig. 2.3. Gimbal .........................................................................................................................10
Fig. 3.1. Different coordinate frames for a 2-axis gimbal .........................................................11
Fig. 4.1. Torque couse by Coulomb friction..............................................................................26
Fig. 4.2. Real vs. estimated friction torque ................................................................................27
Fig. 4.3. Zoom over transition area............................................................................................27
Fig. 4.4. System–observer diagram ...........................................................................................31
Fig. 4.5. Real vs. estimated CFT................................................................................................31
Fig. 4.6. Amplification of real vs. estimated CFT .....................................................................32
Fig. 4.7. Real vs. estimated CFT for high MN ..........................................................................32
Fig. 4.8. Amplification of real vs. estimated CFT for high MN ................................................33
Fig. 4.9. Re
a
lv
se
s
t
i
ma
t
e
dωIy ...................................................................................................34
Fi
g
.4.
10.Zoomov
e
rr
e
a
lv
s
.e
s
t
i
ma
t
e
dωIy .................................................................................34
Fig. 4.11. Real vs. Estimated CFT using EKF.............................................................................37
Fig. 4.12. Zoom over real vs. estimated CFT using EKF ............................................................38
Fig. 4.13. Zoom over real vs. estimated CFT using EKF for high MN .......................................38
Fig. 4.14. Zoom over real vs. estimated CFT using EKF for extremely high MN ......................39
Fig. 4.15. Se
ns
orou
t
pu
tv
s
.ωIy....................................................................................................39
Fi
g
.4.
16.Re
a
lv
s
.e
s
t
i
ma
t
e
dωIy ..................................................................................................40
Fi
g
.4.
17.Zoomov
e
rr
e
a
lv
s
.e
s
t
i
ma
t
e
dωIy .................................................................................41
Fig. 4.18. a) Angular velocity before and after control signal is applied.....................................42
iv
Figures List
Fig. 4.18. b) Zoom over angular velocity before and after control signal is applied...................43
Fig. 4.19. Angular velocity before and after control signal is applied (2º controller) .................43
Fi
g
.4.
20.Zoomov
e
rωIy using 2º controller ...............................................................................44
Fi
g
.4.
21.Zoomov
e
rωIy using 3º controller ...............................................................................44
Fig. 4.22. System response with and without quantization..........................................................46
Fig. 4.23. Sensor output after quantization..................................................................................47
Fig. 4.24. Zoom over sensor output after quantization ................................................................47
Fig. 4.25. Control torque with and without saturation.................................................................48
Fi
g
.4.
26.Zoomov
e
ra
ng
ul
a
rv
e
l
oc
i
t
yωI
ywi
t
ho
uts
a
t
u
r
a
t
i
on....................................................49
Fi
g
.4.
27.Zoomov
e
ra
ng
ul
a
rv
e
l
oc
i
t
yωI
ywi
t
hs
a
t
u
r
a
t
i
on.........................................................49
Fig. 4.28. An
g
ul
a
rv
e
l
o
c
i
t
yωI
yf
or50Hzs
a
mpl
i
ng....................................................................51
Fi
g
.4.
29.An
g
ul
a
rv
e
l
o
c
i
t
yωI
yf
or300Hzs
a
mpl
i
ng..................................................................52
Fi
g
.4.
30.An
g
ul
a
rv
e
l
o
c
i
t
yωIy for 1000Hz sampling .................................................................52
Fi
g
.4.
31.An
g
l
eθ
Iy for 50Hz sampling .......................................................................................54
Fig. 4.32. Complete system diagram for inner gimbal stabilization ............................................55
Fig. 4.33. An
g
l
eθ
Iy for 50Hz sampling and LQ controller..........................................................56
Fi
g
.4.
34.An
g
ul
a
rv
e
l
o
c
i
t
yωIy for proportional controller .........................................................56
Fi
g
.4.
35.An
g
ul
a
rv
e
l
o
c
i
t
yωIy for LQ controller........................................................................57
Fi
g
.4.
36.Zoomov
e
ra
ng
ul
a
rv
e
l
oc
i
t
yωIy for LQ controller ......................................................57
Fig. 4.37. Zoom over real angle vs. estimated angle ...................................................................58
Fi
g
.4.
38.An
g
l
eθ
Iy for 50Hz sampling and proportional controller ...........................................58
Fi
g
.4.
39.An
g
l
eθ
Iy for 50Hz sampling and LQ controller..........................................................59
Fi
g
.4.
40.An
g
ul
a
rv
e
l
o
c
i
t
yωIy for 50Hz sampling and proportional controller.........................59
Fi
g
.4.
41.An
g
ul
a
rv
e
l
o
c
i
t
yωIy for 50Hz sampling and LQ controller .......................................60
Fi
g
.4.
42.An
g
l
eθ
Iy for LQ controller in tracking mode..............................................................60
v
Symbols Glosary
Symbols Glosary
LOS
Line-of-sight
PVG
Piezoelectric vibrating gyroscope
TFGYRO
Transfer Function of gyroscope sensor
KM
Motor constant (motor performance index)
RθPO
Rotational transformation from platform frame to outer frame
Rθ
OI
Rotational transformation from outer frame to inner frame

OI
Relative angular velocity between inner and outer gimbal

PO
Relative angular velocity between outer gimbal and platform
wP(t)
Angular rate of the platform frame
wO(t)
Angular rate of the outer frame
wI(t)
Angular rate of the inner frame
M
Applied moment
h
Angular momentum
I
Moment of inertia vector
G
Gravity gradient moment vector
Tel
Elevation stabilization control torque
T fI
Friction torque about the y-axis of the inner gimbal
Kcf
Coulomb friction coefficient
Kif
Viscous friction coefficient
Taz
Aximuth stabilization control torque
ωIy
Angular velocity over the inner gimbal y-axis
vi
Symbols Glosary
e(t)
Measurement noise
Pk
Estimated error covariance in time k
Kk
Kalman gain in time k
MN
Measurement noise
CKT
Coulomb friction torque
EKF
Extended Kalman filter
SP
Sampling period
SF
Sampling frequency
KP
Proportional gain
L
LQ optimal feedback gain
vii
Section 1. Introduction
1 Introduction
1.1 Introduction to stabilized platforms.
In many engineering applications, a complete servomechanism system often comprises multiple
channels, or axes; for example, a multiple-axis stabilized platform or a multiple-axis machine
tool. The control of such multivariable servomechanisms is, in general, not a simple problem, as
there exist cross-couplings, or interactions, between the different channels [6].
In the case of a multiple-axis stabilized platform, those cross-couplings include extra
nonlinearities that make the task even more difficult. These nonlinearities come not only from
the dynamics of rigid bodies but also from the nonlinear behavior of the bearing friction. This
friction is produced between the motors, used for stabilization, and the axis of rotation.
Stabilized platforms have various uses in many applications. One of the most common
applications is for stabilization of the line-of-sight (LOS). The LOS stabilization system is a
system that maintains the sightline of an electro-optical sensor when it is subjected to external
disturbances such as base motion [5]
Sensing equipment, such as electronic imaging devices, cameras, radars, navigation instruments,
and the like are frequently carried by and operated in a moving vehicle, such as an airplane, that
undergoes rotational motion about its center of rotation. In such an environment, the equipment
is typically mounted on a movable platform that is stabilized with respect to vehicle movements.
The stabilization may be about one, or more, of the vehicle axes. Particular applications involve
the LOS stabilization of a camera, or other imaging device. In such cases, when the vehicle
undergoes rotational motion about its axes, the LOS remains fixed with respect to an inertial
reference frame. This is accomplished by sensing these angular disturbances and generating the
1
Section 1. Introduction
necessary counter-rotations [1]. Typically, gyroscopes haven been used to measure these
angular disturbances, although recently, the use of linear accelerometers has been proposed [7].
For this project, Piezoelectric Vibrating Gyroscopes (PVG) has been chosen as the sensors to
use. These kinds of gyros overcome the disadvantage of the traditional jewel-bearing gyros
without the need of the complex mathematics, involved with the linear accelerometers, of
transforming linear accelerations in angular velocities.
For our project, a 2-axis gimbal - that will contain one visible spectrum camera (Sony, weight
approx.0.3 kg), one infrared camera (Miricle, 1.3 kg) and one laser rangefinder (Vectronix, 0.5
kg) - will be used. The electro-optical sensors should be able to keep their aim fixed at the target
whether this is moving or still. Figure 1.1 shows a descriptive picture of the system.
Figure 1.1 Descriptive picture of LOS system
For achieving this, a multiple sensor array (of gyroscopes) will be attached to each of the axes,
with the possibility to aggregate sensors in the platform itself and tachometers in the rotation
points.
In the last few years much research has been done in LOS-stabilization systems field. Many
control techniques has been proposed, neural networks [], fuzzy logic [], robust control [],
adaptive control []. Although most of these works fail in taking into account many real
variables of the system, like gravity gradient torques, noise and frequency response of sensors or
saturation level for servomechanisms, they have shown some improvements over typical control
methods and could be useful for future upgrade of this work. Instead, we will circumscribe to
2
Section 1. Introduction
more traditional controllers putting larger effort in analyzing how every variable of the system
affects the overall performance.
1.2 Objetives
The main goals of this work are: propose algorithms for the controller driving the motor that
will be placed over the axis of rotation of the inner gimbal; and to analyze the effect of several
practical constraints on the system performance.
The controller will have the mission of stabilizing the Line-Of-Sight (LOS) of a cluster of
cameras and pointing devices, which will be mounted in the 2-axis gimbal. The characteristics
of both, motors and gyros, are provided by the companies producing the components.
1.3 Thesis Layout
This work is divided in four sections. The first is this introduction to the principles and uses of
multiple-axis stabilized platforms especially it’
sapplication to stabilization of the line-of-sight
(LOS).
Section two is devoted to briefly explaining the functioning and technical characteristics of the
main devices to be used in the application, i.e. the 2-axis gimbal, the motors and the gyroscopes.
Section three begins with an introduction to the geometric transformations used to go from one
to another of the multiple-axis gimbal coordinate frames (inner frame, outer frame and platform
frame). Then the rigid body equations of the inner gimbal are developed, describing each
component of the torque vector. The same development is done in the next subsection for the
outer gimbal dynamics. In subsection 3.5, after simplifying the equations using symmetry
properties of the inertia matrix, a space-state representation that includes the sensor dynamics is
developed.
Section four is divided in two parts. The first deals with the design of the observer for the inner
gimbal system where the second is focused on the controller and its constraints.
3
Section 1. Introduction
In subsection 4.1, two different approaches for designing the observer algorithm are presented.
First a first-order linear stochastic differential equation was incorporated to the system of
equations to model the nonlinear friction and a discrete Kalman filter was used to estimate the
angular velocity over y-axis and the nonlinear friction. In the second approach the complete
nonlinear equations were involved and the estimations were performed using an Extended
Kalman filter and assuming the parameter Kcf as one of the state variables.
In subsection 4.2, two different control techniques are analyzed and several practical constraints
for its implementation are studied. First, a simple proportional controller is developed. Three
variations of this controller were simulated to study the improvements of canceling the
nonlinear friction component and of using the estimated states, instead of the sensor output. The
next subsection analyzes several practical constraints that appear when implementing the
controller; these were: quantization step size of the A/D converter, s
a
t
ur
a
t
i
onsl
e
v
e
l
sf
o
rmot
or
’
s
tolerable maximum torque, sampling frequency used in the algorithms, measurement noise and
bandwidth of the sensors. The influence and limitations posed by these constraints was
analyzed. The last subsection presents an alternative augmented observer that will be used to
design an LQ-controller for minimizing both variables: angular velocity and angular
displacement around y-axis of the inner gimbal.
Section five presents a summary with the main conclusions along for suggestions for future
research in the field.
4
Section 2. Gyros, motors and gimbal geometry
2 Gyros, motors and gimbal geometry
2.1 General characteristics of gyroscopes
A gyroscope is a device for measuring or maintaining orientation, based on the principle of
conservation of angular momentum. The essence of the device is a spinning wheel on an axle.
The device, once spinning, tends to resist changes to its orientation due to the angular
momentum of the wheel. In physics this phenomenon is also known as gyroscopic inertia or
rigidity in space.
2.1.1
Gyro specifications
For the development of this project Piezoelectric Vibrating Gyroscopes (PVG) has been chosen
as the sensors for retrieving information of the angular velocities around each axis. This kind of
gyroscope is an angular velocity sensor that uses the phenomenon of Coriolis force, which is
generated when a rotational angular velocity is applied to the vibrator.
This device is surface-mountable so it is possible to be mounted to the desired location by an
automatic surface mounter. Its ultra-small size, of about 0.2cc, and lightweight shape increases
flexibility of installment and allows the sensor to be attached with no noticeable increase in the
size of the apparatus where it is planed to be placed.
Features
 Ultra-small and ultra-lightweight
 Quick response
 Low driving voltage, low current consumption
 Lead type : SMD
5
Section 2. Gyros, motors and gimbal geometry
 Reflow soldering (standard peek temp. 245 degree C)
Characteristics
Supply Voltage
(Vdc)
Maximum
Angular Velocity
(deg./sec.)
Output (at Angular
Velocity=0) (Vdc)
Scale Factor
(mV/deg./sec.)
Linearity
(%FS)
Response
(Hz)
Weight (g)
2.7~5.25
+/-300
1.35
0.67
+/-5
50 max.
0.4
Table 2.1 Gyroscope characteristics
Application
 To reduce the effect of temperature drift (due to change of ambient temperature), a high
pass filter must be connected to sensor output to eliminate DC component.
 To suppress output noise component around 22-25 kHz (resonant frequency of sensor
element), a low pass filter which has higher cut-off frequency than required response
frequency must be connected to sensor output.
The following figure shows a sketch of the gyroscope circuit along with a simplified block
diagram that shows how the output of the sensor goes through the high-pass filter to eliminate
the DC drift, then to the low-pass filter to reduce noise and from there to the A/D converter to
have a discrete signal that the processing circuit and handle.
Figure 2.1 Gyro circuit and block diagram
2.1.2
Gyro modeling
To incorporate the dynamic of the gyros in the future models of the system we need to have a
model of the gyroscope compatible with the rest of the models used. In this case, an attempt to
obtain a transfer function with angular velocity as input was done. From the transfer function it
6
Section 2. Gyros, motors and gimbal geometry
is straightforward to get the state-space representation if required. For this particular case a
second order transfer function with cut-off frequency of 50Hz was chosen.
TFGYRO
r2
2
s 2r s r2
(2.1)
where
r : resonance frequency 50 Hz
: damping factor 0.7
2.2 Brush type DC motors
DC Torque Motor Characteristics
One of the most useful rotating components available to the control system design engineer is
the direct-drive DC torque motor. This versatile control element is a permanent-magnet,
armature-excited, continuous rotation motor with the following features especially suited to
servo system drive and actuation applications:
 No gear train
 Direct mounting on the driven shaft
 High torque at low speeds
 High torque-to-inertia ratio
 High torque-to-power
 Linear torque speed characteristics
 Low electrical time constant
 Convenient for factors
 Simple, rugged construction
 Smooth operation
These features make it possible for the designer to obtain such system performance
characteristics as:
 High coupling stiffness
 Fast response
 Precise positioning
 High tracking accuracy
 Excellent stability
 Low input power
7
Section 2. Gyros, motors and gimbal geometry
 Smooth and quiet operation
 Compact assembly
 Improved system reliability
Figure 2.1 shows a 3D representation of the motor to be used in this project
Figure 2.2 3D representation of the motor
Motor data:
 Tp: Peak Torque. This is the maximum useful (non continuous) torque (in ounce-inches)
that can be obtained at maximum recommended current input.
 KM: Motor Constant. This is the ability of a servo motor to convert electric power input to
torque, a kind of figure of merit that can be used to compare motors in their ability to
produce torque per unit of power input. It is the ratio of torque to the square-root of the
power input.
 TF: Total Breakaway Torque. The friction contributed by the motor to the system
determines the total breakaway torque (in ounces-inches).
It is the sum of the brush-commutator friction, plus the magnetic retarding torques such as
hysteresis drag and slot effect drag.
 JM: Moment of Inertia. The moment of inertia of the armature is measured about the torque
mot
or
’
sa
x
i
so
fr
ot
a
t
i
on.
 TR: Ripple Torque. A small change in torque with armature position is caused by the
switching action of the commutator. The armature rotates through a small angle before its
field is returned to its original position through commutation. This variation is known as
ripple torque and is usually expressed in percent of torque level.
8
Section 2. Gyros, motors and gimbal geometry
2.2.1
Motor modeling
There are two main transfer functions concerning the dynamics of the motor. One related to the
speed/power relation and the other one with the torque/power relation. In our case, because what
we need is to cancel the disturbance torques, we are interested in the second relation. According
to the motor manual this a static equation and therefore there are no dynamics involved. This
can be easily understood if we consider that torques produced by electromagnetic forces act
almost instantly compared to mechanical movements. Here is shown the equation that show
this relation
Torque K M Power (Watts )
(2.2)
Where KM is the motor performance index
From this equation it is clear that after calculating the necessary torque the motor has to exert it
is straightforward to find the needed power.
2.3 Gimbal
A gimbal, also called a gimbal ring, is a mechanical device consisting of two or more rings
mounted on axes at right angles to each other. An object mounted on a three ring gimbal will
remain horizontally suspended on a plane between the rings regardless of the stability of the
base. Gimbals have a wide range of practical uses including aerospace applications.
A gimbal may be used to keep objects level in unstable environments. Gimbals are also
extremely valuable in shipboard and aircraft environments, when measuring instruments such as
chronometers and compasses must be kept level with the horizon. Gimbals may also be used for
aerospace navigation, as they can be set to provide a stable measurement from a specific
reference point such as the earth or sun regardless as to their actual position in space. In this
specific application, our gimbal will hold the cameras and instruments that instead the
compasses will not be kept in horizontal position but will point to a fix position in the inertial
9
Section 2. Gyros, motors and gimbal geometry
frame, or will perform a tracking of an object in that frame. Therefore the selection of the
gimbal configuration is of special importance.
Gimbals employed in aerospace navigation utilize Euler angles to orient an object such as a
spacecraft. This work will also use Euler angles to develop the dynamics of the system. Euler
angles are more intuitive, although not as robust as the quaternions, but can lead to gimbal locks
and are not as efficient. A possible future work could be to develop a stabilization control based
on quaterinions. However, assuming the real angles and angular velocities will not deviate much
about the desired ones the behavior of both systems is rather similar.
Figure 2.3 Gimbal
10
Section 3. Dynamics and Kinematics
3 Dynamics and Kinematics
3.1 Basic coordinate frame transformations
For the 2-axis gimbal problem three different coordinate frames are used. The base or platform
frame, the outer gimbal frame and the inner gimbal frame. All frames are related by
transformation matrices, in this case the sum of a translational and a rotational matrix. Figure
3.1 shows the gimbal structure with the different coordinate frames axis depicted in different
colors along with their angular relations.
Figure 3.1 Different coordinate frames for a 2-axis gimbal
11
Section 3. Dynamics and Kinematics
The relation between the platform frame and the outer gimbal frame is expressed by the sum of
t
wo ma
t
r
i
c
e
s
.One
,Rθ
PO, represents a rotational transformation and the other one, T PO,
represents a translational transformation. For the sake of simplicity figure 3.1 does not depict
the translation between these two frames although it does depict the translation between inner
and outer gimbal frames.
cos PO
RPO 
sin PO


 0
TPO
sin PO
cos PO
0
0
0

1

1 0
0 



0 1
0 



0
0
Tpoz


(3.1)
(3.2)
Coordinates in Outer Frame 
RPO Tpo 

Coordinates in Platform Frame
The same situation happens for the relations between outer and inner gimbal frame where now
Rθ
OI represents the rotational transformation and T OI represents the translational transformation.
cos OI


ROI  0

sin OI

0 sin OI 
1
0 

0 cos OI 

1 0
0 


TOI 
0 1
0 



0
0
Toiz


(3.3)
(3.4)
Coordinates in Outer Frame 
ROI TOI 

Coordinates in Platform Frame
Because our interest is focused on angular velocities, angular accelerations and angles, we can
drop the Translational Matrix that has no influence in how the rate vector of one coordinate
frame relates with the other. The outer frame rotates around the z-axis and the inner frame
around the y-axis so the angles θ
ndθ
PO a
OI will not stay fixed but will vary in time. The
de
r
i
v
a
t
i
v
eofθ
PO depends on the different velocities between the platform coordinate frame
a
n
g
ul
a
rv
e
l
oc
i
t
y(
ωP)a
ndt
heou
t
e
rc
oo
r
di
na
t
ef
r
a
mea
ng
ul
a
rv
e
l
oc
i
t
y(
ωO) around the z-axis. It
is clear f
r
omhe
r
et
ha
tωO can not be solved explicitly without taking into account the forces that
12
Section 3. Dynamics and Kinematics
produce the relative angular velocity between both frames of reference. The same goes for the
i
nne
rc
oo
r
di
na
t
ef
r
a
mea
ng
ul
a
rv
e
l
oc
i
t
y(
ωI), but in this case between the outer coordinate frame
a
n
g
ul
a
rv
e
l
o
c
i
t
y(
ωO)a
ndt
hei
nn
e
rc
oo
r
di
n
a
t
ef
r
a
mea
n
g
ul
a
rv
e
l
oc
i
t
y(
ωO) around the y-axis. .
Asme
nt
i
ona
bov
et
h
ed
e
r
i
v
a
t
i
v
eo
fθ
PO represented by equations is

PO Oz (t ) Pz (t )
(3.5)
Creating the auxiliary matrix
0 
 

PO 0 
 

PO 


(3.6)
We can get the relation between the platform frame and the outer gimbal frame as

O (t ) RPO (t )P (t ) PO
(3.7)
Following an identical procedure

OI Iy (t ) Oy (t )
(3.8)
0 
 
OI 
OI 
 
0 
 

(3.9)
And the relation between the outer frame and the inner gimbal frame is

I (t ) ROI (t )O (t ) OI
(3.10)
Finally we need to calculate the angular acceleration rate of the inner frame. These angular
acceleration rates will be used in the next section to develop the gimbal dynamics model. This
model will be the basis for the rest of the work and will be used to perform the simulations and
to analyze different controllers.
Therefore, differentiating equation 3.10 we get






I (t ) ROI (t ) O (t ) RAUX O (t ) OI OI
13
(3.11)
Section 3. Dynamics and Kinematics
Where
RAUX
sin OI


 0

cos OI
0 cos OI 
0
0 

0 sin OI 

(3.12)
3.2 Eul
e
r
’
smoment equations
The gimbal dynamics model can be derived from the torque relationships about the inner and
outer gimbal body axes based on rigid body dynamics [1].
TheEul
e
r
’
smome
nte
qua
t
i
onsa
r
e


M h I h B h
(3.13)
Where M represents the applied moment and h is the angular momentum. The subscript I
express a derivate in the inertial frame and the subscript B a derivate in the object frame.
If the principal axes of inertia coincide with the coordinate frame, which is our case, performing
the vector product give us three scalar equations

M X I X X 
I Z IY 
Y Z 

M Y IY Y X Z 
I X I Z 
(3.14)

M Z I Z Z X Y 
IY I X 
The following equations that will be used to develop the gimbal dynamics model follow closely
the treatment given in [1] with major differences in the model of the friction.
3.3 Inner gimbal dynamics
We will begin analyzing the dynamics of the inner gimbal. The sum of the kinematics torques
about the inner gimbal is
14
Section 3. Dynamics and Kinematics
M Ix 
t TOIx 
t TGGIx 
t
M Iy 
t Tel 
t TGGIy 
t T fI 
t
M Iz 
t TOIz 
t TGGIz 
t
(3.15)
With
Tel : Elevation stabilization control torque.
T fI : Friction torque about y-axis
TGGIx , TGGIx , TGGIx : Gravity gradient torques about each gimbal axis.
TOIx , TOIz : Torques exerted by outer gimbal on inner gimbal
The elevation stabilization torque Tel - produced by the motor attached to the y-axis of the inner
gimbal –consists of two parts. One has the main function of canceling the disturbances and
therefore to nullify the angular velocity ωIy, this part will be called
Tel, and the other has to
control the inner gimbal in order to move up to the desired angle, this part will be called
Tel.
The gimbal-motor system has been built in a way to avoid or minimize all nonlinear of behavior
of friction. It has proved to be a realistic assumption to consider the friction as the sum of a
linear component, dependant on the relative angular velocity between the inner and outer gimbal
y-axis and a nonlinear component called coulomb friction. The linear component of the friction
is proportional to the viscous friction coefficient KIf. The coulomb friction has a constant value,

Kcf, and its direction depends on the sign of OI . This constant can only be determined
empirically.
The outer gimbal will exert torques around the x and z-axis of the inner gimbal. This torque
will be the necessary to produce the same angular displacement experienced by the outer gimbal
due to the fixed relationship between these axes.
One important external moment is the gravitational moment. An asymmetric body subject to a
gravitational field will experience a torque tending to align the axis of least inertia with the field
direction [4]. For the following development, we assume that the gimbal is at a distance R0 from
15
Section 3. Dynamics and Kinematics
the Ea
r
t
h
’
sc
e
nt
e
ro
fma
s
s
. The reference frame will be defined as follows: the origin of the
reference frame moves with the center of mass (cm) of the inner gimbal. The zR-axis points
towards the cm of the earth (
t
he s
ub
s
c
r
i
p
t“
R” s
t
a
nds f
orr
e
f
e
r
e
n
c
e
)
. The xR-axis is
perpendicular to the zR-axis in the direction of the unmanned plane’
sv
e
l
o
c
i
t
y
. The yR-axis is
perpendicular to both, the zR and xR-axis. The aircraft’
sa
x
i
sf
r
a
mei
sde
f
i
n
e
dby xB, yB and zB
where each axis coincides with the inertia axes (
t
h
es
u
bs
c
r
i
pts
t
a
nds“
B”d
e
not
e
sbody).
The Euler angles are defined as the rotational angles about the body axes as follows: φ,a
bo
ut
the xB-axis; θ
, about the yB-axis; and ψ, about the zB-axis, assuming an initial alignment between
the reference and the body frame.
The gravity gradient vector is defined as
T
G 
Gx Gy Gz 


(3.16)
The force exerted on a mass element due to gravity is
dm 
dF -  3 
r
r 


(3.17)
Where r = R+ρis the di
s
t
a
nc
ef
r
omt
hee
a
r
t
h’
sc
e
n
t
e
rofma
s
st
ot
hema
s
sdm. Since ρ
<<R0, the
moment about the center of mass of the body becomes
dG dF = -
dm
r
3
r
(3.18)
where ρis the radius vector from the body’
scenter of mass to a generic mass element dm.
Because ρ
<<R0, 1/r3 can be approximated as
1
1  3R

 3
1 2 
3
r
R0  R0 
(3.19)
Integration of equation 3.18 over the entire body of mass, together with Eq. 3.19, leads to
3
G 5 
R R dm
2 R0 M
After calculating the scalar and vector products we get the final results
16
(3.20)
Section 3. Dynamics and Kinematics
3
Gx  3 
I z I y 
sin 
2
cos 2 

2 R0
3
Gy  3 
I z I x 
sin 
2
cos 

2 R0
(3.21)
3
Gz  3 
I x I y 
sin 
2
sin 

2 R0
These are the gravity gradient moment components of G.
The gravity moment vector G should be expressed in terms of the angles of the inner gimbal
system of reference. This can be achieved measuring the body axes angular rates relative to the
reference frame together with knowledge of the initial conditions of the Euler angles relative to
the reference frame. As we will show later, the particularities of our case will make this
unnecessary.
If we combine equations 3.14 and 3.15 and replace the general inertia matrices and angular
velocities by the inertia matrices and the angular velocities of the inner frame we get

TOIx 
t I Ix Ix IyIz 
I Iz I Iy 
TGGIx 
t

I Iy Iy IxIz 
I Ix I Iz Tel 
t TGGIy 
t T fI 
t
(3.22)

TOIz 
t I Iz Iz IxIy 
I Iy I Ix TGGIz 
t
The first and third equation of 3.22 have no practical use since the gimbal rotation around the x
and z-axis are will depend completely of the rotation of the outer gimbal. Therefore the torques
will be the necessary ones to accomplish this. This leaves us with the second equation, which
contains the derivative of one of the variables to be controlled (ωIy). Expanding the terms of the
friction torques and the control torques we get

I Iy Iy IzIx 
I Ix I Iz Telw 
t Tel
t TGGIy 
t



K vf OI 
t K cf sgn 
OI 
t 


(3.23)
The goal is to control the elevation axis - inner gimbal y-axis - and the cross-elevation axis inner gimbal z-axis. Therefore, the variables to bec
on
t
r
ol
l
e
da
r
eωIy a
ndωIz. Because we do not
17
Section 3. Dynamics and Kinematics
have direct control over the cross-elevation axis we have to do it indirectly through the azimuth
axis - outer gimbal z-axis.
Using the relations stated in equation 3.11 and 3.3 we get
Ix cos OI Ox sin OI Oz
Iz sin OI Ox cos OI Oz
Ox cos POBx sin POBy
(3.24)
Oy sin POBx cos POBy
The first three equations give us
cos 2 OI Ox sin OI Iz sin 2 OI Ox Ox sin OI Iz
Ix 

cos OI
cos OI
cos POBx sin POBy sin OI Iz

cos OI
(3.25)
We also know that

OI Iy (t ) Oy (t )
(3.26)
Using the relations stated in equations 3.25 and 3.26 it is possible to represent equation 3.23 in
terms of the controlled variables and the base disturbances.

I Iy Iy K vf Iy K vf (cos POBy sin POBx )
Iz 
cos POBx sin POBy sin OI Iz 
I Ix I Iz / cos OI
K cf sgn 
Iy cos POBy sin POBx 


Telw 
t Tel
t TGGIy 
t
(3.27)
Equation 3.27 is useful to simulate and model the real system but for control purposes it is more
practical to use the information of the angular velocities directly from outer gimbal axis instead
of getting the data from the base. A second way to represent these dynamics is in terms of the
controlled variables and the outer gimbal angular velocities as they will be seen by the
controller. Later on it will be shown that the outer gimbal angular velocities will not be known
exactly due to sensor dynamics and noise measurements. Representing equation 3.27 in terms of
the outer gimbal angular velocities give us
18
Section 3. Dynamics and Kinematics

I Iy Iy K vf Iy Iz 
Ox sin OI Iz 
I Ix I Iz / cos OI K vf (Oy )
K cf sgn 
Iy Oy 
t Tel
t TGGIy 
t

Telw 
(3.28)
It can be seen that using the outer gimbal angular velocities not only simplifies the model but
reduce the number of sensors used.
3.4 Outer gimbal dynamics
Weus
et
h
eEul
e
r
’
smome
nte
qua
t
i
o
nst
og
e
tt
her
i
g
i
dbodyt
or
quedy
na
mi
c
sf
ort
heou
t
e
r
gimbal. The total torque vector about the outer gimbal axis is
M O M OT 
MI 
O
(3.29)
Expanding this equation we get the sum of the kinematics torques about each axis of the outer
gimbal
M Ox 
t TBOx 
t TGGOx 
t 
M Ix 
t 


O
M Oy 
t TBOy 
t TGGOy 
t 
M Iy 
t 


O
(3.30)
M Oz 
t Taz 
t TGGOz 
t T fO 
t 
M Iz 
t 


O
M Ix 
t 
,
M 
t 
,
M 
t 
are t
het
or
que
’
sma
t
r
i
c
e
sof the inner gimbal referred to





O  Ix
O  Ix
O
the outer gimbal frame. As in the previous case we have
Taz : Azimuth stabilization control torque.
T fO : Friction torque about z-axis
TGGOx , TGGOy , TGGOz : Gravity gradient torques about each outer gimbal axes.
TBOx , TBOy : Torques exerted by the base on outer gimbal.
In the same manner as before, the azimuth stabilization torque Taz - produced by the motor
attached to the z-axis of the outer gimbal –consists of two parts: disturbance cancellation; and
pointing. Because stabilization is required about the inner z-axis, called cross elevation axis, Taz
is used to indirectly control t
hea
ng
ul
a
rv
e
l
o
c
i
t
yωIz. This fact poses a difficult problem due to
the high nonlinearities that appear in the dynamics of the system.
19
Section 3. Dynamics and Kinematics

As before, the friction can be divided in a viscous friction, proportional to BO and KOf, and the
Coulomb friction, that has a constant magnitude equal to Kfc and whose direction depends on

the sign of BO .
The Outer and Inner torque vectors are defined as
M O [ M Ox , M Oy , M Oz ]
(3.31)
M I [ M Ix , M Iy , M Iz ]
Ap
pl
y
i
ngt
heEu
l
e
r
’
smome
nte
qu
a
t
i
ont
oboth vectors

  

M O 
h h 
I O O O I OO 

 

(3.32)




ROI T M I ROI T 
h I I h ROI T 
I I I I I II (3.33)
M I 
O






Combining with equation 3.29 we can derive the rigid body torque dynamics for the outer
gimbal body as
 

M OT 
I O O O I OO ROI T


 

I



I

I
I
I
I
I




(3.34)
From Equation 3.34 and the symmetry property that states that ROI 1 ROI T we can expand
the first term of the right hand






I O O = I O ROI T I I O ROI T RAUX O OI I O ROI T OI
(3.35)
With this result, equation 3.34 can be expressed as


I O ROI T ROI T I I 
I + 
O I OO + ROI T 
I I II 






I O ROI RAUX O OI I O ROI OI M OT
T
T
(3.36)
As we are interested in the cross elevation axis dynamics, we shall consider only the third
element of the vector shown in equation 3.36. Solving for the first term of the left hand we have
20
Section 3. Dynamics and Kinematics


I Ox cos OI






T
T

I O ROI ROI I I 
I 



 0


3


I Oz sin OI





I Ix cos OI


 0




I Ix sin OI



0
IOy
0





Ix



I Ox sin OI 
 

 
0

 Iy 

 


IOz cos OI 
Iz 



 

3





Ix

I Iz sin OI  

 

0

Iy

 




I Iz cos OI 

Iz 


 

3
0
I Iy
0
(3.37)




I
sin



I
cos


Ix
Iz 
Oz
OI
Oz
OI






I Ix sin OI Ix I Iz cos OI Iz 




cos OI 
I Iz I Oz 
Iz sin OI 
I Ix IOz 
Ix
In this case []3 denotes the third element of the vector. As we can see below, the third element of
the last term of the right hand is equal to zero.

I Ox 0
0 
cos OI



 



T
I O ROI OI   0 I Oy 0  0




3

0 I Oz 
sin OI

0



 0 
  

I Oy OI  0


 0 


3



0 
0 sin OI  





1
0 

OI 
 

0 cos OI 

0 
 

3
(3.38)
After replacing the results obtained above for the cross elevation axis dynamics equation we get


cos OI 
I Oz I Iz 
Iz = sin OI 
I Oz I Ix 
Ix

O I OO + ROI T I I II 


3
(3.39)



+
I O ROI T RAUX O OI  
M OT 
3


3
From equation 3.11, the angular acceleration about the inner x-axis can be obtained.
Substituting into 3.39, expanding the cross products terms and substituting the kinematics
torques leads to
21
Section 3. Dynamics and Kinematics



IT Iz = sin OI 
I Oz I Ix 
Ox 
I OzIx sin OI I IxOz 
OI 
cos OI 
OxOy 
I Oz I Ox sin OI IyIz I Iz + IyOx I Iy cos OI IxIy I Ix 

(3.40)



 
cos OI 
Taz TGGz K Ovf OI KOcf sgn 
OI 

 


This equation could be expanded further to represent the whole dynamics only in terms of the
controlled variables and the base disturbances.
3.5 Augmented inner gimbal dynamics –sensors dynamics
All variables are measured by sensors which have their own dynamics plus noise added at the
output. Therefore, the actual variables can not be known exactly. To deal with this, an
augmented state-space representation of the system is done.
From Eq equation 2.1 we know that the dynamics of the sensors are described as
y
2500
TFSensor  sensor  2
m
s 70 s 2500
(3.41)
Were the input m is the angular velocity to be measured. If we use a state-space representation
 
 
x
0
1


1 
x1  0 



  
2500 70   
2500 


x2 
x2 






ysensor
 
x1 

1 0
 
x2 



(3.42)
Now we are ready to combine equations 3.42 and 3.28 to get a state-space representation of the
inner gimbal dynamics including the sensor dynamics. This representation of the complete
system will be used afterwards to develop the observers and controllers for the system.
22
Section 3. Dynamics and Kinematics
 
x1  
K vf / I Iy
0
0 x1  
K cf / I Iy 
  




sgn 
x2 
0
0
1
x 
0
 Oy 



2  
  Iy
   2500



 0

70 2500 
x3 

x3  


1/ I Iy 



 0 
Telw Tel TGGIy K vf * Oy 




0
 
(3.43)


I Ix I Iz 
/ I Iy 



0
Iz 
Ox sin OI Iz 
/ cos OI



0


ysensor
x1 


0 1 0
x2 




x3 

The gimbal used for this project has a special property: its axes of inertia are symmetrical. We
can see that due to this symmetry the last term in the state equation will be canceled. Also due to
this symmetry, the gravity gradient torque will disappear (see equation 3.21). Furthermore, this
representation is useful for adding measurement noise at the output of the gyroscopes.
Simplifying the state equation and adding the measurement noise we obtain
 
x1  
K vf / I Iy
0
0 x1  
1/ I Iy 
  





x2  0
0
1 
x2  0 
Telw K vf * Oy 



   2500






70

2500
x
0
3   
x3  


K cf / I Iy 


sgn 
 0
Iy Oy 

 


 0

ysensor
(3.44)
x1 


0 1 0
x2 

e


x3 

Were e is the measurement noise of the sensor. This noise is assumed to be white noise with
covariance Ecov.
e(t ) N (0, Ecov )
23
(3.45)
Section 3. Dynamics and Kinematics
It can be seen from the state-space equation that canceling the nonlinear term would leave as
with a linear system and therefore able to apply a controller for LTI systems, e.g. PID controller.
Two important obstacles are the fact that the parameter Kcf is unknown and need to be estimated
in real time, and as stated before that we do not have the precise values of the variables ωIy and
ωOy. It is clear the necessity to develop an observer to be able to apply a negative feedback to
counteract the torque produced by this term. This observer will be developed in the following
section.
24
Section 4. Observers and Controllers
4 Observers and controllers
4.1 Observer for the inner gimbal
Besides the sensor dynamics mentioned before we consider here the common case of noisy
sensor measurements. There are many sources of noise in such measurements. For example,
each type of sensor has fundamental limitations related to the associated physical medium. In
addition, some amount of random electrical noise is added to the signal via the sensor and the
electrical circuits. Therefore analytical measurement models typically incorporate some notion
of random measurement noise or uncertainty. When the variable being measured is planned to
be used as an input for a controller an accurate estimation of it is of utmost importance. If we
consider only the inner gimbal dynamics, there are two variables that are necessary to be
estimated: the inner gimbal angular velocity of the y-axis - ωIy -, and the Coulomb friction

torque (CFT), Kcf · sign( OI ). The first variable will be used to design the controller and the
second is needed for canceling the torque is exerting over the gimbal and also for estimating the
angular velocity. The main challenge is to estimate the nonlinear Coulomb friction. Two
different, although similar, approaches will be used for this purpose.
4.1.1
First order approximation
A simple method to estimate the Coulomb friction is described in [8]. A first-order linear
stochastic differential equation is incorporated to the system of equations to estimate this
friction. This linear equation in no way represents an accurate model for nonlinear Coulomb
friction; however, it is of a form that is compatible with the Kalman filter equations. As it will
be demonstrated, this stochastic differential equation characterizes a slowly changing random
25
Section 4. Observers and Controllers
process that under the right conditions accurately identifies the effects of the actual nonlinear
friction on the system. The algorithm was tested using numerical simulation techniques
As shown in Fig. 4.1, the disturbance torque, associated with the representation of the Coulomb

friction, is either plus or minus depending on the sign of OI .
Figure 4.1 Torque couse by Coulomb friction
If we assume that the switching of the friction from plus to minus is at a relatively low
frequency compared to the sampling frequency, then it is possible to approximate the model for
the friction as being exponentially correlated with time constant that is significantly greater than
the inverse of the sample rate [9], [10], i.e.,
dK cf
1
 K cf w
dt

(4.1)
where w is white noise with zero mean and covariance Wcov.
w(t ) N (0, Wcov )
(4.2)
This simple first-order equation allows the use of the Kalman filter equations for estimating the
Coulomb friction.
A sensitivity study revealed that there was little change in performance once τbecame much
greater than the inverse of the sampling rate.
When the sensor dynamics and noise measurement are not considered, this approximation
works extremely well. Although the equations for this first simple case are not shown, the
results after simulating this model can be seen in figure 4.2.
26
Section 4. Observers and Controllers
Figure 4.2 Real vs. estimated friction torque
Next figure amplifies the plot in the transition point to visualize more clearly the convergence
time and the steady-state error.
Figure 4.3 Zoom over transition area
Sampling period =0.001[sec]
Di
s
t
u
r
banc
eωOy = 0.8 sin(2.28t)[rad/sec]
Coulomb friction coefficient Kcf = 0.1
Viscous friction coefficient Kif = 0.56
Inertia moment IIy = 0.325
27
Section 4. Observers and Controllers
The figures show that the convergence time is almost negligible and the steady-state error,
although not zero, is small enough to have any significant influence. The estimation
convergence rate and steady-state error are greatly deteriorated when the same approximation is
used for the complete system, which means including the sensor dynamics and noise
measurement.
The continuous-space representation of the complete system using the first-order approximation
for the Coulomb friction can be expressed as
 
x1  
K vf / I Iy
  
x2 

 0

   0
x3   2

  
 r


x4 

1/ I Iy
0
1/ 
0
0
0
0
2
r
0 x1  
1/ I Iy 
0







x2
0 
1
 0 
Telw w

x3   0 
1 
0

 


2
r 
x4 
0



 0 
(4.3)
x1 

x2 


0 0 1 0  e

x3 

x4 



ysensor
In order to discretize the system it was assumed Telw, w and e being piece-wise constant during a
sample period, i.e. zero-order-hold for the input and disturbances. The new discrete system can
be represented as
X K 1 Ad X K Bd Telwk Gd wk
ysensor k Cd X K Dd ek
(4.4)
where


Ad e AT 1 
sI A 
1
t T
T A 
Bd 
e dB A1 
Ad I 
B if A nonsingular
0


Cd C
(4.5)
Dd D
The covariance of the measurement and process noise is also influenced due to the discretization
process, therefore
28
Section 4. Observers and Controllers
wk N (0, Wd cov )
ek N (0, Ed cov )
(4.6)
where
T
Wd cov  
e AQe Ad
0
(4.7)
Ed cov E
and T is the sample time.
Within the significant toolbox of mathematical tools that can be used for stochastic estimation
from noisy sensor measurements, one of the most well-known and often-used tools is what is
known as the Kalman filter. The Kalman filter is essentially a set of mathematical equations that
implement a predictor-corrector type estimator that is optimal in the sense that it minimizes the
estimated error covariance, when some presumed conditions are met. Although not all the
required conditions are met, because the system is only an approximation to the real nonlinear
system, the Kalman filter has proven to be flexibly enough to be implemented.
The Kalman filter estimates a process by using a form of feedback control: the filter estimates
the process state at some time and then obtains feedback in the form of (noisy) measurements.
As such, the equations for the Kalman filter fall into two groups: time update equations and
measurement update equations. The time update equations are responsible for projecting
forward (in time) the current state and error covariance estimates to obtain the a priori estimates
for the next time step. The measurement update equations are responsible for the feedback—i.e.
for incorporating a new measurement into the a priori estimate to obtain an improved a
posteriori estimate.
A whole demonstration of the Kalman filter equations can be found in [11]. A summary of the
main equations is shown below
The estimated error covariance to be minimized is defined as

 T






Pk 
xk xk 
xk xk 







29
(4.8)
Section 4. Observers and Controllers
The following procedure uses the system represented in equation 4.4.
The first step is the time update calculations. The equations used are


x k k 1 Ad x k 1 k 1 Bd uk
(4.9)
Pk k 1 Ad Pk 1 k 1 AdT GdWcovGdT
For the measurement update we need first to compute the Kalman gain and then to correct the
estimated states and to compute the new covariance matrix.

K k Pk k 1CdT Cd Pk k 1CdT Ecov

1

ek k 1 yk y k k 1


(4.10)
x k k x k k 1 K k ek k 1

Pk k Pk k 1 Pk k 1CdT Cd Pk k 1CdT Ecov
C P
1
d
k k
1
It is quite common for most applications to use the steady-state Kalman gain instead of the timevarying gain. All the following simulations were run using the steady-state Kalman gain.
For the next simulation, a Kalman filter was also used to estimate the real value of the
distur
b
a
nc
eωOy. In this case it was assumed a sinusoidal disturbance of a specific frequency
(6.2832 rad/sec or equivalently 1Hz) and unknown amplitude and phase. Although this
assumption about the disturbance characteristics could be seen as somewhat arbitrary it is quite
common the case when information about the disturbance is known in advance. It is also
possible to use a nonlinear estimator (e.g Extended Kalman filter) for a disturbance with
uncertain frequency. For disturbances with frequencies much smaller than the s
e
n
s
or
’
s
bandwidth simulations showed that it was unnecessary to use observers. Because the sensor
transfer function is essentially a low-pass filter, low frequencies disturbances are almost not
affected by it, pl
u
s
,t
heKa
l
ma
nf
i
l
t
e
rf
orωOy proved to be almost insensitive to its measurement
noise.
A complete diagram of the system and the estimator can be seen in Figure 4.4
30
Section 4. Observers and Controllers
Figure 4.4 System–
observer diagram
The next figure shows a comparison between the actual Coulomb friction torque (CFT) and the
estimated with the Kalman filter. It can be seen that although the measurement noise for the
sensor was extremely low (
t
hes
e
ns
o
rf
o
rωIy); the estimation error is considerably high. The
convergence rate, although decreased, is still quite high. For all the simulations in this
subsection the measurement noise variance for t
hes
e
ns
orofωOy was equal to 1-3.
Figure 4.5 Real vs. estimated CFT
Where measurement noise: Ecov = 1-10
Figure 4.6 displays an amplification of figure 4.5 were the convergence rate and steady-state
error can be seen with more detail.
31
Section 4. Observers and Controllers
Figure 4.6 Amplification of real vs. estimated CFT
Simulations showed that the main reason for the decrease in the estimation performance is the
measurement noise. Applying the Kalman filter to the complete system without measurement
noise proved to be as efficient as for the simplified system.
Furthermore, a minor increase in measurement noise greatly magnifies the estimation error.
Figure 4.5 shows the increase in the estimation error when the covariance of ek, Ecov, was equal
to 9.9-9
Figure 4.7 Real vs. estimated CFT for high MN
32
Section 4. Observers and Controllers
In the next figure we can visualize that although the convergence time is practically the same as
in the previous simulation the error in the estimation is too high to be use for canceling the
Coulomb friction torque.
Figure 4.8 Amplification of real vs. estimated CFT for high MN
The measurement noise goes straight through the feedback gain K used in the Kalman filter.
This gain has a high value, needed to be able to estimate a nonlinear behavior using a linear
approximation. Therefore any noise will be highly magnified degrading the filter performance.
One way to avoid this is to diminish the Kalman gain but what would cause a reduction of the

convergence time. Because every time there is a switch in the sign the of OI , and consequently
in the torque produced by the Coulomb friction, the estimation process start all over again this
option has no practical use except for very low frequency disturbances. Even under these
conditions decreasing the Kalman gain proved to be unadvisable because slowing down the
convergence rate would cause a resonant response with a considerable overshoot.
One option to improve the performance of the algorithm is to place a digital low pass filter for

the measured variable before calculating the error signal yk y k k 1 .
Simulations showed that placing a low-pass filter after the sensor succeeded in reducing the
estimation error but the trade-off is the reduction in bandwidth. For low cut-frequencies a
noticeably decrease in the convergence time was observed and also a phase-shift in the
33
Section 4. Observers and Controllers
e
s
t
i
ma
t
i
o
no
fωIy. If the cut-off frequencies were too high, almost no reduction of the estimation
error of the CFT was achieved.
However, one of the main advantages of the algorithm is the fact that even if high estimation
errors are obtained for the Coulomb friction torque, the estimation of the inner angular velocity
is rather accurate. This can be seen in fig
ur
e4.
6wh
e
r
et
hea
c
t
u
a
la
nde
s
t
i
ma
t
e
dωIy are plotted.
Figure 4.9 Re
alv
se
s
t
i
mat
e
dωIy
The simulation was run with the same noise conditions, and system parameters, of the first
simulation showed in this section. After zooming for a better visualization of the differences
between both signals we have
Figure 4.10 Zoom over real vs. e
s
t
i
mat
e
dωIy
34
Section 4. Observers and Controllers
Although there is some noise corrupting the estimated ωIy, this is particularly smaller than the
noise over the estimated CFT. As a result the filter provides us with an estimated angular
velocity accurate enough to be used for the control algorithm.
This accuracy tends to degrade when the Coulomb coefficient has higher values and therefore a
major influence in the overall system dynamics.
4.1.2
Extended Kalman filter
To overcome the main drawback of the previous algorithm an alternative method is proposed.
Instead of using a linear approximation compatible with the traditional Kalman filter the
extended Kalman filter for the nonlinear system will be developed.
A Kalman filter that linearizes about the current mean and covariance is referred to as an
extended Kalman filter or EKF. In something akin to a Taylor series, we can linearize the
estimation around the current estimate using the partial derivatives of the process and
measurement functions to compute estimates even in the face of non-linear relationships. The
nonlinear system will be represented as
X k f X k 1 , uk 1 , wk 1 
yk c X k , ek 
(4.11)
As same as with the discrete Kalman filter, the EKF algorithm is divided into two steps: time
update and measurement update. The time update equation, for estimation of Xk based in the
estimation of Xk-1, comes straight from equation 4.11. Consequently


X k k 1 f X k 1 k 1 , uk 1 , 0

(4.12)
Before presenting the whole set of equations we need to define the Jacobian matrices of the
partial derivatives of f and c with respect to X, w, and e. These matrices will be used later in a
similar way to how matrices Ad and Cd were used in the discrete Kalman filter.

fi 

Ak i , j 
X k 1 k 1 , uk 1 , 0 

X 

j 
35
(4.13)
Section 4. Observers and Controllers

fi 

Wk i , j 
X k 1 k 1 , uk 1 , 0 

w

j 
(4.14)

ci 

Ck i , j 
X k k 1 , 0 

X 

j 
(4.15)

ci 

Ek i , j 
X k k 1 , 0 

X 

j 
(4.16)
We are now ready to define properly the time update equations


X k k 1 f X k 1 k 1 , uk 1 , 0

(4.18)
Pk k 1 Ak Pk 1 k 1 A WkWcovWkT
T
k
and the measurement update equations

K k Pk k 1CdT Ck Pk k 1CkT Ek Ecov EkT

1





x k k x k k 1 K k yk c( x k k 1 , uk , 0) 



Pk k Pk k 1 Pk k 1CdT Cd Pk k 1CdT Ek Ecov EkT
(4.19)
C P
1
d
k k
1
The system in 3.44 was discretized considering the unknown parameter Kc as a variable of the
system and using the approximation

X X k 1
X k
T
(4.20)
where T is the sampling period
For T small enough, this discretization method is almost equivalent to the zero-order-hold
method. The Jacobian matrices of this discretized system are
36
Section 4. Observers and Controllers
1 TK vf / I Iy Tsign( x1k 1 k 1 ) / I Iy


0
1
Ak 

0
0

2
0

 T r
g11 0
0
0 

0 g
0
0 
22


Wk 
0
0 g33 0 


0
0 g 44 
0



0
0 

1
T 

T 2
r 1 T r2 

0
0
(4.21)
Ck 
0 0 1 0
Ek 1
The parameters g11, g22, g33, g44 will be used in the design process to tune how the noise w affect
each of the states of the system. Using the matrices from equation 4.21 and the equations 4.184.19, an S-function was programmed in MATLAB to perform the Extended Kalman Filter
algorithm.
The next simulation was the result of applying the EKF with identical conditions as in the first
simulation performed in the previous subsection. Because we are not using any steady-gain, the
filter is a time-varying system, thus the computational load is much higher than when using the
steady Kalman gain. The results are plotted in figure 4.11
Figure 4.11 Real vs. Estimated CFT using EKF
Amplifying the image for one of the periods shows clearly the enormous improvement in the
estimation of the CFT over the previous algorithm, with a relative error less than 0.05%.
37
Section 4. Observers and Controllers
Figure 4.12 Zoom over real vs. estimated CFT using EKF
It is very important to point out that the right choice of the different design-parameters of the
EKF, i.e. matrix W, initial covariance matrix P0 and noise covariance matrices Edcov and Wdcov,
is fundamental for the correct functioning of the filter and usually can only be found empirically
after several tuning of each of the parameters mentioned.
Even after increasing the measurement noise up to the same level as in the second simulation of
the previous subsection the relative estimation error was kept below 0.1%, with some noticeable
increase in the estimation noise. A magnification for this case is shown in figure 4.13
Figure 4.13 Zoom over real vs. estimated CFT using EKF for high MN
38
Section 4. Observers and Controllers
Finally a third simulation was performed with a MN variance six orders higher than before (Ecov
= 0.01). Even under these extremely noisy measurements, the estimator was able to keep the
error in acceptable margins (less than 19%).
Figure 4.14 Zoom over real vs. estimated CFT using EKF for extremely high MN
For a better idea of what kind of noisy measurement we are referring, a plot with the real
angular velocity and the measured by the gyroscope are shown in figure 4.15. Not only the
angular velocity measured is extremely noisy but there is also a substantial phase-shift between
both signals.
Figure 4.15 Sensor output vs. ωIy
The reason for this exceedingly performance of the extended Kalman filter resides in three main
factors that interact with each other:
39
Section 4. Observers and Controllers
 There is no approximation done, so the complete system dynamics are used to estimate the
variables.
 Because the nonlinear behavior of the CFT is taken into account, the convergence process
does not restart every time there is a switch in the direction of the CFT (or equivalently in

the sign of OI ).
 In the previous algorithm the high Kalman gain caused an extreme amplification of the MN,
in this case the Kalman feedback gain is considerably smaller than before thus this
magnification does not occur.
The drawbacks of using the EKF are the slower convergence rate at the beginning of process
and the high computational cost of implementing a time-varying observer.
The estimation of the angular v
e
l
o
c
i
t
yωIy also exceeds the performance achieved with the 1order approximation. For high values of the parameter Kcf this difference is even more
considerable. Ag
r
a
p
h
i
cp
l
o
t
t
i
ngt
hee
s
t
i
ma
t
e
da
n
da
c
t
u
a
lωIy is presented in figure 4.16.
Figure 4.16 Re
alv
s
.e
s
t
i
ma
t
e
dωIy
Zooming figure 4.16 shows the smoother behavior of the estimated signal compared with the 1order approximation and the extremely small misalignment with the r
e
a
lωIy.
40
Section 4. Observers and Controllers
Figure 4.17 Zoom over real vs. estimated CFT ωIy
4.2 Controller for the inner gimbal
The controller is in charge of driving the necessary signal to the motor to control and cancel the
angular velocity around the inner gimbal y-axis and to allow the target pointing and tracking.
The motor dynamics should also be included in the controller equations, but because the relation
power-torque of the motor is a static equation a complete analysis can be done without
including this relation until the implementations phase.
The control algorithm for disturbance rejection and tracking is split in two parts.
Ck K p 
Iy Iydesired CFT
where Iydesired is the desired angular velocity necessary for target tracking.
The second term of the right-hand side is for canceling the nonlinear CFT. If a perfect
cancellation is achieved, the real system (without including sensor dynamics) will became
equivalent to a first order LTI system, hence a P controller should be suitable for the controller
algorithm. KP represents the proportional gain. Although theoretically the system should remain
stable even for extremely high values of KP, due to uncertainties in the system, unavailability of
the actual variables ωIy and Kc, the necessity to implement a discretization that will limit the
41
Section 4. Observers and Controllers
bandwidth, and other factors that will be described in the next section, increasing too much the
value of Kp tends to be unstabilize the system.
The impossibility to use the real variables imposes a slight change in the controller approach,
forcing instead the use of the estimated variables. Thus

 
Ck K p Iy Iydesired CKF


(4.23)
Simulations using three different approaches were performed. In the first case the controller
includes only the proportional term and the output of the gyroscope is taken as the estimation of
ωIy. The second approach includes the observer (in this case the Extended Kalman Filter) used
t
oe
s
t
i
ma
t
e
dωIy and again only the proportional term. Finally, the third approach included the
observer us
e
dt
oe
s
t
i
ma
t
et
hebot
h,ωIy and CFT, the proportional term and the CFT-cancellation
term.
To obtain more realistic results, the observer used in subsection 4.1 to estimate the
di
s
t
ur
b
a
nc
eωOy was neglected and no assumptions were made about the disturbance frequency.
Figure 4.17 a) displays the results of simulating the first controller. For a better idea of the effect
of the controller in rejecting the disturbance the first five seconds of the simulations were run
without any control being applied while after that the controller was activated. Figure 4.18 b)
zooms the response after the controller is applied.
Figure 4.18 a) Angular velocity, before and after control signal is applied
42
Section 4. Observers and Controllers
Figure 4.18 b) Zoom over angular velocity, before and after control signal is applied
Measurement noise standard deviation =1-4
Proportional gain Kp = 310
Disturbance ωOy = 0.8 sin(2πt)[rad/sec]
Sampling period = 0.001 [sec] or equivalently 1000Hz
Although a significant disturbance rejection was obtained, the cancellation attained can not be
considered exceptional. It is important to take into consideration that the disturbance used for
the simulation was rather high, with peaks up to 46 [degrees/sec] and that the measurement
noise was four orders higher than the one used in the second simulation in subsection 4.1. For
very low frequency disturbances and small MN this method come close to the behavior of the
second method shown next. Simulation results for the second controller are plotted in figure
4.19
Figure 4.19 Angular velocity before and after control signal is applied (2º controller)
43
Section 4. Observers and Controllers
Measurement noise standard deviation =1-4
Proportional gain Kp = 1500
Disturbance ωOy = 0.8 sin(2πt)[rad/sec]
It is fairly evident the great improvement achieved by the second controller contrasted with the
first. The variance reduction was above 92%.A more detailed perception of the cancellation
achieved is seen in figure 4.20.
Figure 4.20 Zoom over ωIyusing 2º controller
It is interesting to notice the peaks observed in the angular velocity after the controller is

applied. These peaks take place at the instants when there is a change in the sign of OI and
hence in the direction of the CFT.
For a consistent comparison, the same magnification was done after simulating the third
controller. Figure 4.21 displays the results
Figure 4.21 Zo
omov
e
rωIy using 3º controller
44
Section 4. Observers and Controllers
Clearly, a noteworthy improvement was achieved for the third controller (about a 26% smaller
variance over the previous one). Simulations also showed that when the proportional gain Kp is
reduced, the differences between both controllers are even more noticeable. We can see that
although not crucial, to include the term for canceling the CFT was important for improving the
overall performance.
It important to remark that even when CFT is canceled the peaks mentioned above still appear
in the response. The explanation is simple: even though the estimation of the parameter K cf stays

constant after the convergence period, the estimation in the sign of OI depends on the
estimation of ωIy a
ndωOy. Therefore, the phase-misalignment and estimation error of these

variables cause that for a short period of time, when the sign of OI changes, the control torque
that is supposed to be canceling the CFT is being added to it.
4.2.1
Constraints for the controller
There are four main constraints for the controller when only the inner gimbal is taken into
account. Although all of them are related to each other it is useful to try to analyze the role of
each one in the overall performance.
 Quantization:
Different tests were made simulating the A/D converter quantization process and studying its
effect on the overall performance. It is important to notice that for higher values of K p stronger
are the requirements on the quantization step size for the A/D converter. There is an easy
explanation for this: for high values of Kp,t
h
ea
ng
ul
a
rv
e
l
oc
i
t
y ωIy became very small,
consequently, unless the quantization levels are small enough a lot of information is lost in the
process. To analyze properly the effect of the quantization we need to do it jointly with the
proportional gain because for different values of Kp we will obtain different values of suitable
quantization levels.
Tests exhibited quite an interesting phenomenon. Even for a relatively big quantization step
size, using to convert the continuous signal coming from the sensor that is measuring ωIy to a
45
Section 4. Observers and Controllers
discrete one, the controller performance was barely influenced. This important property can be
visualized clearly in the next example. As before, the proportional gain Kp used in the controller
algorithm was equal to 1500 and the MN standard deviation 1-4.
Figure 4.22 shows the result of simulating the same controller under the same conditions with
the only difference that in the first case no quantization was made for the measured signal used
by the controller and for the second case an A/D converter was placed after the sensors with a
quantization step size of 1.3-3 (assuming a range from 0.65 to -0.65 [rad/sec] this is equivalent to
a 10 bits A/D converter)
Figure 4.22 System response with and without quantization
Disturbance ωOy = 0.8 sin(2πt)[rad/sec]
Even for this large quantization step there is only a slight degradation in the controller
performance when including the A/D converter. For a better visualization an observer was
pl
a
c
e
da
f
t
e
rt
hes
e
ns
orme
a
s
ur
i
ngωOy so the peak
sobs
e
r
v
e
dbe
f
or
edon
’
ta
pp
e
a
r(
t
hepe
r
i
o
d

where OI and its estimation does not coincide is too small to influence the response). The
results di
dn’
td
i
f
f
e
rwh
e
nt
h
eobs
e
r
v
e
rwa
sn
oti
nc
l
ude
d
.
The extreme quantization of the measur
e
dωIy is manifest in figure 4.23. The figure plots the
out
pu
toft
h
eA/
Dc
onv
e
r
t
e
rp
l
a
c
e
da
f
t
e
rt
heg
y
r
os
c
opeus
e
dt
os
e
ns
o
rωIy.
46
Section 4. Observers and Controllers
Figure 4.23 Sensor output after quantization
After amplifying figure 4.23, is evident that the measured angular velocity ωIy, after the
transient period when the observer is still converging, only takes three different values, -1.3e-3,
zero and, 1.3e-3. For smaller values of Kp even bigger quantization step sizes can be used with
the same results.
Figure 4.24 Zoom over sensor output after quantization
Even for this extreme distortion caused by the converter the controller capacity to cancel
disturbances is barely affected, as it was shown in figure 4.22. This is a great advantage because
it allows us to be loose in the requirements of the converter, and consequently in the
microprocessor in charge of dealing with the calculations.
47
Section 4. Observers and Controllers
 Saturation :
Another practical limitation when implementing the controller is the limitation in the torque that
the motor is able to induce. If the control signal exceeds certain maximum levels the motor will
enter in saturation condition. Because of the difficulties in predicting the response of the system
when the motor torque is continuously entering in saturation mode, it is important to analyze the
system behavior when this situation occurs. For the control law proposed before, the parameter
Kp will have a direct effect on the control signal magnitude. For this reason, if the condition of
no saturation is imposed, the maximum allow levels will determine a practical limitation for the
proportional gain Kp. For a better understating the of the consequences of reaching saturation
levels a simulation was done, where the maximum torque that can be produced by the motor is
assumed to be 11% below the maximum theoretical torque that should be produced according to
the control signal. Figure 4.25 plots both motor torques driven by the control signal, with and
without saturation restrictions.
Figure 4.25 Control signal with and without saturation
From the figure above is possible to visualize the small difference between both signals. Next
two figures will show how, even for these slight differences in the torque produced by the
motor, the performance discrepancies are exceptionally large. In figure 4.26, we can see a
48
Section 4. Observers and Controllers
magnification of the steady-state response of ωIy when the torque, the motor is able to exert, has
no limitation.
Figure 4.26 Zoom over angular velocity ωIy without saturation
For a consistent comparison, the same magnification is done for figure 4.27 where the motor
maximum torque is limited by the saturation levels showed in figure 4.25.
Figure 4.27 Zo
omov
e
ra
ng
ul
arv
e
l
oc
i
t
yωIy with saturation
The peak value of ωIy for the controller with saturation limitation was 21 times higher than
when no limits for the torque the motor can produced was imposed and the variance was 275
times higher. It proved to be critical for the control signal to not reach the saturation levels. This
requirement will have to be taken into account when designing the controller algorithm. Unless
49
Section 4. Observers and Controllers
a severe degradation in the performance is consider acceptable for the application, in case of
saturation condition a change of the motor or of the control law will be required.
 Sampling period:
The sampling period used for the observer and controller algorithms are of utmost importance in
defining the system behavior. The two main reasons are:
1. It defines the computational cost of the algorithm
2. It limits the achievable bandwidth of the system
One of the main drawbacks of using a high frequency sampling is that a faster microcontroller,
able to perform all the necessary calculations, will be required for the implementation. The
influence of the sampling period in the system bandwidth will determine another practical
limitation for the proportional gain, hence for the disturbance cancellation effectiveness. The
increase in bandwidth will allow not only a more effective disturbance cancellation but also will
make it possible to cancel a broader range of disturbance frequencies. The sampling period (SP)
is not the only limitation on the range of disturbance frequencies feasible to be cancelled;
an
ot
h
e
ri
mpor
t
a
ntl
i
mi
t
a
t
i
o
ni
st
heba
n
dwi
d
t
hoft
h
es
e
ns
o
r
’
st
r
a
ns
f
e
rf
u
nc
t
i
on.Ont
heot
he
r
hand for very low sampling frequencies the dynamics of the sensor can not be modeled,
therefore the EKF will not include its dynamics, adding another restriction when using low
frequency sampling.
Numerous tests were made to analyze the computational cost, cancellation rates and range of
disturbance frequencies possible of being rejected, for different sampling periods. A special
focus was set on the behavior when using a 50Hz sampling frequency, since this is the projected
sampling frequency (SF) to be implemented in the real application. This frequency is too low to
include any sensor dynamics in the observer algorithm. For a 50Hz SF and a 50Hz sensor
bandwidth, disturbance frequencies up to 2.5Hz were viable to be cancelled efficiently. To study
the major influence of sensor bandwidth on the controller capability to broad the frequency
range for disturbance rejection several simulations using different sensor transfer functions were
50
Section 4. Observers and Controllers
performed. As an example, when the bandwidth of the sensor was increase to 120Hz the 2.5Hz
limit for effective disturbance cancellation was increased to 10Hz, four times higher than
before.
The optimal value of KP for a sampling frequency of 50Hz was equal to 80. Above this value
the system performance started to degrade until becoming completely unstable for values of KP
above 105. Next figure shows the results of applying a 50Hz sampling, the optimal KP and a
sinusoidal disturbance of with frequency of 2Hz and peak amplitude of 8 degrees/sec. The EKF
us
e
dd
i
dn
’
ti
nc
l
udet
hes
e
n
s
ordy
na
mi
c
s
.
Figure 4.28 Angular velocity ωIy for 50Hz sampling
Disturbance ωOy = 0.13 sin(4πt)[rad/sec]
Proportional gain Kp = 80
The results show that although the convergence time is relatively slow a considerable
attenuation was achieved. The steady-state standard variance obtained was 7.32-7.
Increasing the sampling frequency to 300Hz moves the optimal KP to 420. The results after
running the simulation for the same disturbance are shown in figure 4.29. Even for this
sampling frequency it was not convenient to include the sensor dynamics in the observer.
51
Section 4. Observers and Controllers
Figure 4.29 An
gul
arv
e
l
oc
i
t
yωIy for 300Hz sampling
Disturbance ωOy = 0.13 sin(4πt)[rad/sec]
Proportional gain Kp = 420
It can be seen that the convergence time was drastically reduced and the steady-state variance
obtained was 2.93-7, 2.5 times smaller the in the previous case. Finally the results after
simulating the system with a sampling frequency of 1 kHz are displayed in figure 4.30. All the
sensor dynamics were included in the observer algorithm. The convergence rate was not
influenced but the steady-state variance reached was 5.9-8, almost 5 times better than before.
The optimal Kp was found to be equal to1500.
Figure 4.30 An
gul
arv
e
l
oc
i
t
yωIy for 1000Hz sampling
Disturbance ωOy = 0.13 sin(4πt)[rad/sec]
Proportional gain Kp = 1500
52
Section 4. Observers and Controllers
 Estimation error:
Because the controller algorithm includes the estimated variable, an error in the estimation will
affect directly the effectiveness of the controller. The estimation error is directly related to the
noise measurement; therefore this will have a direct impact in the controller design process
limiting the upper limit of the proportional gain.
Fortunately simulations showed that MN of the sensors has small influence over the whole
system. The MN of the sensor measuring ωIy was almost completely suppressed by the used of
the EKF. Now,be
c
a
u
s
et
h
ee
s
t
i
ma
t
i
onofωOy is only used by the controller when estimating the

sign of OI the MN has only influence when facing a change in the sign, thus for a very short
period of time. This influence proved to be less significant than the one produced by the phaseshift caused by the sensor dynamics. Consequently unless the MN of the sensors is particularly
high, the degradation over the performance has not much consequence.
4.2.2
Alternative controller for the inner gimbal
The main goal underneath disturbance rejection is to maintain the aim of the different
instruments, mounted on the gimbal, fixed at the target. This means to keep the angle over the
inner axes (in this case only the y-axis is considered while in the complete system both y and zaxis need to be considered) fix with respect to the inertial frame. This angle will be identified as
θ
Iy; what is consistent with the fact that is time behavior is the integral of the angular velocity
ωIy.Cl
e
a
r
l
y
,a
t
t
e
nua
t
i
ngωIy will reduce the angle variance but this will not assure minimization
of this value nor guarantee that it will have zero mean. Moreover, all simulations using the
controller developed in section 4.2.1 showed a steady-state shift in the angle response. This
shift can be visualized in figure 4.31.
53
Section 4. Observers and Controllers
Figure 4.31 Angle θ
Iy for 50Hz sampling
disturbance : Oy 0.13sin 
2 pi / 9 
proportional gain : K P 80
These results prompted the idea of augment the observer-s
y
s
t
e
mt
oi
nc
l
ud
et
h
ea
ng
l
eθ
Iy. If an
estimation of t
hea
ng
l
eθ
Iy could be obtained many options for the controller method would be

available. Thedi
f
f
i
c
u
l
t
yi
ne
s
t
i
ma
t
i
ngθ
Iy lays in the fact that to simply integrate Iy is not


acceptable. Initial errors in Iy and posterior misalignments between Iy and Iy would make
t
hee
s
t
i
ma
t
e
dθ
image
Iy completely useless. To overcome this obstacle we will make use of the “
processor sensor”used by the system for target recognition and therefore angle measurement.
Although this sensor work at a much lower frequency compared with the gyroscopes the new
EKF will use the information provided by this sensor in every update to correct the current
angle estimation. Simulations showed that even for angle update frequencies as low as 1Hz, or
even below, the new hybrid-sampled EKF work fairly well. A complete block diagram of this
system is represented in figure 4.32.
54
Section 4. Observers and Controllers
Figure 4.32 Complete system diagram for inner gimbal stabilization
The Angle tracker processor bl
oc
kt
ha
ta
p
p
e
a
r
si
nf
i
g
ur
e4.
32r
e
p
r
e
s
e
nt
st
he“
image processor
s
e
ns
o
r
” mentioned above. Using this augmented system, a discrete linear-quadratic (LQ)
controller was designed. Because the system is to be operated in continuous mode, the
stationary feedback gain L was utilized. The LQ controller was design to minimize the
following cost function

J X kT QX k Ruk 
(4.24)
k 0
The right selection of the matrices Q and R are crucial aspects of the controller design process.
The matrix Q will define the weight of the different states of the model while the matrix R
represent the cost of the control signal and will be used to avoid saturation conditions. The
control law minimizing the cost function stated in equation 4.24 come from solving the
following discrete Riccati algebraic equation


S Q AT S SB 
R BT SB  BT S A
1
(4.25)
The feedback control law will be
u Lx
(4.26)
where
1
L R BT S

R R B SB
T
55
(4.27)
Section 4. Observers and Controllers
To show the effectiveness of this new controller, a simulation under the same conditions as the
ones described for figure 4.31 was run. Then angle response can be seen in figure 4.33.
Figure 4.33 Angle θ
Iy for 50Hz sampling and LQ controller
Controller feedback gain : L 
80 500 
We can see that not only the variance was significantly reduced but also the shift observed in
the previous case was completely cancelled.
The variance when using the discrete LQ
controller was 53 smaller than when using the P controller (9.8-7 vs. 1.84-8). In addition, not only
the angle response was dramatically improved but also the angular velocity response exceeded
the cancellation rates achieved with the previous controller. To illustrate this, next two figures
show the time response of Iy for both controllers.
Figure 4.34 An
gul
arv
e
l
oc
i
t
yωIy for proportional controller
56
Section 4. Observers and Controllers
Figure 4.35 An
gul
arv
e
l
oc
i
t
yωIy for LQ controller
For the angular velocity the variance of the discrete LQ controller was 18 times smaller than for
the P controller (4.46-7 vs. 2.49-8).
I
ti
si
nt
e
r
e
s
t
i
ngt
onot
et
h
epe
a
k
sobs
e
r
v
e
di
nωIy at the beginning of the simulation. These
pe
a
k
sc
o
i
nc
i
dee
xa
c
t
l
ywi
t
ht
het
i
me
si
ns
t
a
n
t
swh
e
nt
h
eda
t
af
r
om t
he“
image processor sensor”
is being used to correct the angle estimation. A more clear visualization is shown in figure 4.36.
Figure 4.36 Zoom over ang
ul
arv
e
l
oc
i
t
yωIy for LQ controller
After a number of corrections these peaks start to vanish. In figure 4.37, plotting the real and
estimated angle for the first seconds of the simulations, it can be visualized how the estimated
error convergence almost to zero after approximately six corrections. The image processing
sensor was assumed to be working at a frequency of 1Hz.
57
Section 4. Observers and Controllers
Figure 4.37 Zoom over real angle vs. estimated angle
One of the limitations for this LQ-controller comes from the hybrid-sampled Extended Kalman
filter. The use of this modified algorithm for the observer reduced the range of disturbances
frequencies feasible to be canceled to frequencies up to 2Hz. For frequencies above this value
t
hee
s
t
i
ma
t
i
one
r
r
oro
fθ
Iy, generated by the EKF were too large to be used by the controller.
Again, it is important to remark that increasing the sensor bandwidth will increase this
frequency limit. The degradation of the performance when approaching this frequency is shown
in the next four figures. The first two compare t
het
i
mer
e
s
pons
eo
fθ
Iy for both controllers when
a disturbance of 1.2Hz and the same amplitude as before is exerted on the system.
Figure 4.38 Angle θ
Iy for 50Hz sampling and proportional controller
58
Section 4. Observers and Controllers
Figure 4.39 Angle θ
Iy for 50Hz sampling and LQ controller
Even when the LQ-controller is not as effective as for lower disturbance frequencies is still
surpass the performance of the proportional controller regarding variance and steady-state shift.
The steady-state variance for the LQ-controller was still 6 times smaller compared to the
proportional controller (7.26-9 vs. 1.2-9) and the steady-state shift was almost completely
annulled (more than 3000 times smaller). Finally, last two figures show the time response of
Iy for both controllers.
Figure 4.40 An
gul
arv
e
l
oc
i
t
yωIy for 50Hz sampling and proportional controller
59
Section 4. Observers and Controllers
Figure 4.41 An
gul
arv
e
l
oc
i
t
yωIy for 50Hz sampling and LQ controller
Controller feedback gain: L=[80 1400]
As with the angle response, the efficacy of the LQ-controller in canceling the angular velocity
Iy exceeds the one of the proportional controller; achieving a steady-state variance reduction
greater than 6 (4.12-7 vs. 6.7-8).
Another fundamental advantage of this new controller is the simplicity to apply angle tracking.

Si
mpl
er
e
p
l
a
c
i
ngt
h
es
t
a
t
eθ
desired will cause the system to move
Iy 
Iy of the control law by 
the angle to the desired position. Figure 4.39 plot the results of applying the tracking control
with a desired angle of seven degrees with exactly the same parameters used in the previous
simulation.
Figure 4.42 Angle θ
Iy for LQ controller in tracking mode
60
Section 4. Observers and Controllers
The convergence time although not extremely rapid is quite fast for most practical purposes and
the steady-state response shows the same characteristics as before.
Finally it is important to remark that due to the static relation between the control signal and
motor torque (see equation 2.2), the signal calculated with the control law has to be multiplied
by an adjustment factor as expressed in the following equation when implementing the
controller.
2
controller output 
Power signal driving the motor 

Km


The power signal makes reference to the voltage to be applied to the motor.
61
(4.28)
Section 5. Conclusions
5 Conclusions
5.1 General Conclusions
Although interesting research has been done in the field of LOS stabilization and many papers
has been published, most of that work has a tendency to offer idealistic assumptions related the
real implementation of the system. One of the major contributions of this work is precisely it
focus in analyzing the influences of practical constrains faced when implementing the LOS
stabilization system. Along with this analysis a novel Extended Kalman filter was developed
and an optimal LQ-controller was designed for stabilizing the elevation axis.
The Extended Kalman filter proved to be remarkably accurate in estimating the nonlinear CFT
compared to the approach described in [8]. Using this estimation to cancel the nonlinear torque
showed a great improvement in the cancellation rate achieved. Modifying the Extended Kalman
filter f
ore
s
t
i
ma
t
i
ngt
hea
n
g
l
eθ
Iy allowed the design of an LQ-controller. The LQ-controller
proved to be much more effective than the proportional controller; achieving a significant
r
e
duc
t
i
onofωIy a
ndθ
Iy variances along with a complete cancellation of the θ
Iy steady-state shift
observed when using the proportional controller.
It is interesting to notice the small influence of the quantization step size in the overall
performance. This will allow a considerable reduction in the computational load and the use of
cheaper A/D converters.
Theus
eo
fa
no
bs
e
r
v
e
rt
oe
s
t
i
ma
t
eωIy made the system highly insensitive to the transfer
function –when the sampling frequency was high enough to model it- and measurement noise
oft
h
eg
y
r
o
s
c
opeus
e
dt
os
e
ns
eωIy. The measurement noise o
ft
heg
y
r
os
c
opeu
s
e
dt
os
e
n
s
eωOy
did not affect appreciably the results except for large noise variance.
The disturbance-bandwidth rejection range (DBRR) attested to depend essentially on two
factors: sampling frequency and sensor bandwidth (ofg
y
r
os
c
op
es
e
n
s
i
ngωOy).
62
For high
Section 5. Conclusions
frequency sampling the DBRR was exclusively limited by the sensor bandwidth. Even when
using a 50Hz sampling frequency the sensor bandwidth was the major limitation for the DBRR
for cut-off frequencies below 150Hz; above that frequency the main constraint was imposed by
the sampling frequency. This is of key importance when selecting the gyroscope optimal
characteristics. If the system will work at 50Hz it is pointless to buy gyroscopes with
bandwidths higher than 150Hz.
Finally, simulations showed the extreme degradation produced when saturation conditions were
reached; therefore it will be imperative to avoid these conditions modifying the controller law or
changing the motor.
5.2 Future Work
The main goal of any future work should be to complete the work done in section four, with the
inclusion of the outer gimbal equations to design a controller for the whole system. It would be
also valuable to develop and analyze the possible use of alternative controller designs, e.g.
robust controllers, adaptive controllers, neural networks, and to perform a detailed analysis to
define the best cost-benefit arrangement for the sensors.
63
Section 6. References
6 References
[1] Peter J. Kennedy, Rhonda L. Kennedy, Direct versus indirect line of sight (LOS) stabilization, IEEE
Trans. Controls System Tech. Vol.11, Nº1, 2003, 3-15.
[2] Piezoelectric Vibrating Gyroscopes (GYROSTAR), Murata Manufacturing Co., Ltd, Kyoto, Japan,
November 2002.
[3] Brush Type DC Motors Handbook, Axsys Technologies, 2005.
[4] Marcel J. Sidi, Spacecraft Dynamics and Control: A Practical Engineering Approach, Cambridge
Aerospace Series, Cambridge University Press, 1997.
[5] H. Ambrose, Z. Qu, R. Johnson, Nonlinear robust control for a passive line-of-sight stabilization
system, Proc IEEE, Conference on Control Applications, Sept 2001, 942-947.
[6] T. H. Lee, E. K. Koh, M.K.Loh, Stable adaptive control of multivariable servomechanisms, with
application to a passive line-of-sight stabilization system, IEEE Trans. Industrial Electr. Vol 43, N°1,
1996, 98-105.
[7] Marcelo C. Algrain, James Quinn, Accelerometer Based Line-of-Sight Stabilization Approach for
Pointing and Tracking Systems, IEE Conference on Control Applications, Sept 1993, 159-163.
[8] Bo Li, David Hullender, Mike DiRenzo, Nonlinear Induced Disturbance Rejection in Inertial
Stabilization Systems, IEEE Trans. Control Systems Tech. Vol.3, Nº3, 1998, 421-427.
[9] B. Li, Identification and compensation for Coulomb friction in stochastic systems, Ph.D. dissertation,
Univ. Texas Arlington, Dec. 1994.
[10] D. A. Hullender and B. Li, Application of advanced control techniques to line-of-sight stabilization
systems, Texas Instruments, Inc., Dallas, TX, Final Rep., Feb. 1994.
[11] Greg Welch and Gary Bishop, An Introduction to the Kalman Filter, University of North Carolina at
Chapel Hill, 2001.
64