Abstract
Title:
Implementation of a Control Algorithm for a
Precision Guided Bomb Kit
Submitted By
Group No.
Name of Student
Registration Numbers
Muhammad Bilal
Muhammad Osama Saleem
17ABELT0752
17ABELT0753
Supervisor
Name of Supervisor: Engr. Mehmona Gul
Supervisor’s Designation: Lecturer
DEPARTMENT OF ELECTRONIC ENGINEERING
UNIVERSITY OF ENGINEERING AND TECHNOLOGY
ABBOTTABAD
August, 2021
Title
(Implementation of a Control Algorithm for a Precision
Guided Bomb Kit)
Submitted By
Name of Student
Registration Numbers
Muhammad Bilal
Muhammad Osama Saleem
17ABELT0752
17ABELT0753
A report submitted in partial fulfillment of the requirements for the degree of
B.Sc. Electronic Engineering
Thesis Supervisor:
Supervisor Name: Engr. Mehmona Gul
Designation & Department: Lecturer (Electronics Engineering)
Head of Department Signature: ____________________________________
External Examiner Signature: ____________________________________
Thesis Supervisor Signature:
____________________________________
DEPARTMENT OF ELECTRONIC ENGINEERING
UNIVERSITY OF ENGINEERING AND TECHNOLOGY
ABBOTTABAD CAMPUS
August, 2021
Abstract
Abstract
The demands on modern guided weapon systems are becoming more stringent than in the past. There
is a requirement to have pin-point accuracy, low cost per round, an easy upgrade path, better
performance in a counter measures environment and to be able to acquire and track low observable
targets. Many of the recent conflicts around the world have exacerbated this trend and the demands on
weapon guidance systems and sensor development have risen considerably. Two of the major
components of a guided weapon system are the guidance algorithm and the missile autopilot. The
guidance algorithm issues command to the autopilot to enable the missile to be guided onto the target.
This requires great precision and very fast response from the missile airframe.
Key words: Guided Weapon System，Pin-Point Accuracy ，Guidance Algorithm，Auto Pilot
Innovation Points
Innovation Points
First innovation point.
Second innovation point.
Third innovation point.
IV
List of Abbreviations
List of Abbreviations
The following table describes the significance of various abbreviations and acronyms
used throughout the thesis.
Abbreviation
Meaning
EKF
Extended Kalman Filter
LLA
Longitutde,Latitutde,Altitude
FPA
Flight Path Angle
AOA
Angle of Attack
PA
Pitch Angle
PID
Proportional,Integral,Derivative
ASGM
Air to Surface Guided Missile
IMU
Inertial Measurement Unit
LQG
Linear Quadrant Gaussian
LQR
Linear Quadrant Regulator
V
Table of Contents
List of Figures
Figure 1-1
Flow chart of distribution of tasks in the Project.. ............................................... 8
Figure 2-1
The Inertial Navigation Problem.. ......... 1Ошибка! Закладка не определена.
Figure 2-2
Strap-down Mechanization.. .............................................................................. 18
Figure 3-1
Kalman Filter Recursive Algorithm .................................................................. .18
Figure 4-1
Block Diagram of PID Controller. ..................................................................... 20
Figure 4-2
PID Tuning. ......................................................................................................... 22
Figure 5-1
Command Window Result ……………………………………………………..27
Figure 5-2
Workspace Result ………………………………………………………………27
Figure 5-3
Workspace Result ………………………………………………………………28
Figure 5-4
Simulink Model ……………………………………………………………...28
Figure 5-5
Airframe+Missile Controller……………………………………………………29
Figure 5-6
Kalman Filter Implementation………………………………………………….29
Figure 5-7
StateSpace Implmentation ……………………………………………………..30
Fi gure 5-8 PID Controller+Saturation Block…………………………………………..…..30
Figure 5-9
Block Parameters of PID Controller……………………………………….…..31
Figure 5-10 Block Parameters of Saturation Block………………………………..………...31
Figure 5-11 Corrdinates Calculation Block …………………………………………….…...32
Figure 5-12 Block Parameters of POS > LLA ……………………………………..……….32
Figure 5-13 Guidance Calculator……………………………………………….…………...33
Figure 6-1
Block Diagram of MATLAB Code ……………………………………….…...35
Figure 6-2
Block Diagram of LQR Controller………………………………………...…..36
Figure 6-3
Block Diagram of SIMULINK Model………….……………………………..37
Figure 6-4
Angle Classification …………………………………………………………..37
Figure 6-5
Block Diagram of LQG Controller …………………….……………………..38
VI
Table of Contents
Table of Contents
Abstract .................................................................................................................................... III
Innovation Points ................................................................................................................... IIV
List of Abbreviations................................................................................................................. V
Table of Contents ................................................................................................................ VVII
Introduction and Background ............................................................................................ 1
Purpose and Function ..................................... Ошибка! Закладка не определена.
1.1.1
Sensors ................................................................................................................. 2
1.1.2
Accelrometers ...................................................................................................... 2
1.2 Phases of Guidance ....................................................................................................... 3
............................................................................................. 3
3
………………………........………….4
.4
...4
.4
5
Homming Guidance…………………………………………………………....5
Active Homming……………………………..………………….………..6
………………………………………………6
……………………………………………………..…..6
7
7
Objective of Thesis .................................................................................................... 7
1.4.1
The Road Map of the Project ............................................................................... 8
Goals and Milestones Achieved ................................................................................. 9
Difficulties ................................................................................................................. 9
Outline of Dissertation ............................................................................................... 9
Second Chapter ...................................................... Ошибка! Закладка не определена.
VII
Table of Contents
Introduction .............................................................................................................. 10
Heading (Level-2) .......................................... Ошибка! Закладка не определена.
2.2.1
Importing Pictures .................................... Ошибка! Закладка не определена.
2.2.2
Information about Figures........................ Ошибка! Закладка не определена.
2.2.2.1
Single Picture .................................... Ошибка! Закладка не определена.
2.2.2.2
Adding 2 Pictures Side by Side .................................................................. 13
2.2.2.3
Adding 3 Pictures in a Row .............. Ошибка! Закладка не определена.
2.2.2.4
Adding 4 Pictures ....................................................................................... 14
Chapter Summary .......................................... Ошибка! Закладка не определена.
Third Chapter ......................................................... Ошибка! Закладка не определена.
Importing Tables ............................................ Ошибка! Закладка не определена.
3.1.1
Single Entry in a Row .............................. Ошибка! Закладка не определена.
3.1.2
Multiple Column Entries in a Single RowОшибка! Закладка не определена.
Chapter Summary .......................................... Ошибка! Закладка не определена.
Fourth Chapter ....................................................... Ошибка! Закладка не определена.
Writing Equations .................................................................................................... 20
4.1.1
Single Equation ........................................ Ошибка! Закладка не определена.
4.1.2
Double Equations ..................................... Ошибка! Закладка не определена.
4.1.3
A Set of Multiple Equations ..................... Ошибка! Закладка не определена.
4.1.4
Multiple Column Entries in a Single RowОшибка! Закладка не определена.
Chapter Summary .......................................... Ошибка! Закладка не определена.
Fifth Chapter .......................................................... Ошибка! Закладка не определена.
First Heading (Level-2).................................. Ошибка! Закладка не определена.
5.1.1
Second Heading (Level-3) ....................... Ошибка! Закладка не определена.
5.1.1.1
Fourth Heading (Level-4) ................. Ошибка! Закладка не определена.
5.1.1.2
Fourth Heading ................................. Ошибка! Закладка не определена.
5.1.2
Second Heading (Level-3) ....................... Ошибка! Закладка не определена.
VIII
Table of Contents
5.1.2.1
Fourth Heading (Level-4) ................. Ошибка! Закладка не определена.
5.1.2.2
Fourth Heading ................................. Ошибка! Закладка не определена.
5.1.3
Second Heading (Level-3) ....................... Ошибка! Закладка не определена.
5.1.3.1
Fourth Heading (Level-4) ................. Ошибка! Закладка не определена.
5.1.3.2
Fourth Heading ................................. Ошибка! Закладка не определена.
Chapter Summary .......................................... Ошибка! Закладка не определена.
Sixth Chapter ......................................................... Ошибка! Закладка не определена.
First Heading (Level-2).................................. Ошибка! Закладка не определена.
6.1.1
Second Heading (Level-3) ................................................................................. 36
6.1.1.1
Fourth Heading (Level-4) ........................................................................... 36
6.1.1.2
Fourth Heading ................................. Ошибка! Закладка не определена.
6.1.2
Second Heading (Level-3) ....................... Ошибка! Закладка не определена.
6.1.2.1
Fourth Heading (Level-4) ................. Ошибка! Закладка не определена.
6.1.2.2
Fourth Heading ................................. Ошибка! Закладка не определена.
6.1.3
Second Heading (Level-3) ....................... Ошибка! Закладка не определена.
6.1.3.1
Fourth Heading (Level-4) ................. Ошибка! Закладка не определена.
6.1.3.2
Fourth Heading ................................. Ошибка! Закладка не определена.
Chapter Summary .......................................... Ошибка! Закладка не определена.
Seventh Chapter ..................................................... Ошибка! Закладка не определена.
First Heading (Level-2).................................. Ошибка! Закладка не определена.
7.1.1
Second Heading (Level-3) ....................... Ошибка! Закладка не определена.
7.1.1.1
Fourth Heading (Level-4) ................. Ошибка! Закладка не определена.
7.1.1.2
Fourth Heading ................................. Ошибка! Закладка не определена.
7.1.2
Second Heading (Level-3) ....................... Ошибка! Закладка не определена.
7.1.2.1
Fourth Heading (Level-4) ................. Ошибка! Закладка не определена.
7.1.2.2
Fourth Heading ................................. Ошибка! Закладка не определена.
7.1.3
Second Heading (Level-3) ....................... Ошибка! Закладка не определена.
IX
Table of Contents
7.1.3.1
Fourth Heading (Level-4) ................. Ошибка! Закладка не определена.
7.1.3.2
Fourth Heading ................................. Ошибка! Закладка не определена.
Chapter Summary .......................................... Ошибка! Закладка не определена.
Conclusions and Future Work .......................................................................................... 39
Conclusions .............................................................................................................. 39
Future Work and Recommendations ........................................................................ 39
References ................................................................................................................................ 40
Academic and Research during the Doctoral Study ...... Ошибка! Закладка не определена.
Acknowledgements ........................................................ Ошибка! Закладка не определена.
X
1 Introduction and Background
Introduction and Background
The term missile in the post- World War II era has generally been used synonymously with "guided
missile," due to the wide impact of guided missile technology upon the weapons field. In the
un-guided case, initial conditions (such as train, elevation, powder charge in naval guns) and
exterior ballistic effects are parameters that, along with normal distribution, affect the "fall of
shot." As advances in technology permitted (paralleled by in-creasing threat complexity), the
development of guided missiles made possible a significant increase in terminal accuracy of
military weaponry. The application of automatic control is prevalent in broad regions of missile
technology including:
Underwater Homing Torpedoes
Surface-to-Surface Aerodynamic Guided Missiles
Intercontinental Ballistic Missiles
Air-to-Surface Guided Missiles
Air-to-Air Guided Missiles
Surface-to-Air Guided Missiles
Guided Projectiles
Purpose and Function
Every missile guidance system consists of an attitude control system and a flight path control
system. The attitude control system functions to maintain the missile in the desired attitude on the
ordered flight path by controlling the missile in pitch, roll, and yaw. The attitude control system
operates as an auto-pilot, damping out fluctuations that tend to deflect the missile from its ordered
flight path. The function of the flight path control system is to determine the flight path necessary
for target interception and to generate the orders to the attitude control system to maintain that path.
It should be clear at this point that the concept of "Guidance and Control" involves not only the
maintenance of a particular vehicle's path from point A to B in space, but also the proper behavior
of the vehicle while following the path. A missile that follows a prescribed path half the way to a
1
1 Introduction and Background
target and then becomes dynamically unstable is then incapable of remaining upon the path (or else
fails structurally due to aero-dynamic loading). Such a vehicle, in order to perform properly, must
be "piloted" and capable of responding to control signals.
1.1.1 Sensors
The guidance system in a missile can be compared to the human pilot of an airplane. As a pilot
guides his plane to the landing field, the guidance system "sees" its target. If the target is far away
or otherwise obscured, radio or radar beams can be used to locate it and direct the missile to it.
Heat, light, television, the earth's magnetic field, and Loran have all been found suitable for
specific guidance purposes. When an electromagnetic source is used to guide the missile, an
antenna and a receiver are installed in the missile to form what is known as a sensor. The sensor
picks up, or senses, the guidance information. Missiles that are guided by other than
electromagnetic means use other types of sensors, but each must have some means of receiving
"position reports."
1.1.2 Accelerometers
The heart of the inertial navigation system for ships and missiles is an arrangement of
accelerometers that will detect any change in vehicular motion. To understand the use of
accelerometers in inertial guidance, it is helpful to examine the general principles involved.
An accelerometer, as its name implies, is a device for measuring acceleration. In their basic form
such devices are simple. For example, a pendulum, free to swing on a transverse axis, could be
used to measure acceleration along the fore-and-aft axis of the missile. When the missile is given a
forward acceleration, the pendulum will tend to lag aft; the actual dis-placement of the pendulum
form its original position will be a function of the magnitude of the accelerating force. Another
simple device might consist of a weight supported between two springs. When an accelerating
force is applied, the weight will move from its original position in a direction opposite to that of the
applied force. The movement of the mass (weight) is in accordance with Newton's second law of
motion, which states that the acceleration of a body is directly proportional to the force applied and
inversely proportional to the mass of the body.
If the acceleration along the fore-and aft axis were constant, the speed of the missile at any instant
could be determined simply by multiplying the acceleration by the elapsed time. However, the
2
1 Introduction and Background
acceleration may change considerably over a period of time. Under these conditions, integration is
necessary to determine the speed.
If the missile speed were constant, the distance covered could be calculated simply by multiplying
the speed by time of flight. But because the acceleration varies, the speed also varies. For that
reason, a second integration is necessary.
PHASES OF GUIDANCE
Missile guidance is generally divided into three phases--boost, midcourse, and terminal. These
names refer to different parts of the flight path. The boost phase may also be called the launching
or initial phase.
1.2.1 Boost Phase
Navy surface-to-air missiles accelerate to flight speed by means of the booster component. This
booster period lasts from the time the missile leaves the launcher until the booster burns its fuel.
In missiles with separate boosters, the booster drops away from the missile at burnout. The
objective of this phase is to place the missile at a position in space from where it can either "see"
the target or where it can receive external guidance signals. During the boost phase of some
missiles, the guidance system and the aerodynamic surfaces are locked in position. Other missiles
are guided during the boost phase.
1.2.2 Terminal Phase
The last phase of missile guidance must have high accuracy as well as fast response to guidance
signals. Missile performance becomes a critical factor during this phase. The missile must be
capable of executing the final maneuvers required for intercept within the constantly decreasing
available flight time. The maneuverability of the missile will be a function of velocity as well as
airframe design. Therefore, a terminal guidance system must be compatible with missile
performance capabilities. The greater the target acceleration, the more critical the method of
terminal guidance becomes. Suitable methods of guidance will be discussed in later sections of
this chapter. In some missiles, especially short-range missiles, a single guidance system may be
used for all three phases of guidance, whereas other missiles may have a different guidance
system for each phase.
3
1 Introduction and Background
TYPES OF GUIDANCE SYSTEMS
Missile guidance systems may be classified into two broad categories: missiles guided by
man-made electromagnetic devices, and those guided by other means. In the first category are
those missiles controlled by radar, radio de- vices, and those missiles that use the target as a source
of electromagnetic radiation. In the latter category are missiles that rely on electromechanical
devices or electromagnetic contact with natural sources, such as the stars (self-contained guidance
systems).
All of the missiles that maintain electromagnetic radiation contact with man-make sources may be
further subdivided into two subcategories.
(1)
Control guidance missiles
(2)
Homing guidance missiles
1.3.1
Control Guidance
Control guidance missiles are those that are guided on the basis of direct electromagnetic radiation
contact with friendly control points. Homing guidance missiles are those that guided on the basis of
direct electromagnetic radiation contact with the target. Control guidance generally depends on the
use of radar (radar control) or radio (radio control) links between a control point and the missile.
By use of guidance information transmitted from the control point via a radio or radar link, the
missile's flight path can be guided. This section will use radar control guidance as a model for
discussion because it is by far the most common application of control guidance methods. The
principles discussed may be readily applied to radio (including television) control guidance.
1.3.1.1 Radar Control Guidance
Radar control guidance may be subdivided into two separate categories. The first category is
simply referred to as the command guidance method. The second is the beam-rider method, which
is actually a modification of the first, but with the radar being used in a different manner.
1.3.1.2
Command Guidance
The term command is used to describe a guidance method in which all guidance instructions, or
commands, come from sources outside the missile. The guidance system of the missile contains a
4
1 Introduction and Background
receiver that is capable of receiving instructions from ship or ground stations or from air- craft. The
missile flight-path control system then converts these commands to guidance information, which is
fed to the attitude control system.
1.3.1.3 Beam-rider Method
The main difference between the beam-rider method and the radar command guidance method is
that the characteristics of the missile-tracking radar beam are not varied in the beam-rider system.
The missile has been designed so that it is able to formulate its own correction signals on the
basis of its position with respect to the radar scan axis. The missile's flight path control unit is
sensitive to any deviation from the scan axis of the guidance radar and is capable of computing
the proper flight path correction. An advantage of this type of system is that is requires only one
radar. This radar must, of course, have a conical-scan feature in order to provide both
target-tracking capability and a missile flight-path correction reference axis. A second advantage
is that since the missile formulates its own directional commands, several missiles may be
launched to "ride" the beam simultaneously, without the need for a cumbersome and complicated
multiple-missile command system.
1.3.2 Homing Guidance
Homing guidance systems control the flight path by employing a device in the weapon that reacts
to some distinguishing feature of the target. Homing devices can be made sensitive to a variety of
energy forms, including RF, infrared, reflected laser, sound, and visible light. In order to home on
the target, the missile or torpedo must determine at least the azimuth and elevation of the target by
one of the means of angle tracking mentioned previously. Active homing missiles will also have
the means of determining range of the target if necessary. Tracking is performed by a movable
seeker antenna or an array with stationary electronically scanned arrays in development for
missiles and operational in some torpedoes. Determination of angular error by amplitude
comparison mono-pulse methods is preferred over the older COSRO systems because of the higher
data rate and faster response time; however, phase comparison mono-pulse or interferometer
methods have advantages in some applications. Homing guidance methods may be divided into
three types: active, semi active and passive homing. These methods may be employed in seekers
using any of the energy forms mentioned above, although some methods may be excluded by the
nature of the energy form; for example, one would not build a passive laser seeker or an active or
semi active infrared seeker.
5
1 Introduction and Background
1.3.2.1
Active Homing
In active homing, the weapon contains both the transmitter and receiver. Search and acquisition
are conducted as with any tracking sensor. The target is tracked employing mono-static geometry
in which the returning echo from the target travels the same path as the transmitted energy. An
onboard computer calculates a course to intercept the target and sends steering commands to the
weapon's autopilot. The mono-static geometry allows the most efficient reflection of energy from
the target, but the small size of the missile restricts the designer to high frequencies and low power
output from the transmitter, resulting in short seeker acquisition range.
1.3.2.2 Semi-active Homing
In semi active homing, the target is illuminated by a tracking radar at the launching site or other
control point. The missile is equipped with a radar receiver (no transmitter) and by means of the
reflected radar energy from the target, formulates its own correction signals as in the active
method. However, semi active homing uses bistatic reflection from the target, meaning that
because the illuminator platform and weapon receiver are not co located, the returning echo
follows a different path than the energy incident to the target. Due to its shape and composition, the
target may not reflect energy efficiently in the direction of the weapon. In extreme cases the
weapon may lose the target entirely, resulting in a missed intercept. This disadvantage is
compensated for by the ability to use greater power and more diverse frequency ranges in an
illumination device in a ship, aircraft, or ground station.
1.3.2.3
Passive Homing
Passive homing depends only on the target as a source of tracking energy. This energy can be the
noise radiated by a ship or submarine in the case of a passive homing torpedo, RF radiation from
the target's own sensors in the case of an anti-radiation (ARM) weapon, heat sources such as ship,
aircraft, or vehicle exhausts, contrast with the temperature or visible light environment, or even the
radiation all objects emit in the microwave region. As in the other homing methods, the missile
generates its own correction signals on the basis of energy received from the target rather than from
a control point. The advantage of passive homing is that the counter detection problem is reduced,
and a wide range of energy forms and frequencies are available. Its disadvantages are its
susceptibility to decoy or deception and its dependence on a certain amount of cooperation from
the enemy.
6
1 Introduction and Background
1.3.2.4 Accuracy Homing
Accuracy Homing is the most accurate of all guidance systems because it uses the target as its
source when used against moving targets. There are several ways in which the homing device may
control the path of a missile against a moving target. Of these, the more generally used are pursuit
paths and lead flight paths, which are discussed in a subsequent part of this chapter. Because
mono-pulse methods in weapons seekers are advantageous and are becoming the method of choice
in current weapons.
1.3.2.5 Interferometer (Phase Comparison Mono-pulse)
The interferometer eliminates the requirement for a movable antenna, having instead fixed
antennas mounted at the edge of the airframe or on the wing tips, the result being reduced
complexity and a wider field of view.
The interferometer provides the advantage of wide field of view, flexibility in airframe design,
unobstructed use of weapon interior space, and the ability to cover broad frequency bands without
constraints imposed by limited antenna size. The separation between the antennas governs the
performance of the system, with missile body diameter or fin spread separation as the usual
arrangement. The disadvantage of the interferometer is the angular ambiguity that may exist for
wavelengths less than the separation between the antennas at a specific angle of incidence. If the
distance between the antennas at an angle of incidence is d sin , and is less than d sin , then it is not
possible to determine if the phase angle measured is just that or + n2 radians, where n is any
integer. However, this is a minor problem in most homing systems because the absolute look angle
is not as important as the rate of change of that angle.
Objective of Thesis
 Control Flight Path angle by using input step function to hit targeted location.
 Linear Quadrant Gaussian (LQG) Control design.
 Kalman Filter Design for estimation.
 Working with latitude and longitude.
 Guidance loop and command.
 Finding Azimuth, Range, Miss Distance, Target Distance and Obstacle distance.
