Embed Size (px)
Citation preview
RANGE TO-GO ESTIMATION FOR A TACTICAL MISSILE WITH A PASSIVE SEEKER
A THESIS SUBMITTED TO THE GRADUATESCHOOL OF NATURAL AND APPLIED SCIENCES
OF THE MIDDLE EAST TECHNICAL UNIVERSITY
BY
SUZAN KALE GÜVENÇ
IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR
THE DEGREE OF MASTER OF SCIENCE IN
AEROSPACE ENGINEERING
SEPTEMBER 2015
Approval of the thesis:
RANGE TO-GO ESTIMATION FOR A TACTICAL MISSILE
WITH A PASSIVE SEEKER
submitted by SUZAN KALE GÜVENÇ in partial fulfillment of the requirements for the degree of Master of Science in Aerospace Engineering Department, Middle East Technical University by, Prof. Dr. Gülbin Dural Ünver Dean, Graduate School of Natural and Applied Sciences ________________ Prof. Dr. Ozan Tekinalp Head of Department, Aerospace Engineering ________________ Asst. Prof. Dr. Ali Türker Kutay Supervisor, Aerospace Engineering Dept., METU ________________ Examining Committee Members Prof. Dr. Ozan Tekinalp Aerospace Engineering Dept., METU ________________ Asst. Prof. Dr. Ali Türker Kutay Aerospace Engineering Dept., METU ________________ Prof. Dr. Mübeccel Demirekler Electrical and Electronics Engineering Dept., METU ________________ Asst. Prof. Dr. İlkay Yavrucuk Aerospace Engineering Dept., METU ________________ Asst. Prof. Dr. Yakup Özkazanç Electrical and Electronics Engineering Dept., HACETTEPE ________________
Date: 11.09.2015
iv
I hereby declare that all information in this document has been obtained and
presented in accordance with academic rules and ethical conduct. I also declare
that, as required by these rules and conduct, I have fully cited and referenced
all material and results that are not original to this work.
Name, Last Name : Suzan Kale Güvenç
Signature :
v
ABSTRACT
RANGE TO-GO ESTIMATION FOR A TACTICAL MISSILE
WITH A PASSIVE SEEKER
Kale Güvenç, Suzan
M.S., Department of Aerospace Engineering
Supervisor: Asst. Prof. Dr. A. Türker Kutay
September 2015, 100 pages
Throughout literature, tending to replace or improve Proportional Navigation
Guidance, advanced guidance laws have been proposed. Most of these laws require
the range between the target and the missile (range-to-go) which cannot be measured
by passive seekers. The main objective of this thesis is to perform the estimation of
range to a stationary target for a tactical missile equipped with a passive seeker.
Two different approaches are investigated: the Method of Triangulation and the
Extended Kalman Filter (EKF). The Method of Triangulation which is employed in a
number of fields is used in this work to calculate the range between a stationary
target and a moving missile. The sensitivity of this method to measurement errors in
Inertial Measurement Unit (IMU) and seeker is studied. It is discovered that the error
in range depends on the trajectory of the missile. This relationship is utilized to
adjust the Constant Bearing Midcourse trajectory so that the range error is reduced to
a certain level. Moreover, from sensitivity analysis, the problem of "geometric
dilution" is identified and controlled to obtain a desired accuracy in range. Secondly,
the estimation of range-to-go with an EKF is studied where the system model is
vi
formulated in terms of Modified Polar Coordinates (MSC) for 3D missile-target
geometry. The estimation is performed from LOS (line-of-sight) angle and LOS rate
measurements provided by the gimballed seeker. It is known that the performance of
the filter depends on the observability of the scenario. To help to improve the
performance of the filter when the observability is low, the EKF and the Method of
Triangulation are integrated. The integration is carried out by taking the range output
of the triangulation as one of the measurements provided to the EKF.
Keywords: Range-to-go Estimation, Method of Triangulation, Extended Kalman
Filter, Bearings Only Tracking, Sensitivity Analysis
vii
ÖZ
PASİF ARAYICILI TAKTİK BİR FÜZE İÇİN
KALAN MESAFE KESTİRİMİ
Kale Güvenç, Suzan
Yüksek Lisans, Havacılık ve Uzay Mühendisliği Bölümü
Tez Yöneticisi: Yrd. Doç. Dr. A. Türker Kutay
Eylül 2015, 100 sayfa
Literatürde, Saf Oransal Seyrüsefer Güdüm yöntemini iyileştirmek amacıyla,
gelişmiş güdüm kanunları önerilmiştir. Bu kanunlardan çoğu, füze ile hedef
arasındaki mesafeye (kalan mesafe) ihtiyaç duymaktadır. Ancak, bu bilgi pasif
arayıcılar tarafından ölçülememektedir. Bu çalışmadaki amaç, pasif arayıcılı taktik
bir füze ile sabit bir hedef arasındaki mesafenin kestirimini yapmaktır.
Bu tezde, iki farklı yöntem çalışılmıştır: Üçgen Metodu ve Geliştirilmiş Kalman
Filtresi. Birçok alanda kullanılan Üçgen Metodu bu çalışmada, hareket eden bir füze
ile sabit bir hedef arasındaki mesafenin hesaplanması için uygulanmıştır. Bu
metodun, Ataletsel Ölçüm Birimi (AÖB) ve arayıcı hatasına bağlı hassaslığı
incelenmiştir. Hassaslık analizi sonucunda, kalan mesafe hatasının füzenin
yörüngesine bağlı olduğu sonucu çıkarılmıştır. Bu ilişkiye göre Sabit Bakış Açısı
Güdümlü Arafaz yörüngesinin ayarlanmasıyla, kalan mesafe hatası belirli bir değerin
altına indirilmiştir. Buna ek olarak, "geometrik zayıflık" ile ilgili problem tespit
edilmiş ve istenen seviyede kalan mesafe doğruluğunu elde edilecek şekilde kontrol
altına alınmıştır. İkinci olarak, Geliştirilmiş Kalman Filtresi (GKF) ile kalan mesafe
viii
kestirimi yapılmıştır. Sistem Değiştirilmiş Polar Koordinatları cinsinden 3 boyutlu
füze-hedef geometrisi için modellenmiştir. Kestirim, gimballi arayıcı tarafından
sağlanan Görüş Hattı (GH) açısı ve GH açısal hız ölçümünü kullanılarak
yapılmaktadır. Kestirim performansının senaryonun gözlenebilirliğine bağlı olduğu
bilinmektedir. Gözlenebilirliğin düşük olduğu durumlarda filtrenin performasını
iyileştirmek amacıyla, GKF ile Üçgen Metodu entegre edilmiştir. Bu entegrasyon,
Üçgen Metodu tarafından hesaplanan kalan mesafenin Kalman Filtesine bir ölçüm
olarak alınacak şekilde modellenmesiyle mümkün olmuştur.
Anahtar Kelimeler: Kalan Mesafe Kestirimi, Üçgen Metodu, Genişletilmiş Kalman
Filtresi, Görüş Açısı Tabanlı Takip, Hassaslık Analizi
ix
ACKNOWLEGMENTS
I am very thankful to my supervisor Asst. Prof. Dr. Ali Türker Kutay for his
guidance, advice and helpful criticisms throughout the thesis. I wish to express my
sincere thanks to Prof. Dr. Mübeccel Demirekler for her valuable comments on the
subject of this thesis related to Bearing-Only-Tracking.
I am very thankful to my colleague and friend Gökcan Akalın for sharing his
invaluable experiences on the subject of my thesis.
I would like to forward my appreciation to my friends Naz Tuğçe Öveç and Evrim
Özten who contributed to my thesis with their continuous motivation and friendship.
Special thanks to my love for his endless support on all the matters that troubled me
and for having faith in me throughout my lengthy M.S. experience. Thanks to his
love and encouragement, this thesis is completed.
x
TABLE OF CONTENTS
ABSTRACT ................................................................................................................. v
ÖZ ............................................................................................................................... vii
ACKNOWLEGMENTS .............................................................................................. ix
TABLE OF CONTENTS ............................................................................................. x
LIST OF TABLES .................................................................................................... xiii
LIST OF FIGURES ................................................................................................... xiv
CHAPTERS
1. INTRODUCTION ................................................................................................. 1
1.1 Scope of this thesis ......................................................................................... 3
1.2 Literature Survey ............................................................................................ 5
1.3 Contributions .................................................................................................. 7
1.4 Outline ............................................................................................................ 7
2. MATHEMATICAL MODEL OF HOMING LOOP ............................................ 9
2.1 Assumptions ................................................................................................... 9
2.2 Reference Coordinate Frames ...................................................................... 10
2.3 Missile Dynamics ......................................................................................... 12
2.4 Target Model ................................................................................................ 12
2.5 Missile-Target Relative Kinematics ............................................................. 12
2.6 Navigation .................................................................................................... 14
2.7 Inertial Measurement Unit ........................................................................... 14
2.8 Seeker Model ............................................................................................... 15
3. METHOD OF TRIANGULATION .................................................................... 17
3.1 Method of Triangulation .............................................................................. 17
3.2 Triangulation Algorithm .............................................................................. 19
3.3 Sensitivity Analysis ...................................................................................... 21
3.3.1 Sensitivity to Accelerometer Measurement Error .............................. 23
xi
3.3.2 Sensitivity to Gyroscope Measurement Error .................................... 26
3.3.3 Sensitivity to Look Angle Measurement Error .................................. 29
3.3.4 Conclusion of Sensitivity Analysis .................................................... 33
3.4 Trajectory Design ......................................................................................... 34
3.4.1 Controlling the Range Error due to Accelerometer Bias .................... 35
3.4.2 Controlling the Range Error due to Gyroscope Bias .......................... 36
4. PASSIVE RANGE ESTIMATION .................................................................... 37
4.1 Extended Kalman Filter ............................................................................... 37
4.2 Passive Range Estimation ............................................................................ 39
4.2.1 System Model ..................................................................................... 40
4.2.2 Measurement Model ........................................................................... 43
4.2.3 Observability Issue ............................................................................. 44
4.2.4 Initialization of the Filter .................................................................... 45
4.3 Hybrid Range Estimation ............................................................................. 47
5. SIMULATIONS AND DISCUSSION ............................................................... 51
5.1 Sensitivity Relations of Method of Triangulation ....................................... 51
5.1.1 Validation of Sensitivity Relations ..................................................... 51
5.1.1.1 Validation of the Sensitivity to Accelerometer Bias Error .......... 53 5.1.1.2 Validation of the Sensitivity to Gyroscope Bias Error ................ 55 5.1.1.3 Validation of the Sensitivity to Look Angle Noise Error ............ 56
5.1.2 Trajectory Design According to Sensitivity Relations ....................... 59
5.2 Estimation Performance of Passive EKF ..................................................... 63
5.2.1 Effect of Observability on Filter Performance ................................... 63
5.2.2 Effect of Measurement Uncertainties on Filter Performance ............. 71
5.2.3 Effect of Initial Uncertainties on Filter Performance ......................... 75
5.3 Estimation Performance of Hybrid EKF ...................................................... 80
6. CONCLUSION ................................................................................................... 85
REFERENCES ........................................................................................................... 89
APPENDICES
A. PROOF OF xy=-1 ............................................................................................... 93
xii
B. CLOSED FORM SOLUTION OF CONSTANT BEARING GUIDANCE ....... 97
C. JACOBIANS OF THE EXTENDED KALMAN FILTER ................................ 99
xiii
LIST OF TABLES
TABLES
Table 3.1- Summary of Sensitivity Analysis ............................................................. 33
Table 4.1- Extended Kalman Filter Algorithm .......................................................... 38
Table 5.1- Cases of Parallax Threshold ..................................................................... 56
Table 5.2- Trajectory Design Parameters................................................................... 59
Table 5.3- Monte Carlo Parameters for Accelerometer Bias Error ........................... 60
Table 5.4- Monte Carlo Parameters for Gyroscope Bias Error .................................. 60
Table 5.5- Simulation Parameters for Scenario A & B .............................................. 65
Table 5.6- EKF Parameters ........................................................................................ 65
Table 5.7- LOS Rate and LOS Angle Measurement Noises ...................................... 72
Table 5.8- Initialization of o or r/& Uncertainty ............................................................ 75
Table 5.9- Initialization of rLOR Uncertainties ........................................................... 78
xiv
LIST OF FIGURES
FIGURES
Figure 1.1- Missile Homing Loop ................................................................................ 2
Figure 2.1- Fe , Fb and Flos Frames ............................................................................. 10
Figure 2.2- Flight Path-Angles ................................................................................... 11
Figure 2.3- Missile-Target Relative Geometry .......................................................... 11
Figure 2.4- Look Angles ............................................................................................ 13
Figure 2.5- Seeker Types [6] ...................................................................................... 15
Figure 3.1- Missile-Target Triangular Geometry ....................................................... 18
Figure 3.2- Triangulation Algorithm .......................................................................... 20
Figure 3.3- Error Sources ........................................................................................... 21
Figure 3.4- Planar Triangular Geometry .................................................................... 22
Figure 3.5- Planar Missile-Target Geometry ............................................................. 27
Figure 3.6- Minimum Range Accuracy ...................................................................... 32
Figure 3.7- Parallax Threshold for 10 % Desired Error ............................................. 32
Figure 3.8- Constant Bearing Guidance ..................................................................... 34
Figure 4.1- Hybrid Estimation Algorithm .................................................................. 49
Figure 5.1- Architecture of the Simulation ................................................................ 52
Figure 5.2- Trajectory and LOS rate .......................................................................... 52
Figure 5.3- Range Error due to Accelerometer Bias .................................................. 53
Figure 5.4- Time Step (T) ........................................................................................... 54
Figure 5.5- Difference between the results obtained from (3.13) .............................. 54
Figure 5.6- Range Error due to Gyroscope Bias ........................................................ 55
Figure 5.7- Difference between the result obtained from (3.22) ................................ 56
Figure 5.8- Std. of True Percentage of Range Error due to Seeker Noise ................. 57
Figure 5.9- Look Angle and angle a .......................................................................... 58
Figure 5.10- Comparison of Std. obtained from (3.28) and True Percentage Error
(solid lines) ................................................................................................................. 58
xv
Figure 5.11- Constant Bearing Trajectories for case 1 & 2 ....................................... 59
Figure 5.12- Monte Carlo Output of Range Error due to Accelerometer Bias .......... 61
Figure 5.13- Monte Carlo Output of Per. Range Error due to Gyroscope Bias ......... 62
Figure 5.14- Monte Carlo Output of Range Error due to Gyroscope Bias ................ 62
Figure 5.15- Trajectory of Scenario A & B ............................................................... 64
Figure 5.16- Acceleration Command in Scenario B .................................................. 64
Figure 5.17- Range Estimation (Sc A) ....................................................................... 66
Figure 5.18- Kalman Gain of state 1/r (Sc A) ........................................................... 66
Figure 5.19- Estimation Error in 1/r (Sc A) .............................................................. 67
Figure 5.20- Range Estimation (Sc B) ...................................................................... 68
Figure 5.21- Estimation Error in 1/r (Sc B) ............................................................... 68
Figure 5.22- Kalman Gain of state 1/r (Sc B) ........................................................... 69
Figure 5.23- Range Rate over Range Estimation for Sc A & B ................................ 70
Figure 5.24- LOS rate profiles of Sc A & B .............................................................. 70
Figure 5.25- Estimation Error in /r r& of Sc A & B .................................................. 71
Figure 5.26- ω2 and elλ components of K(7,:) ........................................................... 73
Figure 5.27- Filter std of 1/r ....................................................................................... 74
Figure 5.28- ω2 and elλ components of K(6,:) ............................................................ 74
Figure 5.29- Filter std. of r r/& .................................................................................. 75
Figure 5.30- Estimation Error in /r r& for Init 1-2 ..................................................... 76
Figure 5.31- ω2 and elλ components of K(6,:) for Init 1-2 .......................................... 77
Figure 5.32- Range Rate over Range Estimation for Init 1-2 .................................... 77
Figure 5.33- Range Estimation .................................................................................. 78
Figure 5.34- Estimation Error in 1/r .......................................................................... 79
Figure 5.35- Range Rate over Range Estimation ....................................................... 80
Figure 5.36- Estimation Error in /r r& ....................................................................... 80
Figure 5.37- Range Estimation (Sc A) ....................................................................... 81
Figure 5.38- Estimation Error in 1/ r (Sc A) .............................................................. 82
Figure 5.39- Estimation Error in r (Sc A) .................................................................. 82
xvi
Figure 5.40- Range Estimation (Sc B) ....................................................................... 83
Figure 5.41- Estimation Error in r (Sc B) .................................................................. 84
Figure 5.42- Estimation Error in 1/r (Sc B) ............................................................... 84
Figure 8.1- Displacement and Velocity Vector .......................................................... 94
1
CHAPTER 1
1. INTRODUCTION
The concept of missiles (guided projectiles) originated sometime during World War
I, from the idea of using remote controlled airplanes for the bombardments of enemy
targets [2]. With the aid of the developments made in the area of electronics, the first
missiles were designed in 1950s.
Depending on the operational range, missiles can be divided into two categories;
strategic ballistic missiles and tactical missiles. Strategic missiles, which are
designed to operate much longer distances than tactical missiles, are guided inertially
to intercept a stationary target whose location is known. Tactical missiles, on the
other hand, have the capability to intercept maneuvering or stationary targets with
unknown location. Therefore, unlike strategic missiles, tactical missiles require the
skill of sensing the target motion in real time. With the measurements from an
onboard sensor (namely a “seeker“), tactical missiles are able to track the targets and
adjust its course to achieve the interception.
The performance of each of its subsystems such as propulsion, aerodynamics,
Control Actuation System (CAS), missile computer algorithms, measurement units
etc. determines the overall performance of a missile. For a tactical missile, one of the
most crucial subsystem is the guidance algorithm implemented on the missile
computer. The task of the guidance algorithm is to ensure that the missile meets its
operational and performance requirements by taking the performance of each
2
subsystem into account. In the simplest form, a missile guidance (homing) loop are
illustrated in Figure 1.1.
Figure 1.1- Missile Homing Loop
Commonly, the homing loop of a tactical missile contains at least two sensor units;
an inertial measurement unit (IMU) and a seeker. An IMU is composed of three
accelerometers and three gyroscopes, which measure respectively translational
acceleration and angular velocity of the missile with respect to a non-rotating inertial
frame. The seeker mounted at the nose of the missile is responsible for sensing the
relative motion of the target with respect to the missile.
It is the navigation, guidance and autopilot algorithms implemented on the missile
computer which differs a missile from a rocket. Navigation algorithms integrate IMU
measurements and compute velocity, position, attitude angles, etc. to support
guidance and autopilot algorithms. These outputs together with the measurements
provided by the seeker are then used to mechanize the guidance algorithm. In the
sense of guidance, the flight of a tactical missile can be partitioned into two major
phases; midcourse and terminal phase. Midcourse phase is usually defined as the
period before the seeker is able to acquire information about the target. However, for
some applications this phase continues also after the seeker lock-on until some
conditions with regard to the kinetic energy and/or a desirable relative geometry such
3
as an appropriate attitude prior to terminal phase is achieved [9]. In general, the
primary aim of midcourse guidance is to deliver the missile to the vicinity of the
target by using navigational/seeker information and for some types of missiles (e.g.
surface-to-air, air-to-air) with the help of an additional instrument such as a radar.
The terminal phase is the last and most crucial phase of the flight. In this phase, by
using the information provided by the seeker the missile homes in on the target until
intercept occurs and the missile warhead is detonated. Depending on the guidance
strategy that is applied in midcourse and terminal phase, the command produced by
the guidance algorithm can be a desired acceleration, attitude angle/rate, flight path
angle/rate etc.
The closed loop autopilot dynamics is a minor loop inside the main guidance loop.
The role of the autopilot is to track the guidance command by ensuring a stable
flight. Upon receiving commands from guidance algorithm, autopilot executes fin
deflection commands to the appropriate aerodynamic and/or thrust actuation systems
which in turn forces the missile to track the guidance commands. The resulting
motion alters the missile-target relative geometry, which is sensed by the seeker and
are used to generate the next set of guidance and autopilot commands. Missile
homing loop continues to operate until the interception is accomplished.
1.1 Scope of this thesis
The most important objective of a guided missile is to hit the target. This is
considered as the primary requirement imposed on the terminal guidance algorithms.
The well-known Proportional Navigation Guidance law (PNG), which is frequently
used throughout literature and real applications due to its simplicity to implement, is
sufficient to fulfill this goal especially against stationary and constant speed targets
([1],[2]). However, future tactical systems will be developed to meet new and more
involved requirements. Therefore, tending to replace or improve PNG, advanced
guidance laws have been proposed. Those laws aim to satisfy some specific
requirements which cannot be achieved otherwise with the conventional PNG.
4
Impact-angle-control, impact-time-control or minimum-time-control are good
examples of advanced guidance objectives. Considering damage assessment against
armored tanks, top-attack is desired in order to hit the enemy tank at the roof section
where the armor is the weakest [5]. In the case of torpedoes or anti-ship missiles, the
achievement of a proper impact angle is also important to insure a high killing
probability [4]. Kim et al. [4] and Jeong et al. [3], proposed a biased variation of the
conventional PNG in order to achieve the attack at any desired angle. Here, the bias
term is a function of some measured/calculated navigational information and the
distance between the missile and the target. Another important feature that improves
the warhead effectiveness is the impact time which is particularly important for salvo
attacks, where multiple missiles must intercept the target simultaneously [18]. The
impact-time-control guidance, proposed by Jeon et al. [15], enables the missile-target
interception to be realized at a designated impact time. Moreover, there are also
studies that aim to control both the impact time and the impact angle as proposed in
Ref. [17] and [18]. A common property of impact-time and minimum-time-control
(such as proposed in [19]) strategies is that the time-to-go information is required. In
fact, the time-to-go which cannot be measured by any device is particularly essential
for the guidance laws derived from optimal control theory [21]. Throughout the
literature related to time-to-go based guidance laws, the estimation of time-to-go
(such as proposed in Ref [20]) is performed on the assumption that the range to the
target (called as "range-to-go") is known or measured. To conclude, the price to be
paid for more advanced guidance laws is that more information are required for their
successful implementation compared to the simple PNG [1].
While active or semi-active seekers are able to measure the range, passive seekers
are not. However, with a passive seeker the missile is less likely detected by the
target and the domain of applicable countermeasures are restricted. In that sense,
such systems are more advantageous. The focus of this work is to perform the
estimation of range-to-go to a stationary target from the measurements provided by a
passive seeker. Two different methods for obtaining the range are investigated: the
5
Triangulation and the Extended Kalman Filter. The following section presents the
literature survey related to these methods.
1.2 Literature Survey
Generally, there are two approaches for finding the distance between two objects
from passive measurements: "localization of a stationary target" and " bearings-only
tracking".
Localization of a stationary object has several civilian and military application areas
such as geodetic surveying, submarine localization by sonar, optical range finder, etc
[11]. For single-sensor case where the observer moves, from the bearing
measurement acquired at different points along the trajectory with known relative
distances, the range can be computed. This technique is referred to as Method of
Triangulation. In the absence of measurement errors of bearing readings and
observer locations, the bearing lines will intercept exactly at the true target location.
Since the method have no information about the stochastical properties of the
measurements, it is greatly affected by the measurement errors which directly
propagate into the calculation. It is a disadvantage of this method and if used in an
application it is important to check how the measurement errors in the system
propagate into the calculation. In order to obtain an optimal solution, the information
about the noise statics of the measurements should be taken into account [25]. For
this purpose, the problem is formulated as a Least Square (LS) problem in Ref.[26].
Although the pseudolinear LS estimator is easy to implement, a major drawback is
the large estimation bias due to the correlation between the measurement matrix and
the bearing noise [25].
The bearing-only-tracking is studied in a variety of important applications [22]. The
aim is to estimate the position and velocity of the target using bearing measurements
obtained from a passive seeker. One important feature of bearing only tracking is that
the estimation problem is known to be unobservable prior to observer maneuver [22].
6
Since the problem is nonlinear, it requires nonlinear filtering techniques for its
solution. The Extended Kalman Filter in Cartesian coordinates were used extensively
for this purpose due to its simple implementation. The system model of this filter is
linear and all the nonlinearities are embedded in the measurement model. However,
in Ref. [22] it is shown that, when formulated in Cartesian coordinates, the filter
exhibits unstable behavior characteristics. The reason is that, due to bearing
estimation errors the observability matrix attains full rank, even though the observer
is not maneuvering. This phenomenon is called "false observability", which means
that under unobservable conditions the filter attempts to estimate all states. This
causes the eigenvalues of the covariance matrix to change rapidly, i.e. covariance
matrix exhibits premature collapse. Even in the absence of bearing estimation errors,
when unequal variances are assigned to velocity and position states, (for non-
maneuvering observer case) the observability matrix becomes full rank as well.
Additionally, this initialization procedure which is chosen for practical reasons leads
to ill-conditioning of the covariance matrix. In Ref. [23], the system is formulated in
Modified Spherical Coordinates (MSC). The new model automatically decouples the
observable and unobservable components of the estimated state vector preventing the
covariance collapse and matrix ill-conditioning. Moreover, "false observability"
phenomenon do not occur and the filter behaves as predicted by observability theory
so that the range cannot be estimated without own-ship maneuver.
It is known that the performance of the filter depends on the observability of the
scenario. In fact, it is this feature which differs the estimation with a dynamical
model based filter from the classical triangulation method [23]. However, in the real
scenario, it is not always possible to ensure an observable trajectory. In this thesis, to
improve the performance of the filter for the cases where the observability is low, the
Extended Kalman Filter (EKF) with modified corrdinates and the Triangulation are
integrated.
7
1.3 Contributions
The contributions of this work can be summarized as follows:
• Although the method of triangulation is mainly referred in literature, it has not
been documented for a missile application where the interest is to obtain the
range between a missile and a target.
• The sensitivity of triangulation to measurement errors in IMU and seeker is
studied and expressions that relate the uncertainty of range to the uncertainties in
the measurements are obtained.
• In order to reduce the range error to a desired level, a sample trajectory is
designed. Moreover, the problem of "geometric dilution" is explained and
precautions are taken to circumvent this problem.
• The mathematical model along with the initialization procedure of EKF for 3D
missile-target geometry is described in detail.
• The EKF and the method of triangulation are integrated. As a result of this
integration, the accuracy of range estimation is improved.
1.4 Outline
In Chapter 2, the mathematical model of the homing loop is presented. The
assumptions made to develop a simple 3DOF simulation are listed. Utilized
coordinates frames are defined in order to properly express the motion of the missile
and the target.
In Chapter 3, utilization of the method of triangulation for calculating the range
between a stationary target and a missile is described. The sensitivity of this method
to measurement units such as IMU and seeker has been investigated. Finally, a
sample trajectory design is performed in order to reduce the error in range to a
desired level.
In Chapter 4, from LOS angle and LOS rate measurements provided by a gimballed
seeker, the range estimation is performed with the application of the Extended
8
Kalman Filter (EKF). The system model of the filter is defined in terms of polar
coordinates representing the 3D missile-target kinematics. In Section 4.3, the
triangulation and EKF algorithms are integrated by taking the output of triangulation
as one of the measurements provided to EKF.
In Chapter 5, the validation of the sensitivity analysis of the triangulation is
performed. Examples on the trajectory design are given. The behavior of the Passive
EKF depending on the observability of the scenario, the initialization procedure and
the measurement noises is investigated. Moreover, the results of Hybrid Estimation
are presented.
In Chapter 6, conclusions of this work are presented.
9
CHAPTER 2
2. MATHEMATICAL MODEL OF HOMING LOOP
In this chapter, the main focus is to derive the mathematical model of the subsystems
illustrated in Figure 1.1 to develop a simulation environment that will be used to
implement and test the algorithms proposed in Chapter 3 and 4. Firstly, the
assumptions are listed and the utilized coordinate frames are introduced. Following
that, the mathematical model of each of the subsystem is derived.
2.1 Assumptions
For the purpose of design and analysis of guidance laws, it is crucial to temporarily
depart from more involved 6DOF models and develop a simpler one. In this study,
for the sake of simplicity following assumptions have been made:
I. Missile and target are assumed to be geometric points without inertia. Their
motions are defined by translational dynamics in three dimensions.
II. The fact that the ability of a missile to maneuver is dependent upon; physical
and aerodynamics properties, thrust profile, wind, altitude, etc. has been
neglected. Instead, closed loop lateral dynamics shown as a minor loop in
Figure 1.1 consisting of autopilot, missile aerodynamics and CAS is
represented by an equivalent transfer function.
III. The velocity of the missile is assumed as impulsive constant velocity [1].
IV. Gravitation is not taken into account.
V. Seeker mounted on the missile is assumed to be a 2 axis gimballed passive
seeker.
10
2.2 Reference Coordinate Frames
In this thesis, following coordinate frames are utilized; earth (Fe), body (Fb) and LOS
(Flos) frame which are illustrated in Figure 2.1.
losx
loszlosy
rV
bx
bzby
Figure 2.1- Fe , Fb and Flos Frames
Since the missile at interest is a tactical missile which flies short ranges, the inertial
frame can be assumed as earth fixed reference frame. In this work, the earth frame is
positioned so that the xe axis points towards the launch direction, the ze axes points
downwards towards the direction of gravity and the ye axes according to the right
hand rule points to the appropriate direction.
For 3-DOF missile motion, body frame Fb can be defined by assuming small angle-
of-attack so that the longitudinal axis of body coordinate system coincides with the
velocity vector. The direction cosine matrix that is used to express a vector written in
body frame coordinates in the inertial frame coordinates is given as follows:
3 2e b
az elC R Rγ γ= ⋅( , )ˆ ( ) ( ) (2.1)
where 3 azR γ( ) and 2 elR γ( ) are two sequential rotations around ze and yb defined in
(2.2). The flight path angles are illustrated in Figure 2.2.
11
3 2
cos( ) sin( ) 0 cos( ) 0 sin( )( ) sin( ) cos( ) 0 & ( ) 0 1 0
0 0 1 sin( ) 0 cos( )
az az el el
az az az el
el el
R Rγ γ γ γ
γ γ γ γγ γ
− ⎡ ⎤⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥ −⎣ ⎦ ⎣ ⎦
(2.2)
elγ
azγ
rV
Figure 2.2- Flight Path-Angles
The Euler orientations of the (line-of-sight) LOS vector relative to inertial frame are
defined with &az elλ λ as depicted in Figure 2.3.
elλ
azλ
Figure 2.3- Missile-Target Relative Geometry
The direction cosine matrix from the inertial frame to the LOS frame is written as
follows:
12
3 2e los
az elC R Rλ λ= ⋅( , )ˆ ( ) ( ) (2.3)
where 3 2( ) & ( )az elR Rλ λ are calculated as (2.2).
2.3 Missile Dynamics
In tactical aerodynamic missiles the flight-control system must, by moving the
control surfaces, cause the missile to maneuver in such a way that the achieved
acceleration matches the desired guidance command. The relationship of achieved
and the desired acceleration can be represented by the following transfer function.
com
( ) ma G sa
= (2.4)
where G(s) is the lateral dynamics of the missile which corresponds to the closed
loop system determined by autopilot, aerodynamics and CAS parameters.
2.4 Target Model
The lateral dynamics of the target is assumed to be unity. The target model in the
simulation contains basically of the computation of the position and the velocity as
follows:
e et t
e et t
V a dt
P V dt
=
=
∫∫
( ) ( )
( ) ( )
(2.5)
2.5 Missile-Target Relative Kinematics
The line-of-sight vector depicted in Figure 2.3 is found from the relative position of
the target with respect to the missile as given in (2.6).
e e et mr P P= −( ) ( ) ( ) (2.6)
The look angles illustrated in Figure 2.4 are calculated in the simulation as follows:
13
( )1 ( )
( )1
( )
sin (3)
(2)tan(1)
bel los
blos
az blos
u
uu
ε
ε
−
−
= −
⎛ ⎞= ⎜ ⎟
⎝ ⎠
(2.7)
where blosu ( ) is the unit vector of LOS expressed in body coordinates as:
( )( ) ( , )
( )ˆ
eb b e
los e
ru Cr
= (2.8)
azεelε
Figure 2.4- Look Angles
The look angles are used to calculate the direction cosine matrix to express a vector
defined in LOS frame in body coordinates:
3 2b los
az elC R Rε ε= ⋅( , )ˆ ( ) ( ) (2.9)
Moreover, the angular rate of the LOS vector with respect to earth frame expressed
in earth coordinates is given in (2.10).
( ) ( )
( )/ 2
e ee
los er V
rω ×
= (2.10)
14
2.6 Navigation
The achieved acceleration is measured by the accelerometer, which provides the
acceleration of the missile with respect to the inertial frame expressed in body
coordinates. To obtain the position and the velocity of the missile, the acceleration
vector is transformed into the inertial frame as in (2.11).
e e b bm ma C a=( ) ( , ) ( )ˆ (2.11)
In Navigation algorithm, this acceleration is integrated to calculate the velocity and
the position of the missile as follows:
( ) ( )
( ) ( )
e em m
e em m
V a dt
P V dt
=
=
∫∫
(2.12)
It is now possible to express the flight path angle of the missile relative to the inertial
frame from the knowledge of the velocity components Vx, Vy and Vz as follows:
1
1
sin
tan
zel
yaz
x
VV
VV
γ
γ
−
−
⎛ ⎞= −⎜ ⎟⎝ ⎠⎛ ⎞
= ⎜ ⎟⎝ ⎠
(2.13)
2.7 Inertial Measurement Unit
An IMU is composed of three accelerometers and three gyroscopes, which measure
translational acceleration and angular velocity of the missile with respect to a non-
rotating inertial frame, respectively. In this study, the gyroscope and accelerometer
dynamics are assumed to be unity. The model of the IMU implemented in the
Simulation contains only the error model. The error is modelled with a static bias
component and an additive zero mean, uncorrelated Gaussian noise as given in
(2.14).
15
a x a xb b
a y a y
a z a z
ba a b
b
ηη
η
⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥
= + +⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦
%, ,
( ) ( ), ,
, ,
m m
, .( ) ( )b/e b/e , ,
, ,
q x q x
b bq y q y
q z q z
bbb
ηω ω η
η
⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥= + +⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦
%
(2.14)
where in each channel (i=x,y,z) the bias and noise terms are represented with a ib , &
q ib , and a iη , & q iη , where ( ), ~ 0,ia i aNη σ and ( ), ~ 0,
iq i qNη σ .
2.8 Seeker Model
As illustrated in Figure 2.5, the seeker mounted on a missile is generally categorized
with three types: active, semi-active or passive. Infrared (IR) and radio-frequency
(RF) seekers are two common types of passive systems that are used especially in
tactical missiles. In these systems, unlike others, the target is not illuminated and the
seeker receives energy that emanates from it [6].
Figure 2.5- Seeker Types [6]
In this study, a gimballed passive seeker is considered. The assumptions made for the
seeker model is listed as follows:
16
- Position, velocity or acceleration of the target is not known to the missile. Only
line-of-sight rate and gimbal angles are measured by gyroscopes and encoders
mounted on the seeker gimbal.
- Lock on range (LOR) of the seeker is specified.
- Limited Field of Regard (FOR) for inner and outer gimbals is considered.
- Ideal tracking of the seeker (no delay, robust tracking algorithm, etc.) is assumed.
The total error of the tracking is assumed to be acted on the look angle and LOS
rate measurement.
- The gimbal dynamics is assumed to be unity.
When locked on the target, the encoders mounted on the gimbals provide the look
angle measurements. The look angles are assumed to be contaminated by an
uncorrelated, zero mean additive Gaussian noise defined as:
el el el
az az az
ε ε ηε ε η
= +
= +
%
% (2.15)
where ( ), ~ 0,el az N εη η σ .
The gyroscopes mounted on the gimbals measure the angular rate with respect to the
gimbal frame. Since the tracking is assumed to be ideal, the gimbal frame is equal to
LOS frame. The LOS rate is contaminated by an uncorrelated, zero mean additive
Gaussian noise defined as:
2
3
( ) ( )/ /
0los los
los e los e ω
ω
ω ω η
η
⎡ ⎤⎢ ⎥
= + ⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
% (2.16)
where ( )1 2, ~ 0,Nω ω ωη η σ .
17
CHAPTER 3
3. METHOD OF TRIANGULATION
Triangulation is the most well-known method to calculate the range from an observer
to a stationary object using bearing measurements which are obtained from two
observer locations with known relative distance. This method has been employed in a
number of fields such as geodetic surveying networks, submarine localization by
sonar, parallax determination in astronomy, optical range finder, etc. ([11],[13]). In
this chapter, utilization of this method for calculating the range between a stationary
target and a moving missile is described. The formulation of the triangulation is
given for 3 dimensional missile-target geometry. Moreover, the sensitivity of this
method to measurement errors in IMU and seeker are investigated. Finally, a sample
trajectory design is performed in order to reduce the range error to a desired level.
3.1 Method of Triangulation
The method of triangulation is based on the principle of forming a triangular shape
with one or two observer(s) and a target/object. It is commonly used in the cases
where a single moving observer or two stationary observers are involved [14]. In this
work, triangulation is employed to calculate the range of a missile to a stationary
target. Since the observer (missile) is moving, there is no need in multiple observers:
the bearing measurements are simply acquired at two different points along the
trajectory.
The bearing (look angle) measurements are obtained from the gimballed seeker that
is defined in Chapter 2.8. From bearing measurements and missile's orientation, the
18
direction of the line-of-sight vector can be determined. The LOS vectors at two
instants intersect at the location of the target forming a triangle as illustrated in
Figure 3.1.
a
b
PΔr
kRrkur
1ku −r
Missile @ k
Missile @ -1k
Target Figure 3.1- Missile-Target Triangular Geometry
The baseline of the triangle ΔrP is the displacement of the missile between time k
and k-1 and kur & 1ku −r
are the unit LOS vectors at those instants. Range between the
missile and the target is computed from the law of sines as follows:
sinsink
aR Pb
= Δ ⋅r r
(3.1)
where a and b are obtained from (3.2).
1
1
cos
cos =
−
−
Δ= •
Δ
•
rr
r
r r
k
k k
Pa uP
b u u
(3.2)
The displacement vector is obtained from the position of the missile at k and k-1:
1k kP P P −Δ = −r r r
. Since the displacement vector is expressed in earth coordinates, the
unit vector of LOS is written in earth coordinate system as ( ) ( , ) ( )ˆe e b bu C u= . When
the seeker is locked on the target, the direction of LOS vector with respect to the
body frame is acquired from encoders mounted on the gimbal. The unit vector of
LOS written in body coordinates will be as follows:
19
[ ]( ) ( , )ˆ 1 0 0= Tb b losu C (3.3)
In optical range finders which are based on the method of triangulation, the angle b
shown in Figure 3.1 is called the parallax angle [12]. It is clear that, in order to
employ this method the missile-target geometry should be suitable to form a triangle,
i.e. the parallax angle has to be nonzero. This means also that the missile should not
move along the line-of-sight.
In theory, with true LOS direction and true missile displacement vector, the
calculated range will be 100 % true, i.e. LOS vectors running from the missile will
intersect exactly at the true target location. However, in practice, erroneous
measurements obtained from IMU and seeker will introduce an error in range
calculated from (3.1). Since the measurement errors propagate into the calculation,
the accuracy of range can be directly related to these errors. In order to understand
the sensitivity of triangulation to measurement errors, the aim in Section 3.3 is to
obtain a correlation between the range error and the measurement errors.
3.2 Triangulation Algorithm
The Method of Triangulation is implemented in the simulation as described in Figure
3.2. The Navigation Algorithm provides the position and the orientation of the
missile. In triangulation, the position information is used to calculate the
displacement vector ( ( )Δ eP ). In addition, from missile's orientation and the look
angles measured by the seeker, the unit vector of LOS is obtained.
In the algorithm, the first goal is to find the parallax (b) angle which is calculated
from the dot product of LOS vectors at instants k and k-1. The condition sufficient
for triangulation is that the parallax angle should be nonzero. Therefore, the
calculation of range is enabled when the parallax angle satisfies the following
condition: thb b> in order to avoid the problem of "geometric dilution". As seen, the
20
only design parameter of this algorithm is thb . The process of selection of the value
thb and the problem of geometric dilution is explained in Section 3.3.3.
( )( ) ( )-11 = cos
e eTk kb u u −
( ) sinsin
ek
aR Pb
= Δ ⋅
[ ]( ) ( , ) ( , )ˆ ˆ 1 0 0e Te b b g
k k ku C C=
( , )
3 , 2 ,ˆ ( ) ( )
b g
k kk az elC R Rε ε=
, ,,k kaz elε ε
1
( )
( ) ( ) ( )
( )-1
1 ( )cos
k k
e
e e e
eT
k e
P P P
Pa uP
−
−
Δ = −
⎛ ⎞Δ⎜ ⎟= ⋅⎜ ⎟Δ⎝ ⎠
( , )( ) ˆ,e be
kkP C
( ) ( )/ ,
,b bk b e k
a ω
( ) ( ) ( ) ( )1 1
,e e e e
k k k ku u P P− −
= =
( )
1e
ku −
1( )
keP−
> thb b
( )
1,e
ku −
Figure 3.2- Triangulation Algorithm
Since at every calculation step the triangle have to be suitable to fulfill this condition,
the time interval of range calculation changes depending on the value of LOS rate.
Finally, at the steps when the range is calculated, the LOS unit vector and the
21
position of the missile are stored for the next calculation step to serve as the first
points ("Missile at k-1") of the triangle depicted in Figure 3.1.
3.3 Sensitivity Analysis
The range calculated from (3.1) is a function of the displacement vector and the
orientation of LOS as expressed in (3.4).
( )( , )ˆ, e losR f P C= Δr
(3.4)
The errors in measurements obtained from IMU lead to navigation errors which will
affect both the displacement vector and the LOS orientation. Moreover, the seeker
cannot provide the ideal look angle due to reasons explained in Section 2.6 causing
an error in the LOS orientation as well. These errors propagate into the triangulation
introducing an error in range. In this section, in order to understand how the errors
propagate, the objective is to find an expression that relates the range error to the
uncertainties in the measurements.
Error Sources
Inertial Measurement
Unit
Acceleration Bias Error
Angular RateBias Error
Look AngleNoise
Gimballed Seeker
Figure 3.3- Error Sources
For sensitivity analysis, missile-target geometry is assumed to be planar. The triangle
geometry in pitch ( - e ex z ) plane is depicted in Figure 3.4.
22
b
k 1λ −
kλ
PγkR
PΔr
ezex
M @ k
M @ -1k
T
Figure 3.4- Planar Triangular Geometry
where Pγ is the angle of the displacement vector with respect to the inertial frame,
1k kb λ λ −= − and 1P ka γ λ −= − . For this case, Equation (3.1) reduces to
1
1
sin( ) sign( )sin( )
P kk
k kR P cγ λ
λ λ−
−
−= Δ ⋅
− (3.5)
where to obtain a positive range following sign function is used:
1 1sin( ) / sin( )P k k kc γ λ λ λ− −= − −
Additional assumptions are listed as follows:
In the sensitivity analysis, only the error sources given in Figure 3.3 are taken
into consideration. The acceleration and angular measurement errors are
modeled with a static bias. The look angle measurement obtained from the
gimbal encoders are assumed to be corrupted by a zero mean additive Gaussian
noise.
For simplicity, the accelerometer measurement along with its bias is assumed to
be expressed in inertial coordinate system.
The measurement error of the gyroscope is assumed to only affect the LOS
orientation.
23
3.3.1 Sensitivity to Accelerometer Measurement Error
In this part, the objective is to discover the sensitivity of range obtained from (3.5) to
accelerometer bias error. The accelerometer error will cause an error in missile's
position and as a result an error in the displacement vector which can be expressed as
follows:
P Pδ = Δ −Δrr r% (3.6)
where δr
is the error in the displacement vector and PΔr% is the erroneous
displacement vector. Assuming ideal gyroscope and seeker measurements, the
difference between the true and erroneous range will be as follows: Δ = − %k kR R R .
Inserting / cosx PP P γΔ Δ = and / sinz PP P γΔ Δ = − into (3.5) leads to
1 11
sin cos sign( )sin
λ λ− −Δ + Δ= − ⋅x k z k
kP PR c
b (3.7)
where 1 1sin( )P kc γ λ −= − . From (3.7), the erroneous range is found as:
1 11
( ) sin ( ) cos sign( )sin
δ λ δ λ− −Δ + + Δ += − ⋅% %x x k z z k
kP PR c
b (3.8)
where and z z z x x xP P P Pδ δΔ = Δ + Δ = Δ +% % .Subtracting (3.8) from (3.7) gives the
range error as presented in (3.9).
11
sin( ) sign( )sin
δλ εδ − −Δ = ⋅ kR c
b (3.9)
where / tanx z δδ δ ε− = and 2 2tot x zδ δ δ= + . Here, 1 1sign( ) sign( )c c=% is assumed.
Since the bias of the accelerometer is assumed to be constant throughout the flight,
the x component of the erroneous displacement vector is computed as
24
2 2( )( ) ( )2 2x x x x x
t T tP P t T b P t b⎛ ⎞ ⎛ ⎞+
Δ = + + − +⎜ ⎟ ⎜ ⎟⎝ ⎠ ⎝ ⎠
% (3.10)
where xb is the accelerometer bias in x direction. From (3.10), the displacement error
in x direction xδ is obtained as follows
( )2 22x
x x xbP P T tTδ = Δ − Δ = +% (3.11)
Similarly, the displacement error in z direction is: 2( 2 )/ 2z zb T tTδ = + . The total
displacement error can be expressed as: 2( 2 ) / 2totb T tTδ = + where 2 2= +tot x zb b b
and / tanz x bb b ε− = . Finally, inserting these equalities into (3.9), an expression
between the range error and the accelerometer bias is obtained in (3.12).
2
1 12 sin( ) sign( )
2sinλ ε−
+Δ = ⋅ −tot k b
T tTR b cb
(3.12)
Assuming small b ( sinb b≈ ) and small sampling time: ( )b T tλ= ⋅ & , (3.12) is
reduced to following form:
1 1/ 2 sin( )sign( )( )
λ ελ −
+Δ = ⋅ −
&tot k bT tR b c
t (3.13)
As it is seen from (3.13), the error in range depends on the direction of the bias
vector. If the bias vector is perpendicular to LOS (i.e. 1 90ob kε λ −− = ± ), the range
error will have its maximum value. In real applications, the direction of the bias is
unknown, i.e. the probability of bε being between 0o-360o is uniformly distributed.
To stay on the safe side, the limiting case where the bias is perpendicular to LOS is
considered:
25
max/ 2( )tot
T tR btλ+
Δ =&
(3.14)
Comments on (3.12)-(3.14) are given below:
• As expected, the error in range will be large for large accelerometer bias. It
changes linearly with the bias.
• The error will increase with time (for constant LOS rate). This also means that
(for the same LOS rate profile) the range error will be large for larger flight time
and target distance.
• If the assumption of ( )b T tλ= ⋅ & and T<<t holds, the error will be independent
of the sampling time (T) of the algorithm.
• The range error is inversely proportional to LOS rate. For trajectories with high
LOS rate profile, same accelerometer bias will lead to lower level of range error
than in the case of low LOS rate trajectories. The level of LOS rate depends on
the missile trajectory. Since the trajectory is the product of the guidance strategy
that is employed, it can be controlled or designed with the intention of reducing
the error below a certain level.
Assuming T<<t, following relationship imposes a requirement on the LOS rate
that should be satisfied in order to keep the error below an acceptable threshold (
max,thRΔ ).
max,th
( )tot
t bt R
λ>Δ
& (3.15)
For example, to meet a 100 m threshold requirement, for 20 s maximum flight
time and an accelerometer with 10 mg bias, the LOS rate throughout the flight
should be; ( ) 1.12deg/λ >& t s . If the existing LOS rate profile is above that value,
then it can be said that there is no need to redesign the trajectory. If not, the
26
trajectory should be redesigned to obtain a range error below the threshold. For
this purpose, a sample design has been performed in Chapter 3.4.
If the design of the trajectory for this purpose is not possible because it will
violate some of the other trajectory requirements of the missile such as related to
operational or performance concerns, still from (3.14) one can simply check
whether it is feasible to utilize the method of triangulation for a given
accelerometer bias, maximum flight time and LOS rate profile of the possible
trajectory.
3.3.2 Sensitivity to Gyroscope Measurement Error
In this part, the objective is to discover the sensitivity of the triangulation to
gyroscope bias error. It was assumed that the gyroscope error only affects the LOS
orientation introducing an error in LOS angle as follows:
λ λ λΔ = −%k k k (3.16)
Substituting the erroneous LOS angles into (3.5), leads to
( )( )
1 1
1 1
sin ( )sign( )
sin ( ) ( )γ λ λ
λ λ λ λ− −
− −
− + Δ= Δ ⋅
− + Δ −Δ% %P k k
kk k k k
R P c (3.17)
To find the range error, (3.17) is subtracted from (3.5) which results in
( )1 1 1 1
11 1 1
( ) ( ) cot( )sin( ) sign( )( ) ( ) ( )λ λ λ λ λ γ λ
γ λλ λ λ λ λ λ
− − − −−
− − −
⎛ ⎞Δ − Δ + − Δ −Δ = Δ − ⎜ ⎟⎜ ⎟− − + Δ −Δ⎝ ⎠
k k k k k P kk P k
k k k k k kR P c (3.18)
Here, 1( )k kλ λ −− , 1kλ −Δ , ( )1k kλ λ −Δ − Δ assumed to be small (so that cos(m)=1,
sin(m)=m). Finally, inserting (3.5) into (3.18) results in (3.19) where it is seen that
the range error depends on the instantaneous range.
27
( )1 1 1 1
1 1
( ) ( ) cot( )( ) ( )
λ λ λ λ λ γ λλ λ λ λ− − − −
− −
Δ − Δ + − Δ −Δ = ⋅
− + Δ − Δk k k k k P k
k kk k k k
R R (3.19)
For planar geometry shown in Figure 3.5, from the orientation of the missile ( kθ )
and the seeker look angle ( kε ), the LOS angle is calculated as follows: k k kλ θ ε= + .
If there is an error in kθ and the look angle is ideal, the error in LOS orientation will
be as: k kλ θΔ = Δ .
kλ
LOSezex
kθkε
rV
M
T
Figure 3.5- Planar Missile-Target Geometry
Since the gyroscope bias ( qb ) is assumed to be constant throughout the flight, the
integration of the erroneous angular rate introduces a heading error as follows:
1 & ( )k q k qb t b t Tθ θ−Δ = Δ = + (3.20)
Inserting this into (3.19) and rearranging the equation,
( )1 1
1
( )cot( )λ λ γ λλ λ
− −
−
Δ + − −= ⋅
− +k k k P k
qk k k q
R T tbR b T
(3.21)
For small 1( )λ λ −−k k and small sampling time, 1 1 ( )k k kT T tλ λ λ λ− −− = =& & can be
assumed. As a result, the percentage range error is found as in (3.22).
28
11 ( )cot( )( )
k P kq
k q
R t tbR t b
λ γ λλ
−Δ + −= ⋅
+
&
& (3.22)
Comments on (3.22) are given below:
• An important conclusion is that the accuracy of range will increase with
decreasing range.
• The error in range increases with the increase in bias.
• If the assumption of 1 ( )k k T tλ λ λ−− = & holds, the error will be independent of
sampling time or the parallax angle.
• Let us define sign( ( ))x tλ= & and 1sign( )P ky γ λ −= − . Inserting this into (3.22),
and rearranging the equation:
1 cot( )
kq
k
R b x x y t aR tλ
⎛ ⎞Δ ⎜ ⎟= ⋅ + ⋅ ⋅⎜ ⎟⎝ ⎠& (3.23)
where 1P ka γ λ −= − and ( ) qt bλ >>& is assumed. As proved in Appendix A,
1xy = − is always satisfied. As a result, (3.23) reduces to
1 cot( )
kq
k
R b x t aR tλ
⎛ ⎞Δ ⎜ ⎟= ⋅ −⎜ ⎟⎝ ⎠& (3.24)
From (3.24), it can be concluded that, when 1/ ( ) cott t aλ >& , as t increases the
absolute value of the range error will decrease until 1/ ( ) cott t aλ =& . Then, when
1/ ( ) cott t aλ <& , the absolute value of the range error will increase with time.
Moreover, as a decreases the absolute value of the range error will decrease until
1/ ( ) cott t aλ =& . Then, when 1/ ( ) cott t aλ <& , the absolute value of the error will
increase for decreasing a. For the LOS rate the opposite is true: as ( )tλ& increases
29
the absolute value of the error will decrease until 1/ ( ) cott t aλ =& . If
1/ ( ) cott t aλ <& , the error in range will increase for increasing ( )tλ& .
To conclude, the error due to gyroscope bias is highly dependent on the trajectory.
As stated in the previous section, in order to satisfy an acceptable accuracy of
range, the trajectory can be adjusted. For this purpose, a sample design is
performed in Chapter 3.4.
3.3.3 Sensitivity to Look Angle Measurement Error
In this part, the objective is to discover the sensitivity of range estimate obtained
from triangulation to look angle errors. The look angle measurement provided by the
seeker is assumed to be contaminated by an uncorrelated, zero mean additive
Gaussian noise defined as:
2 21( ) , ( ) 0 , ( ) 0k k k kE E Eεσ ε ε ε ε −= Δ Δ = Δ Δ = (3.25)
The deviation of the LOS angle from its true value is in this case is: k kλ εΔ = Δ .
Assuming 1k kλ λ −− >> 1k kλ λ −Δ −Δ , equation (3.19) is rearranged as follows,
1 1 1 1
1
( ) ( ) cot( )/ ε ε λ λ ε γ λλ λ
− − − −
−
Δ −Δ + − Δ −Δ =
−k k k k k P k
k kk k
R R (3.26)
In this equation, other than the look angle noises 1 and k kε ε −Δ Δ , the remaining
parameters are deterministic. Therefore, the expected value of the percentage error in
(3.26) is zero: ( / ) 0k kE R RΔ = . The variance of /k kR RΔ is the expected value of the
square of (3.26) which is found as in (3.27).
( ) ( )22 2 2/ 2
cot 1 1( / )R R k k
x y b aE R R
bεσ σΔ⋅ ⋅ ⋅ − +
= Δ = ⋅ (3.27)
30
where 1k kb λ λ −= − and 1sign( ( )) sign( )k kx tλ λ λ −= = −& . Since as given previously:
1xy = − , (3.27) reduces to the following equation:
( )22 2/ 2
cot 1 1R R
b abεσ σΔ
⋅ + += ⋅ (3.28)
Comments on (3.28) are given below:
• Similar to the case of gyroscope bias error, as the range between the missile and
target decreases, the accuracy of range improves.
• As a increases, the error in range decreases.
• An important conclusion is that the range error induced by the look angle noise
depends on the parallax angle (b). The error increases with the decrease in
parallax angle. Note that for zero parallax angle, the precision of range is
completely lost. In other words, the sensitivity diverges so that the calculated
range is not reliable at all. In addition, if the parallax angle is too small, even the
small look angle noise may lead to large error in range. The reason is that the
actual value of the parallax angle will be too close to its noisy part. In such a case,
it can be stated that the signal to noise ratio of the triangular geometry is very low.
In literature, this phenomenon is known as "geometric dilution" [12].
In this work, the problem of geometric dilution is handled by imposing a constraint
on the parallax angle as thb b> at each calculation step (shown in Figure 3.2), so that
for a given standard deviation of look angle noise, a desired accuracy in range can be
obtained. This relationship is expressed in (3.29).
( )2cot 1 1des
b abεσ σ
⋅ + +≥ ⋅
(3.29)
where desσ is the desired value of the standard deviation of percentage range error.
This equation is solved for positive parallax angle as given in (3.30).
31
22
22
cot 2 cot
cot
des
th
des
a a
a
b ε
ε
σσ
σσ
⎛ ⎞+ −⎜ ⎟
⎝ ⎠
⎛ ⎞−⎜ ⎟
⎝ ⎠
= (3.30)
The positive solution of thb exists only when following condition is satisfied:
c/ otdes aεσ σ > . From this constraint, it is important to note that by specifying a
threshold on parallax angle, the minimum value of range error that can be achieved
via triangulation will be as follows.
cin otm des aεσ σ= (3.31)
In order to understand how the value of min desσ changes, an example is given in
Figure 3.6. It is seen that, as a decreases the minimum value of the error increases.
Especially for high look angle noises, when a is small, the increase will be
significant. In addition, from this figure, the value of a that satisfies a specified
min desσ can be determined. For example, if the range error is desired to be below 5
%, angle a should be greater than approximately 5o.
Moreover, equation (3.30) is evaluated for 10 %desσ = as given in Figure 3.7. It is
observed that when a is greater than approximately 10o, the value of thb is nearly
constant with respect to a. Therefore, if it is known that a is greater than 10o
throughout the flight, then in the algorithm a constant value can be assigned for thb .
However, if a may be less than 10o, to obtain a desired accuracy in range the value of
thb should be adapted according to the calculation given in (3.30).
32
Figure 3.6- Minimum Range Accuracy
Figure 3.7- Parallax Threshold for 10 % Desired Error
0 5 10 15 20 250
5
10
15
20
25
30
a [deg]
% m
inim
um σ
des
σskr=0.05o
σskr=0.1o
σskr=0.15o
σskr=0.2o
σskr=0.25o
σskr=0.3o
0 5 10 15 20 25 30 35 40 450
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
X: 10Y: 0.02661
b th [r
ad]
a [deg]
bth for %σdes=10
σskr=0.05o
σskr=0.1o
σskr=0.15o
σskr=0.2o
σskr=0.25o
σskr=0.3o
33
3.3.4 Conclusion of Sensitivity Analysis
The sensitivity analysis is summarized in Table 3.1.
Table 3.1- Summary of Sensitivity Analysis
Accelerometer Bias Gyroscope Bias Seeker Noise
t ↑ RΔ ↑ depending on ( ) &cott aλ& −
R↓ − RΔ ↓ RΔ ↓
λ ↑& RΔ ↓ depending on &cott a −
b ↓ − − % RΔ ↑
a ↑ − depending on ( ) &t tλ& % RΔ ↓
Since the accelerometer and gyroscope measurements are integrated in navigation
algorithm, the range error introduced by those measurements changes with time. As
time passes, the range error due to accelerometer bias increases. However, for
gyroscope bias, as stated in Section 3.3.2, the change of the error with respect to time
depends on the LOS rate and the angle a.
The error in LOS orientation is induced by the errors in seeker or gyroscope
measurements. As it is derived in Section 3.3.2 and 3.3.3, the error in LOS
orientation leads to an error in range which improves with decreasing range.
However, for accelerometer bias, the range error is not affected from the missile
moving closer to the target.
For gyroscope and accelerometer measurement errors, how much the errors
propagate into the triangulation depends on the LOS rate of the trajectory. For
accelerometer bias, high LOS rate trajectories produce lower level of range error. In
case of gyroscope bias, the change of range error with respect to LOS rate depends
on time and the value of a. However, for seeker noise, range error does not directly
34
depend on the LOS rate; it depends on the parallax angle. For small parallax angles,
the accuracy in range deteriorates because of the geometric dilution of the triangle.
Moreover, in case of seeker noise, the range error decreases with increasing a. For
gyroscope bias, however, the change of the error with respect to a depends on LOS
rate and time.
3.4 Trajectory Design
As it mentioned in preceding sections, errors in IMU and seeker propagate into the
calculation of range. From the relations derived in (3.13) and (3.22), it is clear that
the range error is highly dependent on the trajectory. In fact, these relationships can
be used to adjust the trajectory of the missile so that the range error is reduced to a
desired level. In the case of seeker error, from (3.29)-(3.30) it is observed that the
range error can be controlled by assigning appropriate values to the threshold of the
parallax angle. Therefore, in this part, only the errors in accelerometer and gyroscope
are taken into consideration.
The aim of this section is not to derive an optimal guidance law that minimizes the
range error. The main objective is to give an idea on how the sensitivity relations can
be used. As an example, for midcourse guidance strategy, the Constant Bearing
Midcourse is considered by which the missile is guided so that bearing (look) angle
to the target is kept constant throughout the flight as illustrated in Figure 3.8.This
strategy can be especially helpful when a desired relative geometry prior to terminal
phase is required. It also prevents the gimbals from reaching its mechanical limits.
cstε ε=bx
Figure 3.8- Constant Bearing Guidance
35
The Constant Bearing Midcourse is formulated by taking the guidance gain of the
Proportional Navigation as 1. The derivation of the closed form solution of this
method is given in Appendix B where the LOS rate profile is found to be:
tan( )( ) cst
f
tt tελ =−
& (3.32)
In the following sections, the value of ε cst will be determined according to the
sensitivity relations derived in Section 3.3.
3.4.1 Controlling the Range Error due to Accelerometer Bias
For Constant Bearing Midcourse, the range error introduced by the accelerometer
bias is found by inserting (3.32) into (3.14) as follows:
max/ 2 ( )
tantot fcst
T tR b t tε+
Δ = ⋅ − (3.33)
As seen from (3.33), the error in range will improve for higher look angles.
Moreover, the error changes parabolically with time. Assuming that the seeker is
locked before launch and t >> T, the maximum value of the variable term on the right
hand side which is a function of time as ( ) ( )ff t t t t= − occurs at / 2ft t= . As a
result, in order to maintain the range error below a specified threshold ( max,desRΔ )
throughout the flight, following inequality must be satisfied.
2
max,
tan4ftot
cstdes
tbR
ε > ⋅Δ
(3.34)
where the maximum condition / 2ft t= is considered. The reason is that, by
selecting cstε based on the maximum condition ensures that the resulting error will
be smaller than this value throughout the flight. It is important to note that the upper
36
limit of cstε is constrained by the mechanical limits of the seeker gimbals:
limit .cstε ε> For the selection of cstε an example is given in Section 5.1.2.
3.4.2 Controlling the Range Error due to Gyroscope Bias
When the flight path rate is assumed to be ignorable during the sampling time,
following assumption can be made: 1P kγ γ −≈ . This leads to equation (3.35) and
(3.36).
1 1 1 1P k k k k cstγ λ γ λ ε ε− − − −− ≈ − = − = − (3.35)
1cot( ) cotP k cstγ λ ε−− ≈ − (3.36)
Substituting (3.32) and (3.36) into (3.22) results in the following equation.
( )2 cotkq f cst
k
R b t tR
εΔ= − (3.37)
As seen from (3.37), similar to (3.33), the accuracy of range improves for higher
look angles. In this case, the percentage error changes linearly with time. The
maximum value of the error occurs at the beginning and at the end of the flight
where 0t = and ft t= , respectively. Different from (3.33), the percentage error will
have its minimum value at / 2ft t= . When the maximum condition is considered
(where 0t = or / 2ft t= ), in order to maintain the percentage error below a specified
threshold ( max,% desRΔ ) during the whole flight, following inequality must be
satisfied.
max,tan
%q f
cstdes
b tR
ε⋅
>Δ
(3.38)
For the selection of cstε , an example is given in Section 5.1.2.
37
CHAPTER 4
4. PASSIVE RANGE ESTIMATION
This Chapter focuses on the estimation of the range with an Extended Kalman Filter
(EKF). Firstly, a brief review of the EKF is given. In Section 4.2, from LOS angle
and LOS rate measurements provided by a gimballed seeker, the estimation of range
is performed. The system model of the filter is defined in polar coordinate frame
representing the 3D missile-target kinematics. In this section, the measurement
model, the necessary and sufficient conditions for observability and the initialization
procedure is provided. In section 4.3, the triangulation and the EKF algorithms are
integrated by taking the range output of the triangulation as one of the measurements
provided to the filter.
4.1 Extended Kalman Filter
Applied firstly in spacecraft navigation problems, Extended Kalman Filter is
originated from the idea that the standard Kalman Filter equations can be employed
to nonlinear systems when the system is linearized around the state estimate of the
filter at each time step [10]. The nonlinear discrete-time system and measurement
model can be expressed in the following general form.
( )( )1 1 1 1, ,
,k k k k k
k k k k
y f y u w
z h y v− − − −=
= (4.1)
where y is the state vector, f is the nonlinear state transition function, w is process
noise, z is the measurement vector, h is the nonlinear measurement function and v is
38
the measurement noise. Here, w and v are assumed to be uncorrelated, zero-mean
(i.e. ( )kE w = ( ) 0kE v = ), Gaussian (normal, N) noises with covariance matrices kQ
and kR , respectively:
( )( )
~ 0,
~ 0,k k
k k
w N Q
v N R (4.2)
The algorithm of EKF ([10]) is summarized in Table 4.. Here, the time propagation
of the system and the residual calculation in the measurement update equation can
still be evaluated as nonlinear functions. However, for the computation of Kalman
gain and state covariance matrix, state transition f and measurement function h need
to be linearized. The linearization of these functions around the state estimate is
given in (4.3)-(4.6). Here, the initial distributions of the initial estimates are assumed
to be Gaussian with mean 0 0y and covariance matrix 0 0P .
Table 4.1- Extended Kalman Filter Algorithm
Initialization ( )00 0
0 00 0 0 0 0 0
ˆ ( )
ˆ ˆ( )( )T
y E y
P E y y y y
=
= − −
Linearization of f
11
1 ˆ 1| 1
kk
k yk k
fAy
−−
−− −
∂=∂
(4.3)
11
1 ˆ 1| 1
kk
k yk k
fGw
−−
−− −
∂=∂ (4.4)
System Time Propagation | 1 1 1| 1 1ˆ ˆ( , )k k k k k ky f y u− − − − −=
Covariance Time Propagation | 1 1 1| 1 1 1 1 1T T
k k k k k k k k kP A P A G Q G− − − − − − − −= +
39
Table 4.1- Continued
Linearization of h ˆ | 1
kk
k yk k
hCy
−
∂=∂
(4.5)
ˆ | 1
kk
k yk k
hHv
−
∂=∂
(4.6)
Kalman Gain ( ) 1| 1 | 1
T T Tk k k k k k k k k k kK P C C P C H R H
−− −= +
Measurement Update | | 1 | 1ˆ ˆ ˆ( ( ))k k k k k k k k ky y K z h y− −= + −
Covariance Update ( ) ( )
( )| | 1
Tk k k k k k k k
T Tk k k k k
P I K C P I K C
K H R H K
−= − −
+
4.2 Passive Range Estimation
In Ref. [23] the state vector of the system model defined in Modified Polar
Coordinates in 2D is given as follows:
/ 1 /T
y r r rλ λ⎡ ⎤= ⎣ ⎦& & (4.7)
where λ& : LOS rate, λ: LOS angle, /r r& : range rate divided by range and 1/ r :
reciprocal of range. In this thesis, the estimation is performed in 3D space, and the
new state vector is defined as in (4.8).
[ ]1 2 3 / 1 / Tel azy r r rω ω ω λ λ= & (4.8)
40
where [ ]( )/ 1 2 3
Telos eω ω ω ω= is the angular rate of the LOS vector with respect to
the inertial frame expressed in inertial frame coordinate system and & el azλ λ are
Euler orientations of the LOS vector depicted in Figure 2.3.
In following sections, the mathematical model of the system and the measurement is
presented. The initialization procedure is explained and comments on the
observability of the filter are given.
4.2.1 System Model
In this section, the mathematical model of the continuous nonlinear system is
derived. A continuous system is defined as follows:
( , )y g y u=& (4.9)
In order to obtain the system function ( , )g y u , the differentiation of the states is
performed.
State 1-3:
By differentiating the expression in (2.10) with respect to time, the first three
equations of the system are obtained as given in (4.10).
( ) ( ) ( )( )/ /
/2 2e e e
elos e t mlos e
e
d r a rdt rrω
ω× ⎛ ⎞= − ⎜ ⎟
⎝ ⎠
& (4.10)
where ( )er is the range vector and ( )1 2 3/ [ ]e
t ma a a a= is the relative acceleration
vector of the target with respect to the missile expressed in earth frame coordinate
system. Range vector written in earth frame coordinates is obtained from the
expression in LOS frame as follows:
[ ]( ) ( , ) ( ) ( , )ˆ ˆ 0 0 Te e los los e losr C r C r= = (4.11)
41
where ( , )ˆ e losC is given in (2.3). Finally, inserting (2.3) and (4.11) into (4.10), leads to:
( )( )
( )
2 3( ) ( )
/ 1 3 /
2 1
sin cos sin /sin cos cos / 2
cos cos sin /
el el aze e
los e el az el los e
el az az
a a rra a rr
a a r
λ λ λω λ λ λ ω
λ λ λ
⎡ ⎤+⎛ ⎞⎢ ⎥= − + − ⎜ ⎟⎢ ⎥ ⎝ ⎠⎢ ⎥−⎣ ⎦
&& (4.12)
From (4.12), it is seen that the range is at the denominator of the equation. As stated
in Ref. [23], in order to minimize the linearization error, the reciprocal of range is
chosen as the state rather than the range itself. Moreover, in the original formulation
given in Ref. [23], to prevent the filter from estimating the range for unaccelerated
missile motion, the range rate over range is chosen as the state instead of range rate.
State 4-5:
The relation between the angular rate of the LOS vector with respect to inertial frame
( ( )/
elos eω ) and the angular rate of Euler orientations ( &az elλ λ ) are given in (4.13).
( , ) ( ) ( ) ( ) ( , )/ /1 1/
0 00
0ω ω ω λ
λ
⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥= + = +⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦⎣ ⎦
&
&
los e e los los los eellos e los e
az
C C (4.13)
where [ ]( )/ 1 2 3
Telos eω ω ω ω= . From (4.13), the derivatives of LOS angles with respect
to time are obtained in terms of ( )/
elos eω as follows:
( )2 1
3 1 2
cos sin
tan cos sin
λ ω λ ω λ
λ ω λ ω λ ω λ
= −
= + +
&
&el az az
az el az az
(4.14)
State 6:
The derivative of range rate divided by range is calculated as
22
2d r r r r rdt r r r rr⎛ ⎞ ⎛ ⎞= − = −⎜ ⎟ ⎜ ⎟⎝ ⎠ ⎝ ⎠
& && & && & (4.15)
42
From the second derivative of range vector expressed as ( )losr with respect to inertial
frame given in (4.16), r&& can be obtained.
( )( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )/ / / /2
los los los loslos los los los loslos e los e los e los ea r r r rω ω ω ω= + × + × + × ×&&& & (4.16)
where [ ]( ) 0 0 Tlosr r=&& && . Inserting the first row of (4.16) into (4.15), leads to (4.17).
2
23 1 2
22 1
1 2 3
( cos cos sin sin sin )
( cos sin )( cos cos cos sin sin ) /
ω λ ω λ λ ω λ λ
ω λ ω λλ λ λ λ λ
⎛ ⎞ ⎛ ⎞= − + + +⎜ ⎟ ⎜ ⎟⎝ ⎠ ⎝ ⎠
+ −
+ + −
& &
el az el az el
az az
az el el az el
d r rdt r r
a a a r
(4.17)
State 7:
The derivative of the reciprocal of range can be found simply as
21d r
dt r r⎛ ⎞ = −⎜ ⎟⎝ ⎠
& (4.18)
Finally, from (4.12), (4.14), (4.17) and (4.18), the nonlinear system model is
obtained as follows:
( )( )
( )
( )
7 2 4 3 4 5 1 6
7 1 4 3 5 4 2 6
7 4 2 5 1 5 3 6
2 5 1 5
3 4 1 5 2 52 26 3 4 1 5 4 2 5 4
2 5 1
sin cos sin 2
sin cos cos 2cos cos sin 2
cos( ) sin( )tan( ) cos( ) sin( )( , )
( cos cos sin sin sin )
( cos si
+ −
− + −
− −−
+ += =− + + +
+ −
&
y a y a y y y y
y a y a y y y yy y a y a y y y
y y y yy y y y y yy g y u
y y y y y y y y y
y y y 25
7 1 5 4 2 4 5 3 4
6 7
n )( cos cos cos sin sin )
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎛ ⎞⎢ ⎥⎜ ⎟⎢ ⎥⎜ ⎟⎢ ⎥⎜ ⎟⎜ ⎟+ + −⎢ ⎥⎝ ⎠⎢ ⎥
−⎢ ⎥⎣ ⎦
yy a y y a y y a y
y y
(4.19)
43
where the relative acceleration is the input of the system. Since in this thesis the
target is assumed to be non-maneuvering, the input is equal to the negative of missile
acceleration: ( ) ( )/
e et m ma a= − which is provided by the accelerometers mounted on the
missile.
Since EKF is implemented in discrete time domain, the system given in (4.19) should
be discretized. The discretization is performed on the assumption of first order Euler
integration as follows:
1 1 1 1 1 1( , ) ( , )− − − − − −= + ⋅ =k k k k k k ky y T g y a f y a (4.20)
The linearized form of state transition function f defined in (4.3)-(4.4) is given in
Appendix C.
In the original formulation proposed in Ref. [23], the process noise was not included.
However, in this work, to take the input/accelerometer error and unmodelled
dynamics into account the process noise is included as an additive Gaussian noise
acted on the input:
( )1 2 3
2~ 0, , diag([ ])σ σ σ=k a a aw N Q Q (4.21)
4.2.2 Measurement Model
Originally, as stated in Section 2.8, the measurements provided by the seeker are the
gimbal (look) angles and the LOS rate vector expressed in LOS coordinates.
However, to simplify the Kalman equations and to eliminate the possible
linearization error, in the measurement model of the EKF the LOS rate is assumed to
be expressed in earth coordinates and instead of gimbal angles the LOS angles is
assumed to be measured by the seeker. As a result, the measurement vector is
defined as follows:
44
( )/
Telos e el azz ω λ λ⎡ ⎤= ⎣ ⎦ (4.22)
Since these measurements are also in the state vector, the measurement function h is
linear as given in (4.23).
[ ]5 5 5 2 5 50k k k k k k kz C y H v I y I v× × ×= + = + (4.23)
where the measurement noise is assumed to be uncorrelated, zero-mean, Gaussian
noise with covariance matrix R as defined in (4.24).
( ) 2 2 2 2 2~ 0, , diag([ ])ω ω ω λ λσ σ σ σ σ=kv N R R (4.24)
Here, each channel in LOS angle and LOS rate measurements are assumed to be
independent with equal standard deviations and ω λσ σ respectively.
4.2.3 Observability Issue
In Ref. [24] it is stated that, for a target travelling with constant velocity, the
necessary condition for a scenario to be observable is that the observer should
execute a maneuver. However, this condition is not sufficient: the observer have to
maneuver in a way such that the resulted LOS angle history can be distinguishable
from those corresponding to the unaccelerated case. In Ref [23], the modified states
of the system model are chosen so that the filter behaves as predicted by
observability theory.
In Ref. [29], the results related to observability derived in Ref. [24] is extended to the
case where the target is stationary. It turns out that, for a stationary target, as long as
the LOS rate is not zero the range can be estimated even when the observer does not
manuever. However, when the filter proposed in Ref. [23] is utilized for stationary
targets (as in this thesis), the range will still be unobservable to the filter without a
manuever. The reason of that is explained as follows: It can be observed from the
first three equations of (4.19), that a relation between the LOS rate and the reciprocal
45
of range ( 7y ) only exits when the acceleration is non-zero. In the sixth equation
similar to the first three equations, 7y is multiplied by the acceleration components.
Therefore, for zero acceleration, the relation between /r r& and 1 / r also vanishes
which means that the information about range cannot be acquired from the /r r&
estimate either.
To concluded, even the problem is observable, since in the system model the relation
between range and other state estimates vanishes when the acceleration is zero, for
the filter the problem remains unobservable. Thus, in case of stationary targets, the
states and the system model proposed in Ref. [23] should be modified so that the
estimation of range-to-go can be realized when the observer does not maneuver. This
is evaluated as a future work.
On the other hand, it is clear that the estimation of /r r& is still possible when the
acceleration is zero. From the first three equations of (4.19), it can be also concluded
that, since the /r r& and LOS rate components are in multiplication, the estimation of
/r r& only possible when the LOS rate is not zero.
4.2.4 Initialization of the Filter
For scenarios where the range is unobservable, the estimation will rely on the initial
estimate of range. Therefore, the initialization of the filter is an important issue. The
initialization procedure of the filter is presented in subsequent sections.
State 1-5: The LOS rate ( ( )/
elos eω ) and the LOS angle ( & )el azλ λ defined in the system
model are also the measurements provided by the seeker. Hence, these measurements
obtained at the step when the algorithm is initiated can be used as their initial
estimates. Similarly, &ω λσ σ defined in the measurement model can be assigned as
the uncertainty in initial estimates as well.
46
State 6: The range rate ( r& ) is equal to the component of the relative velocity of the
target with respect to the missile along the LOS vector: t mr rr V V= −& . Assuming zero
target velocity, the initial condition of range rate will be: (0)o mrr V= −& where (0)mr
V
can be simply found from the first component of the velocity vector expressed in
LOS frame coordinates as: ( ) ( , ) ( )ˆlos los e em mV C V= .
The variation in range rate divided by range ( /o or rΔ & ) can be expressed in terms of the
variation in range rate ( orΔ & ) as follows:
/o oo o o o
r ro o o
r r r rr r r
+ Δ ΔΔ = − =&
& & & & (4.25)
From (4.25), the standard deviation of /o or rΔ & is found to be: / /r r r oo o orσ σ=& & where
or is the initial range-to-go estimate. Since in this work, the target is stationary, for
the initial uncertainty of range rate ( roσ & ) small values can be assigned.
State 7: It is clear that, in order to be able to perform the range estimation, the
seeker lock-on has to be achieved. Assuming the midcourse guidance algorithm prior
to lock-on ensures that the target is inside the Field-of-View (FOV) of the seeker, the
lock-on will be realized when the range to the target is equal to the Lock on Range
(LOR). Therefore, the LOR value is set as the initial estimate of the range. The
deviation of the actual LOR from the theoretical value can be determined from test
results.
The variation in reciprocal range ( 1/ roΔ ) in terms of variation in range ( orΔ ) can be
expressed as follows:
1/ 21 1
( )o o
roo o o o o o o
r rr r r r r r r
Δ ΔΔ = − = ≅
+Δ +Δ (4.26)
47
where o or r>> Δ is assumed and o LORr r= . As a result, the standard deviation of
1/roΔ is: 2/r r LORo o
rσ σ= .
To conclude, the initial condition of the state estimates and covariance matrix is
determined as in (4.27).
( )0|0 /
222 2 2 2 2
0|0 2
ˆ (0) (0) (0) (0) / 1 /
diag
Teel az m LOR LORlos e r
r ro o
LOR LOR
y V r r
Pr rω ω ω λ λ
ω λ λ
σ σσ σ σ σ σ
⎡ ⎤= −⎣ ⎦⎛ ⎞⎡ ⎤⎛ ⎞⎛ ⎞⎜ ⎟⎢ ⎥= ⎜ ⎟⎜ ⎟ ⎜ ⎟⎜ ⎟⎢ ⎥⎝ ⎠ ⎝ ⎠⎣ ⎦⎝ ⎠
& (4.27)
4.3 Hybrid Range Estimation
As stated in Section 4.2.3, the performance of the filter depends on the observability
of the scenario. This feature is what differs the estimation with a model based filter
from the classical triangulation. To improve the performance of the filter even when
the observability lacks, the EKF and Triangulation Algorithms are integrated. The
integration is performed by taking the range output of the triangulation as one of the
measurements provided to the filter as
( )/ 1 /
Teel azlos ez rω λ λ⎡ ⎤= ⎣ ⎦ (4.28)
Here, to minimize the linearization error, the range measurement is expressed as
reciprocal of range. The new measurement model is given in (4.29).
( )= ⋅ +k k kz C y h v
5 5 5 2
1 6
00 1
× ×
×
⎡ ⎤= ⎢ ⎥⎣ ⎦
IC
(4.29)
where kv is the measurement noise with the following covariance matrix.
48
2 2 2 2 2 2diag([ ])ω ω ω λ λσ σ σ σ σ σ=trik rR (4.30)
where σtrir is the standard deviation of the triangulated range error. Since the state is
reciprocal of range but the actual measurement error is acted on the range, the sixth
component of ( )kh v in (4.29) is nonlinear. The error of reciprocal of range 6( )Δh r in
terms of the error of range rΔ can be found from (4.31).
6 21 1( )
( )Δ Δ
Δ = − = ≈+ Δ + Δ
r rh rr r r r r r r
(4.31)
where >> Δr r is assumed. Following that, the measurement noise matrix H is
derived as follows:
266,6 72
( ) 1∂ Δ= = =
∂Δh rH y
r r
21 5 7diag([1 ])×=kH y
(4.32)
The Hybrid Estimation algorithm is described in Figure 4.1. The measurement model
consisting of the measurement vector ( kz ), the measurement matrix ( kC ), the
covariance matrix of the measurement ( kR ) and the measurement noise matrix ( kH )
are switched according to the "flag_tria" obtained from the Triangulation Algorithm.
The "flag_tria" takes the value of 1 whenever the following condition is satisfied:
thb b> meaning that the range calculated by triangulation is available. Thus, if
flag_tria==1, the measurement model is switched from the passive model to the
hybrid model defined in (4.28)-(4.32).
49
kr
( )
( ) ( )( )
1
| 1 | 1
| | 1 | 1
1
ˆ ˆ ˆ( ( ))
T T Tk k k k k k k k k k k
k k k k k k k k k
T
k k k k k k
T Tk k k k k
K P C C P C H R H
y y K z h y
P I K C P I K C
K H R H K
−
− −
− −
−
= +
= + −
= − −
+
1| 1 1| 1
| 1 1 1| 1 1
1 11 1
1 1ˆ ˆ
| 1 1 1| 1 1 1 1 1
ˆ ˆ( , )
,k k k k
k k k k k k
k kk k
k ky y
T Tk k k k k k k k k
y f y u
f fA Gy w
P A P A G Q G− − − −
− − − − −
− −− −
− −
− − − − − − − −
=
= =∂ ∂
= +
| 1ˆk ky −
|ˆk ky
, , ,k k k kz C H R
( )1
emk
a−
( )/ , ,
k kke
el azlos eω λ λ Passive Measurements
Hybrid Measurements
TriangulationAlgorithm
Figure 4.1- Hybrid Estimation Algorithm
50
4
51
CHAPTER 5
5. SIMULATIONS AND DISCUSSION
In this chapter, the performance of the algorithms described in Chapter 3 and Chapter
4 are studied. In the first part, the validations of the sensitivity relations derived in
Section 3.3 are performed and examples for the trajectory design are given. In the
second part, the behavior of the Passive EKF depending on observability of the
scenario, measurement noises and initialization is investigated. Finally, the
estimation results for the Hybrid EKF are presented.
5.1 Sensitivity Relations of Method of Triangulation
5.1.1 Validation of Sensitivity Relations
In this section, the objective is to validate the equations derived in Section 3.3 which
give the sensitivity of range obtained from the Triangulation to measurement errors
in IMU and seeker. The structure of the simulation is modified as illustrated in
Figure 5.1 so that the measurement errors of IMU and seeker do not cause a change
in the trajectory and only affects the range calculation. Moreover, the model of IMU
is modified so that the acceleration bias is added to the accelerometer measurement
defined in earth coordinates and in the navigation algorithm the measurements from
gyroscope are only utilized in calculating the Euler Angles.
The true range error and its percentage are calculated in the simulation as follows.
52
Δ = −true true triR R R
% 100truetrue
true
RRRΔ
Δ = × (5.1)
The sample trajectory used in this section is given in Figure 5.2.
coma
triR
rtP
rmP
trueR
Figure 5.1- Architecture of the Simulation
Figure 5.2- Trajectory and LOS rate
0 5 10 1550
100
150
200
250
300
350
400
h [m
]
time [s]
0 5 10 15
-4
-3.5
-3
-2.5
-2
-1.5
-1
-0.5
time [s]
LOS
rate
[deg
/s]
53
5.1.1.1 Validation of the Sensitivity to Accelerometer Bias Error
In this part, the range error obtained from (3.12) and (3.13) is compared to the
actual/true range error calculated from (5.1). The accelerometer error parameters are
assigned as 50ε = ob & 30mg=totb . Two cases are considered where the threshold of
the parallax angle is taken respectively as: ,1 8 mrad=thb & ,2 15 mrad=thb . The
results are presented in Figure 5.3-Figure 5.5.
From Figure 5.3, since there is no assumption made to derive equation (3.12), the
error calculated from (3.12) is equal to the actual value of the range error. However,
the derivation of (3.13) involves the assumption of ( )b T tλ= ⋅ & . Whenever this
assumption is violated, the error obtained from (3.13) diverges from the actual value
of the range error (this difference is illustrated in Figure 5.5). Because of ,2 ,1th thb b> ,
the time step (T) of the algorithm is greater for case 2 and therefore is the difference
given in Figure 5.5. However, for both cases this difference is quite small relative to
the total error given in Figure 5.3. Moreover, since the parallax angle is constant, T
changes inversely proportional to the change in the LOS rate given in Figure 5.2.
Figure 5.3- Range Error due to Accelerometer Bias
0 5 10 15-180
-160
-140
-120
-100
-80
-60
-40
-20
0
time [s]
Δ R
[m]
case 1
0 5 10 15-180
-160
-140
-120
-100
-80
-60
-40
-20
0
time [s]
case 2
trueEqn 3.12Eqn 3.13
54
Figure 5.4- Time Step (T)
Figure 5.5- Difference between the results obtained from (3.13)
and the actual range error
0 2 4 6 8 10 12 14 16 180
0.5
1
1.5T
[s]
time [s]
case 1case 2
0 2 4 6 8 10 12 14 16 18-15
-10
-5
0
5
10
Δ R
true- Δ
R(3
.13)
[m]
time [s]
case 1case 2
55
5.1.1.2 Validation of the Sensitivity to Gyroscope Bias Error
In this section, the validation of the sensitivity relations derived in (3.21) and (3.22)
is performed. The same cases are considered for the parallax angle thresholds. For
the gyroscope bias, o200 /qb h= is assigned. The results are given in Figure 5.6 and
Figure 5.7.
In derivation of (3.21), 1( )k kλ λ −− , 1kλ −Δ , ( )1k kλ λ −Δ −Δ are assumed to be small so
that their cosine and sine are equal to the first order series expansions. Since this
assumption is true, the range error obtained from (3.21) is very close to the actual
error. Furthermore, in derivation of (3.22), 1 ( )k k T tλ λ λ−− = & is assumed. As given in
Figure 5.7, whenever this assumption is violated, there will be a difference between
the true range error and the result obtained from (3.22). Since T is greater for case 2,
so is the difference given in Figure 5.7. However, as the difference is below 1 %, it
can be evaluated as ignorable.
Figure 5.6- Range Error due to Gyroscope Bias
0 5 10 15-15
-10
-5
0
5
10
15
20
25
time [s]
% Δ
R
case 1
0 5 10 15-15
-10
-5
0
5
10
15
20
time [s]
case 2
trueEqn (3.21)Eqn (3.22)
56
Figure 5.7- Difference between the result obtained from (3.22)
and the actual range error
5.1.1.3 Validation of the Sensitivity to Look Angle Noise Error
Since the look angle error is stochastic, the validation of (3.28) is carried out via
Monte Carlo analysis. The cases that will be considered are given in Table 5..
Table 5.1- Cases of Parallax Threshold
[mrad]thb
Case 1 8
Case 2 13
Case 3 15
In the simulation, the standard deviation of look angle noise is assigned as:
0.01oεσ = and at each Monte Carlo run another random variable with the same
standard deviation is created for the look angle noise. In the post-process of the
0 2 4 6 8 10 12 14 16 18-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
1.2%Δ
Rtru
e-%Δ
R(3
.22)
time [s]
case 1case 2
57
Monte Carlo runs, the standard deviation of the true range error at any given time j
are calculated from the unbiased variance estimate given in (5.2) [28].
( )22
1
11
n
i jji
xn
σ μ=
= −− ∑ (5.2)
where j denotes the number of samples taken at each run, n is the number of Monte
Carlo runs and 1
/n
j ii
x nμ=
=∑ is the mean of these runs at each time step j.
Figure 5.8- Std. of True Percentage of Range Error due to Seeker Noise
The standard deviation of the true percentage range error obtained from (5.2) is given
in Figure 5.8. It is observed that, as the threshold of parallax angle increases the
percentage error decreases as stated in Section 3.3.3. Moreover, since the look angle
is greater than 10o as given in Figure 5.9, the percentage error is nearly constant.
The comparison of the result obtained from (3.28) and true standard deviation are
given in Figure 5.10. The major assumption in deriving (3.26) was: 1k kλ λ −− >>
0 2 4 6 8 10 12 14 16 180
0.5
1
1.5
2
2.5
3
3.5
Mon
te C
arlo
σ o
f %Δ
Rtru
e
time [s]
bth=8mradbth=13mradbth=15mrad
58
1k kλ λ −Δ −Δ . Since in this case the assumption does not hold, the difference between
the actual standard deviation and the result obtained from (3.28) is significant.
Figure 5.9- Look Angle and angle a
Figure 5.10- Comparison of Std. obtained from (3.28) and True Percentage
Error (solid lines)
0 2 4 6 8 10 12 14 16 180
5
10
15
20
25
[deg
]
time [s]
true look anglea for bth=8mrad
0 2 4 6 8 10 12 14 16 180
0.5
1
1.5
2
2.5
3
3.5
Mon
te C
arlo
of σ
% Δ
R
bth=8mradbth=13mradbth=15mradeqn 3.28 bth=8mradeqn 3.28 bth=13mradeqn 3.28 bth=15mrad
59
5.1.2 Trajectory Design According to Sensitivity Relations
In this section, two cases are considered where in the first case only the
accelerometer bias and in the second case only the gyroscope bias is present in the
IMU error model. The bearing angle cstε is computed according to the parameters
given in Table 5.2.
Table 5.2- Trajectory Design Parameters
Case 1: Accelerometer Error Case 2: Gyroscope Error
Desired Range Error
Threshold max, 100 mdesRΔ = max,% 10desRΔ =
Max. Flight Time 18 sft = 18 sft =
Bias Error 26 mgtotb = 150 /oqb h=
The minimum bearing angles that satisfy the specifications for case 1 and 2 are
computed from (3.34) and (3.38) respectively as: 12.16ocstε = − & 7.45o
cstε = − . The
trajectories for these values are presented in Figure 5.11.
Figure 5.11- Constant Bearing Trajectories for case 1 & 2
0 2 4 6 8 10 12 14 16 180
50
100
150
200
250
300
350
400
h [m
]
time [s]
case 1 εcst= -12.16o
case 2 εcst= -7.45o
60
To show how the range error introduced by the accelerometer and gyroscope errors
changes, a Monte Carlo analysis is performed for each case. The Monte Carlo
variables related to acceleration and gyroscope errors are presented in Table 5.3 and
Table 5.4. In order to cover all possible values of the acceleration bias in x and z
channels, the direction of the bias is uniformly (U) distributed between 0o-360o.
Table 5.3- Monte Carlo Parameters for Accelerometer Bias Error
Bias direction ~ (0 ,360 )o ob Uε
Magnitude of bias ~ (20 mg,5 / 3 mg)totb N
Table 5.4- Monte Carlo Parameters for Gyroscope Bias Error
Magnitude of gyro bias o o~ (140 / ,10 / 3 / )qb N h h
In the simulation, the position of the target and the initial position of the missile is set
as: [ ] [ ]3500 0 0 , 0 0 200t mP m P m= = and the threshold of the parallax angle is
chosen to be: 5 mradthb = . The outputs of the Monte Carlo runs are presented in
Figure 5.12-Figure 5.14. In these figures, the red line represents the root-mean-
square value of the error calculated from (5.3) [28].
2
1
1rn
iji
xn =
= ⋅∑ (5.3)
where j denotes the number of samples taken at each run and n is the number of
Monte Carlo runs. In order to store the information of the sign of the result rj is
multiplied by sign( ix ).
From Figure 5.12, as it was defined earlier in (3.33), it is observed that the range
error due the accelerometer bias changes parabolically with time and the maximum
61
value occurs at tf / 2. As expected, at the maximum condition the error is kept below
100 m.
Figure 5.12- Monte Carlo Output of Range Error due to Accelerometer Bias
The percentage error due to gyroscope bias is given in Figure 5.13. It is observed that
the percentage error changes linearly with time. The maximum value occurs at both
ends of the flight and the minimum value at tf / 2 as also stated in Section 3.4.2.
Moreover, since the magnitude of the gyroscope bias is modeled as a Gaussian
distribution with 140 o/h mean and 10/3 o/h one sigma standard deviation, the
amplitude of the noise can take values greater than 150 o/h which was specified for
the trajectory design in Table 5.2. Therefore, for some runs the range error exceeds
10 % threshold error. Another reason is due to the assumptions made to derive the
equation (3.22).
From Figure 5.14, it seen that the total error changes parabollicaly with time. The
reason is that, since the range can be approximated for small look angles by
( )true m fR V t t= − ([1]), the total range error becomes a parabola with respect to time
as expressed in (5.4).
0 2 4 6 8 10 12 14 16 18-100
-80
-60
-40
-20
0
20
40
60
80
100Δ
Rtru
e [m]
time [s]
62
( )2 cot ( )truetrue true q f cst m f
true
RR R b t t V t tR
εΔΔ = ≈ − ⋅ − (5.4)
Figure 5.13- Monte Carlo Output of Per. Range Error due to Gyroscope Bias
Figure 5.14- Monte Carlo Output of Range Error due to Gyroscope Bias
0 2 4 6 8 10 12 14 16 18
-10
-5
0
5
10
% Δ
Rtru
e/Rtru
e
time [s]
0 2 4 6 8 10 12 14 16 18-400
-350
-300
-250
-200
-150
-100
-50
0
50
ΔR
true [m
]
time [s]
63
5.2 Estimation Performance of Passive EKF
In this section, firstly, the estimation performance depending on the observability of
the scenario is illustrated. For this purpose, two scenarios are selected which one is
unobservable and the other becomes observable after the missile maneuvers at a
specified time. Secondly, the effect of the measurement errors on the closed loop
filter dynamics is studied. Finally, the behavior of the filter depending on the
uncertainty in the initial range is investigated.
As the first five states are also the measurements provided to the filter, their
estimates will involve basically the filtering/removal of the noisy parts of the signals.
Therefore, in this section, only the results related to the estimation of /r r& and 1 / r
are given.
5.2.1 Effect of Observability on Filter Performance
To show the effect of the observability on the estimation performance two scenarios
are selected as presented in Figure 5.15. In scenario A, after the missile is launched,
it climbs with constant angle that is equal to the launch attitude. Here, since the
gravity is ignored, the acceleration command is zero. In scenario B, at a given time
the missile executes a maneuver so that the flight path angle approaches zero
resulting in level flight at constant altitude. The acceleration command produced by
the midcourse guidance is given in Figure 5.16.
The simulation and the filter parameters are presented in Table 5.5 and Table 5.6
respectively. As it is seen, the filter operates at its design conditions where the
standard deviations &ω λσ σ assigned in the filter are equal to those that are assigned
for the LOS rate and LOS angle measurement noises implemented in the Simulation.
64
Figure 5.15- Trajectory of Scenario A & B
Figure 5.16- Acceleration Command in Scenario B
0 2 4 6 8 10 12 14 16 180
100
200
300
400
500
600
700h
[m]
time [s]
ScAScB
0 2 4 6 8 10 12 14 16 180
5
10
15
20
25
30
35
time [s]
a com
[m/s
2 ]
65
Table 5.5- Simulation Parameters for Scenario A & B
Missile location [ ]0 0 50mP m= −
Target location [ ]4000 0 0tP m=
Initial attitude 10ooγ =
Standard deviation of LOS angle
measurement noise 0.1λσ = o
Standard deviation of LOS rate
measurement noise 0.05ωσ = o
Table 5.6- EKF Parameters
Standard deviation of LOS rate
measurement noise assigned in the filter 0.05ωσ = o
Standard deviation of LOS angle
measurement noise assigned in the filter 0.1λσ = o
Standard deviation of initial estimate of
range
10 m/sroσ =&
Standard deviation of initial estimate of
range
1000 mroσ =
Lock-on-Range Prediction 3000 mLORr =
Standard deviation of process noise 1 2 3
20.1 m/sσ σ σ= = =a a a
The range estimation in Scenario A is presented in Figure 5.17. Here, since the
acceleration is zero, the range is unobservable to the filter. Therefore, Kalman gains
are close to zero as given in Figure 5.18 which means that the filter is unable to
update the 1/r estimate from the measurements provided. As a result, the estimate
and the standard deviation given in Figure 5.17 and Figure 5.19 will be the outcomes
of the time propagation part of the filter. From Figure 5.19, it is seen that since the
66
estimate of 1/r is not corrected, the estimation error increases remaining in +/- 1 σ
theoretical limit calculated by the filter.
Figure 5.17- Range Estimation (Sc A)
Figure 5.18- Kalman Gain of state 1/r (Sc A)
0 2 4 6 8 10 12 14 16 18500
1000
1500
2000
2500
3000
3500
4000
4500
r [m
]
time [s]
trueest
0 2 4 6 8 10 12 14 16 18-6
-5
-4
-3
-2
-1
0
1x 10
-5
Kal
man
Gai
n of
1/r
time [s]
K(7,1)K(7,2)K(7,3)K(7,4)K(7,5)
67
Figure 5.19- Estimation Error in 1/r (Sc A)
In Scenario B, at the time the maneuver is executed the information about the range
can be acquired. From Figure 5.20, it is seen that, the estimate quickly converges to
the true value. This can also be observed from estimation error in Figure 5.21. Here,
after the problem becomes observable, the covariance update is realized and the
estimation error remains in the theoretical bound. Moreover, the Kalman gain given
in Figure 5.22 shows that the gain related to LOS rate (K(7,2)) is much larger than
the gain related to LOS angle (K(7,4)). In fact, K(7,4) is close to zero indicating that
the estimation of range is obtained mainly from LOS rate measurement, rather than
the LOS angle. The effect of measurement noises will be discussed in Section 5.2.2.
0 2 4 6 8 10 12 14 16 18-6
-4
-2
0
2
4
6x 10-4
erro
r in
1/r
[m- 1]
time
est. errfilt σ
68
Figure 5.20- Range Estimation (Sc B)
0 2 4 6 8 10 12 14 16 18-2
-1.5
-1
-0.5
0
0.5
1
1.5
2x 10-4
erro
r in
1/r
[m- 1]
time
est. errfilt σ
8 10 12 14 16 18
-1
-0.5
0
0.5
1
1.5
x 10-5
est. errfilter 2*σ
Figure 5.21- Estimation Error in 1/r (Sc B)
0 2 4 6 8 10 12 14 16 18500
1000
1500
2000
2500
3000
3500
4000
4500r [
m]
time [s]
trueest
69
Figure 5.22- Kalman Gain of state 1/r (Sc B)
The estimation of /r r& for both scenarios are given in Figure 5.23 and Figure 5.25.
As it is seen, for unaccelerated missile motion in Scenario A the estimation of /r r&
is still possible as stated in Ref. [23]. The reason that the estimation error of /r r&
given in Figure 5.25 is initially out of the bound is because the actual initial error is
greater than the initial uncertainty assigned in the filter. The effect of initialization
will be discussed in Section 5.2.3.
The LOS rate of Scenario B is lower than that of Scenario A as shown in Figure 5.24.
Since the estimation of /r r& is directly related to the LOS rate, the observability of
/r r& is lower in Scenario B than Scenario A. This can be observed from the “Zoom
View” in Figure 5.25 where the estimation error firstly increases and then decreases
with the increase in LOS rate.
4 6 8 10 12 14 16 18-0.005
0
0.005
0.01
0.015
0.02
0.025
0.03
Kal
man
Gai
n of
1/r
time [s]
K(7,1)K(7,2)K(7,3)K(7,4)K(7,5)
70
Figure 5.23- Range Rate over Range Estimation for Sc A & B
Figure 5.24- LOS rate profiles of Sc A & B
0 2 4 6 8 10 12 14 16 18
-0.25
-0.2
-0.15
-0.1
-0.05
-0.025
rDot
/r [s
-1]
time [s]
true Sc Aest Sc Atrue Sc Best Sc B
0 2 4 6 8 10 12 14 16 18-0.25
-0.2
-0.15
-0.1
-0.05
0
LOS
rate
[deg
/s]
time [s]
ScAScB
71
0 2 4 6 8 10 12 14 16 18-0.02
-0.015
-0.01
-0.005
0
0.005
0.01
time [s]
erro
r in
rDot
/r [s
-1]
est. err ScAest. err ScBfilter 2*σ ScAfilter 2*σ ScB
8 10 12 14 16 18-1
-0.5
0
0.5
1x 10-3
Figure 5.25- Estimation Error in /r r& of Sc A & B
5.2.2 Effect of Measurement Uncertainties on Filter Performance
The closed loop dynamics of the filter is determined by the Kalman Gain. This gain
modifies the eigenvalues of the closed loop system according to the relative
accuracies of LOS rate and LOS angle measurements. To understand how the
measurement errors affect the filter dynamics, the cases listed in Table 5.7 are
studied. Here, Scenario B is applied along with the parameters given Table 5.5 and
Table 5.6. The results are presented in Figure 5.26-Figure 5.29. The estimation of
range is illustrated after the problem becomes observable.
From Figure 5.26- Figure 5.28, it is seen that in case 1, 1ess credibility is given to
LOS angle compared to LOS rate measurement. The Kalman Gains associated with
elλ are quite small indicating that the estimation process is based mainly on LOS rate
72
measurement. Therefore, increasing λσ further as in case 2 will have little effect on
the estimation.
Table 5.7- LOS Rate and LOS Angle Measurement Noises
[deg]λσ [deg/ ]sωσ
Case 1 0.1 0.05
Case 2 0.2 0.05
Case 3 0.02 0.05
Case 4 0.02 0.1
Case 5 0.02 0.2
Case 6 0.02 0.03
In case 3, decreasing λσ to 0.02o improves the steady state standard deviation of 1/r
and /r r& . In this case, as the LOS angle measurement is more reliable, elλ
component of K(7,:) and K(6,:) gain increases. However, for K(7,:), since the
increase of elλ component is small and 2ω component is nearly the same compared to
case 1-2, the transient part of 1/r is not effected much. The reason that the increase
in elλ component of K(7,:) is insignificant is due to the fact that the accuracy of
estimating the range from LOS rate measurement is still higher than from LOS angle
measurement. Moreover, for /r r& estimation, the increase of elλ component of
K(6,:) results in a slightly faster response.
In case 4-5, since the LOS rate measurement is less reliable compared to case 3, 2ω
component of K(7,:) and K(6,:) gain decreases. This is compensated by increasing
elλ component and as a result the steady state standard deviations of /r r& and 1/r is
nearly kept the same as in case 3. However, the decrease of 2ω component leads to a
slower response. For /r r& estimation, it can be concluded that the relative values of
73
2ω and elλ components of K(6,:) have great impact on the transient behavior: the
decrease of 2ω component and the increase of elλ component caused a rise in the
undershoot as seen in case 4-5.
In case 6, as the LOS rate is more reliable, the gain associated with 2ω increases
which is responsible of faster response. However, the steady state values of
estimation errors are close to case 3.
Figure 5.26- ω2 and elλ components of K(7,:)
7.5 8 8.5
0
0.01
0.02
0.03
0.04
0.05
0.06
time [s]
ω2 component of K(7,:)
7.5 8 8.5-0.005
0
0.005
0.01
0.015
0.02
0.025
time [s]
λel component of K(7,:)
case 1case 2case 3case 4case 5case 6
case 1case 2case 3case 4case 5case 6
74
7.5 7.6 7.7 7.8 7.9 8 8.1 8.2 8.3 8.4 8.50
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
x 10-4
filte
r std
of 1
/r [m
-1]
time [s]
case 1case 2case 3case 4case 5case 6
8 8.5 9 9.5 10 10.50
0.2
0.4
0.6
0.8
1x 10-5
Figure 5.27- Filter std of 1/r
Figure 5.28- ω2 and elλ components of K(6,:)
0 2 4 6 8 10-0.05
0
0.05
0.1
0.15
0.2
time [s]
ω2 component of K(6,:)
0 2 4 6 8 10-0.05
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
time [s]
λel component of K(6,:)
case 1case 2case 3case 4case 5case 6
case 1case 2case 3case 4case 5case 6
75
Figure 5.29- Filter std. of r r/&
5.2.3 Effect of Initial Uncertainties on Filter Performance
In this work, since initially there is no information about the range, the initialization
of range estimate is based on LOR prediction. Hence, it is important to understand
how the uncertainty in initial estimate of range affects the filter performance.
Table 5.8- Initialization of o or r/& Uncertainty
Init 1 ro
LORrσ &
Init 2 2ro
oLOR
rr
σ⋅ &
Firstly in this section, for the initialization of the uncertainty in /o or r& the methods
given in Table 5.8 are studied. In these methods, the error in /o or r& is assumed to be
0 1 2 3 4 5 6 7 8 9 100
0.5
1
1.5
2
2.5
3
3.5
4x 10-3
filte
r std
of r
Dot
/r [m
-1]
time [s]
case 1case 2case 3case 4case 5case 6
76
induced by the error in range-rate and by the error in range, respectively. The
comparison of these methods are given in Figure 5.30-Figure 5.32. The parameters
are the same as in Table 5.5 and Table 5.6.
Since in this work the target is assumed to be stationary and the error in (0)mrV
(introduced by the seeker and accelerometer noises) is small, the contribution of the
error in range-rate to the error in /o or r& is ignorable. In fact, the error in /o or r& is
introduced mainly by the uncertainty of the initial range. Thus, as seen from Figure
5.30, when initialized by the second method, the filter is informed that the initial
condition of /o or r& is less reliable, than in the first case. The Kalman gains
associated with /r r& increase as given in so that the initial estimate can be corrected.
As a result, the transient behavior becomes more responsive. Moreover, from Figure
5.30, since the theoretical limits in case “Init 2” increases, the estimation error will
now remain inside the limits.
Figure 5.30- Estimation Error in /r r& for Init 1-2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-0.05
-0.04
-0.03
-0.02
-0.01
0
0.01
0.02
0.03
0.04
0.05
erro
r in
rDot
/r [s
- 1]
time
Init1 est. errInit1 filt 2*σInit2 est. errInit2 filt 2*σ
77
Figure 5.31- ω2 and elλ components of K(6,:) for Init 1-2
Figure 5.32- Range Rate over Range Estimation for Init 1-2
Secondly, to illustrate the effect of the uncertainty in initial range ( roσ ), the cases
listed in Table 5.9 are studied. In all cases the initial range estimate is taken as:
3000mLORr = . Moreover, the uncertainty of /r r& is initialized by the second
0 1 2 3 4 50
0.2
0.4
0.6
0.8
1
1.2
1.4
time [s]
ω2 component of K(6,:)
0 1 2 3 4 50
0.002
0.004
0.006
0.008
0.01
0.012
0.014
0.016
0.018
0.02
time [s]
λel component of K(6,:)
Init1 estInit2 est
0 1 2 3 4 5 6 7 8 9-0.09
-0.085
-0.08
-0.075
-0.07
-0.065
-0.06
-0.055
-0.05
-0.045
rDot
/r [s
-1]
time [s]
trueInit1 estInit2 est
78
initialization procedure defined in Table 5.8. The results are given in Figure 5.33-
Figure 5.36.
Table 5.9- Initialization of rLOR Uncertainties
[ ]or mσ
Case 1 400
Case 2 1000
Case 3 3000
Case 4 5000
As seen from Figure 5.33, the value of the initial uncertainty (orσ ) does not affect
the estimation of range until the missile executes a maneuver. This is due the fact
that up to that point the estimate is determined by the time propagation part of the
filter which is basically equal to the following integration:
7, 7, 1 7, 1 6, 1k k k ky y T y y− − −= − ⋅ . However, as seen from Figure 5.35, since /r r& is
observable before the maneuver and the initialization is performed by "Init 2", orσ
have an influence on the estimation of /r r& .
Figure 5.33- Range Estimation
7.4 7.6 7.8 8 8.2 8.40
1000
2000
3000
4000
5000
time [s]
7.4 7.6 7.8 8 8.2 8.41600
1800
2000
2200
2400
2600
2800
3000
r [m
]
time [s]
truecase 4 est.case 3 est.
truecase 2 est.case 1 est.
79
Figure 5.34- Estimation Error in 1/r
As seen from Figure 5.34 and Figure 5.36, in case 1, since the initial uncertainty is
assigned 400 m but actually it is 1003.12 m, the resulting error will be outside the
theoretical limits. Since the filter evaluates that the initial condition of 3000mLORr =
is more reliable than in the case 2-4, the Kalman gain will be smaller leading to a
slower response.
As the initial uncertainty is increased the filter quickly attempts to correct the initial
condition by increasing the Kalman gain. As a result, the transient behavior becomes
more responsive. When the initial uncertainty is assigned to big as in case 4, 1/r and
/r r& estimates exhibit large over/undershoots at the transient part. If such estimates
are utilized in an advanced guidance algorithms, the performance of the missile may
degrade leading to undesirable acceleration commands. To prevent this, some
precautions can be taken, such as assigning small initial uncertainty of range esimate,
filtering the estimates, restricting the estimates between given limits, utilizing the
estimates after the transient part is over etc.
7.4 7.5 7.6 7.7 7.8 7.9 8-1
-0.5
0
0.5
1x 10-3
time [s]
7.4 7.6 7.8 8-2
-1.5
-1
-0.5
0
0.5
1
1.5
2x 10-4
erro
r in
1/r [
m-1
]
time [s]
case 4 est.errcase 4 filt σcase 3 est.errcase 3 filt σ
case 2 est.errcase 2 filt σcase 1 est.errcase 1 filt σ
80
Figure 5.35- Range Rate over Range Estimation
Figure 5.36- Estimation Error in /r r&
5.3 Estimation Performance of Hybrid EKF
In this section, the results of the Hybrid EKF are presented for Scenario A and B.
The standard deviation of the range information provided by the Triangulation is
assigned as: 100 mσ =trir . Moreover, the threshold of the parallax angle is taken as:
25 mradthb = .
0 1 2 3 4-0.12
-0.1
-0.08
-0.06
-0.04
-0.02
time [s]
0 1 2 3 4-0.08
-0.075
-0.07
-0.065
-0.06
-0.055
-0.05
-0.045
rDot
/r [s
-1]
time [s]
truecase 2 est.case 1 est.
truecase 4 est.case 3 est.
0 0.5 1 1.5 2
-0.1
-0.05
0
0.05
0.1
time [s]
0 1 2 3
-0.02
-0.01
0
0.01
0.02
erro
r in
rDot
/r [s
-1]
time [s]
case 2 est.errcase 2 filt σcase 1 est.errcase 1 filt σ
case 4 est.errcase 4 filt σcase 3 est.errcase 3 filt σ
81
The results of the Hybrid EKF for Scenario A are presented in Figure 5.37-Figure
5.39. As seen in Figure 5.37, the passive EKF is updated with the range information
obtained from the triangulation. The filter is still unobservable between the times
where the update does not take place. Between the updates the estimate is determined
by the time-propagation part of the filter and in each step of update the initial
condition of the time-propagation part is corrected.
The reason that the estimation error given in Figure 5.38 is outside the theoretical
limits is because trirσ assigned in the filter does not represent the actual case. From
Figure 5.39, it can be observed that between 2-4 s the error in triangulated range
exceeds the 3 σ± ⋅trir limit. The hybrid measurement model can be improved by
taking the sensitivity of the triangulation to look angle errors into account.
Figure 5.37- Range Estimation (Sc A)
0 2 4 6 8 10 12 14 16 180
500
1000
1500
2000
2500
3000
3500
4000
4500
r [m
]
time [s]
trueEKF+tritri
82
Figure 5.38- Estimation Error in 1/ r (Sc A)
Figure 5.39- Estimation Error in r (Sc A)
0 2 4 6 8 10 12 14 16 18-1.5
-1
-0.5
0
0.5
1
1.5x 10-4
erro
r in
1/r
[m- 1
]
time
est. errfilt σ
0 2 4 6 8 10 12 14 16 18-200
0
200
400
600
800
1000
1200
erro
r in
r [m
]
time [s]
EKF (passive)EKF+tri (hybrid)tri
83
The results of the Hybrid EKF for Scenario B are presented in Figure 5.40-Figure
5.42. As a result of the integration of triangulation and EKF, the estimation
performance prior to the maneuver is improved. Since the update from the
triangulation leads to a decrease of the estimation error, when the system becomes
observable the overshoot in the transient part will be smaller compared to Figure
5.20.
Figure 5.40- Range Estimation (Sc B)
0 2 4 6 8 10 12 14 16 180
500
1000
1500
2000
2500
3000
3500
4000
4500
r [m
]
time [s]
trueEKF+tritri
84
Figure 5.41- Estimation Error in r (Sc B)
Figure 5.42- Estimation Error in 1/r (Sc B)
0 2 4 6 8 10 12 14 16 18-400
-200
0
200
400
600
800
1000
1200er
ror i
n r [
m]
time [s]
EKF (passive)EKF+tri (hybrid)tri
0 2 4 6 8 10 12 14 16 18-1.5
-1
-0.5
0
0.5
1
1.5x 10
-4
erro
r in
1/r
[m- 1
]
time
est. errfilt σ
85
CHAPTER 6
6. CONCLUSION
In this thesis, the estimation of the relative range of a stationary target with respect to
a missile is studied. The estimation is performed by utilizing the measurements
obtained from a gimballed passive seeker and missile's navigational information.
Two different approaches are investigated; the Method of Triangulation and the
Extended Kalman Filter.
The method of triangulation which is employed in a number of fields is used in this
work to calculate the range between a stationary target and a moving missile. The
formulation of triangulation is given for 3D missile-target geometry. Since this
method have no information about the stochastical properties of the measurements
and thus no filtering mechanism, it is greatly affected by the measurement errors
which directly propagate into the calculation. In this work, the sensitivity of range to
measurement errors in IMU and seeker is studied. An expression that relates the
uncertainty of range estimation to the uncertainties in these measurements is derived.
From sensitivity analysis, in case of gyroscope and accelerometer error, it is
concluded that the range error depends on the trajectory which is a product of the
guidance strategies that is employed. In order to give an idea on how the sensitivity
relations can be used, the Constant Bearing Midcourse trajectory is considered. Here,
the constant bearing angle is selected according to the sensitivity relations so that the
range error is below a desired level.
86
Moreover, from sensitivity analysis, the problem of "geometric dilution" is identified
which is also a main concern in satellite positioning. This problem is handled by
imposing a condition on the parallax angle at the steps where the range calculation is
performed so that for a given standard deviation of look angle noise, a desired value
of range error can be obtained. In addition, the maximum accuracy of this method for
a given look angle standard deviation is found.
Secondly, the EKF which is a recursive estimation algorithm is formulated for 3D
missile-target geometry. The initialization of the algorithm is described for each state
in detail. It is known that the performance of this filter depends on the observability
of the scenario. The necessary condition for a scenario to be observable is that the
observer should execute a maneuver. In fact, it is this feature which differs the
estimation with a Kalman based filter from the classical triangulation method. To
help to increase the performance of the filter even when the observability lacks, the
EKF and Triangulation Algorithms are integrated. The integration is performed by
taking the range output of the triangulation as one of the measurements provided to
the filter. It is noted that, the measurement model of the hybrid filter can be improved
by taking the sensitivity of range to look angle errors into account. As a result of this
integration, the accuracy of range estimation is improved compared to the method of
triangulation and EKF with only passive measurements. The improvement is
especially obvious for unobservable scenarios.
The present study can be improved with the following works:
• In order to understand how the measurement errors of IMU and seeker in yaw
plane propagate into the triangulation, the sensitivity analysis can be formulated in
3D space.
• The sensitivity analysis can be performed for more realistic case where the
accelerometer measurement is expressed in body frame and the gyroscope
measurement error introduces an error in the displacement vector.
87
• The performance of the EKF under off-design conditions where the actual
measurement errors are different from the standard deviations assigned in the
filter, the input acceleration is corrupted by the IMU error model, the seeker
gimbals are not unity, etc. should be studied.
• In case of stationary targets, the states of the EKF can be modified so that the
estimation of range-to-go can be realized when the observer does not maneuver.
• The performance of the range estimation can be shown on an advanced guidance
law that utilizes the range information in its mechanization.
88
89
REFERENCES
[1] P. Zarchan, Tactical and Strategic Missile Guidance, AIAA Tactical Missile
Progress in Astronautics and Aeronautics, Vol. 124, 1990.
[2] G. M. Siouris, Missile Guidance and Control Systems, Springer Verlag New
York, Inc., 2004.
[3] S.K. Jeong, S.J. Cho, E.G. Kim. “Angle Constraint Biased PNG”, 5th Asian
Control Conference, 2004.
[4] B.S., Kim, J.G. Lee, H.S. Han. “Biased PNG Law for Impact with Angular
Constraint”, IEEE Transactions on Aerospace and Electronic Systems, Vol. 34,
No. 1, 1998.
[5] N.R. Iyer. “Recent Advances in Anti-tank Missile Systems and Technologies”,
SPIE Conference on Photonic Systems and Applications in Defense and
Manufacturin, Vol. 3898, 1999.
[6] N.F. Palumbo. “Guest Editor’s Introduction: Homing Missile Guidance and
Control”, John Hopkins APL Technical Digest, Vol. 29, No.1, 2010.
[7] P.A. Hawley, R. A. Blauwkamp. "Six-Degree-of-Freedom Digital Simulations
for Missile Guidance, Navigation, and Control", John Hopkins APL Technical
Digest, Vol. 29-1,2010.
[8] C. Lin, Modern Navigation, Guidance, and Control Processing, Prentice-Hall,
1991.
[9] E. Song, M. Tahk, “Real-time midcourse missile guidance robust against launch
conditions”, Control Engineering Practice, Vol.7, Issue 4,1999.
[10] D. Simon. Optimal State Estimation: Kalman, H Infinity, and Nonlinear
Approaches. Wiley-Interscience, June 2006.
[11] L.G. Taff. "Target Localization From Bearings-Only Observations", IEEE
Transactions on Aerospace and Electronic Systems, Vol.33, No.1. 1997.
90
[12] R. Pieper, A.W. Cooper, G. Pelegris. "Passive Range Estimation Using Dual-
Baseline Triangulation", Optical Engineering, Vol.35, No.3. 1996.
[13] W.J. Smith,. Modern Optical Engineering: The Design of Optical Systems,
McGraw-Hill, 1990.
[14] P. Koparde, V. P. Panakkal. "Target Range Computation Using Stationary
Passive Single Sensor Measurements by Batch Processing", Radar Conference,
2012.
[15] I.S. Jeon, J.I. Lee, M. J. Tahk. "Impact-time-control guidance law for anti-ship
missiles", IEEE Transactions on Control Systems Technology, Vol. 2, No. 14,
2006.
[16] E. J. Ohlmeyer, C.A. Phillips. “Generalized Vector Explicit Guidance,”. Journal
of Guidance, Control, and Dynamics, Vol. 29, No. 2, 2006.
[17] I.S. Jeon, J.I. Lee, M. J. Tahk." Guidance Law to Control Impact Time and
Angle", IEEE Transactions on Aerospace and Electronic Systems, Vol. 43, No.
1, 2007.
[18] Harl, H., Balakrishnan, S.N. "Impact Time and Angle Guidance With Sliding
Mode Control", IEEE Transactions on Control Systems Technology, Vol. 20,
No. 6, 2012.
[19] Hull, D.G., Radke, J.J, Mack, R.E.. "Time-to-Go Prediction for Homing Missiles
Based on Minimum-Time Intercepts", Journal of Guidance, Vol.14, No.5, 1991.
[20] C.K. Ryoo, H. Cho, M. Tahk. "Time-to-Go Weighted Optimal Guidance With
Impact Angle Constraints", IEEE Transactions on Control Systems Technology.
Vol. 14, No. 3, 2006.
[21] M. Tahk., C.K. Ryoo, H. Cho. "Recursive Time-To-Go Estimation for Homing
Guidance Missiles", IEEE Transactions on Aerospace and Electronic Systems.
Vol. 38, No. 1, 2002.
[22] Aidala, V. J., "Kalman Filter Behavior in Bearings-Only Tracking
Applications", IEEE Transactions on Aerospace and Electronic Systems, Vol.
AES-15, July 1979, pp. 29-39.
91
[23] Aidala, V. J., and Hammel, S. E., "Utilization of Modified Polar Coordinates for
Bearings-Only Tracking", IEEE Transactions on Automatic Control, Vol. AC-
28, Aug. 1983, pp. 283-294.
[24] S. C Nardone, V. J.Aidala , "Observability Criteria for Bearings-Only Target
Motion Analysis", IEEE Transactions on Aerospace and Electronic Systems,
Vol. 17, No. 2, 1981.
[25] K. Doğançay, G. Ibal. "3D Passive Localization in the Presence of Large
Bearing Noise", IEEE Signal Processing Conference, 2005.
[26] W. H. Foy.“Position-location solutions by Taylor series estimation”, IEEE
Trans. on Aerospace and Electronic Systems, Vol. 12, No. 2, 1976.
[27] N.A. Shneydor. Missile Guidance and Pursuit. Horwood Publishing. 1998.
[28] P. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation
Systems, Artech House, 2008.
[29] M. Tahk, H. Ryu, E. Song. “Observability Characteristics of Angle-Only
Measurement Under Proportional Navigation", 34st Society of Instrument and
Control Engineers Conference, 1995.
92
93
APPENDIX A
A. PROOF OF xy=-1
In this section the goal is to show: 1= −xy where sign( ( )) λ= &x t and
1sign( )γ λ −= −P ky . Firstly, for planar missile-target geometry, the relation between
the LOS rate and the look angle will be derived.
From (2.10), the angular rate of LOS vector expressed in LOS frame is defined in
(8.1).
( ) ( )( )
/ 2ω ×=
los loslos
los er V
r (8.1)
where for planar geometry; ( ) [ 0 0]=los Tr r is the LOS vector,
( ), ,[ 0 ]⊥= ll
los Tr r rLOS LOS
V V V is the relative velocity vector and ( )/ [0 ( ) 0]ω λ= &los T
los e t
is the angular rate of LOS vector written in LOS coordinates. After these are inserted
into (8.1), the LOS rate is obtained as follows.
,( )λ ⊥= −& r LOSV
tr
(8.2)
Since in this work the target is assumed to be stationary, the relative velocity vector
is equal to the minus of the missile velocity. Thus: , ,⊥ ⊥= −r mLOS LOSV V . The
component of the missile velocity perpendicular to LOS can be found from following
equation.
94
( ) ( , ) ( )cos 0 sin cos
0 1 0 0 0sin 0 cos 0 sin
ε ε ε
ε ε ε
−⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥ ⎢ ⎥= = =⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦
mlos los b b
m m m
VV C V V (8.3)
As a result: , sin ε⊥ =m mLOSV V . Inserting this into (8.2),
sin ( )( ) ελ =& mV ttr
(8.4)
From the equation given in (8.4), since 0 & 0> > mR V , following can be written.
sign( ( ) ( )) 1λ ε = +& t t (8.5)
As shown in Figure 8.1, the angle of the displacement vector can be expressed in
terms of the angular difference ( γΔ p ) of the velocity vector as follows:
1γ γ γ−= + ΔP k p .
Pγ
PΔrr
V
k 1γ −
PγΔ
M@ k
M @ k-1 Figure 8.1- Displacement and Velocity Vector
From 1 1 1ε λ γ− − −= −k k k , the difference of 1γ λ −−P k is rewritten in terms of the look
angle as:
1 1γ λ ε γ− −− = − + Δp k k p (8.6)
Since the look angle is greater than the angular difference 1ε γ− > Δk p , the sign of
(8.6) will be determined by the sign of 1ε −− k as given in (8.7).
95
1 1 1sign( ) sign( ) sign( )γ λ ε γ ε− − −− = − + Δ = −p k k p k (8.7)
Finally, inserting (8.7) into (8.5), results in,
1sign( ( )( )) 1λ γ λ −− = = −&p kt xy (8.8)
96
97
APPENDIX B
B. CLOSED FORM SOLUTION OF CONSTANT BEARING
GUIDANCE
In Proportional Navigation Guidance, the flight path rate (γ& ) will be equal to the
LOS rate (λ& ) multiplied by the navigation gain N as given in (8.9) [27].
( ) ( )γ λ= && t N t (8.9)
where λ ε γ= +& & & . It is clear that, if N=1, the flight path angle rate equals the LOS
rate: γ λ= && . As a result, the bearing rate will be zero: 0ε =& , leading to constant
bearing angle throughout the flight. The aim in this section is to provide the closed
form solution of the LOS rate for the Constant Bearing Guidance.
The acceleration of the LOS vector with respect to the inertial frame is found as
follows:
( )2 ( )
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )/ / / /2 2ω ω ω ω= + × + × + × ×&&& &
loslos los los los los los los los
los e los e los e los ee
d r r r r rdt
(8.10)
From this equation, the normal component of the acceleration for planar geometry is
found as in (8.11).
98
2λ λ⊥ = − −&& & &a r r (8.11)
The acceleration command produced by the Proportional Navigation Guidance in
case of N=1 is expressed as follows:
,com λ⊥ = &a V (8.12)
When the closed loop dynamics is assumed as unity, (8.11) and (8.12) will be equal.
Moreover, the angle between the velocity vector and the LOS vector is assumed to be
small so that following linear expressions can be obtained: = −& mr V and
( )= −m fr V t t . As a result of equating (8.11) and (8.12), the differential equation in
(8.13) is found.
0ft tλλ − =−
&&& (8.13)
Solving this equation for the initial condition of ( )o otλ λ=& & , results in
( )λ λ−
=−
& & f oo
f
t tt
t t (8.14)
The initial condition is defined according to (8.4) as ( ) sin /λ ε=&o m o ot V r where
( )cosε= −o m o f or V t t and ε ε=o cst . Finally, the closed form solution of LOS rate for
Constant Bearing Guidance is obtained as follows:
tan( ) ελ =−
& cst
ft
t t (8.15)
99
APPENDIX C
C. JACOBIANS OF THE EXTENDED KALMAN FILTER
In this sections, the linearization of the state transition f defined in (4.3)-(4.4) are
presented. The linearization of the transition function f around the state estimate is
shown as follows.
-1-1
-1 ˆ -1| -1
kk
k yk k
fAy∂
=∂
(8.16)
Following this differentiation, the transition matrix is found as in (8.17).
[ ]
( )( )
1 2 3 4 5 6 7
1 4 7 2 5 3 5 3 7 5 6
1 2 5 3 5 6
2 4 7 1 5 3 6 5
; ; ; ; ; ;
[ 1 2 , 0, 0, cos sin sin 6 , cos cos ,
2 , sin cos sin ]
[ 0 , 1 2 , 0 , ( cos cos sin ),
A A A A A A A A
A Ty Ty a y a y y Ta y y y
Ty T a y a y y
A Ty Ty a y a y y
=
= − −
− +
= − − −
3 7 5 6 2 1 5 3 5 6
3 4 7 5 2 6 1 6
7 5 1 6 2 6 3 5 2 6 1 6
4 6
cos sin , 2 , ( sin cos cos )]
[ 0 , 0 , 1 2 , sin ( cos sin ), cos ( cos sin ), 2 , cos ( cos sin )]
[ sin ,
Ta y y y Ty T a y a y y
A Ty Ty y a y a yTy y a y a y Ty T y a y a y
A T y T
− − +
= − − −
− + − −
= − 6 1 6 2 6
5 6 5 5 62
1 6 2
2 6 6
6 5
15
[ cos tan , tan sin , , ( cos sin ) / cos
cos , 0, 1, ( cos sin ), 0, 0]
tan ( cos sin ) 1,0,0]T T T T y y y y y
T y
y T y y y y
A y y y yy y y y
+
− +
− +
=
(8.17)
100
2 2 2
26 1 1 5 6 3 5 6 5 2 5 6 6
22 2 5 2 5 6 3 5 5 6 1 5 6 6
5 3 5 1 6 5 2 5
2
6
2
2 cos ( cos cos sin sin si
[2 ( cos cos cos cos sin cos cos sin ),
2 ( cos cos cos cos sin sin cos cn ),
os sin ),
A y y y y y y y y y y y y
y y y y y y y y y y y y y yT y y y y y y y y
T
Ty
− + −
− + +
+ +
−
=
1 5 6 3 5 2 5 6 3 5 1 6 5
2 5 6 7 3 5 1 6 5 2 5 6
7 5 2 6 1 6 2 6 1 6 1
(2( cos cos - sin cos sin ) ( cos + cos sin ... ... sin sin ) - ( cos cos sin sin sin )), ( cos ( cos - sin ) - ( cos - sin ) (2 cos
T y y y y y y y y y y y y yy y y y a y a y y a y y
T y y a y a y y y y y y
+ ⋅
+ + +
⋅ 6 2 6
5 2 6 1 6 3 5 1 6 5 2 5 6
4
1 5 6 3 5 2 5 6
2 sin )... ... 2sin ( cos - sin )( cos cos sin sin sin )), 1 - 2 ( cos cos - sin cos sin )]
y y yy y y y y y y y y y y y y
TyT a y y a y a y y
+
+ + +
+
7 7 4[ 0, 0, 0, 0, 0, - , 1 - ]A Ty Ty=
where the state estimate at k given k-1 is: [ ]1 2 3 4 5 6 7-1ˆ Tk ky y y y y y y y= .
Moreover, the linearization of f with respect to input noise is derived as follows:
1 11
1 1ˆ ˆ1| 1 1| 1
6 4 6 4 5
6 4 6 4 5
6 4 5 6 4 5
1
6 4 5 6 4 5 6 4
0 sin cos sinsin 0 cos cos
cos sin cos cos 0 0 0 0
0 0 0cos cos cos sin sin
0 0 0
− −−
− −− − − −
−
= =∂ ∂
⎡ ⎤⎢ ⎥− −⎢ ⎥⎢ ⎥−⎢ ⎥= ⎢ ⎥⎢ ⎥⎢ ⎥
−⎢ ⎥⎢ ⎥⎣ ⎦
k kk
k ky yk k k k
k
f fGw a
Ty y Ty y yTy y Ty y y
Ty y y Ty y yG
Ty y y Ty y y Ty y
(8.18)