7
1 Introduction and Background
1.4.1 The Road Map of the Project
Main domain
MATLAB
Implementation
Theory
+ Simulink
Enhancement
Modeling
Software
Design
First
FPGA
Hardware
Implementation
Further
work
Accuracy
Improvement
Design
Modeling
Analysis
Prototype Model
Improveme
nt
Second
Design
Third
Analysis
Finalization
Modeling
Design
Fourth
Modeling
Prototype Model
Air to Air Missile
Air to Surface
Applications
Cruise Missile
Vehicle Tracking
System
Missile
Figure 1-1
Implementation
Flow chart of distribution of tasks in the Project.
8
1 Introduction and Background
Goals and Milestones Achieved
The following milestones in the prescribed research are completed.
 Accuracy Improvement.
 White Noise Reduction.
 PID Tuning.
Difficulties
 Data Collection.
 Implementation of Control Algorithm.
 Error Minimization.
 Communication gap due to Covid.
Outline of Dissertation
An outline of this dissertation follows. In Chapter 2, we will discuss about Inertial Navigation
for Missile Guided System. In Chapter 3, we will discuss about Kalman Filtering Techniques.
In Chapter 4, we will discuss about PID Tuning. Chapter 5 focuses on the techniques for
solving and analyzing the Kalman Filter Control Algorithm using MATLAB and
SIMULINK. Chapter 6 mainly elaborates that the How our Model Works. Finally,
conclusions of the work and recommendations for possible future investigations are presented in
Chapter 7.
9
2 Second Chapter
Inertial Navigation for Missile Guided System
In this chapter we will discuss about Inertial Navigation for Missile Guided System.
Introduction
An accurate inertial reference based on measurements of missile angular velocity and
acceleration is needed for all of the major guidance and control functions of a guided missile. For
example, intercept of a target would not be possible without a good inertial reference system to
stabilize target line-of-sight measurements for the computation of missile guidance commands.
This chapter provides an overview of inertial navigation for guided missiles. Missile navigation
data (position, velocity, and attitude) are needed for missile guidance and control. Furthermore,
this chapter describes in-flight alignment techniques that can be used to increase the accuracy of
the missile navigation-system data by incorporating external non-inertial navigation-aiding data
using a navigation Kalman filter. For guided missile systems, this aiding often is provided by an
external radar track of the missile and/or Global Positioning System (GPS) receiver
measurements.
Background
Inertial navigation has been a key element of missile system design since the 1950s. Traditionally,
the focus has been on strategic- and precision-strike systems. In these applications,
terminal-position accuracy is the primary objective of the navigation system. In guided missile
systems in which a terminal seeker is used to sense and track an air or ballistic missile threat, a
critical function of the inertial navigation system (INS) is to provide accurate seeker-attitude
information and, therefore, allow accurate pointing of the seeker for acquisition of a target. In
addition, the navigation system provides essential data for guidance and flight-control functions.
This chapter also discusses more recent advances in navigation for guided missiles. These
advances have been motivated by several factors. The historical use of a semi-active RF seeker
with a wide field of view placed less demand on the accuracy of the navigation system for
pointing information. The use of wide-field-of-view
seekers also was consistent with the fact that accurate navigation systems were high in cost,
heavy in weight, large in volume, and, therefore, not suitable for tactical guided missiles.
However, as lower-cost, smaller, and more reliable inertial measurement units (IMUs) have
become readily available, missile systems have been able to employ higher-accuracy,
smaller-field-of-view seekers such as infrared or high-frequency RF technology without the need
to perform an angle search. The use of advanced seeker technology naturally leads to better
overall performance against more stressing targets. A second consideration is that targeting
10
2 Second Chapter
information may be improved by the use of multiple sensors. As sensor alignment errors and
target-track errors are taken into account, it is desirable to minimize the alignment error between
the missile seeker and the targeting reference.
A third consideration is the missile guidance system configuration before seeker acquisition.
Typically, a missile is guided by uplinks that are based on filtered radar measurements of both the
missile and the target. In an alternative approach, called inertial midcourse guidance, the tracking
radar still provides filtered targeting data and unfiltered missile-position measurement data,but
the missile itself computes the guidance commands. This latter approach places greater reliance
on the missile navigation and guidance systems in an attempt to improve overall system
performance.
2.2.1 Unaided Inertial Navigation
Most commonly, an INS is used to determine the position, velocity, and orientation of a vehicle
moving relative to the Earth’s surface. The INS computations are based on gyroscope
measurements of inertial angular velocity to determine the orientation of a triad of accelerometers.
The accelerometer measurements, in turn, are integrated to estimate vehicle velocity and position.
There are two fundamental approaches to INS mechanization. Because of the dynamic ranges
and error sensitivities of earlier gyro technologies and computer limitations, platform systems
were the most common mechanization approach before the 1990s. In these systems, the inertial
instruments are placed on a stabilized platform that is gimbaled with respect to the host vehicle,
making the measurements insensitive to rotational motion. Although platform mechanization still
is used today in many applications, such as aircraft, cruise missiles, and ships, it is not suitable
for tactical missiles
because of the cost, volume, and weight.
2.2.2 Mathematics behind Inertial Navigation System
During the 1970s, gyroscopes with lower error sensitivity to angular rate were developed.
Concurrent advances in computer technology led to interest in strap-down systems in which the
inertial instruments are rigidly attached to the host vehicle. Here, the sensor measurements are
mathematically transformed to a stabilized reference frame to remove the effects of vehicle
motion. Although the computations associated with a strap-down INS are conceptually simple,
the mechanization can be quite complex because of the multiplicity of rotating coordinate frames
involved. As shown in Fig. 1, the strap-down INS measures angular velocity and acceleration of
the missile body relative to inertial coordinates, but these measurements are sensed in the rotating
frame of the missile body denoted by the coordinates of the inertial measurement unit case, (ib,
jb, kb). Moreover, the desired navigation solution typically is formed relative to a second,
11
2 Second Chapter
rotating Earth-centered Earth-fixed (ECEF) coordinate frame, (ie, je, ke), having angular velocity
ie, relative to the inertial frame. If the position vector of the missile, r, over the Earth’s surface is
desired (latitude, longitude, and altitude), then a model for the ellipsoidal shape of the Earth’s
surface must be used reference frame.
Figure 2-1
2.2.2.1
The Inertial Navigation Problem.
Inertial Measurement Unit
Figure 2-2 illustrates an example set of navigation computations. The gyro and accelerometer
measurements are accumulated over a measurement interval and compensated by
factory-measured errors, typically bias and scale factor versus temperature. Coning and sculling
compensation are approximations to account for the vehicle’s rotational motions during the
measurement interval, and size compensation accounts for the fact that the accelerometers cannot
be physically collocated, so a lever-arm term caused by case rotation must be removed. The
coning and sculling compensations may be performed within the IMU internal software or in the
navigation computer. The compensated body-angle increments then are used to compute a
body-to-navigation-frame transformation. This process typically is implemented via computation
of a body-attitude quaternion and an associated orientation vector. The orientation vector is a
direct function of the gyro incremental-angle measurements. Although shown in Fig. 3 as a
direction cosine matrix transformation, an equivalent quaternion transformation often is used to
transform the compensated incremental velocity measurement vector, vc, from the body frame to
the navigation frame. The resulting incremental velocity terms are summed and compensated per
Eq. 1 to produce the computed velocity. The linear velocities are converted to angular velocity, and then used to update the direction cosine matrix that describes the orientation of the
navigation frame relative to the Earth frame. Latitude and longitude may be extracted from the
direction cosine matrix. Altitude is computed separately by using velocity in the vertical direction
(kn).
12
2 Second Chapter
The gyroscope and accelerometer technologies used in navigation systems vary considerably in
construction and accuracy. Gyroscope technologies fall into the categories of mechanical gyros
that depend on the angular momentum of a spinning mass, vibratory gyros that depend on
Coriolis acceleration effects, or optical gyros. The most common gyros used today for navigation
in tactical missiles are optical and employ either ring-laser or optical-fiber technologies.
Accelerometers are constructed by using either pendulous or resonant-beam technologies.
Figure 2-2
2.3
Strap-down Mechanization.
NAVIGATION-SYSTEM DESIGN AND TESTING
In this section, we describe the process used to design a guided missile navigation system.
Typically, there are four major steps involved in the development of a navigation system: the
initial requirements specification and formulation of error budgets; the initial design phase,
which often involves the aid of a covariance simulation; the implementation of the actual
navigation equations and Kalman filter algorithms.
2.3.1
Navigation Requirements and Error Budgets
The initial phase of the navigation-system design involves the development of the system’s
fundamental requirements. During this initial stage of the design, the navigation-system design
engineer must become knowledgeable about the overall weapon system and its intended modes
of operation. The designer should be aware of what types of external information will be
available and what types of information will be required from the navigator to support the other
guided missile subsystem functions. In short, to guarantee overall mission success, the design
engineer should have a good understanding of how the navigation system fits within the overall
weapon system to ensure development of a design that will satisfy overall weapon-system
performance.
Usually during the initial stage of missile design, the system engineers will have a good
understanding of the top-level objectives of the overall guided missile system. Furthermore, the
13
2 Second Chapter
system designer will need to identify the specific hardware components that will be included
in the missile along with their basic, key performance parameters. For missile navigation, the
field of regard of the seeker is a very important driver of navigation system performance
requirements.
2.3.2
Covariance Simulation
Once the top-level navigation-system requirements have been determined and a general
architecture of a missile navigation system has been chosen, the actual navigation-system design
begins. A particularly useful tool during the initial stages of the design is a covariance simulation.
In covariance simulation, it is the uncertainty of the navigation state that is important and not the
state vector itself. The basis of a covariance simulation is the recursive Kalman filter error
covariance equations that were discussed previously in Aided Inertial Navigation. Another
advantage of covariance analysis is that a single run of a covariance simulation provides a
statistical assessment of system performance. Hence, covariance analysis avoids the need to
perform a very large number of individual simulation runs to carry out a statistical assessment of
system performance.
In the design of a navigation system, the designer must strike a balance between a conflicting set
of hardware and software objectives. Better performance may be achieved by using better sensors
or by including more states within the Kalman filter model. Covariance analysis provides a
systematic approach to quickly evaluate alternate system implementations. Another benefit of
covariance analysis is that it allows the designer to easily determine the dominant error sources
within the system and to determine whether it is feasible to reduce the error through either better
hardware or more detailed modeling. In practice, covariance analysis can be a very effective tool
in predicting performance, refining error budgets for components and external sensors, and
tuning the Kalman filter in aided-navigation systems.
14
3 Third Chapter
Kalman Filter
Introductory paragraph about this Chapter, that how this work is relevant to your research and
details about the connectivity of this Chapter in your research.
Introduction
The Kalman filter has long been regarded as the optimal solution to many tracking and data
prediction tasks. Its use in the analysis of visual motion has been documented frequently. The
standard Kalman filter derivation is given here as a tutorial exercise in the practical use of some
of the statistical techniques outlined in previous sections. The filter is constructed as a mean
squared error minimizer, but an alternative derivation of the filter is also provided showing how
the filter relates to maximum likelihood statistics. Documenting this derivation furnishes the
reader with further insight into the statistical constructs within the filter.
The purpose of filtering is to extract the required information from a signal, ignoring everything
else. How well alter performs this task can be measured using a cost or loss function. Indeed we
may define the goal of the alter to be the minimization of this loss function.
3.1.1 Mean squared error
Many signals can be described in the following way;
𝑦𝑘 = 𝑎𝑘 𝑥𝑘 + 𝑛𝑘
where; yk is the time dependent observed signal, ak is a gain term, xk is the information bearing
signal and nk is the additive noise. The overall objective is to estimate xk. The difference between
the estimate of ^ xk and xk itself is termed the error.
f (ek) = f (𝑥𝑘 - 𝑥̂𝑘 )
The particular shape of f (ek) is dependent upon the application, however it is clear that the
function should be both positive and increase monotonically. An error function which exhibits
these characteristics is the squared error function;
f (ek) = (𝑥𝑘 − 𝑥̂𝑘 )^2
Since it is necessary to consider the ability of the filter to predict many data over a period of time
a more meaningful metric is the expected value of the error function;
loss function = E (f (ek))
15
3 Third Chapter
3.1.2 Maximum likelihood
The above derivation of mean squared error, although intuitive is somewhat heuristic. A more
rigorous derivation can be developed using maximum likelihood statistics. This is achieved by
rendering the goal of the alter to ending the x^ which maximizes the probability or likelihood of y.
That is;
max [P (y|𝑥̂)]
Assuming that the additive random noise is Gaussian distributed with a standard deviation of k
gives;
[P (𝑦𝑘 |𝑥̂
̂𝑘 )] = 𝑘𝑘 exp - (yk-𝑎𝑘 𝑥̂𝑘 /2δ^2 )^2
where KK is a normalization constant. The maximum likelihood function of this is;
[P (𝑦 |)] = 𝑘𝑘 exp - (yk-𝑎𝑘 𝑥̂𝑘 /2δ^2 )^2
Which leads to;
logP (yjx^) = 2 X k (yk k2ak x^k)2 + constant
The driving function of equation 11.9 is the MSE, which may be maximized by the variation of
x^k. Therefore the mean squared error function is applicable when the expected variation of yk is
best modeled as a Gaussian distribution. In such a case the MSE serves to provide the value of
x^k which maximizes the likelihood of the signal yK.
3.1.3 Kalman Filter Derivation
Before going on to discuss the Kalman filter the work of Norbert Wiener, should first be
acknowledged. Wiener described an optimal Unite impulse response (FIR) filter in the mean
squared error sense. His solution will not be discussed here even though it has much in common
with the Kalman filter. discussed to say that his solution uses both the auto correlation and the
cross correlation of the received signal with the original data, in order to derive an impulse
response for the alter. Kalman also presented a prescription of the optimal MSE filter. However
Kaman’s prescription has some advantages over Weiner's; it sidesteps the need to determine the
impulse response of the filter something which is poorly suited to numerical computation.
Kalman described his filter using state space techniques, which unlike Wiener's prescription,
enables the filter to be used as either a smoother, alter or a predictor.
The latter of these three, the ability of the Kalman filter to be used to predict data
has proven to be a very useful function. It has led to the Kalman filter being applied to a wide
range of tracking and navigation problems.
16
3 Third Chapter
3.1.4 State space derivation
Assume that we want to know the value of a variable within a process of the form;
𝑥𝑘+1 = 𝜙𝑥𝑘 + 𝑤𝑘
where; xk is the state vector of the process at time k, (nx1); is the state transition matrix of the
process from the state at k to the state at k + 1, and is assumed stationary over time, (nxm); wk is
the associated white noise process with known covariance, (nx1) Observations on this variable
can be modeled in the form;
zk = 𝐻𝑥𝑘 + 𝑈𝑘
where; zk is the actual measurement of x at time k, (mx1); H is the noiseless connection between
the state vector and the measurement vector, and is assumed stationary over time (mxn); vk is the
associated measurement error. This is again assumed to be a white noise process with known
covariance and has zero cross-correlation with the process noise, (mx1)
As shown in section ?? for the minimization of the MSE to yield the optimal alter it must be
possible to correctly model the system errors using Gaussian distributions. The covariance’s of
the two noise models are assumed stationary over time and are given by;
Q = E [𝑤𝑘 𝑤𝑘𝑇 ]
R = E[𝑈𝑘 𝑈𝑘𝑇 ]
The mean squared error is given by 11.5. This is equivalent to;
𝑝𝑘 = E [𝑒𝑘 𝑒𝑘𝑇 ]
where; Pk is the error covariance matrix at time k, (nxn).
Equation 11.14 may be expanded to give;
𝑝𝑘 = E [𝑒𝑘 𝑒𝑘𝑇 ]
= E [(𝑥𝑘 - 𝑥̂𝑘 )( 𝑥𝑘 - 𝑥̂𝑘 )^𝑇]
Assuming the prior estimate of 𝑥̂𝑘 is called 𝑥̂𝑘′ , and was gained by knowledge of the system. It
possible to write an update equation for the new estimate, combing the old estimate with
measurement data thus;
𝑥̂𝑘 = 𝑥̂𝑘′
+ 𝑘𝑘 (Zk - H𝑥̂𝑘′
)
where; Kk is the Kalman gain, which will be derived shortly.
17
3 Third Chapter
The Kalman filter as a chi-square merit function
The objective of the Kalman alter is to minimize the mean squared error between the actual and
estimated data. Thus it provides the best estimate of the data in the mean squared error sense.
This being the case it should be possible to show that the Kalman alter has much in common with
the chi-square. The chi-square merit function is a maximum likelihood function, and was derived
earlier.
It is typically used as criteria to t a set of model parameters to a model a process known as least
Square setting. The Kalman filter is commonly known as a recursive least squares (RLS) fitter.
Drawing similarities to the chi-square merit function will give a different perspective on what the
Kalman filter is doing.
The chi-square merit function is;
X= ∑[zi-h(ai,x)/ δ]^2
where; zi is the measured value; hi is the data model with parameters x, assumed linear in a; δ i is
the variance associated with the measured value.
The optimal set of parameters can then be defined as that which minimizes the above function.
Expanding out the variance gives;
x^2=∑ 1/ δi δi[zi-h(ai ,x)]^2
Representing the chi-square in vector form and using notation from the earlier Kalman
derivation;
𝑥𝑘2 = [zk- h (a, 𝑥𝑘 )] 𝑅 −1 [zk- h (a, 𝑥𝑘 )]^T
where; R1 is the matrix of inverse squared variances, i.e. 1/ δi δi.
Fig 3-1 Kalman Filter Recursive Algorithm
18
3 Third Chapter
The above merit function is the merit function associated with the latest, kth, measurement and
provides a measure of how accurately the model predicted this measurement.
Given that the inverse model covariance matrix is known up to time k, the merit function up to
time k may be re-written as;
𝑥𝑘2−1 = (𝑥𝑘−1 - 𝑥̂𝑘−1 ) 𝑃𝑘𝑡−1
(𝑥𝑘−1 - )^T
−1
It is assumed that the estimated model parameters are a close approximation to the actual model
parameters Therefore it may be assumed that the derivatives of the actual model and the
estimated model are the same Further, for a system which is linear in a the model derivative is
constant and may be written as;
𝛥𝑥 ℎ(a, 𝑥𝑘 ) = 𝛥𝑥 ℎ(a, 𝑥̂𝑘 ) = 𝐻
Substituting this into equation 11.39 gives;
d 𝑥 2 /dx= 2𝑃𝑘′−1 𝛥𝑥𝑘 + 2𝐻 𝑇 𝑅−1 𝐻𝛥𝑥𝑘 − 2𝐻 𝑇 𝑅−1[ 𝑍𝑘 − ℎ(𝑎, 𝑥̂𝑘 )]
d 𝑥 2 /dx=2[𝑃𝑘′−1 + 𝐻 𝑇 𝑅 −1 𝐻] 𝛥𝑥𝑘 − 2𝐻 𝑇 𝑅 −1 [𝑍𝑘 − ℎ(𝑎, 𝑥̂𝑘 )]
−1
𝛥𝑥𝑘 = [𝑃𝑘′−1 + 𝐻 𝑇 𝑅−1 𝐻] 𝐻 𝑇 𝑅−1 [𝑍𝑘 − ℎ(𝑎, 𝑥̂𝑘 )]
−1
X=𝑥̂𝑘 + [𝑃𝑘′−1 + 𝐻 𝑇 𝑅−1 𝐻] 𝐻𝑇 𝑅−1 [𝑍𝑘 − ℎ(𝑎, 𝑥̂𝑘 )]
Comparison of equation allows the gain, Kk to be identified as;
−1
𝑘𝑘 = [𝑃𝑘′−1 + 𝐻 𝑇 𝑅−1 𝐻] 𝐻𝑇 𝑅−1
Giving a parameter update equation of the form;
𝑥𝑘 = 𝑥̂𝑘 + 𝑘𝑘 [𝑍𝑘 − ℎ(𝑎, 𝑥̂𝑘 )]
Equation is identical to and describes the improvement of the parameter estimate using the error
between measured and model projected values.
19
4 Fourth Chapter
PID Controller
A proportional–integral–derivative controller (PID controller or three-term controller) is a control
loop mechanism employing feedback that is widely used in industrial control systems and a
variety of other applications requiring continuously modulated control. A PID controller
continuously calculates an error value as the difference between a desired set-point (SP) and a
measured process variable (PV) and applies a correction based on proportional, integral,
and derivative terms (denoted P, I, and D respectively), hence the name.
In practical terms it automatically applies an accurate and responsive correction to a control
function. An everyday example is the cruise control on a car, where ascending a hill would lower
speed if only constant engine power were applied. The controller's PID algorithm restores the
measured speed to the desired speed with minimal delay and overshoot by increasing the power
output of the engine.
PID Equations
PID controllers are used in more than 95% of closed-loop industrial processes.
+
KP (1+1/TiS+TdS)
Fig 4-1 Block Diagram of PID Controller
U(S)/E(S) = GPID(S) = KP (1+1/TiS+TdS)
Typical steps for designing a PID Controller are:
 Determine what characteristic of system needs to be improved.
 Use KP to reduce the rise time.
 Use KD to reduce the overshoot and settling time.
 Use KI to eliminate the steady state error.
20
Plant
5 Fifth Chapter
The effects of increasing the each controller parameters KP, kI, kD.
Response
Rise Time
Over Shoot
Settling Time
Steady state Error
Kp
Decrease
Increase
Minor Change
Decrease
Ki
Decrease
Increase
Increase
Eliminate
Kd
Minor Change
Decrease
Decrease
No change
Table 4-1 Effect of Parameter
4.1.1 Applicability
The use of the PID algorithm does not guarantee optimal control of the system or its control
stability. Situations may occur where there are excessive delays: the measurement of the process
value is delayed, or the control action does not apply quickly enough. In these cases lead–lag
compensation is required to be effective. The response of the controller can be described in terms
of its responsiveness to an error, the degree to which the system overshoots a set point, and the
degree of any system oscillation.
4.1.2 Tuning a PID Controller
System model is required for techniques we have studied (Root Locus, Bode Plots).System
models may be determined using system identification techniques, such measuring output for an
impulse or step input. Traditional control design methods are less appropriate if the system is
unknown. Most PID controllers are tuned on-site due to machine and process variations. The
theoretical calculations for an initial setting of PID parameters can be by-passed using a few
tuning rules.
21
4 Fourth Chapter
How do the PID parameters affect system dynamics?
4 major characteristics of the closed-loop step response.

Rise Time:
The time it takes for the plant output y to rise beyond 90% of the desired level for the
first time.

Overshoot:
How much the peak level is higher than the steady state, normalized against the steady
state?

Settling Time:
The time it takes for the system to converge to its steady state.

Steady-state Error:
The difference between the steady state output and the desired output.
Fig 4-2 PID Tuning
22
5 Fifth Chapter
Software Work
In this chapter we will discuss the MATLAB Code, Simulink work and Output waveforms.
MATLAB Code
% Missile Guidance System
clc; close all; clear all;
format short;
A = [-1.064 1.000; 290.26 0.00];
B = [-0.25; -331.40];
C = [-123.24 0.00; 0.00 1.00];
D = [-13.51; 0.00];
states = {'AoA', 'q'};
inputs = {'delta_c'};
outputs = {'Az', 'q'};
sys = ss(A,B,C,D,'statename',states,...
'inputname',inputs,...
'outputname',outputs);
%TF
TFs = tf(sys);
TF = TFs(2,1);
disp(pole(TF));
%LQR Weight Matrices
Q = [0.1 0; 0 0.1];
R = 0.5;
%LQR Gain
[K,S,e] = lqr(A,B,Q,R);
23
4 Fourth Chapter
fprintf('eigenvalues of A-BK\n');
disp(eig(A-B*K));
fprintf('Feedback gain K');
disp(K)
%Closed Loop System
Acl = A-B*K;
Bcl = B
syscl = ss(Acl,Bcl,C,D,'statename',states,...
'inputname',inputs,...
'outputname',outputs);
%TF Close loop
TF = tf(syscl);
TFc = TF(2,1);
%LQG Kalman Filter Design
G = eye(2);
H = 0*eye(2 );
%Kalman Q,R noise matrices
Qbar = diag(0.00015*ones(1,2));
Rbar = diag(0.55*ones(1,2));
%Define noisy system
sys_n = ss(A,[B G],C,[D,H]);
[kest,L,P] = kalman(sys_n,Qbar,Rbar,0);
%Kalman gain observer closed loop
Aob = A-L*C;
%Display observer eigenvalues
fprintf('observer eigenvalues\n');
24
5 Fifth Chapter
disp(eig(Aob));
%Noise time constant (I choose)
dT1 = 0.5;
dT2 = 0.4;
% Missile parameters
R = 6371e3; %earth radius
vel = 1021.08; %speed (m/s)
m2f = 3.2811; %meters to feet
%target location
LAT_TARGET = 34.6588;
LON_TARGET = -118.769745;
ELEV_TARGET = 795; %m - MSL
%Initial location
LAT_INIT = 34.2329;
LON_INIT = -119.4573;
ELEV_INIT = 10000; %m p-MSL
%Obstacle location
LAT_OBS = 34.61916;
LON_OBS = -118.8429;
d2r = pi/180; % degree to radians
25
4 Fourth Chapter
%convert to radians
l1 = LAT_INIT*d2r;
u1 = LON_INIT*d2r;
l2 = LAT_TARGET*d2r;
u2 = LON_TARGET*d2r;
dl = l2-l1;
du = u2-u1;
%haversine formula
a = sin(dl/2)^2 + cos(l1)*cos(l2)*sin(du/2)^2;
c = 2*atan2(sqrt(a),sqrt(1-a));
d = R*c; %horizontal distance (in m)
%Initial range (Pythagoras theorem)
r = sqrt(d^2+(ELEV_TARGET-ELEV_INIT)^2);
%Initial azimuth
yaw_init = azimuth(LAT_INIT,LON_INIT,LAT_TARGET,LON_TARGET);
yaw = yaw_init*d2r;
%Initial Flight path angle
dh = abs(ELEV_TARGET-ELEV_INIT);
FPA_INIT = atan(dh/d); %rad
26
5 Fifth Chapter
5.1.1 MATLAB Results
Fig 5-1 Command Window Result
Fig 5-2 Workspace Result
27
4 Fourth Chapter
Fig 5-3 Workspace Result
Simulink Work
Fig 5-4
Simulink Model
28
5 Fifth Chapter
5.2.1 Airframe + Missile LQG Controller
Fig 5-5
Airframe + Missile LQG Controller
5.2.1.1 Kalman Filter Implementation + LQR Controller
Fig 5-6
Kalman Filter Implementation + LQR Controller
29
4 Fourth Chapter
5.2.1.2 State Space Implementation
Fig 5-7
State Space Implementation
5.2.2 PID Controller + Saturation Block
Fig 5-8
PID Controller + Saturation Block
30
5 Fifth Chapter
Fig 5-9 Block Parameters of PID Controller
Fig 5-10 Block Parameters of Saturation Block
31
4 Fourth Chapter
5.2.3 Coordinates Calculation
Fig 5-11 Coordinates Calculation Block
Fig 5-12
Block Parameters of POS > LLA
32
5 Fifth Chapter
5.2.4 Guidance Calculator
Fig 5-13
Guidance Calculator
5.2.4.1 Guidance Code
function [FPA,RNG,D_OBS,ALT,warn] = fcn(OBS, CURRENT, TARGET)
%#codegen
R = 6371e3;
d2r = pi/180;
%OBSTACLE LOCATION
LAT_OBS = OBS(1);
LON_OBS = OBS(2);
%OBSTACLE THRESHOLD (ZONE)
thres = 2000; %2km
%TARGET LOCATION
LAT_TARGET = TARGET(1);
LON_TARGET = TARGET(2);
ELEV_TARGET = TARGET(3); %m- MSL
%CURRENT LOCATION
ELEV_CUR = CURRENT(1);
LAT_CUR = CURRENT(2);
LON_CUR = CURRENT(3);
%% distance to target
l1 = LAT_CUR*d2r;
u1 = LON_CUR*d2r;
l2 = LAT_TARGET*d2r;
u2 = LON_TARGET*d2r;
dh = abs(ELEV_TARGET-ELEV_CUR);
33
4 Fourth Chapter
dl = l2-l1;
du = u2-u1;
%haversine formula
a = sin(dl/2)^2 + cos(l1)*cos(l2)*sin(du/2)^2;
c = 2*atan2(sqrt(a),sqrt(1-a));
d = R*c; %horizontal distance (in m)
%% distance from the obstacle
l3 = LAT_OBS*d2r;
u3 = LON_OBS*d2r;
dl = l3-l1;
du = u3-u1;
%haversine formula
a = sin(dl/2)^2 + cos(l1)*cos(l2)*sin(du/2)^2;
c = 2*atan2(sqrt(a),sqrt(1-a));
d_obs = R*c; %horizontal distance (in m)
%% current range (from target) - range > distance
range = sqrt(d^2+dh^2);
%% calculate command flight path setpoint based on d_obs, range
if abs(d_obs)>=thres
w = 0;
FPA_CMD = atan(dh/d);
else
%when in the obstacle zone, triger warning
w = 1;
FPA_CMD = 0;
end
%output variables
FPA = FPA_CMD;
RNG = range;
D_OBS = d_obs;
warn = w;
ALT = ELEV_CUR;
34
5 Fifth Chapter
How our MATLAB and SIMULINK Model Works?
In this chapter we will discuss about How our MATLAB and SIMULINK Model Works.
MATLAB Code Explanation
In this section we will explain the MATLAB code.
Fig 6-1 Block Diagram of MATLAB Code
6.1.1 Open Loop System
An Open-loop system, also referred to as non-feedback system, is a type of continuous control
system in which the output has no influence or effect on the control action of the input signal.
x˙ = Ax + Bu
y = Cx + Du
35
4 Fourth Chapter
6.1.2 LQR Controller
Modern control theory has made a significant impact on the aircraft industry in recent years.
LQR is a method in modern control theory that used state-space approach to analyze such a
system. Using state space methods it is relatively simple to work with a multi-output system.
Fig 6-2
Block Diagram of LQR Controller
Xhat = Ax + Bu
The feedback control law that minimizes the value of the cost is:
u = -kx
Where k is the controller gain that is determined by following way
K = R-1BT P
and here P is the unique positive definite solution to Algebric Riccati Equation (ARE). The ARE
is as follows
AT P + PA - PBR-1BT P + Q = 0
6.1.3 Close Loop System
A Closed-loop Control System, also known as a feedback control system is a control system
which uses the concept of an open loop system as its forward path but has one or more feedback
loops (hence its name) or paths between its output and its input.
Acl = A-B*K
6.1.4 Kalman Filter
Kalman filter can be used to estimate a system state when it cannot be measured directly. Optimal
estimate algorithm that predicts the parameter i.e location, speed and direction.
Consider the discrete plant
x(n+1) = Ax(n)+B(u(n)+w(n))
y(n) = Cx(n)
with Additive White Gaussian Noise w(n) on the input u(n) and data.
Yv(n) = Cx(n) + v(n)
Where v(n) is some Gaussian Noise.
36
5 Fifth Chapter
6.1.5 Haversine Formula
The haversine formula determines the great-circle distance between two points on a sphere given
their longitudes and latitudes
Simulink Model Explanation
Fig 6-3 Block Diagram of SIMULINK Model
6.2.1 Angle Classification

Flight Path Angle:
The angle between the inertial reference axis and the missile velocity vector is called the
flight-path angle gamma.
 Angle of Attack:
The angle from the velocity vector to the missile centerline is called the angle of attack
(AOA) alpha.
 Pitch Angle:
The angle from the inertial reference to the missile centerline is called the pitch angle theta.
Fig 6-4
Angle Classification
37
4 Fourth Chapter
6.2.2 LQG Controller
Linear Quadratic Gaussian (LQG) control is a modern state space technique for designing
optimal dynamic regulators. It enables you to trade off regulation performance and control effort,
and to take into account process and measurement noise.
Fig 6-5
Block Diagram of LQG Controller
This regulator has state-space equations
The goal is to regulate the output y and zero. The plant is subject to disturbances and is driven by
controls.
Where w and v are white noise.
6.2.3 POS to LLA Block
This block converts x,y,z coordinates to Latitude and Longitude.
38
References
Conclusions and Future Work
Conclusions
In this dissertation, the exploration of ……. is presented. …... The main research content
and innovations of the thesis are posited as following:
1.
Based on …..
2.
In order to improve the performance of ……..
3.
The following approaches were used…
4.
General analytical method[5], Symmetrical hemisphere approach and conventional DH
method were applied for kinematic computations based on simplification from structural
geometry as well as motion symmetry of the mechanism. The simulated results of the
kinematics solutions in MATLAB and MapleSIM are applied on the prototype model and
the results were evaluated experimentally.
5.
A novel design of…..
6.
Methodology and experimental results were.....
Future Work and Recommendations
Various novel designs of …… were presented and described in the dissertation. The scope
for future work in the design aspect includes the following recommendations:

The design procedure has to be revisited and certain ...

Using software tools, structure optimization ….

Elimination of undesired ...

Extensive dynamic simulations ..

Simulations are required to[6] be established using software techniques ...

Development of the prototype …
39
References
References
[1]
Brian A. White, and Antonios Tsourdos, MODERN MISSILE GUIDANCE DESIGN.
[2]
Scott M. Bezick, Alan J. Pue, and Charles M. Patzelt, Inertial Navigation for Guided Missile
Systems.
[3]
Britting, K. R., Inertial Navigation Systems Analysis, John Wiley & Sons, Hoboken.
[4]
Paul B. Jackson, Overview of Missile Flight Control Systems.
[5]
Labane Chrif a, Zemalache Meguenni Kaddac,a*,Aircraft Control System Using LQG and
LQR Controller with Optimal Estimation-Kalman Filter Design.
[6]
M. Grewal and A. Andrews, “Kalman Filtering Theory and Practice Using MATLAB”,
(Second ed.), New York, NY USA: John Wiley & Sons, Inc., 2001.
[7]
D. Simon, “Optimal state estimation: Kalman, H-infinity, and nonlinear approaches”, John
Wiley & Sons, 2006.
[8]
Curtis P. Mracek ∗ and D. Brett Ridgely, Missile Longitudinal Autopilots: Connections
between Optimal Control and Classical Topologies.
[9]
Paul Zarchan, Tactical and Strategic Missile Guidance, Fourth Edition.
[10]
F.L. Lewis and V.L. Syrmos, “Optimal Control, Second Edition”, John Wiley & Sons, Inc.
[11]
Taeklim Kim and Tae-Hyoung Park, Extended Kalman Filter (EKF) Design for Vehicle
Position Tracking Using Reliability Function of Radar and Lidar.
[12]
Huang, L.; Zhe, T.; We, J.; Pei, C.; Chen, D. Robust Inter-Vehicle Distance Estimation Method
based on Monocular Vision. IEEE Access 2019, 7, 46059–46070.
[13]
Chen, B.; Pei, X.; Chen, Z. Research on Target Detection based on Distributed Track Fusion
for Intelligent Vehicles. Sensors 2020, 20, 56.
[14]
Omar, R.; Garcia, C.; Aycard, O. Multiple Sensor Fusion and Classification for Moving Object
Detection and Tracking. IEEE Trans. Intell. Transp. Syst. 2016, 17, 525–534.
[15]
Kai Yang et al 2021 J. Phys.: Conf. Ser. 1721 012038, Design and Implementation of
Trajectory Planning Algorithm for UAV Borne Air-to-Surface Missile.
[16]
Zhang Yilin,Jiang Qi,et. 2013 Tactical Missile Technology.
[17]
Meng Xiuyun.Principles of Missile Guidance and Control System Beijing.
[18]
Ma Fei,Ma Qinghua,Yang Kai 2015 Journal of Projectiles, Rockets, Missiles and Guidance .
40