Applied Mathematics in Integrated Navigation Systems, Third Edition (AIAA Education) [3 ed.] 1563479273, 9781563479274

Citation preview

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Applied Mathematics in Integrated Navigation Systems

Third Edition

Applied Mathematics in Integrated Navigation Systems Third Edition Robert M. Rogers Rogers Engineering & Associates Gainesville, Florida

EDUCATION SERIES Joseph A. Schetz Series Editor-in-chief Virginia Polytechnic Institute and State University Blacksburg, Virginia

Published by American Institute of Aeronautics and Astronautics, Inc. 1801 Alexander Bell Drive, Reston, VA 20191-4344

American Institute of Aeronautics and Astronautics, Inc., Reston, Virginia

1 2

3 4 5

Library of Congress Cataloging-in-Publication data on file ISBN-13: 978-1-56347-927-4 ISBN-10: 1-56347-927-3

Copyright # 2007 by the American Institute of Aeronautics and Astronautics, Inc. All rights reserved. Printed in the United States of America. No part of this publication may be reproduced, distributed, or transmitted, in any form or by any means, or stored in a database or retrieval system, without the prior written permission of the publisher. Data and information appearing in this book are for informational purposes only. AIAA is not responsible for any injury or damage resulting from use or reliance, nor does AIAA warrant that use or reliance will be free from privately owned rights.

AIAA Education Series Editor-in-Chief Joseph A. Schetz Virginia Polytechnic Institute and State University

Editorial Board Takahira Aoki University of Tokyo

Rakesh K. Kapania Virginia Polytechnic Institute and State University

Edward W. Ashford Karen D. Barker Brahe Corporation

Brian Landrum University of Alabama Huntsville

Robert H. Bishop University of Texas at Austin

Timothy C. Lieuwen Georgia Institute of Technology

Claudio Bruno University of Rome

Michael Mohaghegh The Boeing Company

Aaron R. Byerley U.S. Air Force Academy

Conrad F. Newberry Naval Postgraduate School

Richard Colgren University of Kansas

Mark A. Price Queen’s University Belfast

Kajal K. Gupta NASA Dryden Flight Research Center

James M. Rankin Ohio University

Rikard B. Heslehurst Australian Defence Force Academy

David K. Schmidt University of Colorado Colorado Springs

David K. Holger Iowa State University

David M. Van Wie Johns Hopkins University

Foreword

We are very happy to present the third edition of Applied Mathematics in Integrated Navigation Systems by Robert M. Rogers. The first two editions have been very well received, and we are certain that this comprehensive and in-depth treatment of such a timely and important topic in the aerospace field, as well as others, will be equally well received by the technical community. The book has sixteen chapters divided into two main parts and several appendices, including a bibliography, all in more than 400 pages. There is a detailed Preface and software for download. Robert Rogers is extremely well qualified to write this book, because of his broad and deep expertise in the area. His command of the material is excellent, and he is able to organize and present it in a very clear manner. The AIAA Education Book Series aims to cover a very broad range of topics in the general aerospace field, including basic theory, applications and design. Information about a complete list of titles can be found on the last page of this volume. The philosophy of the series is to develop textbooks that can be used in a university setting, instructional materials for continuing education and professional development courses, and also books that can serve as the basis for independent study. Suggestions for new topics or authors are always welcome. Joseph A. Schetz Editor-in-Chief AIAA Education Series

Table of Contents

Preface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Part 1.

xiii

Elements of Integrated Navigation Systems

1.

Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3

2.

Mathematical Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 Vector/Matrix Algebra . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Vector/Matrix Calculus . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Linearization Techniques. . . . . . . . . . . . . . . . . . . . . . . . 2.4 Direction Cosine Matrices . . . . . . . . . . . . . . . . . . . . . . . 2.5 Miscellaneous Mathematical Topics . . . . . . . . . . . . . . . . . 2.6 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7 7 13 17 19 30 32 32

3.

Coordinate Systems and Transformations 3.1 Coordinate Systems . . . . . . . . . . . 3.2 Coordinate Frame Transformations . 3.3 Chapter Summary . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . .

............... ............... ............... ............... ...............

49 49 54 64 65

4.

Earth Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Ellipsoid Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Ellipsoid Gravity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

71 71 79 87 87

5.

Terrestrial Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1 Strap-Down Navigation Systems . . . . . . . . . . . . . . . . . . 5.2 Local-Level Navigation-Frame Mechanization Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 Perturbation Form of Navigation System Error Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

101 101

ix

. . . .

102 105

x 5.4 5.5 5.6 5.7

Navigation System Attitude Error Equations: C Formulation. . . . . . . . . . . . . . . . . . . . . . . . Navigation System Error Equations Using Alternative Velocity Error . . . . . . . . . . . . . . . . . . . . . . . . Vertical Channel . . . . . . . . . . . . . . . . . . . . . . . . Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

....

113

. . . .

113 116 119 120

. . . .

. . . .

. . . .

6.

Navigation Sensor Models . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1 Gyro Performance Characterizations . . . . . . . . . . . . . . . 6.2 Sensor Error Models . . . . . . . . . . . . . . . . . . . . . . . . . 6.3 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

135 135 140 149 149

7.

Navigation Aids . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.1 Doppler Velocity Sensors . . . . . . . . . . . . . . . . . . . . . . 7.2 Tactical-Air-Navigation Range . . . . . . . . . . . . . . . . . . . 7.3 Global-Positioning-System Range . . . . . . . . . . . . . . . . . 7.4 Forward-Looking Infrared Line-of-Sight Systems . . . . . . . 7.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

153 154 157 160 169 172 172

8.

Kalman Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1 Recursive Weighted Least Squares: Constant Systems . . . . 8.2 Recursive Weighted Least Squares: Dynamic Systems . . . . 8.3 Discrete Linear Minimum Variance Estimator . . . . . . . . . 8.4 U – D Factored Form . . . . . . . . . . . . . . . . . . . . . . . . . 8.5 Summed Measurements . . . . . . . . . . . . . . . . . . . . . . . 8.6 Combined Estimate from Two (or More) Kalman Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.7 Derivative-Free Estimation. . . . . . . . . . . . . . . . . . . . . . 8.8 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

181 182 187 191 194 201

Part 2. 9.

204 207 221 222

Applications

Strap-Down Inertial Sensor Laboratory Calibration 9.1 Navigation Mechanization Review. . . . . . . . . 9.2 Sensor Error Model . . . . . . . . . . . . . . . . . . 9.3 Solutions for Sensor Errors . . . . . . . . . . . . . 9.4 Data-Collection Rotation Sequences . . . . . . . 9.5 Observation Equations . . . . . . . . . . . . . . . . 9.6 Processing Sequences. . . . . . . . . . . . . . . . . 9.7 Simulated Laboratory Data Calibration . . . . . . 9.8 Chapter Summary . . . . . . . . . . . . . . . . . . .

. . . . . . . .

........ ....... ....... ....... ....... ....... ....... ....... .......

237 238 238 239 239 241 244 247 250

xi 10.

Flight-Test Evaluations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.1 Optical Tracking Trajectory Reconstruction . . . . . . . . . . 10.2 Tactical-Air-Navigation/Inertial-Navigation-Unit Reconstruction . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.3 Vehicle Dynamics with Radar Tracking Trajectory Reconstruction . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.4 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . .

251 252

Navigation System Ground Alignment . . . . . . . . . . . . Initial Coarse Alignment and Resulting Errors . . . . . . . . Fine-Alignment Kalman Filter . . . . . . . . . . . . . . . . . . Simulated Ground Fine Alignment. . . . . . . . . . . . . . . . Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . .

275 276 279 281 287

256 260 273

11.

Inertial 11.1 11.2 11.3 11.4

12.

Integration via Kalman Filtering: Global-Positioning-System Receiver . . . . . . . . . . . . . . . . . . . . 12.1 Global-Positioning-System Receiver Kalman Filter Configurations . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.2 Inertial-Navigation-System Configuration Kalman Filter . . 12.3 Simulated Global-Positioning-System-Receiver Inertial-Navigation-System Kalman Filter Operation . . . 12.4 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . .

290 290

13.

In-Motion Alignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.1 Transfer Alignment . . . . . . . . . . . . . . . . . . . . . . . . . 13.2 Alignment Without Benefit of Attitude Initialization . . . . 13.3 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . .

305 306 315 324

14.

Integrated Differential Global Positioning System/Dead-Reckoning Navigation . . . . . . . . . . . . . . . . . . . 14.1 Dead-Reckoning Navigation Equations . . . . . . . . . . . . . 14.2 Dead-Reckoning System Error Model. . . . . . . . . . . . . . 14.3 Differential Global-Positioning-System Position Observations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.4 Integrated Dead-Reckoning/Differential Global-Positioning-System Implementation. . . . . . . . . 14.5 Test Conditions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.6 Test Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.7 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . .

15.

Attitude-Determination and Estimation . . . . . . . . . . . . . . . . . 15.1 Terrestrial Attitude-Determination . . . . . . . . . . . . . . . . 15.2 Attitude-Determination by Iteration . . . . . . . . . . . . . . . 15.3 Attitude Estimation . . . . . . . . . . . . . . . . . . . . . . . . . 15.4 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . .

289

299 304

327 328 329 332 332 333 334 337 339 339 344 347 359

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

xii 16.

Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 361

A.

Pinson Error Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 365 A.1 Navigation System Error Model . . . . . . . . . . . . . . . . . . 365 A.2 Global-Positioning-System Latitude and Longitude Position as Measurements . . . . . . . . . . . . . . . . . . . . 374

B.

Orbital Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 377 B.1 Orbital Position in Earth-Centered Frame . . . . . . . . . . . . 377 B.2 Orbital Velocity in Earth-Centered Frame . . . . . . . . . . . . 384

C.

Coarse-Alignment Error Equations C.1 Position Error Equations. . . C.2 Position Error Dynamics. . . C.3 Attitude Error Equations. . . C.4 Velocity Error Equations . .

. . . .

.................... ................... ................... ................... ...................

387 387 388 390 392

D. Fine-Alignment Error Equations . . . . . . . . . . . . . . . . . . . . . . 393 E.

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 395

F.

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . F.1 Inertial Sensors. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . F.2 Kalman Filtering. . . . . . . . . . . . . . . . . . . . . . . . . . . . . F.3 Navigation Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . F.4 Orbital Mechanics . . . . . . . . . . . . . . . . . . . . . . . . . . . .

399 399 399 400 400

Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 401 Supporting Materials . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 409

Software download information can be found at the end of the book on the Supporting Materials page.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Preface

The subject of integrated navigation systems covered in this book is designed for those directly involved with the design, integration, and test and evaluation of navigation systems. It is assumed that the reader has a background in mathematics, including calculus. Integrated navigation systems is the combination of an onboard navigation solution (position, velocity, and attitude) and independent navigation data (aids to navigation) to update or correct navigation solutions. In this book, this combination is accomplished with Kalman filter algorithms. This presentation is segmented into two parts. In the first part, elements of basic mathematics, kinematics, equations describing navigation systems/ sensors and their error models, aids-to-navigation, and Kalman filtering are developed. Detailed derivations are presented and examples are given to aid in the understanding of these elements of integrated navigation systems. Problems are included to expand the application of the materials presented. This edition includes software for selected Chapter, Section and Exercise material in a companion CD to enhance the learning experience of the reader. The included software has been developed using MATLAB/SimulinkTM version 6.5 by The MathWorks, Inc.

Part 1: Elements of Integrated Navigation Systems The concept of integrated navigation systems is introduced in Chapter 1 by illustrating different elements of operational navigation systems. The majority of the material presented addresses navigation systems that are mechanized with inertial sensors, accelerometers, and gyros to form an inertial navigation system to provide navigation state data: position, velocity, attitude, etc. To remove errors resulting from initialization and sensor errors, the navigation system’s state data are combined with other independent data from aids-tonavigation. This combination is accomplished via a Kalman filter algorithm. This algorithm implements mathematical models relating a navigation system’s state errors to corresponding errors in aids-to-navigation data. Aids-to-navigation data can include position, velocity, line-of-sight angles, etc., from various onboard sources, e.g., global positioning system (GPS) receivers, Doppler velocity sensors, optical sensors, radar, etc. Relating navigation states to aids-to-navigation data via a mathematical model is the principal xiii

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

xiv

PREFACE

problem in integrated navigation systems and is dealt with extensively in Part 1 of this book. Mathematical preliminaries are presented in Chapter 2 to familiarize the reader with notation and operations with vectors, matrices, and their calculus. This review provides the basis for later developments of navigation and Kalman filter equations. Examples applying the material presented include the formation of the least-squares estimation problem. This formulation is used repeatedly in the development of Kalman filter equations as presented in this book. Linearization techniques for nonlinear equations are presented and are applied later to nonlinear navigation equations to obtain linearized forms for Kalman filter implementations. Direction cosine matrices are used to transform vector components among various reference coordinate frames. The formation and dynamics of these matrices are developed. Various coordinate systems in current use, and transformations between them, are illustrated in Chapter 3. These coordinate systems are used to reference navigation states: position, velocity, and attitude, determined by the navigation solution and used for relating information from aids to navigation to navigation states’ reference frames. Coordinate systems that are currently used for navigation systems are presented in detail, including Earth-centered, Earth-fixed (ECEF), local geodetic, wander azimuth, and line-of-sight. Examples of position and rate errors for various wander azimuth coordinate frame definitions are developed. Geometrical shape and gravitational models for representing the Earth are presented in Chapter 4 to account for the Earth’s oblate shape and mass distribution. Relationships between ECEF position x– y– z components and local-level latitude, longitude, and altitude positions are developed. A vehicle’s position change in geographical coordinates is related to the local Earth relative velocity and Earth curvature. A general development of Earth gravitational potential and acceleration is presented and along with illustrations of simplifications for use in navigation. Equations describing terrestrial navigation are developed in Chapter 5. Many linearized forms of these equations are developed, including perturbation and an alternative velocity error representation that yields the “computer frame” form. Simplified forms of these error equations used in Kalman filter implementations are presented. The inertial navigation system’s vertical axis control and stabilization, assuming a barometric pressure reference, is examined, and the corresponding error equations for a Kalman filter implementation presented. The historically significant Pinson error model is reexamined. Functional characterizations of inertial sensors used in navigation systems are developed in Chapter 6. Performance characterizations for ring-laser gyro and fiber-optic gyro sensors are developed. Generalized error models for gyros and accelerometers are presented that represent these sensors’ fixed errors, e.g., biases, scale factor, and misalignments. Time-varying dynamic error models for sensor random errors, useful for numerical simulations, are presented based on continuous system and stochastic differential equation approaches. Functional characterizations of aids to navigation are presented in Chapter 7. These aids provide a source of independent navigation information that can be related to navigation states. These data are used in a navigation Kalman filter implementation to estimate errors associated with the navigation system.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

PREFACE

xv

Navigation aids presented include Doppler velocity sensors, tactical air navigation (TACAN) ranging, GPS-satellite ranging, and line-of-sight sensors. Examples relating the information from these aids in their respective reference frames to that used by the navigation system’s Kalman filter are presented. In Chapter 8, Kalman filtering equations are developed based on a recursive weighted least-squares approach. The Kalman filter algorithm is used to estimate navigation system errors by combining information available from the aids to navigation with navigation solution system data to obtain an optimal estimate of the navigation system’s errors. Various forms of the Kalman filter algorithm are presented, including the conventional, Joseph’s, and U –D factored forms. Kalman filtering techniques are extended to include modifications to the algorithm for including summed measurements and combining outputs of several independent parallel filters to form a single optimal estimate. Finally, a filtering algorithmic approaches that are derivative free, the Unscented Kalman Filter (UKF) and the Divided Difference Kalman Filter (DDKF), are presented. Part 2: Applications Presented in the second part are case studies illustrating the application of the elements in Part 1 to integrated navigation systems. In Chapter 9, the first of these case studies addresses the inertial navigation sensors’ laboratory or factory calibration. In this case study, navigation error dynamic equations are treated as algebraic equations and a tracking model implemented in a Kalman filter is used to reconstruct higher derivative error states. With these higher derivative states available, the fixed sensor errors are estimated from a series of rotations of an accelerometer/gyro cluster assembly. Results are presented from simulated inertial sensor data, allowing a direct evaluation of calibration accuracies. Trajectory reconstruction is used during flight test and evaluation of inertial navigation systems to provide an improved source of independent navigation state data by which to gauge the performance of the navigation system under test. Three trajectory reconstruction approaches are presented in Chapter 10. A least-squares technique using ground-based, line-of-sight tracking; a Kalman filter implementation using an onboard inertial navigation system aided with TACAN ranging; and a Kalman filter modeling a sounding rocket’s dynamics integrated with ground-based radar track data are presented. Data from actual flight tests are used to illustrate the results obtained from these approaches. Autonomous inertial navigation system ground alignment is illustrated in Chapter 11. Initial coarse alignment based on accelerometer and gyro sensor outputs is presented, with a characterization of the relationship between sensor error and the resulting alignment accuracy. An illustration of a final fine alignment using Kalman filtering is also presented. Simulated data are used to illustrate the accuracy of alignments achieved. In Chapter 12, an operational GPS receiver unit integrated with an inertial navigation unit is the first integrated navigation system case study. In this case study, the GPS receiver’s internal Kalman filter is used to estimate errors associated with the navigation state data being provided to the unit. The GPS receiver

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

xvi

PREFACE

and inertial navigation unit are loosely coupled. The Kalman filter implementation used by the GPS receiver is less complete in that it does not incorporate inertial sensor error states within its state vector. Outputs from the receiver’s Kalman filter are combined with the inertial navigation unit’s uncorrected outputs to form an optimal estimate of position, velocity, etc. Simulated data are used to illustrate the accuracy of the receiver’s Kalman filter estimates. In-motion alignment of an inertial measurement unit is presented in Chapter 13. This case study, which also includes establishing another navigation solution form, is presented for two major subcases. The first is a conventional transfer alignment with attitude initialization provided by another inertial navigation system. This application permits the use of a small attitude error model implementation in the alignment Kalman filter. The second subcase is an alignment without initial attitudes. This application requires a reformulation of the error model to accommodate large attitude errors. Both approaches are illustrated using results obtained with actual recorded flight test data. The next case study is for a differential GPS- (DGPS) aided dead-reckoning (DR) land navigation system and is presented in Chapter 14. In this case study, higher rate navigation data, as required by an autonomous vehicle for real-time steering and control, are provided at a comparable level of accuracy as available from the lower-rate DGPS position outputs. The DR navigation solution is formed using outputs from a single-axis gyro and a Doppler radar ground speed sensor. In this case study, test data from an actual real-time implementation are used to illustrate the benefits and performance available from this integration. In Chapter 15, attitude determination and estimation case studies are presented. These case studies address navigation problems, like those of the previous chapter, that use less than a complete suite of accelerometer and gyro sensors. The problem of attitude determination involves determining a transformation matrix that transforms known information from one reference frame to another. This problem has applications in both terrestrial and spacecraft navigation. Attitude estimation continues with the integration of a navigation solution, in this case only attitude, with other sensor data via a Kalman filter. Examples of these two applications are presented. Summarized in Chapter 16 are several design options available for an integrated navigation system based on one of many possible navigation frame implementations. These options illustrate various navigation system error models discussed and developed in this book that are candidates for implementation in a Kalman filter algorithm. Mathematical methods presented provide the reader, whether involved in design, integration, or test and evaluation, the tools to establish a new integrated navigation system or evaluate an existing one. Appendices contain additional material to supplement the material presented in the earlier chapters. This additional material is included to expand upon the material presented in the chapters and to help the book be more of a standalone document. While treating specific subjects, notation is used that is consistent with publications relating to those subjects. However, because a variety of subjects are covered, the same notational variables assume different meanings in various parts of the book. The reader is cautioned not to assume that the symbology in one subject area has the same meaning in another. Finally, a practical aspect

PREFACE

xvii

of presenting the variety of material limits the depth to which each area can be developed. The reader is encouraged to investigate those areas that are only summarized to the level necessary to understand their application in an integrated navigation system.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Robert M. Rogers June 2007

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Part 1 Elements of Integrated Navigation Systems

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

1 Introduction

Navigation systems are used for land, sea, airborne, and space vehicles. These systems provide an operator and/or control system with the necessary information to effect some action in response to data provided by these systems. For example, this action can be a course correction indication for an aircraft pilot or a feedback control signal to guide an autonomous vehicle. These systems incorporate onboard sensors coupled with a computer, permitting selfcontained operations with little or no assistance required from sources external to the vehicle. The core of the navigation system is a set of sensors combined with a computer that can provide a relatively stable and accurate source of navigation data. These systems output navigation state data, which usually include position, velocity, and attitude. As a result of imperfections in navigation sensors and computational errors, errors develop in the navigation state data and grow in time. The host vehicle’s operating environment also influences the error growth rate. Long-term error growth is minimized by including other sensors that provide independent and/or redundant navigation data, that is, position, in an integrated system that optimally combines this independent data source with the core navigation system. These independent sensors, referred to as navigation aids, are characterized by long-term error stability, which can complement the short-term error stability of the navigation system’s sensors. When combined within a computer algorithm, such as a Kalman filter, errors from both the core sensors and navigation aids can be estimated to reduce the integrated navigation system’s errors. The resulting navigation system will exhibit improved performance, even if independent data are used intermittently or are not available for a short time span. The majority of this book addresses navigation systems that are mechanized with accelerometer and gyro inertial sensors. These inertial sensors provide sensed accelerations (velocity changes over a time interval) and rates (attitude changes over a time interval). Accelerometers and gyros are mounted in orthogonal triad clusters and enclosed within an inertial measurement unit (IMU) to provide three components of acceleration and rate outputs. These outputs are provided to a computer-implemented numerical integration process that computes a navigation solution yielding a complete set of navigation state data, that is, position, velocity, and attitude. These mechanizations are generally referred to as an inertial navigation unit when enclosed within a case that can 3

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

4

INTEGRATED NAVIGATION SYSTEMS

be easily removed and replaced. Implementations that include the inertial sensors, computer, and navigation aids are referred to as an inertial navigation system (INS). Other navigation systems that use fewer than the full three-axis accelerometer and gyro sensors are also presented. These systems include a dead-reckoning system and attitude reference systems. The dead-reckoning system uses speed (distance traveled) and heading sensors to compute a dead-reckoning navigation solution. This solution is less complete and generally provides only position and heading navigation state data. Two attitude reference systems are presented. One uses accelerometers and a magnetic sensor combined with data from a global positioning system (GPS) receiver. The other uses only gyros aided with a strapdown star sensor. Within each of these types, there is a range of integration approaches. The navigation system’s core sensors provide information as a result of movement. These sensors are fixed to the vehicle, that is, a strap-down IMU. Therefore, these sensors provide information about the movement of the vehicle as reference to the vehicle’s frame of reference. The information in this reference frame might not be useful to a pilot or guidance system. Therefore, a navigation solution is established in a reference frame that allows its data to be used conveniently and allows data from other sources, that is, navigation aids, to be easily incorporated. This is accomplished by establishing a navigation frame that is relatively fixed. The literature indicates a variety of navigation system reference frames that have been used in integrated navigation systems. Examples include inertial (stellar referenced), Earth-referenced (north-referenced azimuth), and wander azimuth (free azimuth movement). Even within these examples, there are additional levels of definition, for example, which axis is aligned with what direction and what order one axis is rotated with respect to another. The navigation frame selection can be arbitrary, at the discretion of the designer, or the frame’s definition can be specified. In either case, the ability to establish mathematical methods to convert data from one axis definition to another is of primary importance in integrated navigation systems. Methods for establishing reference frames, transforming information between frames, and describing motion in these frames are presented using many navigation reference frames for illustration. To enhance reliability, navigation systems have been implemented using multiple navigation units or implementations that incorporate both inertial and deadreckoning navigation solutions. For example, some commercial and military aircraft have two or more inertial units, each operating simultaneously. If a unit fails during flight, then the other unit’s outputs are available. In some cases, three units are installed, allowing their outputs to be compared. If one unit is much different from the other two, then that unit might be faulty and is no longer considered valid (inoperative). If two navigation units are available, the outputs from two units can be combined with outputs from available navigation aids to form two independent navigation systems. The outputs from these two independent systems can be optimally combined to establish a better source of navigation state data. The use of several inertial units can be too costly or not feasible for other reasons; however, other sensors that can be used to enhance system reliability

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

INTRODUCTION

5

might be available. If, for example, airspeed and heading sensors are available to implement a dead-reckoning navigation solution, then this implementation’s outputs can be optimally combined with navigation aids to yield an improved integrated dead-reckoning navigation system. Implementations such as these are used as the only navigation systems in many applications. Dead-reckoning implementations are generally less accurate than inertial systems—not just as a result of the quality of the sensors used but also as a result of the simplifying assumptions used. When included as part of an integrated navigation system that also includes an INS, the dead-reckoning navigation solution represents a fall-back source of navigation data if the primary source, that is, the INS, becomes inoperative. Whether the navigation system is based on inertial sensors or dead-reckoning sensors, multiple sources of redundant information—navigation aids—can be optimally combined within a single integrated navigation system to improve its navigation state data. Navigation aids that provide independent sources of position are available, that is, the GPS. Other navigation aids include sources of velocity, for example, Doppler and GPS; sources of relative position to ground-based stations, that is, tactical air navigation; and sources of position relative to fixed landmarks, for example, airborne radar and optical line-of-sight sensors. Combining data from multiple navigation aids with the core navigation system can not only enhance the integrated system’s reliability but also improve the quality of resulting navigation state data. Methods for integrating different navigation aids to an integrated navigation system are presented. A fully integrated navigation system structured to provide a high level of reliability is one that implements a failure mode hierarchy to ensure that the most accurate navigation solution is implemented with whatever sensors remain operative. Fully integrated systems such as these can include both inertialbased navigation systems (including more than one inertial unit) and dead-reckoning-based navigation systems. Whether single or multiple inertial units are used, a dead-reckoning implementation is used (by itself or as part of a structured navigation system), or multiple sources of navigation aid data are available, the methods presented in this book are used in integrated navigation systems such as these. Integrated navigation systems incorporate elements from diverse fields of study, including kinematics, inertial and navigation aid sensors, and optimal estimation. Integration of navigation systems with navigation aids into an integrated navigation system requires the development of mathematical descriptions of reference frames, descriptions of motion in those frames (kinematics), and corresponding models for navigation aids. This book develops a mathematical framework used to accomplish this integration and builds upon the inertial navigation systems analysis approach by Britting [1]. The optimal combination of navigation sensors/systems and navigation aids is accomplished with the Kalman filter algorithm. For the reader without prior academic experience, the approach used in developing this algorithm is based on the weighted recursive least-squares approach of Sage [2]. Kinematic equations used to describe motion are, in general, nonlinear time varying differential equations. Equations describing the relationship between this motion and navigation aid data are also generally nonlinear and time

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

6

INTEGRATED NAVIGATION SYSTEMS

varying. To facilitate the implementation of an integrated navigation system via the Kalman filter algorithm, navigation state equations are linearized to establish dynamic equations of navigation state errors. The literature indicates that error representations used to express navigation state errors are varied and nonunique. The diversity of choices available precludes the definition of a single system dynamic and measurement model for an integrated navigation system. At one level, the choice is the integrated navigation system’s navigation reference frame selection. At another level, given the navigation frame’s definition, is the choice of which of several assumptions to use in establishing error equations for implementation within the Kalman filter algorithm. Because these choices are design decisions, methods to establish different error equations based on defined assumptions are presented. The goal of this book is to provide the reader, whether involved with design, integration, or test and evaluation, with the necessary mathematical tools to evaluate an integrated navigation system’s design.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2 Mathematical Preliminaries

This chapter presents a review of the mathematical techniques used in this study of integrated navigation systems. The material presented is brief; therefore, the reader is encouraged to seek more in-depth information than that presented in this review. The following subjects are treated in this chapter: vector/matrix algebra, vector/matrix calculus, linearization techniques, direction cosine matrices, and miscellaneous mathematical topics. The basic algebraic vector and matrix operations, for example, addition, subtraction, multiplication, and inversion (division), are summarized. Next, vector and matrix calculus, including differentiation of scalars and vectors with respect to scalars and vectors, is reviewed and applied to the development of a least-squares estimate. This is also the approach used later for developing the Kalman filter algorithm. The calculus of vectors and matrices continues with the linearization of nonlinear equations to obtain linearized equations for perturbations about a nominal solution. The conventional Kalman filter algorithm assumes a linear system, and, used frequently in integrated navigation systems, the extended Kalman filter also requires certain linearized terms from this process (see Chapter 13). The establishment of coordinate systems and transformations of vector components between different systems, using direction cosine matrices, is presented next. The dynamic equation that describes the evolution of this matrix, which is used extensively in this treatment of integrated navigation systems, is also developed. Finally, presented are miscellaneous mathematical topics, including the angular velocity addition theorem and quaternions. In this chapter, and subsequent chapters that compose Part 1 of this book, problems are included, most with step-by-step instructions that, if followed, lead to the solution found in the problem statement. The purpose of these problems is to expand on the material presented, and they are drawn from actual experiences in integrated navigation system applications.

2.1 Vector/Matrix Algebra The use of vectors and matrices greatly facilitates the development and compact expression of equations and algorithms used in integrated navigation systems. Their use permits results to be expressed in a general form that has 7

8

INTEGRATED NAVIGATION SYSTEMS

wide applicability without resorting to expressing results in vector component form. Our use requires only a limited amount of vector/matrix algebra. 2.1.1 State-Space Vector A vector is an arrangement of scalar quantities with governing manipulation rules. A column vector x of “n” scalar quantities xi is expressed as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2

3 x1 6 x2 7 6 7 x¼6 . 7 4 .. 5 xn

(2:1)

The transpose of the column vector in Eq. (2.1) is expressed as  xT ¼ x 1

x2

   xn



(2:2)

This is in the form of a row vector. 2.1.2 Matrices A vector can also be considered as an n  1 matrix. The matrix A of n  m scalar quantities aij is expressed as 2

a11 6 a21 6 A¼6 6  4  an1

a12 a22   an2

    

3 a1 m a2 m 7 7  7 7  5 anm

(2:3)

This matrix can also be considered as an arrangement of vectors. The transpose of a matrix is obtained by interchanging its rows and columns and is expressed as 2

a11 6 a12 6 AT ¼ 6 6  4  a1m

a21 a22   a2m

3    an1    an2 7 7   7 7   5    amn

(2:4)

2.1.3 Vector/Matrix Manipulations Compatible vectors and matrices are manipulated according to the following elementary operations: Addition: C ¼AþB

or

cij ¼ aij þ bij

(2:5)

MATHEMATICAL PRELIMINARIES

9

Subtraction: C ¼AB

or

cij ¼ aij  bij

(2:6)

Multiplication: C ¼ AB

or

cij ¼

n X

aik  bkj

(2:7)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

k¼1

In addition and subtraction, compatible vectors/matrices have the same number of rows and columns. For multiplication, the number of columns of the left matrix must equal the number of rows of the right matrix. Matrix inversion is the parallel for the division operation. The inverse of the square n  n matrix A is defined such that AA1 ¼ A1 A ¼ Inn

(2:8)

where the identity matrix Inn is defined as 2

Inn

1 60 6 ¼6 4 0

0 1  0

3  0  07 7 7 .. . 5 0 1

(2:9)

The matrix inverse is defined if the matrix to be inverted is nonsingular. A test to determine if a matrix is nonsingular is that its determinate is nonzero: j Aj = 0

(2:10)

There are special rules for vector/matrix manipulation. First, the order of matrix multiplication is important. In general, matrices do not commute: AB = BA

(2:11)

The transpose of a product is the product of the transposed matrices in reverse order: ðABÞ ¼ BT AT

(2:12)

Examples of the preceding operations are presented in the following. The inner product (three-space vector dot product parallel) of two vectors is xT y ¼

n X i¼1

xi yi ¼ x1 y1 þ x2 y2 þ    þ xn yn

(2:13)

10

INTEGRATED NAVIGATION SYSTEMS

The outer product of a n  1 vector x and a m  1 vector y 2 3 2 x1 y1 x1 y2    x1 6 x2 7   6 x2 y1 x2 y2    T 7 6 xy ¼ 6 4  5 y1 y 2    y m ¼ 4    xn xn y1 xn y2   

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Matrix/vector product. given as

is 3 x1 ym x2 ym 7 7  5 xn ym

(2:14)

The product of a compatible vector and matrix is y ¼ Ax

(2:15)

or, written out in component form and with multiplication completed, 2 3 2 32 3 y1 a11 a12    a1n x1 6y 7 6a 6 7 7 6 2 7 6 21 a22    a2n 76 x2 7 6 7¼6 76 7 4  5 4     54  5 ym

am1

am2



amn

xn

2

3 a11 x1 þ a12 x2 þ    þ a1n xn 6 a x þ a x þ  þ a x 7 22 2 2n n 7 6 21 1 ¼6 7 4 5 

(2:16)

am1 x1 þ am2 x2 þ    þ amn xn Reviewing the condition of compatible operations from this example, an m  1 vector y results from the product of a m  n matrix A and an n  1 vector x. The number of columns of the matrix must equal the number of rows of the vector multiplied. Products between matrices must also be compatible. Similarity transformation. In Eq. (2.15), x and y are related as y ¼ Ax. If the vectors x and y are transformed into another set of vectors by a nonsingular invertible matrix B as u ¼ Ax

(2:17)

v ¼ By

(2:18)

then the vectors u and v are related by a similarity transformation. This is demonstrated in the following steps: v ¼ By ¼ BAx ¼ BAB1 u

(2:19)

where the matrix (BAB 21) is the similarity transformation. 2.1.4

Three-Space Vectors

Certain operations specialized to three-dimensional “space” vectors will be used. The vector cross product can be expressed as a matrix/vector product.

MATHEMATICAL PRELIMINARIES

11

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

The cross product of two vectors a and b is expressed as 2 3 a2 b3  a3 b2 6 7 a  b ¼ 4 a3 b1  a1 b3 5 a1 b2  a2 b1 2 32 3 b1 0 a3 a2 6 76 7 0 a1 54 b2 5 ; ðaÞ b ¼ 4 a3 a2 2

0 6 ¼ 4 b3 b2

a1 b3 0 b1

0

b3 32

3 b2 a1 76 7 b1 54 a2 5 ; ðbÞ a 0

a3

¼ b  a

(2:20)

These forms demonstrate that a vector cross product can be formed as the product of a matrix and a vector where the matrix is skew-symmetric. A skewsymmetric matrix has the property that its transpose is the negative of the matrix, that is, AT ¼ A (2:21) The following vector triple cross product is applied in later developments: a  (b  c) þ b  (c  a) þ c  (a  b) ¼ 0

(2:22)

Example 2.1 U – D Factorization of Symmetric 3 3 3 P Matrix Assume that a symmetric matrix (a matrix whose elements in opposite pairings about the matrix’s diagonal are equal) P can be factored into the following form: P ¼ UDU T where the U matrix is an upper triangular matrix with unity diagonal elements, the D matrix is a diagonal matrix with nonzero diagonal entries, and all other elements are zero. Then, what are the relationships between the original symmetric P matrix elements and the elements of the U and D matrices so defined? For the simple 3  3 matrix, this equation is written in expanded form as 2

p11

6 4 p12 p13

p12 p22 p23

p13

3

2

1 u12

u13

32

d1

0

0

3

7 6 7 76 p23 5 ¼ 4 0 1 u23 54 0 d2 0 5U T p33 0 0 1 0 0 d3 2 d1 þ u12 d2 u12 þ u13 d3 u13 u12 d2 þ u13 d3 u23 6 ¼4 d2 u12 þ u23 d3 u13 d2 þ u23 d3 u23 d3 u13 d3 u23

u13 d3

3

7 u23 d3 5 d3

12

INTEGRATED NAVIGATION SYSTEMS

Equating elements in reverse order, the following are obtained: d3 ¼ p33 p23 u23 ¼ d3 p13 u13 ¼ d3 Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

d2 ¼ p22  u23 d3 u23 u12 ¼

p12  u13 d3 u23 d2

and d1 ¼ p11  (u12 d2 u12 þ u13 d3 u13 ) The preceding equations can be summarized in the following algorithm [3]: for j ¼ n, n 2 1, . . . , 2 dj ¼ pjj for, k ¼ 1, . . . , j 2 1 ukj ¼ pkj/dj for i ¼ 1, . . . , k pik ¼ pik 2 uij dj ukj and d1 ¼ p11 In this particular form for the U – D factorization algorithm form, the upper diagonal elements of the P matrix are destroyed.

Example 2.2

Matrix Inversion Lemma

This lemma [2] is used in later developments for the Kalman filtering algorithm and is presented here to illustrate the algebra of matrix manipulations. This lemma is stated as follows: if A, B, and D are invertible matrices such that A1 ¼ B1 þ C T D1 C and (D þ CBC T )1 exists, then A ¼ B  BC T (D þ CBC T )1 CB The following demonstration of the proof of this is summarized from [2].

MATHEMATICAL PRELIMINARIES

13

Step 1: Premultiply the first equation by A: A½A1 ¼ B1 þ CT D1 C)I ¼ AB1 þ AC T D1 C Step 2: Postmultiply this result by B: ½I ¼ AB1 þ AC T D1 C B)B ¼ A þ ACT D1 CB Step 3: Postmultiply this result by C T: Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

½B ¼ A þ AC T D1 CBC T )BC T ¼ AC T þ AC T D1 CBC T ¼ AC T D1 D þ ACT D1 CBC T ¼ AC T D1 (D þ CBC T ) Step 4: Postmultiply by (D þ CBC T )1 : ½BC T ¼ AC T D1 (D þ CBC T )1 )BC T (D þ CBC T )1 ¼ ACT D1 Step 5: Postmultiply by CB: ½BC T (D þ CBC T )1 ¼ ACT D1 CB ) BC T (D þ CBC T )1 CB ¼ ACT D1 CB Step 6: Subtracting both sides from B yields  1 B  BCT D þ CBC T CB ¼ B  ACT D1 CB Step 7: Using the result of step 2 for B on the right-hand side (RHS),  1 B  BC T D þ CBC T CB ¼ A þ AC T D1 CB  AC T D1 CB ¼ A which is the desired result.

2.2 Vector/Matrix Calculus The calculus of vectors and matrices follows similar manipulation rules. A few of these manipulations are reviewed in this section. The derivative of a vector (matrix) with respect to a scalar is a vector (matrix) whose elements are derivatives with respect to that scalar. (In this example, the scalar is time-t.) 2 3 dx1 6 dt 7 6 7 6 dx2 7 7 dx 6 ¼ 6 dt 7 (2:23) 7 dt 6 6 .. 7 6 . 7 4 dx 5 n dt

14

INTEGRATED NAVIGATION SYSTEMS

The derivative of a scalar with respect to a vector is defined as the row vector  @a @a ¼ @x @x1

@a @x2



@a @xn

 (2:24)

If the scalar a is defined as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

a ¼ yT x ¼ y1 x1 þ y2 x2 þ    þ yn xn

(2:25)

then @a  ¼ y1 @x

y2

   yn



(2:26)

The derivative of a vector with respect to another vector is defined as the matrix. For example, if the vector y is defined as y ¼ Ax, or written out in component form as in Eq. (2.16), 2

3 2 3 y1 a11 x1 þ a12 x2 þ    þ a1n xn 6 y2 7 6 a21 x1 þ a22 x2 þ    þ a2n xn 7 6 7¼6 7 4  5 4 5  ym am1 x1 þ am2 x2 þ    þ amn xn

(2:27)

From the definition of the derivative of a scalar with respect to a vector, the first component becomes @y1  ¼ a11 @x

a12

   a1n

@y2  ¼ a21 @x

a22

   a2n



(2:28)



(2:29)

The second is

Continuing for each element in the vector y, then ordering the results into a vector, the result is summarized in the following vector/matrix form 2

a11 6 a21 @y @ ¼ ðAxÞ ¼ 6 4  @x @x am1

a12 a22  am2

3    a1n    a2n 7 7¼A   5    amn

(2:30)

Example 2.3 Least-Squares Parameter Estimation The objective of this example is to develop an expression for an optimal estimate of a vector composed of unknown constants given available data that are related to those constants, that is, polynomial curve fitting.

MATHEMATICAL PRELIMINARIES

15

Given: x-vector of n unknown constant parameters z-vector of m available data quantities from which to estimate x Only indirect and imperfect access to the unknown constants is available via the measurements z. The vector z is linearly related to x, but each element z is corrupted with additive error v as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

z ¼ Hx þ v

(2:31)

Here, H is an m  n known matrix, where m  n (more measurements than unknown parameters) and with rank n (n linearly independent rows or columns) and v is a vector of m unknown additive errors. Form a cost or penalty function that is an inner product as shown here: JLS ¼ (z  Hx)T (z  Hx)

(2:32)

Then, the objective is to minimize the difference, in a least-squares sense, between the available data in the vector z and an estimate of these data based on estimates for unknown constant contained within the vector x and the known matrix H. The estimate for the vector x is obtained by minimizing JLS with respect to x. An extrema (maximum or minimum) is obtained by setting the derivative of JLS with respect to x to zero and solving for x. First expanding Eq. (2.32), JLS ¼ zT z  zT Hx  xT H T z þ xT H T Hx and then, the derivative of this scalar with respect to x is obtained in the following sequence: @JLS ¼ 0T  zT H  (H T z)T þ (H T Hx)T þ xT H T H @x ¼ 2(zT H þ xT H T H) Setting this result to zero for an extrema, canceling the 2, and transposing the entire equation yields H T Hx ¼ H T z Because it is assumed that the matrix H is of full rank, the matrix resulting from the product (H T H) is invertible. The expression for x that minimizes or maximizes the cost function becomes x^ ¼ (H T H)1 H T z

(2:33)

This expression produces a minimum value of JLS because the second derivative @2 JLS ¼ 2(H T H) @x2 is a positive (definite) quantity.

(2:34)

16

INTEGRATED NAVIGATION SYSTEMS Important properties associated with the estimate just obtained are listed here: 1) Estimation error:

1 ; x  xˆ ¼ x  (H T H)1 H T z

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¼ x  (H T H)1 H T (Hx þ v) ¼ x  (H T H)1 (H T H)x  (H T H)1 H T v ¼ (H T H)1 H T v

(2:35)

2) Residual (difference between actual and predicted data): n ; Hx þ v  H xˆ ¼ H(x  xˆ ) þ v ¼ H1 þ v ¼ H(H T H)1 H T v þ v ¼ ½I  H(H T H)1 H T v

(2:36)

If errors v are random zero mean processes such that E½v ; 0

and

Ebv vT c ; R

(2:37)

then the following additional properties are obtained. 3) Expected estimation error: E½x  xˆ  ¼ Eb(H T H)1 H T v ¼ (H T H)1 H T E½v ¼0

(2:38)

The estimation error is zero mean. The mathematical expectation operator E[ ] is used extensively in this book. Its development is beyond the scope of this book. However, because all errors considered in this book are Gaussian, this operator can be considered to be operating on a random variable yielding a known mean and variance. 4) Estimation error covariance:  E½(x  xˆ )(x  xˆ )T  ¼ E ½(H T H)1 H T v½(H T H)1 H T vT ¼ (H T H)1 H T E½v vT H(H T H)T ¼ (H T H)1 H T RH(H T H)T ;P

(2:39)

MATHEMATICAL PRELIMINARIES

17

5) Residual error covariance: E½(z  H xˆ )(z  H xˆ )T  ¼ ½I  H(H T H)1 H T E½v vT   ½I  H(H T H)1 H T T ¼ ½I  H(H T H)1 H T R

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

 ½I  H(H T H)1 H T T

(2:40)

2.3 Linearization Techniques The equations describing terrestrial navigation are nonlinear differential equations. Linearized error forms are desirable for use in the Kalman filter algorithm. In this section, techniques are presented to obtain a linearized error form of general nonlinear equations. Consider the following continuous-time nonlinear vector differential equation: x˙ ¼ f (x, t)

(2:41)

where the dot over the variable denotes the derivative with respect to the scalar t, time. Assume that a nominal solution x0 satisfies this equation. Examine the perturbations or errors dx about this nominal solution with time fixed, where the new value of x is defined as x ¼ x0 þ d x

(2:42)

Taking the time derivative of this equation yields x˙ ¼ x˙ 0 þ dx˙ Upon substitution, the original differential equation becomes x˙ 0 þ dx˙ ¼ f (x0 þ dx, t) Expand f (x0 þ dx, t) in a Taylor-series about the nominal solution x0 and for time t fixed: f (x0 þ dx, t) ¼ f (x0 , t) þ

@f (x, t)

dx þ HOT @x x¼x0

(2:43)

where HOT represents higher-order terms beyond the first linear term that are not included. The original differential equation is then expressed as

@f (x, t)

dx x˙ 0 þ dx˙  f (x0 , t) þ @x x¼x0

18

INTEGRATED NAVIGATION SYSTEMS

Because x0 is also a solution to the original differential equation, x˙ 0 ¼ f (x0 , t)

(2:44)

By subtracting, the linearized equation for the continuous-time error becomes

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

dx˙ ¼

@f (x, t)

x¼x0 dx @x

(2:45)

In this equation, only first-order terms have been retained from the Taylor-series expansion. A concept used in the development of the dynamic Kalman filter equations is the state transition matrix. The preceding differential equation is used to establish the values of the linearized state dx at a future time instant t þ Dt. With the assumption of a sufficiently small time step Dt, such that a Taylor-series time expansion retains only the first-order term, the continuous-time error equation can be rewritten as

dx(t þ Dt)  dx(t) þ dx˙ (t)Dt ¼ dx(t) þ

@f dx(t)Dt @x

or  @f dx(t þ Dt)  I þ Dt dx(t) @x

(2:46)

Defining the matrix in parentheses as the state transition matrix, this equation can be rewritten as

dx(t þ Dt) ; FtþDt dx(t)

(2:47)

where FtþDt  I þ

@f Dt @x

(2:48)

More formal definitions and methods of determining this matrix exist. The partial derivative in this equation will later be referred to by the letter F: F;

@f @x

(2:49)

With this notation, the state transition matrix becomes FtþDt  I þ FDt

(2:50)

MATHEMATICAL PRELIMINARIES

19

A more formal derivation of the state transition matrix, for linear timeinvariant systems, shows that this matrix is the exponential matrix e FDt, and the form in Eq. (2.50) is just a truncation after the first term in the exponential expansion FtþDt ¼ eFDt

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¼ I þ FDt þ F 2

Dt2 Dt3 Dt4 þ F3 þ F4 þ  2! 3! 4!

(2:51)

Improved accuracy can be obtained by retaining more terms, however, with a higher computational cost. 2.4 Direction Cosine Matrices 2.4.1 Coordinate Transformations Important matrices used in this treatment of integrated navigation systems are direction cosine matrices (DCMs). These matrices relate a vector’s components in one coordinate frame to another frame. The following development illustrates how this matrix’s elements are formed by considering the transformation of a vector’s components, from an original coordinate frame, into components defined in another frame, with the second frame being rotated relative to the original. Consider the vector r shown in Fig. 2.1. In the coordinate frame x, this vector is denoted as rx. In the rotated frame y, this vector is unchanged in length and is denoted as ry. It is desired to determine the component values associated with this vector in the rotated frame y. This frame is rotated with respect to the x frame by the angle c. Figure 2.1 is redrawn in Fig. 2.2 with emphasis on a few triangle segments used to develop expressions for the vector components. Using simple

y2

x2

ψ

r y1

ψ x1

Fig. 2.1

Vector/coordinate frame.

20

INTEGRATED NAVIGATION SYSTEMS x2

y2

ψ

γ Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

r y1

η

ψ

β

x1

Fig. 2.2 Vector/coordinate frame components.

trigonometry, the following relationships are established: x1 ¼ h cos c ) h ¼

b ¼ h sin c ) b ¼

x1 cos c

x1 sin c cos c

y1  h ¼ g sin c ¼ (x2  b) sin c y2 ¼ g cos c ¼ (x2  b) cos c y1 ¼ x2 sin c  b sin c þ h y2 ¼ x2 cos c  b cos c y1 ¼ x2 sin c  x1

sin c x1 sin c þ cos c cos c

sin c cos c cos c   (1  sin2 c) y1 ¼ x1 þ x2 sin c cos c y2 ¼ x2 cos c  x1

y2 ¼ x1 sin c þ x2 cos c or y1 ¼ cos c x1 þ sin c x2

(2:52)

y2 ¼  sin c x1 þ cos c x2

(2:53)

MATHEMATICAL PRELIMINARIES

21

The preceding two equations are written in vector/matrix form as y ¼ Cx

(2:54)

where the transformation matrix C is 

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

C¼

cos c  sin c

sin c cos c

 (2:55)

This matrix relates vector components in the x frame to components in the y frame. Direction cosine matrices have several important properties. First, the determinant is unity, jC j ¼ 1

(2:56)

Next, the transpose is equal to the inverse, C T ¼ C 1

(2:57)

Finally, the rows and columns of a DCM form orthogonal unit vectors. The dot product of two rows or columns of this matrix is zero, and the cross product of two rows or columns of the matrix yields the remaining row or column. These properties are illustrated next with the simple DCM just given. Determinant:



cos c sin c

jC j ¼

 sin c cos c

¼ cos2 c þ sin2 c ¼ 1 Inverse: CT C ¼



cos c

 sin c

sin c

cos c

" ¼



cos c

sin c

 sin c

cos c

cos2 c þ sin2 c



 cos c sin c þ sin c cos c

 sin c cos c þ cos c sin c   1 0 ¼ 0 1

sin2 c þ cos2 c

Orthogonality: 

cos c sin c

    sin c ¼  cos c sin c þ sin c cos c cos c ¼0

#

22

INTEGRATED NAVIGATION SYSTEMS

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Coordinate frames used in this treatment of navigation systems are defined in a right-handed sense. That is, the direction of rotation formed by curling ones fingers is in the direction of the thumb on the right hand. In the examples in Figs. 2.1 and 2.2, the rotation was about a third axis perpendicular to the plane of the paper. This third axis completes the right-handed Cartesian frame. The transformed vector component along this third axis remains unchanged with the rotation about that axis. This is illustrated in Fig. 2.3. For the case of three axes, the earlier C matrix is rewritten as 2

cos c C ¼ 4  sin c 0

sin c cos c 0

3 0 05 1

(2:58)

Several features about this matrix and the way it is established can be identified from its form just given. First, elements of the DCM row/column about which the rotation occurs are either 0 or 1. Second, the other elements in the DCM are either sin or cos of the angle of rotation, with cosines being on the diagonal and sines being off the diagonal. Finally, the negative sign on the sine term is associated with the rotated frame’s component, which is rotated “outside” of the quadrant formed by the original frame’s axes. A vector’s components described in one frame can be described in another frame of arbitrary orientation with respect to the original frame by a transformation matrix composed of three sequential rotations (Euler angles) starting from the original frame’s axes. These rotations are illustrated in Fig. 2.4, and the

y2

x2 ψ

y1 ψ

x1

x3, y3

Fig. 2.3

Single rotation in three-axis coordinate frame.

MATHEMATICAL PRELIMINARIES x2′, x2″ x2′″

x2 ψ

φ

θ Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

23

ψ

x1″, x2′″ x1′ x1

θ x3, x3′ x3″ φ

x3′″

Fig. 2.4

Three rotations in three-axis coordinate frame.

primes are used to represent intermediate axes in the sequence of rotations. The final y frame corresponds to the triple primed x axes. Written in vector form, the transformation is ry ¼ Cxy rx

(2:59)

where the transformation DCM Cxy transforms the components of the r vector from the x frame to the y frame. Mathematically, this transformation is accomplished by transforming through three sequential rotations as shown in Eqs. (2.60 –2.62). These rotations proceed as a c rotation about x3 ; a u rotation about x02 resulting from the first rotation; and, finally, a f rotation about x001 resulting from the second rotation, where the primes denote the intermediate frames generated by sequential rotations. Rotation 1: x2′

2

3 2 x01 cos c 4 x02 5 ¼ 4  sin c x03 0

sin c cos c 0

x2

32 3 0 x1 0 5 4 x2 5 x3 1

(2:60) x1′

ψ x3, x3′

x1

24

INTEGRATED NAVIGATION SYSTEMS Rotation 2: 2

3 00

2

x1 cos u 4 x002 5 ¼ 4 0 x003 sin u

32

x1′

x1″

3

x01  sin u 5 4 x02 5 0 x03 cos u

0 1 0

x3″

θ x3″, x3′

(2:61)

x3, x3′

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Rotation 3: y2 x2′, x2″ φ

2

3 2 1 y1 4 y2 5 ¼ 4 0 y3 0

0 cos f  sin f

32 00 3 x1 0 sin f 54 x002 5 x003 cos f

(2:62) x3″

x1″, y1

y3

Combining the sequential rotations into a single matrix, 00

0

ry ¼ Cxy rx ¼ Cxy00 Cxx0 Cxx rx

(2:63)

The resulting DCM (transposed) is written as 2

cu cc Cyx ¼ 4 cu sc su

Example 2.4

cf sc þ sf su cc cf cc þ sf su sc sf cu

3 sf sc þ cf su c c sf cc þ cf su sc 5 cf cu

(2:64)

Using DCMs to Obtain Local Azimuth and Elevation to Stars

It is desired to relate the astronomical position of a star or some other body to a set of local-level azimuth and elevation angles. With knowledge of the local position (latitude) and the astronomical body’s position (local hour angle and declination), the local line-of-sight direction (azimuth and elevation) can be determined. A local north –east – down (N – E – D) reference frame is shown in Fig. 2.5. From the origin of this frame, the point P is located to correspond to the approximate direction of the star Polaris. This corresponds to the origin’s latitude. The point Z is in the direction of the zenith directly above the origin. From the meridian formed by the points N, P, and Z, the local hour angle t and the declination d establish the astronomical body R position. The azimuth Az and elevation h angles are to be obtained in terms of the local hour angle and declination.

MATHEMATICAL PRELIMINARIES

25

Z

x′, x′′

90 − δ

x′′′

P

ϕ

E R t

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

h x N

Az D

Fig. 2.5

North– east –down reference frame.

Astronomical triangle. One approach is to use the astronomical triangle and spherical triangle trigonometry to establish these relationships. This approach will be illustrated first. The astronomical triangle is formed by the points P, Z, and R in Fig. 2.5 and is redrawn in Fig. 2.6. For a spherical triangle with sides a, b, and c, with opposing angles A, B, and C, sin A sin B sin C ¼ ¼ sin a sin b sin c and cos a ¼ cos b cos c þ sin b sin c cos A Identifying the local hour angle t with A in this equation, the sides a, b, and c correspond to (90 2 h), (90 2 d), and (90 2 f), respectively, and yield the following equation for elevation: sin h ¼ sin d sin f þ cos d cos f cos t

x′, x ″

90 – ϕ

90 – h t x′″

90 – δ

Fig. 2.6

Astronomical triangle.

26

INTEGRATED NAVIGATION SYSTEMS

The ratio of sine’s above yields sin (360  Az) sin t ¼ sin (90  d) sin (90  h)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

from which the following equation for azimuth is obtained:  cos d sin t sin Az ¼ cos h By moving the angle identifications around the triangle, the following alternate form for the azimuth equation can be obtained: sin d cos f  cos d sin f cos t cos Az ¼ cos h

Direction cosines. With reference to Fig. 2.5, an original set of axes x –y –z is aligned parallel to the N – E – D axes shown. A sequence of three rotations aligns the x axis with the final orientation, which is along the line of sight to the astronomical object passing through the point R. First, a rotation about the y axis, through latitude f, aligns the original x axis direction with the point P to form the intermediate x0 axis as x'

2

cf C1 ¼ 4 0 sf

0 1 0

3

E, y, y'

φ

N, x

sf 0 5 cf z' D, z

Second, a rotation about the intermediate x0 axis through the hour angle t is shown 2

1 C2 ¼ 4 0 0

0 ct st

3

0 st 5 ct

E, y'

x', x"

τ

y"

z'

z"

This is shown as a t rotation, where t ¼ –t. Finally, a rotation about what is now the y00 axis, through 90 2 d, to align the final x000 axis with the astronomical body is shown as R, x'''

2 C3 ¼ 4

c(90  d) 0 0 1 s(90  d) 0

3

x''

Δ

y'', y'''

s(90  d) 5 0 c(90  d) z'''

This rotation is shown as a D rotation, where D ¼ 90 2 d.

z''

MATHEMATICAL PRELIMINARIES

27

Consider another sequence of two rotations, again starting with the original x– y– z axes parallel to the N – E – D axes, however, through different angles to arrive at the same orientation just obtained. First, an azimuth rotation, through Az, about the z axis is shown as 2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

cAz C4 ¼ 4 sAz 0

sAz cAz 0

E, y y'

3

0 05 1

x' N, x

Az D, z, z'

This rotation is followed by a rotation, through the elevation angle h, about the intermediate y0 axis to align the final x00 axes with the astronomical body: R, x''

2

ch C5 ¼ 4 0 sh

3

x'

h

0 s h 1 0 5 0 ch z' D, z, z'

Because the sequences just described start with the same x –y –z axes and end with the alignment of their corresponding final x axis with astronomical body, the first row’s elements of the following matrix product are equivalent: C3 C2 C1 ¼ C5 C4 Equating 1:3 elements of the resulting matrices yields the following equation for the elevation angle: sin h ¼ sin d sin f þ cos d cos t cos f which is identical to that obtained by using the astronomical triangle approach. Equating the 1:2 elements of this matrix product yields the following equation for the azimuth angle: sin Az ¼

 cos d sin t cos h

The same expression for the azimuth angle, as just obtained using the astronomical triangle approach, is found by equating the 1:1 elements of the matrix product as cos Az ¼

sin d cos f  cos d cos t sin f cos h

28

INTEGRATED NAVIGATION SYSTEMS

The ratio of the sine and cosine functions forms the following tangent function: tan Az ¼

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2.4.2

cos d sin t sin d cos f  cos d cos t sin f

Direction Cosine Matrix Rates

The time rate of change of the DCM is important to the later development of navigation state equations. This matrix differential equation governs the evolution of the corresponding DCM. This equation will be developed for the simple single-axis rotation presented in Fig. 2.1, then generalized for an arbitrary three-axis rotation. Consider the incremental rotation Dc over the time interval Dt of the y frame shown in Fig. 2.7. Examining the moving y-frame components of the vector r in Fig. 2.7, the future time t þ Dt components can be expressed in terms of the current time t components as y1 (t þ Dt) ¼ y1 (t) þ Dcy2 (t)

(2:65)

y2 (t þ Dt) ¼ y2 (t)  Dcy1 (t)

(2:66)

or, in vector/matrix form,    y1 (t þ Dt) 1 ¼ y2 (t þ Dt) Dc

Dc 1



y1 (t) y2 (t)

 (2:67)

Define 

 Dc ; I  DC 1

1 Dc

(2:68)

where  DC ;

y2 (t + Δt)

y2 (t) Δψ

0 Dc Dc 0

 (2:69)

x2 ψ

Δψ y1 (t) Δψ y1 (t)

r

y1 (t + Δt) Δψ y1 (t) ψ

x1

Fig. 2.7

Rotating vector/coordinate frame components.

MATHEMATICAL PRELIMINARIES

29

At time t, the components along the y frame can be expressed in terms of the x frame components by the C matrix determined earlier. This is expressed by y(t) ¼ C(t)x

(2:70)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

In terms of the x frame components, the values of y at the current time t þ Dt are expressed by y(t þ Dt) ¼ ½I  DCC(t)x ¼ C(t þ Dt)x

(2:71)

The time rate of change of the C matrix is defined as _ ¼ lim C(t þ Dt)  C(t) C(t) Dt!0 Dt ¼ lim

Dt!0

C(t)  DCC(t)  C(t) Dt

¼ lim  Dt!0

DC C(t) Dt

(2:72)

or y C_ x (t) ¼ Vyx=y (t)Cxy (t)

(2:73)

where the skew-symmetric rotation matrix V is Vyx=y ¼



0 v3

v3 0

 (2:74)

The rotation matrix V subscripts and superscript are interpreted as a rotation of the y frame relative to the x frame referenced in the y frame. The transposed equivalent is x C_ y ¼ Cyx (Vyx=y )T

¼ Cyx Vyx=y The second of these expressions is the result of the skew-symmetric form of the V matrix. (Its transpose is the negative of the original matrix.) Expressing this final result for a three-axis rotation, x C_ y ¼ Cyx Vyx=y

(2:75)

30

INTEGRATED NAVIGATION SYSTEMS

where 2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Vyx=y

0 ¼ 4 v3 v2

v3 0 v1

3 v2 v1 5 0

(2:76)

and the rotations vi (i ¼1, 2, and 3) correspond to rotations about the x, y, and z axes, respectively, of the y frame with respect to the x frame referenced in the y frame and written in vector notational form as vyx=y . The skew-symmetric matrix in Eq. (2.76) is equivalent to the angular velocity vector’s cross product ðvyx=y Þ. The skew-symmetric rotation matrix in Eq. (2.76), referenced in one frame, can be related to a rotation matrix’s rates referenced in another frame by using the similarity transformation Vyx=y ¼ Cxy Vxx=y Cyx

(2:77)

Note that the sense of the rotation, that is, y relative to x, is unchanged. The sense of rotation is reversed by changing the order of the subscripts of this matrix, accompanied with a corresponding change in sign of the matrix.

2.5

Miscellaneous Mathematical Topics

2.5.1 Angular Velocity Addition Theorem Use is made later of the angular velocity addition theorem. This theorem states that for angular velocity vectors referenced in a common frame,

va=z ¼ va=b þ vb=c þ    þ vy=z

(2:78)

2.5.2 Other Direction Cosine Matrix Dynamical Descriptions The earlier DCM can be expressed in terms of an orientation unit vector l and a rotation angle d about this vector as (see Fig. 2.8) Cyx ¼ cos d I þ sin d(l) þ (1  cos d)l lT

(2:79)

Using the following vector relationship,

l lT ¼ I þ (l  )(l)

(2:80)

This matrix can also be written as Cyx ¼ I þ sin d(l) þ (1  cos d)(l)(l)

(2:81)

Quaternions are a four-parameter representation of a transformation matrix and are defined as follows: d (2:82) q0 ¼ cos 2

MATHEMATICAL PRELIMINARIES

31

x2

λ

δ

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

β α

x1 γ

x3

Fig. 2.8

Orientation and rotation angles.

d d q1 ¼ l1 sin ¼ cos a sin 2 2 d d q2 ¼ l2 sin ¼ cos b sin 2 2 d d q3 ¼ l3 sin ¼ cos g sin 2 2

(2:83) (2:84) (2:85)

where cos2 a þ cos2 b þ cos2 g ¼ 1

(2:86)

The angles a, b, and g define the orientation of a unit vector defined with respect to the coordinate axes, as shown in Fig. 2.8. The preceding DCM can be expressed using quaternions as 2

q20 þ q21  q22  q23 x 4 Cy ¼ 2(q1 q2 þ q0 q3 ) 2(q1 q3  q0 q2 )

2(q1 q2  q0 q3 ) q20  q21 þ q22  q23 2(q2 q3 þ q0 q1 )

3 2(q1 q3 þ q0 q2 ) 2(q2 q3  q0 q1 ) 5 2 q0  q21  q22 þ q23

(2:87)

where qi satisfies the following orthogonality condition: q20 þ q21 þ q22 þ q23 ¼ 1

(2:88)

32

INTEGRATED NAVIGATION SYSTEMS

The dynamics of qi are described by the following vector differential equation: 2 3 q1 0 6 7 d6 1 q v 6 27¼ 6 3 dt 4 q3 5 2 4 v2 q0 v1

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2

v3 0 v1 v2

v2 v1 0 v3

32 3 v1 q1 6 q2 7 v2 7 76 7 v3 54 q3 5 0 q0

(2:89)

Initialization values for qi can be obtained from DCM elements. Advantages for using quaternions include a reduction of dynamic variables, that is, from six for the direction cosine matrix to four for quaternions, and the elimination of possible singularities for three-variable Euler angle equations (see Chapter 5 problems). Disadvantages include nonlinear terms in the result and the necessity of renormalizations within the computational cycles to maintain properties already discussed for DCMs. 2.6

Chapter Summary

Vector/matrix algebra and calculus topics were reviewed in this chapter. These operations have been applied to the development of a least-squares estimate. An extension of this approach to weighted least squares (see Problem 2.4) will be used to develop the Kalman filter algorithm in Chapter 8. The linearization of nonlinear equations to obtain linearized forms of these equations was also illustrated. These techniques are required to linearize nonlinear navigation state equations, developed in Chapter 5, to obtain state error equations for implementation in the Kalman filter algorithm. Transformations of vector components between different coordinate systems using DCMs were presented. Sequential rotations relative to intermediate coordinate frames were used to establish a transformation matrix relating vector components in one coordinate system to the components of that vector in another coordinate system, with the two coordinate systems being of arbitrary orientation to each other. The dynamic equations that describe the evolution of direction cosine matrices were developed. Direction cosine matrix dynamic equations are used in Chapter 5 to describe the attitude and position dynamics portion of navigation state equations (see Sec. 5.2). Several miscellaneous mathematical topics, including the angular velocity addition theorem and a quaternion description for the DCM, were presented without development. The angular velocity addition theorem is used extensively in later developments of navigation state equations.

Problems 2.1

Consider the problem of estimating an unknown constant from data that contain additive error. Many samples of data are available. Available data are related to the unknown constant as z i ¼ a þ vi

MATHEMATICAL PRELIMINARIES

33

where a ¼ constant to be estimated and v ¼ zero mean additive error with each sample. Using the least-squares estimate formed earlier in Example 2.3, find an estimate of the unknown constant a. First, assume two samples are available from which to form an estimate. The problem can be reformulated as a vector as z ¼ ha þv

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

or 

z1 z2



    1 v ¼ aþ 1 v2 1

Assume that additive errors are independent and uncorrelated, that is,  R¼

r1 0

0 r2



Using definitions for the measurement matrix H and unknown vector x, obtain the following expressions for the estimate and estimation error variance: 1 a^ ¼ (z1 þ z2 ) 2 1 p ¼ (r1 þ r2 ) 4 The least-squares estimate of the constant with zero mean additive error is just the average of the data. Extend this result for the case of three measurements to obtain the following: 1 a^ ¼ (z1 þ z2 þ z3 ) 3 1 p ¼ (r1 þ r2 þ r3 ) 9 Generalize this result to show that, in the general case of m measurements, the estimate of the constant and the estimation error variance becomes a^ ¼

m 1X zi m i¼1

p¼

m 1 X ri m2 i¼1

34

INTEGRATED NAVIGATION SYSTEMS And, if additive errors are identically distributed with variance r, the estimation error variance becomes p¼

1 r m

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

This result shows that the estimate of the constant is the mean or average of the data samples and that the uncertainty associated with the estimate is reduced by the number of samples used to determine the estimate.

2.2

Consider the problem of estimating radial position error rate circular error probable (CEP) from time-tagged radial position errors. From Example 2.3, an estimate of the slope m is desired where the intercept b is defined as zero. The measurement matrix (vector) for m observations becomes 2

3 t1 6 t2 7 6 7 6 7 h ¼ 6 t3 7 6 . 7 4 .. 5 tm for m radial position error observations 2

3 z1 6 z2 7 6 7 6 7 z ¼ 6 z3 7 6 .. 7 4 . 5 zm Show that the least-squares estimate for slope m can be expressed as Pm Pm zi ti i¼1 zi hi ^ ¼ Pi¼1 x^ ¼ Pm 2 ¼ m m 2 h i¼1 i i¼1 ti where the subscripts correspond to vector components. To establish the CEP, a number of sample sets are used and an estimate of each set’s slope computed. For N test sample sets, if errors associated with the radial error rate are Gaussian, then the CEP is computed by

CEPRER

vffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi u N uX ^ 2i =N ¼ 0:83t m i¼1

where RER is radial error rate.

MATHEMATICAL PRELIMINARIES 2.3

35

Gyro static test data are analyzed to characterize statistically independent error contributors. This exercise applies the least-squares methodology to estimate these error contributors. Data groups are formed corresponding to different time spans t, and corresponding variances are computed s2 ðtÞ. The model defined for these variances (square-root) is a1=2 a1 sðtÞ ; a0 þ pffiffiffi þ t t

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

The a’s in this equation are unknown and are to be determined. Assume a vector defined in terms of these parameters as 2

a0

3

x ¼ 4 a1=2 5 a1 Show that the H matrix row (vector) for each observation is  hi ¼ 1

1 1 pffiffiffiffi ti ti

T

The variances so computed are known as Allan variances, and the coefficients of the correspond to the following error contributors: Quantization:

s2Q ðtÞ  fQ

 1 t2

Angle random walk:

s2rw ðtÞ

 1  frw t

and Bias stability:

s2b ðtÞ 2.4

  fb

1 t0



This exercise develops a weighted least-squares estimate, extending Example 2.3. The weighted least-squares cost function form is used in the development of the Kalman filter algorithm. The cost function is modified by weighting the vector inner product with a symmetric matrix W as follows: JWLS ¼ (z  Hx)T W(z  Hx)

36

INTEGRATED NAVIGATION SYSTEMS Expand terms to yield JWLS ¼ zT Wz  xT H T Wz  zT WHx þ xT H T WHx T

¼ a  xT y0  y00 x þ xT (Ax) where

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

a

¼ scalar (no functional dependence on x)

y0 ¼ H T Wz ¼ (n  1) column vector y00T ¼ zT WH ¼ (1  n) row vector A ¼ H T WH ¼ (n  n) matrix Recall Eqs. (2.24) and (2.25): a ¼ xT y ¼ yT x @a ¼ yT @x With the following expression for the vector y y ¼ Ax and, from Eq. (2.26), its derivative is @y ¼A @x These definitions are used in the minimization of the cost function J with respect to the vector x. Take the partial derivative of each term in the preceding expanded equation to yield @JWLS @ ¼ y0 T  y00 T þ (xT y) @x @x ¼ zT WH  zT WH þ xT H T WH þ xT H T WH ¼ 2(zT WH þ xT H T WH) Setting this result to zero, transposing, and solving for the vector x, (xT H T WH)T ¼ (zT WH)T ) (H T WH)x ¼ (HW)z

MATHEMATICAL PRELIMINARIES

37

or xˆ ¼ (H T WH)1 H T Wz The following properties are to be developed as in Example 2.3, replacing the weighting matrix W with the matrix R 21. 1) Estimation error: Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

1 ; x  xˆ ¼ x  (H T R1 H)1 H T R1 z ¼ x  (H T R1 H)1 H T R1 (Hx þ v)  1 ¼ x  H T R1 H (H T R1 H)x  (H T R1 H)1 H T R1 v ¼ (H T R1 H)1 H T R1 v If the errors are random zero mean processes such that E½v ; 0

and

E½v vT  ; R

then 2) Estimation error covariance:  E½(x  xˆ )(x  xˆ )T  ¼ E ½(H T R1 H)1 H T R1 v½(H T R1 H)1 H T R1 vT ¼ (H T R1 H)1 H T R1 E½vvT R1 H(H T R1 H)1 ¼ (H T R1 H)1 H T R1 RR1 H(H T R1 H)1 ¼ (H T R1 H)1 (H T R1 H)(H T R1 H)1 ¼ (H T R1 H)1 ;P 2.5

This exercise exploits the orthonormal properties of direction cosine matrices. Consider the following form for a DCM: C ; ½ujvjw where its columns are orthogonal unit vectors. With this property, uv¼w vw¼u

38

INTEGRATED NAVIGATION SYSTEMS and wu¼v From these cross products, the third element of each can be obtained as u1 v2  u2 v1 ¼ w3

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

v1 w2  v2 w1 ¼ u3 and w1 u2  w2 u1 ¼ v3 Returning to the definition of the preceding DCM, the following are obtained for the DCM’s third “row” in terms of the other rows: C11 C22  C21 C12 ¼ C33 C12 C23  C22 C13 ¼ C31 and C13 C21  C23 C11 ¼ C32 These last three equations are useful in relating three of the DCM’s elements to its others. This property is beneficial because it allows a numerical integration algorithm that implements only two of the three rows of the DCM, integrating six rather than nine differential equations. 2.6

Additional terms in state transition matrix are developed in this exercise. Continue the expansion of the perturbation state in a Taylor-series beyond the first term

dx(t þ Dt) ¼ dx(t) þ

@dx(t) 1 @2 dxðtÞ 2 Dt þ Dt þ    @t 2 @t2

where @dx=@t ; dx˙ , etc. Then, by interchanging the order of differentiation, obtain the following  @ 2 dx @ @ @f @f @dx @f @f ¼ ¼ (dx˙ ) ¼ dx ffi dx @t2 @t @t @x @x @t @x @x assuming that @f =@xjx¼x0 ; F is a constant matrix. Substitute this expression, using the F notation, into the preceding expansion, to obtain

MATHEMATICAL PRELIMINARIES

39

the following:

dxðt þ DtÞ ¼ dxðtÞ þ FDtdx þ 12F 2 Dt2 dx þ      ¼ I þ FDt þ 12F 2 Dt2 þ    dxðtÞ ; Ft,tþDt dxðtÞ

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Define the state transition matrix as Ft,tþDt ; bI þ FDt þ 12F 2 Dt2 þ   c ) eFDt 2.7

Obtain a linear equation relating the change in range to perturbations about nominal position. The three-dimensional position components Dx, Dy; pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ffi and Dz are used to form range as r ¼ Dx2 þ Dy2 þ Dz2 . Defining the nominal relative position vector as 2

Dx

3

6 7 7 Dx ¼ 6 4 Dy 5 Dz and its perturbation as 2

dx

3

6 7 7 dx ¼ 6 4 dy 5

dz then use the truncated Taylor-series expansion to evaluate the perturbed range as rðDx þ dxÞ  rðDxÞ þ ð@r=@DxÞdx to obtain the range error dr ¼ rðDx þ dxÞ  rðDxÞ related to perturbed position vector as 

dr 

2.8

Dx r

Dy r

2 3  dx 7 Dz 6 6 dy 7 4 5 r dz

In this exercise, a numerical algorithm solution to the DCM differential equation is developed. The homogeneous matrix differential equation X_ ðtÞ ¼ AðtÞX ðtÞ

40

INTEGRATED NAVIGATION SYSTEMS has the solution X ðtÞ ¼ Fðt,t0 ÞX ðt0 Þ

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

where the state transition matrix can be expressed in terms of the matrix exponential 2t 3 ð Fðt,t0 Þ ¼ exp4 A(t)dt5 t0 ADt

e  2 3 4 2 Dt 3 Dt 4 Dt þA þA  I þ ADt þ A 2! 3! 4! for small Dt ¼ t – t0 and constant A. Consider the following form of the DCM differential equation: C_ yx ¼ Vyx=y Cxy The skew-symmetric rotation matrix corresponds to the preceding matrix A. Then, the product A Dt is written as 2 3 0 Du3 Du2 ADt ¼ 4 Du3 0 Du1 5 Du2 Du1 0 Show that, up to the fourth-order term, the approximation to the matrix exponential can be expressed as 2 3 aDu3 þ bDu1 Du2 aDu2 þ bDu1 Du3 1  b(Du22 þ Du23 ) eADt  4 aDu3 þ bDu1 Du2 1  b(Du21 þ Du23 ) aDu1 þ bDu2 Du3 5 aDu2 þ bDu1 Du3 aDu1 þ bDu2 Du3 1  b(Du21 þ Du22 ) where a;1

3 X

Du2i =6

i¼1

and 3 1 X Du2 =24 b;  2 i¼1 i

Once initialized, the DCM is integrated between time steps using this expression.

MATHEMATICAL PRELIMINARIES 2.9

41

A numerical algorithm solution for a nonhomogeneous vector differential equation is developed in this exercise. The vector differential equation x˙ (t) ¼ Ax(t) þ u has the solution

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

x(t) ¼ eADt xðt0 Þ þ A1 (eADt  1)u where    Dt Dt2 Dt3 þ A3 A1 eADt  1  I þ A þ A2 Dt 3! 4! 2 up to the fourth-order. Using the definition of the matrix A from Problem 2.8, show that this equation can be expressed as   A1 eADt  I 2 3 bDu3 þ cDu1 Du2 bDu2 þ cDu1 Du3 a þ cDu21 6 7  4 bDu3 þ cDu1 Du2 a þ cDu22 bDu1 þ cDu2 Du3 5Dt bDu2 þ cDu1 Du3

bDu1 þ cDu2 Du3

a þ cDu23

where b was given earlier and c ¼ 1/6.

2.10

Solve for quaternions in terms of elements of the Cyx matrix. The trace of this matrix is x x x tr½Cyx  ; Cy11 þ Cy22 þ Cy33

¼ 4q20  1 Obtain the following: q1 ¼

x x Cy32  Cy23 4q0

q2 ¼

x x Cy13  Cy31 4q0

q3 ¼

x x Cy21  Cy12 4q0

These expressions provide quaternion values, from DCM elements, to initialize the quaternion differential equation (2.89).

42 2.11

INTEGRATED NAVIGATION SYSTEMS This exercise reformulates the quaternion dynamics in Eq. (2.89) and establishes several quaternion relationships to be used later in the Attitude Determination and Estimation chapter. Rewrite Eq. (2.89) as 1 q˙ ¼  Z y v 2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

where the matrix Z y is defined as 2

q3

q0

6 6 q3 6 Z ;6 6 q2 4 y

q0 q1

q1

q2

q0

q3

q2

3

q2

3

7 q1 7 7 7 q0 7 5 q3

Define another matrix Z x as 2

6 6 q3 6 Zx ; 6 6 q2 4 q1

q0 q1 q2

7 q1 7 7 7 q0 7 5 q3

and show the following relationships: T

Cyx ¼ Z x Z y , 2.12

Z x Cyx ¼ Z y ,

T

Z y q ¼ 031

and

T

T

Z y Z y ¼ Z x Z x ¼ I33

Confirm the similarity transformation in Eq. (2.77) in the following. Consider the following rotation vector change in coordinate (transformation) from the x frame to the y frame, where the parentheses ( ) is the place where the rotation to and from frames is usually located.

vyð Þ ¼ Cxy vxð Þ 2 32 x 3 v1 C11 C12 C13 6 76 7 6 76 7 ¼ 6 C21 C22 C23 76 vx2 7 4 54 5 vx3 C31 C32 C33 2 3 C11 vx1 þ C12 vx2 þ C13 vx3 6 7 6 7 ¼ 6 C21 vx1 þ C22 vx2 þ C23 vx3 7 4 5 x x x C31 v1 þ C32 v2 þ C33 v3

MATHEMATICAL PRELIMINARIES

43

The similarity transformation is defined as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Vyð Þ ¼ Cxy Vxð Þ Cyx 2 C11 C12 6 ¼ 4 C21 C22 C31 C32

32

0 76 x C23 54 v3 vx2 C33 C13

vx3 0

vx1

32 vx2 C11 x 76 v1 54 C12 0 C13

C21 C22 C23

C31

3

7 C32 5 C33

Equating the 3:2 elements of this product     Vyð Þ32 ¼ vy1 ¼ C31 vx3 C22 þ vx2 C23 þ C32 vx3 C21  vx1 C23   þ C33 vx2 C21 þ vx1 C22 ¼ ðC22 C33  C32 C23 Þvx1 þ ðC31 C23  C33 C21 Þvx2 þ ðC32 C21  C31 C22 Þvx3 ¼ C11 vx1 þ C12 vx2 þ C13 vx3 Because ðC22 C33  C32 C23 Þ ¼ C11 , etc. Equate the two other elements to complete the demonstration of the similarity transformation. 2.13

This is an advanced exercise for the development of an iterative algorithm for orthonormalization of a DCM. It is desired to minimize the deviation of the computed DCM and the true DCM (C  C) subject to the constraint that the true DCM satisfies CT C ¼ I Consider the following scalar cost function: h T    i J ¼ tr C  C C  C þ L C T C  I where the preceding constraint has been included in the cost function via the Lagrange multiplier matrix L and L is assumed to be a symmetric matrix. Minimizing this function (taking the partial derivative with respect to C and equating the result to the null matrix) yields the following:   033 ¼ 2 C T þ (I þ L)C T The value of C that minimizes the cost function becomes  þ L)1 C^ ¼ C(I where it is assumed that the matrix (I þ L) is nonsingular. Next, the preceding constraint equation is used to eliminate (1 þ L) in the result just obtained. Substituting that result into the

44

INTEGRATED NAVIGATION SYSTEMS constraint equation, C^ T C^ ¼ I ¼ ðI þ LÞ1 C T C ðI þ LÞ1 and solving for (I þ L) yields  1=2 I þ L ¼ +(C T C)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

ˆ equation yields Substituting this expression into the preceding C  1=2  C T C) C^ ¼ +C( An iterative form of this equation can be obtained by first forming the error matrix E ; C T C  I or C T C ¼ I þ E then  þ E)1=2 C^ ¼ +C(I If the error matrix E is sufficiently small, then (I þ E) – 1/2 can be expressed in the series form   1 13 2 (I þ E)1=2  I  E þ E  O E3 2 24 Retaining only the first two terms in this series and substituting into the second Cˆ equation yields  1 ^  CC I E 2    1T   ¼C I C CI 2 3 1 ¼ C  C C T C 2 2 The iterative character of this equation can be established by using the following replacements: C^

 Cnþ1

MATHEMATICAL PRELIMINARIES

45

and C

 Cn

Then, the iteration algorithm becomes 3 1 Cnþ1 ¼ Cn  Cn CnT Cn 2 2 Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2.14

The inverse of a partitioned matrix is considered in this exercise. Consider the matrix P  X P xx P Pxy ¼ yx

yy

where it is assumed that the matrices S and Syy are nonsingular. Develop the following inverse of S in terms of its partitioned elements: X1

 ¼

P P1 P 1  " P 0 yy xx  xy yx I xy 0 P P1 

I P P1 T

  I  0

yy



xy

0 P1

#

yy

yy

I

and the following expression for the determinate of

P

X P P P P

P



¼ xx  xy 1 yy yy yx

Hint: Use the following transformation to transform this matrix into a diagonal form: "

2.15

I 0

# P #" P P1 #" P I 0 xx xy yy xy P P P PT I  1 I yy xy yx yy "P # " P P1 P I 0 yy xx  xy yx P1 PT ¼ P P  yy xy yx yy "P # P P1 P 0 yy xx  xy yx ¼ P 0 yy 

0

#

I

A factor of the symmetric P matrix presented in Example 2.1 is considered again by developing a square-root factor defined by P ; SST

46

INTEGRATED NAVIGATION SYSTEMS where the columns of square-root matrix are written in terms of vectors as  S ¼ v1

v2

   vn



Show that the P matrix can be expressed as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

P¼

n X

vi vTi

i¼1

Hint: Establish the factors by considering the matrix P expressed in terms of a 3  3 matrix. 2

2

3

p11

p12

p13

s11

0

6 P¼6 4 p12

p22

7 6 6 p23 7 5 ¼ 4 s12

s22

p13

p23

p33

s23

2

s211 6 ¼6 4 s11 s12

s13

s11 s12

3

s11

s12

s13

76 6 0 7 54 0

s22

7 s23 7 5

s33

0

0

s33

3

s11 s13

7 s12 s13 þ s22 s23 7 5

s212 þ s222

s11 s13

32

0

s12 s13 þ s22 s23

s213 þ s223 þ s233

Define the vectors and resulting outer product as 2

s11

2

3

s11

3

2

6 7 6 7 T 6 7 7 v1 ¼ 6 4 s12 5 v1 v1 ¼ 4 s12 5 s11 s12 s13 2

s13 3

0

2

3

0

3 s211 s11 s12 s11 s13  6 7 s13 ¼ 4 s11 s12 s212 s12 s13 5 2

0

 6 6 7 6 7 v2 ¼ 4 s22 5 v2 vT2 ¼ 4 s22 5 0 s22 s23 ¼ 4 0 s23 2

s33

2

s222

3 2 0 0  6 6 7 6 7 T v3 ¼ 4 0 5 v3 v3 ¼ 4 0 5 0 0 s33 ¼ 4 0 0 0

3

0

0

0

s33

3

7 s22 s23 5 s223

0 s22 s23

s23

s213

s11 s13 s12 s13

0

3

0 7 5

0 0 s233

MATHEMATICAL PRELIMINARIES Then, the P matrix becomes P ¼ v1 vT1 þ v2 vT2 þ v3 vT3 and the elements for these vectors become

=

=

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

s211 ¼ p11 s12 ¼ p12 s11 s13 ¼ p13 s11 s222 ¼ p22  s212 s23 ¼

( p23  s12 s13 ) s22

s233 ¼ p33  (s213 þ s223 )

47

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

3 Coordinate Systems and Transformations

Navigation system states—position, velocity, and attitude—are defined with reference to coordinate frames, and there are a number of different frames in current use. Because there are differences, the survey of these systems presented in this chapter is intended to help reader understand information that is communicated using these frames. This chapter presents a survey of coordinate systems and coordinate frame transformations. Coordinate systems that are currently used in navigation systems are presented first. These include several navigation frames, including Earth-centered, local geodetic, and wander azimuth. Certain local geodetic frames are usually associated with specific Earth-centered frames. These and their associations are presented. Body-axis frame definitions are also presented. Like the association between certain local geodetic frames and Earth-centered frames, various body frames are also associated with specific local geodetic frames. These associations are also presented. For the coordinate systems defined, transformation matrices (direction cosine matrices) are developed to transform navigation variables from one frame to another. Depending on the axis system definitions, different forms of the corresponding transformation matrices are obtained. The concept of an external error vector associated with a direction cosine matrix is presented, and the relationship between this external error and errors in Euler angles is illustrated for position and attitude matrices.

3.1 Coordinate Systems Coordinate systems are established to allow information to be exchanged between interfacing systems in a consistent manner. This section reviews the definitions of some commonly used coordinate systems. 3.1.1

Earth-Centered, Earth-Fixed Frame

The Earth-centered, Earth-fixed (ECEF) frame is fixed within the Earth and its rotation, and it is centered at the Earth’s center. Axis definitions in current use vary. Shown in Figs. 3.1, 3.2, and 3.3 are illustrations of three possible ECEF frames. In the first frame, shown in Fig. 3.1, the z axis is parallel to and aligned with the direction of the Earth’s rotation. In the equatorial plane, the x 49

50

INTEGRATED NAVIGATION SYSTEMS ze ωe

N E z''

#3

x''

y''

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

D #2 ϕ

ye z' λ

y' #1 x'

z y xe

Fig. 3.1

start

x

ECEF coordinate frame with z axis along Earth’s rotation axis.

axis locates the Greenwich meridian, and the y axis completes the right-hand system. In Figs. 3.2 and 3.3, the direction of the Earth’s rotation is parallel to the y and x axes, respectively. In Fig. 3.2, the z axis locates the Greenwich meridian, and the x axis completes the right-hand system. In the third frame, the z axis locates the Greenwich meridian, and the y axis completes the right-hand system. Illustrated in these figures are intermediate frames resulting from single axis rotations. Sequences of rotations will be used to form the direction cosine matrices needed to transformation vector components from the ECEF frame to a local geodetic frame. 3.1.2 Earth-Centered Inertial Frame In Figs. 3.1 – 3.3, a corresponding Earth-centered inertial (ECI) frame exists and is established by the direction of the Earth’s rotation. This inertial frame is fixed to an inertial reference. The further definition of an inertial frame is not used in this treatment of navigation systems. If the reader is incorporating navigation aids that are based on stellar updates, then the inertial reference would have to be defined. 3.1.3

Local Geodetic Frame

Usually associated with the ECEF frames shown in Figs. 3.1, 3.2, and 3.3 are local geodetic (geographic) frames.

COORDINATE SYSTEMS AND TRANSFORMATIONS ye

ωe

N

U

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

E y''

z''

x'

#2

ϕ xe y'

x'

λ

#1 z'

y start

x z ze

Fig. 3.2 ECEF coordinate frame with y axis along Earth’s rotation axis.

xe

ωe

N

U z''

x'' W

ye

#2 y''

ϕ x'

λ

#1 y'

z'

x y ze

start z

Fig. 3.3 ECEF coordinate frame with x axis along Earth’s rotation axis.

51

52

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

3.1.4

INTEGRATED NAVIGATION SYSTEMS Wander Azimuth Frame

The wander-azimuth (WA) frame is a local geodetic frame that is in current use for many navigation systems, and there are several WA frame definitions. The WA frames are usually associated with local geodetic frames shown in Figs. 3.1 –3.3 and are the result of an additional rotation about the geodetic frame’s vertical axis. In Fig. 3.4, the WA frame is rotated with respect to the local geodetic frame z axis (down) by the WA angle a. For the last two frames in Figs. 3.2 and 3.3, the corresponding WA frame and angle are defined as shown in Figs. 3.5 and 3.6. In these figures, the WA frame is rotated with respect to the local geodetic frame z axis (up) by the WA angle a. It will be shown later that the angle a, whose rotation direction is about the z axis (down) axis (shown in Fig. 3.4), satisfies the following differential equation:

a˙ ¼ l˙ sin f

(3:1)

whereas the corresponding equation for the other two WA frames, whose rotation direction is about the z axis (up) axis (shown in Figs. 3.5 and 3.6), is

a˙ ¼ l˙ sin f

(3:2)

ze ωe

finish N αX

n

E Yn

#3

D, Z n ϕ

ye λ

xe

Fig. 3.4

Local-level WA frame for Fig. 3.1.

COORDINATE SYSTEMS AND TRANSFORMATIONS ye ωe

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

N Xnα

U, Z n Yn

E

ϕ

xe λ

ze

Fig. 3.5

Wander azimuth for Fig. 3.2. xe ωe

finish N Xnα

U, Z n

#2 W

ye

Yn ϕ

λ

ze

Fig. 3.6

Wander azimuth for Fig. 3.3.

53

54

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

3.1.5

INTEGRATED NAVIGATION SYSTEMS Body Frame

The body frame is rigidly attached to and defined within the vehicle carrying the navigation system. For strap-down navigation units, the body frame is usually identical to, or the result of a fixed transformation relative to, the navigation system inertial sensor’s case frame. Conventional definitions for this frame have the x axis along the vehicle longitudinal axis, the z axis downward, and the y axis pointed outward, completing the right-hand set. This frame is shown in Fig. 3.7 with yaw C, pitch u, and roll f rotations about navigation frame X –Y –Z axes. The corresponding body frame for the WA frame in Fig. 3.6 is illustrated in Fig. 3.8. The rotations follow the same yaw c, pitch u, and roll f sequence as in Fig. 3.7. 3.1.6

Geographic Local Horizon Line of Sight

The line of sight (LOS) is defined in terms of azimuth Az and elevation El relative to a geographic frame (north –east – down or N – E – D), as shown in Fig. 3.9. The first rotation is about D through the Az angle, then a rotation through El to establish the LOS axis.

3.2

Coordinate Frame Transformations

This section develops several coordinate frame transformation matrices defined in the preceding section. Because several ECEF frames are used, a subscript within parentheses is used to denote the axis about which the Earth’s rotation is defined for the corresponding frames as a means to distinguish the associated matrix formed for one frame from that used for another. Xn

xb

ψ

zb ϕ

yb

θ

Yn

Zn

Fig. 3.7

Body coordinate frame for Fig. 3.4 WA frame.

COORDINATE SYSTEMS AND TRANSFORMATIONS Zn

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

xb yb ϕ

zb

Xn

θ

ψ

Yn

Fig. 3.8

Body coordinate frame for Fig. 3.6 WA frame.

los N El

Az

E Az El D

Fig. 3.9

Geographic horizon line-of-sight frame.

55

56

INTEGRATED NAVIGATION SYSTEMS

3.2.1

Earth-Centered to Earth-Fixed to Local Geodetic

Referring to Fig. 3.3, the transformation matrix from ECEF to local geodetic proceeds with the following sequence of two rotations: Rotation 1 (positive rotation l about xe): 2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

1 0 C1 ¼ 4 0 cl 0 sl Rotation 2 (positive rotation f about 2 cf C2 ¼ 4 0 sf

3 0 sl 5 cl

(3:3)

3 sf 0 5 cf

(3:4)

ye0 ): 0 1 0

where ye0 represents the transformed y axis after rotation 1. The resulting ECEF to local geodetic (north – west – up) transformation matrix is 2 3 cf sfsl sfcl g C e ð xÞ ¼ 4 0 cl sl 5 (3:5) sf cfsl cfcl

3.2.2 Earth-Centered, Earth-Fixed to Navigation Frame (Wander Azimuth) This transformation matrix requires an additional transformation from the local geodetic to the navigation frame through the WA angle a, as shown in Fig. 3.6. Cen ð xÞ ¼ Cgn ð xÞCeg ð xÞ 2 c a sa 6 ¼ 4 sa ca 0

0

0

3

7 0 5Ceg ð xÞ

(3:6)

1

Or, 2

c ac f Cen ð xÞ ¼ 4 sacf sf

casfsl þ sacl sasfsl þ cacl cfsl

3 casfcl þ sasl sasfcl þ casl 5 cf cl

(3:7)

Note that  T Cne ð xÞ ¼ Cen ð xÞ

(3:8)

COORDINATE SYSTEMS AND TRANSFORMATIONS 3.2.3

57

Earth-Centered, Earth-Fixed to Local Geodetic

Referring to Fig. 3.1, the transformation matrix from ECEF to local geodetic proceeds with the following sequence of three rotations: Rotation 1 [positive rotation (right-hand sense) l about ze]: 2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

cl C1 ¼ 4 sl 0

3 sl 0 cl 0 5 0 1

(3:9)

Rotation 2 (positive rotation f about ye0 , but with a change in sign to change the sense): 2

cf C2 ¼ 4 0 sf

3 0 sf 1 0 5 0 cf

(3:10)

Rotation 3 (align axes from up– east –north to north– east –down): 2

0 C3 ¼ 4 0 1

0 1 0

3 1 05 0

(3:11)

or 2

sfcl Ceg ðzÞ ¼ 4 sl cfcl

3.2.4

sfsl cl cfsl

3 cf 0 5 sf

(3:12)

Earth-Centered Inertial to Local Geodetic Frame

The rotation from ECI to ECEF, as a result the Earth’s rotation, is defined by the direction of the Earth’s rotation. The angle resulting from this rotation is defined as

u ¼ vi=e t

(3:13)

The preceding transformation from ECEF to local geodetic is postmultiplied by an additional transformation resulting from a rotation about the z axis through the

58

INTEGRATED NAVIGATION SYSTEMS

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

angle u as Cig ðzÞ ¼ Ceg ðzÞCie ðzÞ 2 c u su 6 g ¼ Ce ðzÞ4 su cu 0 0 2 c l sl 6 ¼ C3 C2 4 sl cl 0 0 2 c ð l þ uÞ 6 ¼ C3 C2 4 sðl þ uÞ 0

0

3

7 05 1 32 0 cu 76 0 54 su 1 0 s ð l þ uÞ cðl þ uÞ 0

su

0

3

7 cu 0 5 0 1 3 0 7 05 1

(3:14)

or 2

3 sfcðl þ uÞ sfsðl þ uÞ cf Cig ðzÞ ¼ 4 sðl þ uÞ cðl þ uÞ 0 5 cfcðl þ uÞ cfsðl þ uÞ sf

3.2.5

(3:15)

Body-to-Navigation Frame

Referring to Fig. 3.7, the transformation from the X –Y – Z navigation frame to the body x– y– z axes proceeds through yaw, pitch, and roll rotations. This transformation was developed in Sec. 2.4. This sequence of rotations produces the following transposed transformation matrix: 2 3 cuccaz cwscaz þ swsuccaz swscaz þ cwsuccaz Cbn ðzÞ ¼ 4 cuscaz cwccaz þ swsuscaz swccaz þ cwsuscaz 5 (3:16) su s wc u c wc u For the navigation frame shown in Fig. 3.8, the body-to-navigation frame transformation matrix is developed using the reverse of the sequences just used. The transformation matrix is produced through a sequence of four rotations. The first three transform from the body to an intermediate navigation frame that is consistent with the body axis illustrated in Fig. 3.8. That is, with zero rotations, the x, y, and z axes for the two frames are parallel. The resulting intermediate navigation frame is then rotated about its x axis by 180 deg, which is common to both intermediate and final navigation frame axes. The required transformation matrix proceeds with the following sequence of rotations. Note that although the rotations are indicated as negative, the individual rotations are formed as positive rotations, and then the sign of the angle is changed to change the sense of the rotation.

COORDINATE SYSTEMS AND TRANSFORMATIONS

59

Rotation 1 (2w about xb): 2

1 C1 ¼ 4 0 0

3 0 sw 5 cw

(3:17)

3 0 su 1 05 0 cu

(3:18)

0 cw sw

Rotation 2 (2u about yb0 ):

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2

cu C2 ¼ 4 0 su Rotation 3 (2 caz about z00b): 2

ccaz C3 ¼ 4 scaz 0

scaz ccaz 0

3 0 05 1

(3:19)

Rotation 4 (180 deg about x000 b ): 2

1 C4 ¼ 4 0 0

0 1 0

3 0 0 5 1

(3:20)

The body-to-navigation frame transformation matrix is 2

ccaz cu Cbn ð xÞ ¼ 4 scaz cu su 3.2.6

ccaz susw  scaz cw scaz susw  ccaz cw cusw

3 ccaz sucw þ scaz sw ccaz sw  scaz sucw 5 cucw

(3:21)

Geodetic to Line of Sight

Referring to Fig. 3.9, the transformation matrix from geodetic to line of sight proceeds with the following sequence of two rotations. Rotation 1 (Az about D): 2 3 cAz sAz 0 C1 ¼ 4 sAz cAz 0 5 (3:22) 0 0 1 Rotation 2 (El about E0 ): 2

cEl C2 ¼ 4 0 sEl

0 1 0

3 sEl 0 5 cEl

(3:23)

60

INTEGRATED NAVIGATION SYSTEMS

The resulting geographic to line-of-sight transformation matrix is 2

CgLOS

cElcAz ¼ 4 sAz sElcAz

cElsAz cAz sElsAz

3 sEl 0 5 cEl

(3:24)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Example 3.1

Line-of-Sight Angles and Linearization Using Relative Positions Define the relative position of an object being optically tracked with respect to the tracking device’s position as Dx ¼ x  xs Dy ¼ y  ys Dz ¼ z  zs where x, y, and z are the object’s local position coordinates and xs, ys, and zs are the tracking system’s local position coordinates. It is assumed that the tracking device is mechanized with elevation as the inner gimbal. The equations for these measurements are obtained by a sequence of rotations, first azimuth and second elevation, transforming the Cartesian axis components into the line-of-sight frame as 2

3 2 Dx0 cEl 4 Dy00 5 ¼ 4 0 Dz00 sEl

0 1 0

32 sEl cAz 0 54 sAz cEl 0

sAz cAz 0

32 3 0 Dx 0 54 Dy 5 1 Dz

where cEl, sEl are the cosine and sine of the elevation gimbal angle and cAz, sAz are the cosine and sine of the azimuth gimbal angle. Completing the preceding matrix multiplications yields the following. In this equation, the component of position along the line of sight is range, and the others are defined as zero— defining the line-of-sight angles. 2 3 2 cElcAz r 4 0 5 ¼ 4 sAz 0 sElcAz

cElsAz cAz sElsAz

32 3 sEl Dx 0 54 Dy 5 cEl Dz

where r is the range that is not provided by an optical tracking system. This matrix equation is expanded into component form for each of the last two rows. From these two equations, expressions for the azimuth and elevation can be obtained in terms of the relative positions. Table 3.1 summarizes the azimuth and elevation equations and their partial derivatives with respect to the relative positions.

COORDINATE SYSTEMS AND TRANSFORMATIONS

61

Table 3.1 Az and El measurements and partial derivatives Partial derivatives with respect to ( )

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Measurement equation tan Az ¼

Dy Dx

tan El ¼

Dz cAzDx þ sAzDy

x

y

Dy Dx2 þ Dy2 cAzDz ð Þ2 þ Dz2

Dx Dx2 þ Dy2 sAzDz ð Þ2 þ Dz2

z —— ðÞ a ð Þ2 þ Dz2 a

aa( ) ¼ cAzDx þ sAzDy:

Example 3.2

Wander Angle Rate for Cen(x)

The equation describing the rate of change in the Earth-to-navigation frame direction cosine matrix is established using the general form presented in Sec. 2.4. The elements of the Cen(x) matrix are given in Eqs. (3.7) and (3.8). This example develops the expression in Eq. (3.2) for the WA angle rate. The rate of change of the direction cosine matrix is (momentarily dropping the preceding x subscript) n C_ e ¼ Vne=n Cen

The navigation-to-Earth angular rotation vector, referenced in the navigation frame, is redefined for notational purposes as 2

vne=n

3 rx ; 4 ry 5 rz

Substituting the elements of this vector into the proper elements for the skewsymmetric form of the preceding vector cross product yields, for selected elements, n C_ e 11 ¼ rz Cen 21  ry Cen 31 n C_ e 31 ¼ ry Cen 11  rx Cen 21 n C_ e 32 ¼ ry Cen 12  rx Cen 22

Referring to the elements of the direction cosine matrix for the left and right side of these equations, these equations can be rewritten as sacfa˙  casff˙ ¼ sacfrz  sfry cff˙ ¼ cacfry þ sacfrx sfslf˙  cfcll˙ ¼ ðcasfsl þ saclÞry  ðsasfsl þ caclÞrx

62

INTEGRATED NAVIGATION SYSTEMS

Solving the second equation for f˙ yields

f˙ ¼ cary þ sarx Substituting this equation into the third equation yields

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

   sary  carx l˙ ¼ cf Substituting the equation for f˙ into the first equation yields 

   a˙  rz cf ¼  sary þ carx sf

which, after using the equation for l˙, gives the equation for WA rate

a˙ ¼ l˙ sin f þ rz To obtain Eq. (3.2) requires that the angular rate about the z axis be zero:

rz ¼ 0 Example 3.3 Angular Position and Latitude/Longitude/Wander Angle Errors This example introduces the concept of an external error vector, angular position error, associated with the position direction cosine matrix. The angular position error vector du is related to the computed or corrupted DCM, C¯ne (x) (indicated with an overbar) and true DCM Cne (x) (without the overbar) as in the following equation: n C e ; ½I  ðduÞ Cen

where 2

1 ½I  ðduÞ ¼ 4 duz duy

duz 1 dux

3 duy dux 5 1

The Cne (x) matrix from Sec. 3.2 is 2

c ac f Cen ð xÞ ¼ 4 sacf sf

casfsl þ sacl sasfsl þ cacl cfsl

3 casfcl þ sasl sasfcl þ casl 5 cf cl

This form holds whether the matrix is computed or true. If computed, then the corresponding latitude, longitude, and wander angles can be expressed in terms

COORDINATE SYSTEMS AND TRANSFORMATIONS

63

of their true value plus error as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

f¯ ¼ f þ df l¯ ¼ l þ dl a¯ ¼ a þ da The objective is to relate the error angles in these equations to the elements of angular position error vector du. It is assumed that du is given and expressions for dw, dl, and da are to be obtained. This is accomplished by selecting elements in the computed C ne (x) matrix, expanding them in terms of the trigonometric relationships for sums of angles (assuming small error angles), and equating those expressions to their corresponding terms from the matrix multiplication in the first equation just given. Beginning with the Cne zx term, sf¯ ¼ sf þ cfdf ¼ Cen zx þ duy Cen xx  dux Cen yx This implies, from the definition of Cne (x) just given, cfdf ¼ duy Cen xx  dux Cen yx ¼ duy ðcacfÞ  dux ðsacfÞ or, after canceling the cf term, yields the following equation for latitude error:

df ¼ duy ca þ dux sa Next, the Cne zy elements are used to solve to longitude error cf¯ sl¯ ¼ cfsl þ sfsldf  cfcldl ¼ Cen zy þ duy Cen xy  dux Cen yy which becomes sfsldf  cfcldl ¼ duy Cen xy  dux Cen yy From the definition of Cne (x) and the equation for df just given   sfsl duy ca þ dux sa  cfcldl ¼ duy ðcasfsl þ saclÞ  dux ðsasfsl þ caclÞ or, after expanding terms, casfslduy þ sasfsldux  cfcldl ¼ casfslduy þ saclduy þ sasfsldux  cacldux

64

INTEGRATED NAVIGATION SYSTEMS

Cancelling terms yields cfcldl ¼ saclduy  cacldux After cancelling cl, the following equation for longitude error dl is obtained: 

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

dl ¼

cadux  saduy cf



The position errors can be related to the latitude and longitude errors. Combining equations for the latitude and longitude errors, the following equations for the angular position errors are obtained:

dux ¼ cacfdl þ sadf duy ¼ sacfdl þ cadf Finally, from the Cne xx term, ca¯ cf¯ ¼ caci  sacfda  casfdf ¼ Cen xx þ duz Cen yx  duy Cen zx which becomes, after cancelling terms, sacfda  casfdf ¼ duz Cen yx  duy Cen zx From the equation for the angular position error duy just given, sacfda  casfdf ¼ duz ðsacfÞ  ðsacfdl þ cadfÞðsfÞ ¼ sacfduz þ sacfsfdl  casfdf After cancelling terms, the following equation for WA error da is obtained:

da ¼ duz  sfdl This example illustrates the relationship between the external error associated with the position direction cosine matrix and errors in latitude, longitude, and wander-azimuth angles that are used to form this DCM.

3.3

Chapter Summary

Several coordinate systems that are currently used for integrated navigation systems were presented, and their associated transformation matrices were developed. The awareness of different coordinate systems and their usage should reinforce the notion that assumed coordinate system definitions need to be verified prior to using data defined in different systems and consistency in their use to ensure a successful navigation system.

COORDINATE SYSTEMS AND TRANSFORMATIONS

65

The concept of position and attitude errors associated with their corresponding direction cosine matrices was presented. Linearized navigation state error equations for position and attitude errors, to be developed in Sec. 5.3, will be based on these error definitions.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Problems 3.1

This exercise develops the relationships between Cne (y) matrix and latitude, longitude, and wander angles. Referring to Fig. 3.2, show that the corresponding navigation-to-Earth frame direction cosine matrix (transposed) is 2

cacl  sasfsl Cen ð yÞ ¼ 4 sacl  casfsl cfsl where

s ac f c ac f sf

3 casl  sasfcl sasl  casfcl 5 cfcl

 T Cne ( y) ¼ Cen ( y)

and the following relationships: 2

3 2 n 3 Ce 21 0 6 7 6 7 vni=e ¼ Cen (y)4 vi=e 5 ¼ vi=e 4 Cen 22 5 Cen 23 0   f ¼ sin1 Cne 23  e  C l ¼ tan1 ne 13 Cn 33  e  C a ¼ tan1 ne 21 Cn 22

3.2

This exercise develops the wander angle rate for Cen(y). Show that the following expressions hold:

f˙ ¼ sary  carx   cary þ sarx ˙ l ¼ cf ˙ a˙ ¼ l sin f þ rz Note that the expression for the WA rate here is the same as that obtained in Example 3.2.

66 3.3

INTEGRATED NAVIGATION SYSTEMS This exercise examines the relationship between angular position and latitude/longitude/wander angle errors. Show that the following expressions hold for the ECEF coordinate frame Cen(y):

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

df ¼ duy sa  dux ca   sadux þ caduy dl ¼ cf dux ¼ sacfdl  cadf duy ¼ cacfdl þ sadf da ¼ duz  sfdl Note that the expression for the WA error is again the same as that obtained in Example 3.3. 3.4

This exercise continues to relate the external error vector, tilt errors, to errors associated with the Euler angles associated with the attitude direction cosine matrix Cnb(x). That is, tilt error vector f is to be related to yaw, pitch, and roll errors—dc, du, and dw. Tilt errors are defined in terms of the computed and true body-to-navigation frame direction cosine matrix n C b ; bI  ðfÞcCbn

where 2

1 ½I  ðfÞ ¼ 4 fz fy

fz 1 fx

3 fy fx 5 1

The Cnb(x) matrix is given in Eq. (3.21) as 2

ccaz cu Cbn ð xÞ ¼ 4 scaz cu su

ccaz susw  scaz cw scaz susw  ccaz cw cusw

3 ccaz sucw þ scaz sw ccaz sg  scaz sucw 5 cucw

This equation is assumed to hold whether it is computed or true. If computed, then the corresponding yaw, pitch, and roll angles can be expressed in terms of their true values plus error

c¯ az ¼ caz þ dc u¯ ¼ u þ du w¯ ¼ w þ dw

COORDINATE SYSTEMS AND TRANSFORMATIONS

67

The objective is to relate the preceding error angles to the tilt errors. This is accomplished by selecting elements in the computed Cnb(x) matrix, expanding them in terms of the trigonometric relationships for sums of angles, and equating those expressions to their corresponding terms from the matrix multiplication given in the first equation just given. Beginning with the Cnb zx terms,

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

su¯ ¼ su þ cudu ¼ Cbn zx þ fy Cbn xx  fx Cbn yx This implies from the definition of Cnb(x) cudu ¼ fy Cbn xx  fx Cbn yx     ¼ fy ccaz cu  fx scaz cu or, after cancelling the cu terms,

du ¼ ccaz fy þ scaz fx Continue in a similar fashion to obtain the following:   dcaz ¼ fz  ccaz fx  scaz fy tu and

dw ¼ 3.5

scaz fy  ccaz fx cu

Referring to Fig. 3.1, show that the corresponding navigation-to-Earth frame direction cosine matrix is 2

casfcl  sasl Cne (z) ¼ 4 casfsl þ sacl c ac f

sasfcl  casl sasfsl þ cacl sacf

and the following relationship: 2

vni=e

3 0 ; V ¼ Cen (z)4 0 5 vi=e

3 cfcl cfsl 5 sf

68

INTEGRATED NAVIGATION SYSTEMS

3.6

By paralleling the steps in Example 3.2, obtain the following expression for the WA rate for the ECEF coordinate frame Cne (z): a˙ ¼ l˙ sin f þ rz

3.7

In this exercise, body-referenced tilt errors are related to Euler-angle errors for the Cnb(z) matrix. Define body-referenced tilt errors fb as the result of the following product:

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598



T n Cbn C b ¼ I  Cnb ðfÞCbn   ¼ I  fb 

Establish the following relationships for the components of fb:

fbx ¼ dw  sudcaz fby ¼ cwdu  swcudcaz fbx ¼ swdu  cwcudcaz 3.8

This exercise develops relationships between the attitude error and quaternion error from the preceding chapter. From Problem 2.10, obtain the following equations for quaternion errors: dC11 þ dC22 þ dC33 ¼ 8q0 dq0

dq 1 ¼

dC32  dC23 C32  C23  dq0 4q0 4q20

¼

dC32  dC23 q1  d q0 4q0 q0

dq 2 ¼

dC13  dC31 q2  d q0 4q0 q0

dq 3 ¼

dC21  dC12 q3  d q0 4q0 q0

Define a computed DCM similarly as just shown i C b ; bI  ðfÞcCbi

¼ Cbi  ðfÞCbi and obtain the error matrix

dCbi ¼ ðfÞCbi

COORDINATE SYSTEMS AND TRANSFORMATIONS

69

Expand terms in this error matrix, and insert them into the quaternion error equations just given to obtain the following relationship between quaternion errors and attitude errors: 2 3 dq1 q0 6 dq2 7 1 6 q3 6 6 7 4 dq3 5 ¼ 2 4 q2 dq0 q1 Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2

q3 q0 q1 q2

3 q2 2 3 f1 q1 7 74 f2 5 q0 5 f3 q3

which can be rewritten, using the results from Exercise 2.11, associating the y frame with the moving body frame and the x frame as the fixed inertial frame, 1 dq ¼ Z b f 2 where 2

q0 6 q3 Zb ; 6 4 q2 q1

q3 q0 q1 q2

3 q2 q1 7 7 q0 5 q3

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

4 Earth Models

This chapter presents two important modeling elements of an integrated navigation system: ellipsoid geometry and ellipsoid gravity. Like the different navigation frames presented in Chapter 3, there are many models for the Earth’s geometry and gravity. These models are based on parameters that have been assigned different values for different uses. For integrated navigation systems, the Earth’s shape is modeled by simple oblate spheroid, and the gravity models reflect a similar simplification. The local-level frames discussed in Chapter 3 are to be maintained as locally level as a vehicle moves over the Earth’s surface. This movement results in an angular rotation defined by the Earth’s geometrical shape, that is, its radius of curvature. The numerical integration of this motion yields the vehicle position. An accurate model of the Earth’s shape is necessary so that an accurate position results from that integration. The radius of curvature expressions developed in this chapter are used in Chapter 5’s developments of navigation system dynamic equations. The Earth’s gravity influences accelerations sensed by the inertial system’s instruments. Models for gravity are maintained within the navigation system to determine what part of the sensed acceleration is caused by vehicle dynamics and what is caused by the Earth’s gravitational attraction. The most widely used current Earth model is the World Geodetic System (WGS-84) [4]. Approximations to the shape and gravity models that are later used in the linearization of navigation state equations presented in Chapter 5 are developed. 4.1

Ellipsoid Geometry

Earth-shape model parameters are illustrated in Fig. 4.1, which shows the elliptical cross section for the ellipsoid. The z axis is along the rotation axis, and b is a radial distance in the x –y plane of the equator. Several geometrical parameters used in definitions for Earth-shape models are illustrated in Fig. 4.1. The following relationships for eccentricity 1, ellipticity e, and flattening f define the ellipsoid’s shape and other variables: !1=2 rp2 1¼ 1 2 (4:1) re e¼1

rp re

71

(4:2)

72

INTEGRATED NAVIGATION SYSTEMS z

h

r

rp

2

+φ

φ

φc

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

π

β

re

Fig. 4.1

Earth-shape model geometry.

f ¼

1 e

(4:3)

4.1.1 Earth-Centered X – Y –Z Coordinates from Latitude and Longitude Variables of interest include X – Y– Z ECEF position coordinates based on surface-referenced latitude, longitude and altitude coordinates. These coordinates as affected by the Earth-shape model parameters, will be developed in the following. The Earth’s surface, as shown in Fig. 4.1, is described by the following equation for an ellipse:

b2 z2 þ ¼1 re2 rp2

(4:4)

To determine b and z in terms of the preceding shape parameters and the geodetic latitude f, first the geocentric latitude fc is determined. The local slope, as shown in Fig. 4.1, is found from the differential of Eq. (4.4). 2b db 2z dz þ 2 ¼0 re2 rp

(4:5)

b rp2 dz ¼ 2 z re db

(4:6)

or

EARTH MODELS

73

From inspection of Fig. 4.1, p  dz 1 ¼ tan þ f ¼  2 db tan f

(4:7)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

or, combining the results from the last two equations,

b rp2 1 ¼ 2 z re tan f

(4:8)

rp2 ¼ (12  1) re2

(4:9)

z ¼ (1  12 ) tan f b

(4:10)



From the definition in Eq. (4.1),

Then,

Again, by inspection of Fig. 4.1, z ¼ tan fc b

(4:11)

Equating Eqs. (4.10) and (4.11), the following equation for the geocentric latitude is obtained in terms of the eccentricity and geodetic latitude: tan fc ¼ (1  12 ) tan f

(4:12)

From Eq. (4.4),

2

b ¼

re2

z2 1 2 rp

! ¼ re2 

re2 2 1 z2 z ¼ re2  rp2 (1  12 )

(4:13)

Combining this equation with Eq. (4.10) z ¼ (1  12 ) tan w b

)

z ¼ b(1  12 ) tan w

)

z2 ¼ b2 (1  12 )2 tan2 w

74

INTEGRATED NAVIGATION SYSTEMS

yields for b2

b2 ¼ re2 

1 b2 (1  12 )2 tan2 w (1  12 )

¼ re2  b2 (1  12 ) tan2 w

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¼

re2 ½1 þ (1  12 ) tan2 w

and solving for beta re ½1 þ (1  12 ) tan2 w1=2 re ¼ 2 ½1 þ tan w  12 tan2 w1=2 re ¼ 2 2 ½1 þ sin w=cos w  12 (sin2 w=cos2 w)1=2 re ¼ 2 2 2 ½( sin w þ cos w)=cos w  12 (sin2 w=cos2 w)1=2

b¼

Or

b¼

re cos f (1  12 sin2 f)1=2

(4:14)

And z can be obtained by substituting Eq. (4.14) into Eq. (4.10): z ¼ b(1  12 ) tan w ¼

re cos w sin w (1  12 ) 1=2 2 2 cos w ½1  1 sin w

Or z¼

re (1  12 ) sin f (1  12 sin2 f)1=2

(4:15)

From these equations, the X –Y – Z position coordinates in the ECEF frame of a point of the surface can be determined. From Fig. 4.2, the position in the x –y plane is given in terms of the radial position b and longitude l as x ¼ b cos l

(4:16)

y ¼ b sin l

(4:17)

EARTH MODELS

75

y

λ

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

x

Fig. 4.2

Earth-shape model X– Y (equatorial) plane.

From these equations and the preceding results, the following are obtained: x¼

re cos f cos l (1  12 sin2 f)1=2

(4:18)

y¼

re cos f sin l (1  12 sin2 f)1=2

(4:19)

Approximating the altitude above the surface following Britting [1], equations for the X – Y– Z coordinates become ECEF

x

yECEF zECEF

4.1.2



 re ¼ þ h cos f cos l (1  12 sin2 f)1=2   re ¼ þ h cos f sin l (1  12 sin2 f)1=2   re (1  12 ) ¼ þ h sin f (1  12 sin2 f)1=2

(4:20) (4:21) (4:22)

Radii of Curvature

The radii of curvature along lines of constant longitude and latitude are defined as Rmeridian and Rnormal, respectively. Terrestrial navigation systems are mechanized or implemented such that the local-level frame is maintained while the vehicle is changing position. These radii of curvature will be used later to determine the change in the navigation frame’s orientation with change in position.

76

INTEGRATED NAVIGATION SYSTEMS

From calculus, the radius of curvature of an arc, in this case of constant longitude, is given by "  2 #3=2 , dz d2 z + 2 (4:23) Rmeridian ¼ 1 þ db db

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Evaluating the second derivative from Eq. (4.6), rp4 d2 z ¼ 2 3 2 re z db

(4:24)

Substituting this result with Eqs. (4.6) and (4.9) into Eq. (4.23) yields Rmeridian ¼

½z2 þ (1  12 )2 b2 1=2 rp2 (1  12 )

(4:25)

After substituting the expressions for b in Eq. (4.14) and z in Eq. (4.15), this radius of curvature becomes re (1  12 ) (4:26) Rmeridian ¼ (1  12 sin2 f)3=2 The radius at the ellipsoid surface along constant latitudes is b. The radius of curvature normal to the ellipsoid surface at the point of tangency, at a given latitude, Table 4.1 Reference ellipsoid Clarke 1866 Clarke 1880 International Bessel Everest Modified Everest Australian National South American 1969 Airy Modified Airy Hough Fischer 1960 (South Asia) Fischer 1960 (Mercury) Fischer 1968 WGS-60 WGS-66 WGS-72 WGS-84

Reference ellipsoid constants [4] re, m

f

6,378,206.4 6,378,249.145 6,378,388 6,377,397.155 6,377,276.345 6,377,304.063 6,378,160 6,378,160 6,377,564.396 6,377,340.189 6,378,270 6,378,155 6,378,166 6,378,150 6,378,165 6,378,145 6,378,135 6,378,137

294.9786982 294.465 297 299.1528128 300.8017 300.8017 298.25 298.25 299.3249646 299.3249646 297 298.3 298.3 298.3 298.3 298.25 298.26 298.257223563

EARTH MODELS

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Table 4.2

77

WGS-84 ellipsoid constants [4]

Defining parameters Equatorial radius re, m Angular velocity vi/e, rad/s Earth’s gravitational constant m, m3/s2 Second gravitational constant J2

Value 6,378,137 7.292115  1025 3.986005  10þ14 1.08263  1023

Derived constants Flattening f Polar radius rp, m First eccentricity 1 Gravity at equator gWGS0 , m/s2 Gravity formula constant gWGS1 Mean value (normal) gravity g, m/s2

Value 298.257223563 6,356,752.3142 0.0818191908426 9.7803267714 0.00193185138639 9.7976446561

is given as Rnormal ¼

b cos f

From Eq. (4.14), this radius of curvature becomes re Rnormal ¼ 2 (1  1 sin2 f)1=2

(4:27)

(4:28)

Constants for several reference ellipsoid models are presented in Table 4.1. The number of different models reflects the changing knowledge and different applications. Table 4.2 lists several defining constants and resulting derived constants, specifically for the WGS-84 model. Example 4.1 Latitude, Longitude, and Geographic Frame Inertial Rates Latitude and longitude rates. Motion over the surface of the ellipsoid is along the arc defining the surface. The rate of change of latitude is along the meridian and is governed by the curvature along this arc as vnorth f˙ ¼ Rmeridian þ h The rate of change of longitude is found from veast l˙ ¼ b or, from Eq. (4.27),

l˙ ¼

veast ðRnormal þ hÞ cos f

Note that this equation for longitude rate contains a mathematical singularity at the Earth’s poles. If polar operations are to be considered in the navigation system’s design, then other methods to evolve position must be chosen, for example, direction cosine matrices or quaternions. This form is used only in this example to illustrate the formation of rotation rate vectors.

78

INTEGRATED NAVIGATION SYSTEMS

Local frame rate. The longitude rate is an angular rate relative to the ECEF frame about the z axis shown in Fig. 3.1. A local-level g frame is formed with a nonzero latitude. The longitude rate is transformed into an intermediate frame g0 transformed through the latitude f. This places the longitude rate rotation vector in the same coordinate frame as the latitude rate. These two rate vectors are added using the angular velocity addition theorem to obtain the angular rate vector in the intermediate g0 frame as 2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

0

vge=g0

cf ¼4 0 sf

0 1 0

3 32 3 2 sf 0 0 0 54 0 5 þ 4 f˙ 5 cf l˙ 0

The intermediate g0 frame is parallel to the geographic g frame; however, the orientation of the g0 frame is not the same as that of the g frame. The rotation vector in the g0 frame can be transformed into the g frame as follows: 2 3 0 0 1 0 vge=g ¼ 4 0 1 0 5vge=g0 1 0 0 0

Substituting the preceding equations for f˙ and l˙ into the equation for vge=g0 and using this change in orientation transformation, the Earth geographic frame angular rotation vector becomes 2 3 veast 6 (Rn þ h) 7 6 7 vnorth 6 7 g ve=g ¼ 6 7 6 (Rm þ h) 7 4 veast 5 tan f (Rn þ h) Geographic frame inertial rate. frame is

The Earth’s rotation expressed in ECEF 2

vei=e

3 0 ¼4 0 5 vi=e

Transforming into the geographic frame as before, this Earth rate angular rotation vector becomes 2 3 vi=e cos f 5 0 vgi=e ¼ 4 vi=e sin f Applying the angular velocity addition theorem, the total angular velocity of the local-level geographic frame with respect to the inertial frame can be expressed as vgi=g ¼ vge=g þ vgi=e

EARTH MODELS or

2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

vgi=g

6 6 6 ¼6 6 4

vi=e cos f þ

veast (Rn þ h)

79 3

7 7 vnorth 7 7 (Rm þ h) 7 5 veast tan f vi=e sin f  (Rn þ h)

4.2 Ellipsoid Gravity 4.2.1 Gravitation Potential The gravitation potential for the Earth is formulated in the following from [1]. Consider the geometry defined in Fig. 4.3. The differential potential is defined as being proportional to the elemental mass dm(r, b, l) divided by the magnitude of the distance between this mass element and another point as dU ¼ G

dm jr  rj

(4:29)

In this figure, the mass element dm(r, b, l), located at r, and the other point p(r, f, u), located at r, and their position dependencies are defined in spherical coordinates. z

φ

r

β

γ

dm ρ,β,λ . (ρ,β,λ)

ρ y

λ θ

θ −λ

x

Fig. 4.3 Gravitation elements spherical geometry.

80

INTEGRATED NAVIGATION SYSTEMS

An expression for the magnitude of the distance in Eq. (4.29) is formed next. First, from the definition of vector dot products, (4:30) r  r ¼ jrjjrj cos g then, the cosine of the angle g becomes cos g ¼

rr jrjjrj

(4:31)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

The two positions indicated in the figure are expressed as 2

3 sin f cos u r ¼ r4 sin f sin u 5 cos f

(4:32)

and 2

3 sin b cos l r ¼ r4 sin b sin l 5 cos b

(4:33)

Using these forms, the vector dot product in Eq. (4.30) becomes r  r ¼ r r(sin f cos u sin b cos l þ sin f sin u sin b sin l þ cos f cos b) ¼ rr½sin f sin b(cos u cos l þ sin u sin l) þ cos f cos b ¼ r r½sin f sin b cos (u  l) þ cos f cos b

(4:34)

And, Eq. (4.31) can be written as cos g ¼ cos f cos b þ sin f sin b cos (u  l)

(4:35)

Returning to the magnitude of the relative positions jr  rj ¼ (r 2 þ r2  2r r cos g)1=2

(4:36)

the inverse of the value becomes jr  rj1 ¼

 1=2 1 r2 r 1 þ 2  2 cos g r r r

(4:37)

The terms in the parentheses on the right-hand side of this equation can be considered small compared to one. Using the following series expansion for the nth power n(n þ 1) 2 n(n þ 1)(n þ 2) 3 (1 + x)n ¼ 1 + nx þ x + x þ  2! 3! 8 x2 , 1

(4:38)

EARTH MODELS

81

and the small variable defined as x;

r2 r  2 cos g 2 r r

(4:39)

then, using Eqs. (4.38) and (4.39), Eq. (4.37) can be expanded as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2

jr  rj1

3  r r2 6 1  r 2  2 r cos g 7 6 7 6 7  2 2 7 16 3 r r 7 cos ¼ 6  2 g þ 6 7 2 r r6 8 r 7  2 3 6 7 4 5 r 5 r   2 cos g þ    r 16 r 2 2 3 r 1 r2 2 cos 1 þ g þ (3 cos g  1) 6 7 r 2 r2 6 7 3 6 7 1 1r 3 7 ¼ 6 þ 3 (5 cos g  3 cos g) 6 7 r6 2r 7 4 1 r4 5 4 2 þ 4 (35 cos g  30 cos g þ 3) þ    8r 2 3 r r2 P 1 þ (cos g ) þ P (cos g ) 1 2 6 7 r r2 6 7 3 7 16 r 6 7 ; 6 þ P3 (cos g) 7 3 r6 r 7 4 r4 5 þ 4 P4 (cos g) þ    r 1  1 X r k Pk (cos g) ¼ r k¼0 r 1 2



(4:40)

where the following definitions for Legendre polynomials have been used: P1 (cos g) ; cos g 1 P2 (cos g) ; (3 cos2 g  1) 2 1 P3 (cos g) ; (5 cos3 g  3 cos g) 2 1 P4 (cos g) ; (35 cos4 g  30 cos2 g þ 3) 8 etc.

(4:41)

82

INTEGRATED NAVIGATION SYSTEMS

Integrating the differential potential function in Eq. (4.29) over the range of variables encompassing the mass, the gravitational potential becomes ðððX 1  k r Pk (cos g)dm r k¼0 ððð ððð G G r P1 (cos g)dm ¼ dm þ r r r ð ð ð  2 ð ð ð  3 G r G r þ P2 (cos g)dm þ P3 (cos g)dm þ    r r r r ððð ð ð ð  2 GM G G r þ 2 r cos g dm þ P2 (cos g)dm ¼ r r r r ð ð ð  3 G r þ P3 (cos g)dm þ    r r

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

U¼

G r

(4:42)

To exploit their properties, the following Legendre polynomial derivative relationship is used: Pjk (n) ¼ (1  n2 )1=2

dj Pk (n) dnj

(4:43)

Defining the variable n as n ; cos f

(4:44)

then, the derivative polynomials become d n ¼ sin f dn   d 1 P12 ¼ (1  cos2 f)1=2 (3n2  1) ¼ 3 sin f cos f dn 2  2  d 1 P22 ¼ (1  cos2 f) 2 (3n2  1) ¼ 3 sin2 f dn 2

P11 ¼ (1  cos2 f)1=2

(4:45)

etc. Using these expressions, the gravitational potential in Eq. (4.42) is written as ð ð ð  ð ð ð  2 GM G r G r P1 (cos g)dm þ þ P2 (cos g)dm þ    U¼ r r r r r ð ð ð  GM G r ¼ ½P1 (cos f)P1 (cos b) þ P11 (cos f)P11 (cos b) cos (u  l) dm þ r r r # ð ð ð  2 " P2 (cos f)P2 (cos b) þ 13 P12 (cos f)P12 (cos b) cos (u  l) G r dm þ    þ 1 2 r r P2 (cos f)P22 (cos b) cos 2(u  l) þ 12 (4:46)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

EARTH MODELS

83

Factoring terms with the p positions’ f dependency out of the mass integrals ððð GM G rP1 (cos b)dmP1 (cos f) þ 2 U¼ r r ððð G þ 2 rP11 (cos b) cos (u  l)dm P11 (cos f) r ððð G r2 P2 (cos b)dm P2 (cos f) þ 3 r ððð G 1 þ 3 r2 P12 (cos b) cos(u  l)dm P12 (cos f) r 3 ððð G 1 þ 3 r2 P22 (cos b) cos 2(u  l)dm P22 (cos f) þ    (4:47) r 12 Expanding the cosine of the difference of the two angles, this equation is rewritten as ððð GM G þ 2 rP1 (cos b)dm P1 (cos f) U¼ r r ððð G þ 2 rP11 (cos b) cos ldm P11 (cos f) cos u r ððð G þ 2 rP11 (cos b) sin ldm P11 (cos f) sin u r ððð G þ 3 r2 P2 (cos b)dm P2 (cos f)dm r ððð G 1 r2 P12 (cos b) cos ldm P12 (cos f) cos u þ 3 r 3 ððð G 1 þ 3 r2 P12 (cos b) sin ldm P12 (cos f) sin u r 3 ððð G 1 þ 3 r2 P22 (cos b) cos 2ldm P22 (cos f) cos 2u r 12 ððð G 1 þ 3 r2 P22 (cos b) sin 2ldm P22 (cos f) sin 2u þ    (4:48) r 12 Or, to summarize this equation in a compact form, the gravitational potential becomes U;

1 GM X 1 þ kþ1 r r k¼1 (

 Ak Pk (cos f) þ

k h X j¼1

Bjk Pjk (cos f) cos ju þ Ckj Pjk (cos f) sin ju

) i

(4:49)

84

INTEGRATED NAVIGATION SYSTEMS

where the elemental mass dm ; D(r, b, l)r2 sin bdrdbdl

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

and other coefficients in the summations are defined as ððð rkþ2 D(r, b, l)Pk (cos b) sin bdrdbdl Ak ; G Bjk ; 2G Ckj

(k  j) (k þ j)

(k  j) ; 2G (k þ j)

ððð ððð

(4:50)

(4:51)

rkþ2 D(r, b, l)Pjk (cos b) cos jl sin bdrdbdl

(4:52)

rkþ2 D(r, b, l)Pjk (cos b) sin jl sin bdrdbdl

(4:53)

If it is assumed that the Earth possesses axial symmetry about the vertical axis, then D(r, b, l) ; D(r, b)

(4:54)

and the integrals in Eqs (4.52) and (4.53) equate to zero as ð 2p

ð 2p sin jl dl ¼ 0

0

cos jl dl ¼ 0 ) Bjk ¼ Ckj ¼ 0 8 j

(4:55)

Also, if it is assumed that the axis of symmetry passes through the center of the Earth’s mass, then ððð G r cos gdm ¼ 0 (4:56) r2 With these assumptions, the gravitational potential simplifies to 1 m X Ak U¼ þ Pk (cos f) r k¼2 r kþ1

(4:57)

where, for notional simplification, GM ; m.

4.2.2

Gravitational Acceleration

The potential function in Eq. (4.57) is rewritten, including only the first nonzero coefficient (see Problem 4.9) and defining the coefficient A2 in terms of J2. UJ2 (r, wc ) ¼

  r 2 1 m e 1  J2 (3 sin2 wc  1) r r 2

(4:58)

EARTH MODELS

85

where, for a point on the surface, r 2 ¼ b2 þ z 2 and sin wc ¼

z r

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Then, the potential becomes U J2 ¼

 r 2 1  z2  m e 1  J2 3 1 r r 2 r

(4:59)

The gradient of this potential, with respect to b and z, yields the gravitational acceleration in those directions:  r 2 1  z2  @U b e ¼  3 m 1  J2 1 Gb ; 3 r r 2 @b r    r 2 1  b z2  m b re 2 1 z2 e þ þJ2 2 2 1 þ J2 3 2 3 r r r 2 r 2 r2 r r  r 2 1  z2  r 2 1  z2  m e e ¼  2 1  J2 1  2J2 1 3 3 r r 2 r 2 r r r 2  z2  b e J2 3 r r r     z2  3 r 2  z2  b m 3 re 2 e ¼  2 1 þ J2 13 2  J2 r r r r 2 r 2 r        m 3 re 2 z 2 b (4:60) ¼  2 1 þ J2 15 r r r 2 r  r 2 1  z2  @U z e ¼  3 m 1  J2 3 Gz ; 1 r 2 @z r r    r 2 1  2z 2z3  m z re 2 1 z2 e þ þJ2 2 2 3 3 2 4 1  J2 r r 2 r r r 2 r r             m re 2 1 z 2 re 2 1 z2 13 13 ¼  2 1 þ J2 þ 2J2 r r 2 r 2 r r r 2 3  z2  z e 22 þ J2 r 2 r r        z2  z 2 m 3 re z 2 3 re 2 J J 13 22 þ 2 ¼ 2 1þ 2 r r r 2 r 2 r r        m 3 re 2 z 2 z (4:61) 35 ¼  2 1 þ J2 r r 2 r r

86

INTEGRATED NAVIGATION SYSTEMS

The component in the b is resolved into x and y components to yield the gravitational accelerations in Earth-centered coordinates as    z 2  x m 3 re 2 J þ  1 þ 1  5 2 R2 R 2 R R    z 2  y m 3 re 2 Gy ¼  2 1 þ J2 þ  15 R R 2 R R    z 2  z m 3 re 2 Gx ¼  2 1 þ J2 þ  35 R R 2 R R

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Gx ¼ 

(4:62) (4:63) (4:64)

where the radial position to the location in Earth-centered coordinates is R¼

pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi x 2 þ y2 þ z2

(4:65)

These equations are approximate with only the first two terms, m and J2, included. The J2 term is the first nonspherical model term included in the gravitational model, and its contribution to the model is to include effects caused by the Earth’s oblateness.

4.2.3 Gravity Gravity is the acceleration on a rotating Earth. The preceding gravitation acceleration model is referenced to non-rotating frame. This acceleration is adjusted to account for the centripetal acceleration caused by the Earth’s rotation. Including this acceleration, the gravity vector is g ¼ G  Vi=e Vi=e r

(4:66)

WGS gravity model. The magnitude of gravity at the surface of the WGS-84 ellipsoid can be expressed in the following form [4]:

g ¼ gWGS0

(1 þ gWGS1 sin2 f) (1  12 sin2 f)1=2

(4:67)

This simple mathematical form to describe gravity motivates the use of locallevel referenced navigation state equations, which are much simpler than alternatives, that is, ECEF navigation state equations. Gravity is affected by altitude (the inverse square law of acceleration). The variation of gravity with altitude is included in the model form in the preceding equation by the additional term  2ðh=RÞ.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

EARTH MODELS

87

Gravity anomalies. If the Earth conformed to a homogenous ellipsoid, the local gravity vector direction would be normal to the ellipsoid surface (see Problem 4.10). Gravity anomalies are deviations from the gravity model as maintained in the navigation system. An analytical gravity model is limited in that it cannot include the many possible variations. Higher-order models are used in special applications, that is, precision navigation systems; however, most terrestrial navigation systems’ performance goals can be satisfied by models that include only the few terms already discussed. Gravity deviations are represented by the following gravity vector referenced in the local-level geographic frame [1]: 2

3 jg gg ¼ 4 hg 5 g

(4:68)

where j is the meridian deflection of the vertical (positive about east), h the normal deflection of the vertical (positive about north), and g the gravity magnitude. The gravity deflections just given can be several arc seconds in magnitude. These deflections result in tens of micro gs of unmodeled acceleration. Both j and h are relatively constant over short distances.

4.3

Chapter Summary

Models for the Earth’s geometrical shape were presented, and equations for converting latitude, longitude, and altitude position into ECEF positions and the reverse were developed. These conversions are used in the global-positioningsystem case study presented later in Chapter 12. Expressions for Earth radii of curvature were developed for use in the numerical integration of navigation state equations that describe vehicle motion (see Sec. 5.2). Simplified gravity models based on WGS-84 definitions were presented. These models describe gravity at the surface of an ellipsoid defined as nominally normal to that ellipsoid. This simple mathematical form to describe gravity shows that the use of local-level referenced navigation state equations is much simpler than alternatives, that is, ECEF navigation state equations. Approximations to shape and gravity models developed in this chapter are used later in the linearization of navigation state equations (see Sec. 5.3).

Problems 4.1

This exercise demonstrates the sensitivity of Rnormal to latitude. From Eq. (4.28), obtain the following: @Rnormal Rnormal sfcf12 ¼ @f (1  12 s2 f)

88

INTEGRATED NAVIGATION SYSTEMS then

dRnormal / 12 df Rnormal

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

to demonstrate that the sensitivity of Rnormal to latitude is of order 12 , a small quantity. Show the same sensitivity of Rmeridian. 4.2

This exercise demonstrates the iteration method to convert ECEF positions to latitude/longitude/altitude. The Newton iteration algorithm [5] for finding the root of the continuously differentiable nonlinear function f(x) can be stated as xiþ1 ¼ xi 

f (xi ) f 0 (xi )

for

i ¼ 0, . . . , n

until the change in the computed root xiþ1 is small compared to the previous value. Establish the iteration function to find the latitude f (f) ¼ zECEF  (x2ECEF þ y2ECEF )1=2 tf þ

r e 12 s f ¼0 (1  12 s2 f)1=2

whose derivative with respect to latitude f becomes f 0 (f) ¼ 

(x2ECEF þ y2ECEF )1=2 re 12 cf þ 2 c f (1  12 s2 f)3=2

and use the algorithm form

fiþ1 ¼ fi  4.3

f (fi ) f 0 (fi )

This exercise uses the substitution method to convert ECEF to latitude/ longitude/altitude. Using the results of Sec. 4.1, demonstrate the following relationships: h¼

qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi x2ECEF þ y2ECEF

1 re  cos f (1  12 sin2 f)1=2

From Eq. (4.11), tan fc ¼

(x2ECEF

zECEF þ y2ECEF )1=2

EARTH MODELS

89

Obtain the following equation for the geodetic latitude:  tan f ¼ tan fc

½re =(1  12 sin2 f)1=2 þ h ½re (1  12 )=(1  12 sin2 f)1=2 þ h



Or redefine terms to obtain the following: 

½R þ h ½R(1  12 ) þ h

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

tan f ¼ tan fc



where R;

(1 

12

re sin2 f)1=2

Assume a two-step procedure to solve for geodetic latitude. First, approximate the values for R  re and h¼0 Using these values in the second equation for tan f, compute a first approximation to the geodetic latitude, and, with that result in the equation for h, compute a first approximation to altitude. Using these results for geodetic latitude and altitude, recompute these values using 1) the same equation for tan f (but now using the equation for R to obtain a refined value for geodetic latitude) and 2) the same equation for h to also obtain a refined value for altitude. Finally, to complete the Earth surface positions, longitude is computed from Eqs. (4.20) and (4.21):  ECEF  y l ¼ tan xECEF 1

This value of longitude is used with the results of the iteration for latitude and altitude. 4.4

Analytical substitution is used in this exercise to obtain a first-order solution for geodetic latitude and altitude accounting for the Earth’s oblateness. Consider the following figure showing the geocentric latitude and its

90

INTEGRATED NAVIGATION SYSTEMS first-order correction indicated. From this figure, obtain the following equations: z (x2 þ y2 )1=2 (1  12 ) z sin f1 ¼ 2 ½(x þ y2 )(1  12 )2 þ z2 1=2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

tan f1 ¼

z φc

φ

(x

2

+ y2

) (1 − ε ) ½

2

(x2 + y2)½ and cos f1 ¼

(x2 þ y2 )1=2 (1  12 ) ½(x2 þ y2 )(1  12 )2 þ z2 1=2

Substitute these equations into the equation for altitude to obtain its firstorder solution:  h1 ¼ 1 þ

1=2 z2 (x2 þ y2 )(1  12 ) (  2 2 1=2  (x þ y )  re = 1 þ

4.5

z2 (x2 þ y2 )(1  12 )

1=2 )

This exercise forms vne=n for a wander-azimuth frame. Consider the wanderazimuth frame presented in Fig. 3.6. The horizontal components of the geographic frame referenced rates are vwest rnorth ¼  Rnormal þ h vnorth rwest ¼ Rmeridian þ h Use the transformation 

rx ry





ca ¼ sa

sa ca



rnorth rwest



from the geographic frame to the wander-azimuth frame to obtain the following rate components: 

   c2 a s2 a 1 1 þ  þ vx saca Rn þ h Rm þ h R m þ h Rn þ h  2    c a s2 a 1 1 þ  ry ¼ v x þ vy s ac a Rm þ h Rn þ h Rn þ h R m þ h

rx ¼ vy

EARTH MODELS

91

where 

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

4.6

vnorth vwest



 ¼

ca sa

sa ca



vx vy



This exercise demonstrates approximations to gravitational variation. Using the results of Sec. 4.2, make the assumption that the J2 term is negligible to obtain the following: @G ¼ v2s (3eeT  I) @r where

v2s ¼

m R3

and the position unit vector is 2 3 x 14 5 e¼ y R z Note that gravity can be approximated as g  m=R2 and the preceding frequency becomes v2s ¼ g=R2 — the Schuler frequency (see Example 5.1). 4.7

This exercise shows the variation of gravity with altitude. In Sec. 4.2, it was indicated that gravity varied with altitude. Assume a spherical Earth and the following simplified inverse square law gravity model:  2 R g ¼ g0 Rþh where g0 is the gravity magnitude at zero altitude h. Take the partial derivative of this equation with respect to altitude to obtain   @g g ¼ 2 @h Rþh

4.8

This exercise demonstrates navigation frame position error induced Earth rate error. For most terrestrial navigation applications, Earth rate is assumed constant and known. As will be shown later, Earth rate referenced in the local navigation frame is used in formulating navigation state equations. This transformed Earth rate can contain error if the local position is in error. Form the computed navigation frame referenced Earth rate by

92

INTEGRATED NAVIGATION SYSTEMS transforming the known Earth rate with a computed Earth-to-navigation frame direction cosine matrix as n  ni=e ¼ C e vei=e v

¼ ½I  (du)Cen vei=e

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Show that the resulting navigation frame Earth rate error becomes

dvni=e ; v¯ ni=e  vni=e ¼ (du)vni=e 4.9

Develop an expression for the gravitational potential assuming the Earth possesses a homogeneous density D(r, b, l) ; D Show for the ellipsoid shape x 2 z2 þ ¼1 a2 b2

)

b2 x 2 þ a2 z 2 ¼ a2 b2

x ¼ r sin b

)

x2 ¼ r 2 sin2 b

z ¼ r cos b

)

z2 ¼ r 2 cos2 b

r2 ¼

a 2 b2 b2 sin2 b þ a2 cos2 b

Then, using this expression, show that the mass integral becomes ððð M;

D(r, b, l)r2 sin bdrdbdl ðð

¼ 2p D

r2 sin bdrdb

ð

 1 3 r(b) ¼ 2p D r sin bdb 3 0 ð 2 3 3 p sin b ¼ pa b D db 2 3=2 2 2 2 3 0 (b sin b þ a cos b)   2 2 ¼ pa3 b3 D 3 ab2 4 ¼ pa2 bD 3

EARTH MODELS

93

and, the first coefficient in Eq. (4.57) equates to zero ððð A1 ; G

r3 DP1 (cos b) sin bdrdbdl

ðð ¼ 2pDG

r3 sin b cos bdrdb

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

ð

 1 4 r(b) ¼ 2pDG r sin b cos bdb 4 0 ðp 1 4 4 sin b cos b ¼ pa b DG db 2 sin2 b þ a2 cos2 b)2 2 (b 0   ðp 1 d 1 1 ¼ pa4 b4 DG db 2 2 2 2 2 2 2 0 db 2(a  b ) (b sin b þ a cos b)   1 1 1 1 ¼ pa4 b4 DG  2 2(a2  b2 ) a2 a2 ¼0

Show that the first non-zero coefficient in the gravitational potential equation becomes ððð A2 ; G

r4 DP2 (cos b) sin bdrdbdl

ðð

1 r4 (3 cos2 b  1) sin bdrdb 2 r(b) ð 1 5 ¼ pDG r (3 cos2 b  1) sin bdb 5 0 ðp 1 5 5 (3 cos2 b  1) sin b ¼ pa b DG db 2 5=2 2 2 2 5 0 (b sin b þ a cos b)   1 5 5 4 (a2  b2 ) ¼ pa b DG  5 3 a3 b 4 ¼ 2pDG

4 1 ¼  pa2 bDG (a2  b2 ) 3 5 1 2 ¼ MG (a  b2 ) 5

94

INTEGRATED NAVIGATION SYSTEMS Defining the following 12 ¼ 1 

b2 a2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

r 2 ¼ x2 þ y2 þ z2 z cos f ¼ r and showing the potential, including terms up to the second coefficient, become GM A2 þ 3 P2 (cos f) r r   GM 1 1 2 1 2 þ 3 GM (a  b ) (3 cos2 f  1) ¼ r r 5 2    2  2 GM GM b 3z  r 2 2 a 1  2 þ ¼ a r2 r 10r 3   GM a2 2 ¼ (x þ y2  2z2 )12 1þ 10r 4 r

Uk¼2 ¼

4.10

This exercise completes steps between the first and second lines in Eq. (4.46). Recall the expression from Eq. (4.35): cos g ¼ cos f cos b þ sin f sin b cos (u  l) Because P1 (cos n) ¼ cos n and, from Eqs (4.45) P11 (cos n) ¼ sin n then the first term in the second line in Eq. (4.46) becomes P1 (cos g) ¼ P1 (cos f)P1 (cos b) þ P11 (cos f)P11 (cos b) cos (u  l)

EARTH MODELS

95

Continue, and demonstrate that the second term in this equation can be written as 1 P2 (cos g) ¼ (3 cos2 g  1) 2 1 3 ¼ (3 cos2 f cos2 b þ sin2 f sin2 b  1) 2 2 Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

þ 3 sin f cos f sin b cos b cos (u  l) 3 þ sin2 f sin2 b cos 2(u  l) 4   1 3 ¼ 3 cos2 f cos2 b þ sin2 f sin2 b  1 2 2 1 þ (3 sin f cos f)(3 sin b cos b) cos (u  l) 3 1 þ (3 sin2 f)(3 sin2 b) cos 2(u  l) 12 ¼ P2 (cos f)P2 (cos b) 1 þ P12 (cos f)P12 (cos b) cos (u  l) 3 1 þ P22 (cos f)P22 (cos b) cos 2(u  l) 12 4.11.

This exercise examines the relationship between the surface normal and direction of gravity for a oblate spheroid. Consider the equation of an ellipsoid surface with axes defined in Fig. 4.1. s(b, z) ¼

b2 z 2 þ 1 re2 rp2

Components of the surface gradient vector are computed as 2 3 b 7 rs re (1  12 )1=2 6 re2 6 7 n; ¼ 1=2 4 5 2 z jrsj ½1  12 (b=re )  2 2 re (1  1 ) where 1 is defined in Eq. (4.1). An angle is defined as the arc tangent of the ratio of the b and z components of this vector.

g ; tan1



(1  12 )b z



96

INTEGRATED NAVIGATION SYSTEMS Now, consider the gravitation acceleration from Eqs. (4.60) and (4.61) and only including terms up to J2.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

  z2  b m 3 re 2 Gb ¼  2 1 þ J2 15 r r r 2 r    z2 z m 3 re 2 G z ¼  2 1 þ J2 35 r r 2 r r From these gravitational components, gravity components are written as gb ¼ Gb þ v2 b gz ¼ Gz Again define an angle from the b and z components of gravity as was just done with the surface normal

d ; tan1

  gb gz

Substituting from the preceding,   1 z2  b m 3 re 2 þ v2 b  2 1 þ J2 15 C B r r r 2 r C    d ¼ tan1 B     A @ 2 2 m 3 re z z  2 1 þ J2 35 r r 2 r r   1 0   z2 3 re 2 v2 r3 1 þ J2 15 b  C B r m 2 r C   ¼ tan1 B z2  A @ 3 re 2 1 þ J2 35 z r 2 r   (1 þ 11 )b ; tan1 (1 þ 12 )z   b  tan1 (1 þ 11 )(1  12 ) z  b  tan1 ½1 þ (11  12 ) z 0

Using WGS-84 Earth model parameters in Table 4.2 for shape and gravity, show that the difference between the angles 12 and (11  12 ) is less than 105 radians.

EARTH MODELS

97

Note: the 1’s in the fourth and last rows in the preceding development are assumed to be small and defined as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

 z2  v2 r 3 3 re 2 15 11 ¼ J 2  r m 2 r       3 re 2 z 2 12 ¼ J 2 35 r 2 r Then, assuming r  re , the following is obtained for the difference in 1’s. (11  12 ) ¼ 3J2 þ

v2 re3 m

Therefore, the surface normal and gravity directions, predicted from the preceding equations that include terms only up to J2 in the gravitational model, are in very close agreement.

4.12

An alternate form for gravity is developed in this exercise. Modify the potential in Eq. (4.58) to include Earth’s rotation as   r 2 1 m 1 v2 r 3 cos2 wc e 2 1  J2 (3 sin wc  1) þ VJ2 (r,wc ) ¼ r r 2 m 2 Establish the following gradients: gr ;

@V @r

  r 2 1 m 1 v2 r 3 cos2 wc e 2 ¼  2 1  J2 (3 sin wc  1) þ r r 2 m 2     2 2 m 2 re 2 1 3 v r cos2 wc 2 þ þ J2 (3 sin wc  1) þ r r 2 m r 2 2 3 r 2 1 v2 r 3 cos2 wc e 2 1 1  2 J2 (3 sin wc  1) þ 7 m6 r m 2 7 ¼  26 r 2 2 3 2 5 r 4 3 v r cos wc e 2 J2 (3 sin wc  1)  r m 2  # 2 2 3 v r 1  sin w m 3 re 2  c 3 sin2 wc  1  ¼  2 1  J2 r r m 2     m 3 re 2 v2 r 3 9 re 2 v2 r 3 J2    sin2 wc ¼  2 1  J2 r r m r m 2 2

98

INTEGRATED NAVIGATION SYSTEMS and 1 @V r @ wc   r 2 1 m 1 v2 r 3 2 cos wc sin wc e ¼ 2 J2 (3  2 sin wc cos wc )  r r 2 m 2     m re 2 v2 r 3 ¼  2 3J2 þ sin wc cos wc r r m

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

gw c ;

Defining the magnitude of gravity as

jgj ¼

qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi g2r þ g2wc

and using the intermediate variables to ease forming the square root of their squares, 3 re 2 a ; J2 r 2

and

b;

v2 r 3 m

Then, rewrite these expressions as

m ½1  a(3 sin2 wc  1)  b cos2 wc  r m ¼  ½2a þ b sin wc cos wc r

gr ¼  gw c

which results in the following for the gravity squared: g2 ¼

 m 2 2 ½1 þ (a  b)  (3a  b) sin2 wc 2 r

Returning to the original variables, show that the magnitude of gravity becomes

jgj ¼

m r2

 1þ

     3 re 2 v2 r 3 9 re 2 v2 r 3 J2    J2 sin2 wc r m r m 2 2

EARTH MODELS

99

Evaluated at the equator and poles, show that the gravity magnitude becomes

wc ¼ 0

)

p 2

)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

wc ¼

  m 3 v2 re2 ge ¼ 2 1 þ J2  re m 2 "  2 # m re gp ¼ 2 1  3J2 rp rp

at the equator

at the pole

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

5 Terrestrial Navigation

In this chapter, the following is presented for an integrated terrestrial navigation system: strap-down navigation system functional flow, local-level navigation frame mechanization equations, perturbation form of navigation system error equations, navigation system attitude error equations—psi formulation, navigation system error equations using alternative velocity error, and baro-inertial vertical channel mechanization and error equations. Inertial navigation systems for terrestrial navigation can be, and have been, mechanized in either a gimballed platform or as a strap-down unit. Most current navigation systems use strap-down sensors mechanized as a strap-down navigator. The mathematical forms for these two mechanizations differ only slightly; therefore, only equations describing a strap-down mechanization are presented in detail. Equations describing navigation states (position, velocity, and attitude) for strap-down navigation system implementation referenced to a local-level navigation frame are developed. The resulting differential equations are nonlinear. In developing these equations, the objective is to form them in terms of sensed accelerations and rates, rather than as applied and reactionary forces and moments, as in the case of forming dynamic equations of motion. These sensed accelerations and rates are provided by inertial sensors and are described in Chapter 6. An integrated navigation system combines navigation state data, generated by these dynamic equations, with independent redundant data in a Kalman filter algorithm. The form of the algorithm used requires a linearized error formulation of the navigation equations; with several error representations available. Many different approaches (design alternatives) are possible in forming these linearized equations, each with their advantages and disadvantages. Examples of different linearized forms are presented. Terrestrial navigation systems stabilize a normally unstable vertical axis (channel) by using outputs from a barometric altimeter. The source of this instability is gravity variation with altitude. A typical vertical channel stabilization implementation and its associated errors are presented. Appendix A contains additional developments for the historically significant Pinson error model.

101

102

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

5.1

INTEGRATED NAVIGATION SYSTEMS Strap-Down Navigation Systems

Illustrated in Fig. 5.1 is a functional block diagram for a strap-down navigation system mechanization. In this mechanization, three accelerometers and three gyros are mounted in orthogonal triads and rigidly attached to the vehicle body. Motions sensed by the gyros, that is, vbi=b , are in coordinates fixed to the body. The body-referenced accelerometer outputs f b are transformed from the body to the navigation frame in the navigation computer using the Cbk transformation matrix. This matrix is generated in the navigation computer by combining the rate outputs from the body-fixed gyros and the navigation frame rates created by the vehicle’s movement. The function performed by the navigation computer for a strap-down system is accomplished physically for a gimballed navigation system by torquing the stabilized platform. The gimballed navigation systems’ sensors are mounted on a mechanically stabilized platform whose alignment coincides with the navigation frame. Outputs from the accelerometers f k fixed in the navigation frame are integrated to establish velocity and position of the vehicle. Computations are performed in the navigation computer, and results from that computation are used to physically torque the stabilized platform to maintain its alignment with the navigation frame. Each of these mechanizations has advantages and disadvantages. These generally center on cost vs performance trades. However, outputs from the two are not necessarily interchangeable. For example, body rate information, available from the strap-down navigation system, is not available or is of poor quality from the gimballed system. Mathematical developments and equations presented in the following generally apply to both mechanizations. However, it is assumed that inertial navigation sensors are strap-down in describing the influence of these sensor errors to navigation errors.

gk f

Strap-down gyro & accelerometer triad

b

f

k

k Cb

gravity

+ +

Position, velocity, rates, etc. computation

Transformation Matrix computation b

ω b/k

ω bi/b

Fig. 5.1

+

+

b ω i/k

Strap-down local-level mechanization.

TERRESTRIAL NAVIGATION 5.2

103

Local-Level Navigation-Frame Mechanization Equations

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Differential equations describing navigation states are developed in the following sections. In developing these equations, the objective is to form expressions for navigation states in terms of sensed accelerations and rates available from accelerometers and gyros, respectively. The resulting differential equations, one vector and two matrix, can be numerically integrated using mathematical techniques developed in Chapter 2. 5.2.1

Velocity Equation

The Earth relative velocity vector vn in the rotating navigation frame n is defined in terms of the rotating Earth-centered Earth-fixed frame e position as (see Problem 5.5) vn ; Cen r˙e

(5:1)

n v˙ n ; Cen r¨ e þ C_ e r˙e

(5:2)

The time derivative of Eq. (5.1) is

Each of the terms in this equation will be developed in the following. The objective for the end result is an equation that relates the accelerometer’s specific force output to this rate of change of velocity in the navigation frame. The Earth-centered Earth-fixed position vector re is related to the nonrotating inertial frame i position as re ¼ Cie ri

(5:3)

The time rate of change of this vector is e

r˙e ¼ Cie r˙i þ C_ i ri

(5:4)

The time rate of change of the direction cosine matrix Cie is e C_ i ¼ Cie Vii=e

(5:5)

which, when substituted into Eq. (5.4), yields r˙e ¼ Cie (˙ri  Vii=e ri )

(5:6)

Taking the second time derivative of Eq. (5.6) results in i

e

_ ri ) þ C_ (˙ri  Vi ri ) r¨ e ¼ Cie (¨ri  Vii=e r˙i  V i=e i i=e ¼ Cie (¨ri  Vii=e r˙i )  Cie Vii=e (˙ri  Vii=e ri ) ¼ Cie (¨ri  2Vii=e r˙i þ Vii=e Vii=e ri )

(5:7)

104

INTEGRATED NAVIGATION SYSTEMS

where, in the second line, the Earth’s rotation rate is assumed to be constant _i ¼0 V i=e

(5:8)

The time rate of change of the direction cosine matrix Cen is

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

n C_ e ¼ Vne=n Cen

(5:9)

Substituting Eqs. (5.6), (5.7), and (5.9) into Eq. (5.2) and using Eq. (5.1) yield, after some manipulations, v˙ n ¼ Cen Cie (¨ri  2Vii=e r˙i þ Vii=e Vii=e ri )  Vne=n Cen Cne vn ¼ Cin (¨ri  2Vii=e Cni vn  2Vii=e Vii=e ri þ Vii=e Vii=e ri )  Vne=n vn ¼ Cin (¨ri  2Vii=e Cni vn  Vii=e Vii=e ri )  Cin Vie=n Cni vn h i ¼ Cin r¨i  (Vie=n þ 2Vii=e )Cni vn  Vii=e Vii=e ri

(5:10)

where, in the second step in this equation, the following relationship is used: r˙ i ¼ Vii=e ri þ Cni vn This equation can be obtained using Eqs. (5.1) and (5.4). The specific force f n is the sensed output of accelerometers transformed into the navigation frame. This specific force is a combination of inertial and gravitational acceleration: f n ¼ Cin r¨i  Gn

(5:11)

Gravitational acceleration can be expressed as the sum of gravity and centripetal acceleration (induced by the Earth’s rotation): Gn ¼ gn þ Vni=e Vni=e rn

(5:12)

Using the similarity transform on the product of Earth rates Vni=n Vni=n ¼ Cin Vii=n Cni Cin Vii=n Cni

(5:13)

and combining Eqs. (5.11) and (5.12) and rearranging terms yield r¨i ¼ Cni ( f n þ Gn ) ¼ Cni ( f n þ gn þ Cin Vii=e Vii=e Cni rn ) ¼ Cni ( f n þ gn ) þ Vii=e Vii=e ri

(5:14)

TERRESTRIAL NAVIGATION

105

Substituting Eq. (5.14) into Eq. (5.10) yields v˙ n ¼ Cin bCni ( f n þ gn )  (Vie=n þ 2Vii=e )Cni vn ¼ f n þ gn  Cin (Vie=n þ 2Vii=e )Cni vn which, after using the similarity transformation for the angular rates, becomes

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

v˙ n ¼ f n  (Vne=n þ 2Vni=e )vn þ gn

(5:15)

This equation provides the basic description of the Earth relative velocity evolution in a local level navigation frame, and is applicable to both local-level geodetic and to wander-azimuth frames. Specification of the frame is only necessary when writing out components and defining the V rotation matrix elements, thus, this equation is also applicable to either gimballed or strap-down navigation system mechanizations. 5.2.2

Attitude Equations

In this treatment of integrated navigation systems, the attitude dynamics equation is represented by the direction cosine matrix differential equation. The general form for the direction cosine matrix rate was described in Sec. 2.4. In this form, once initialized, it can be integrated without specific expressions for each of its elements. This attitude dynamics evolution is obtained from the following: C_ bn ¼ Vnb=n Cbn

(5:16)

The skew-symmetric matrix Vnb=n is formed from the rotation vector vnb=n . This vector is obtained using the angular velocity addition theorem

vnb=n ¼ vni=n  Cbn vbi=b

(5:17)

The rotation vector vbi=b represents the outputs of the strap-down gyros. Several methods can be used to evolve attitudes. These include integration of 1) six elements of Eq. (5.16) with the remaining three computed as in Problem 2.5; 2) integration of four quaternions or other equivalent representations for the direction cosine matrix transformation (see Sec. 2.5); or 3) integration of three Euler-angle equations (see Problem 5.6). Which method to select is a tradeoff involving considerations of vehicle operating environment, computer capability, and accuracy requirements. 5.2.3 Position Equations The position evolution is expressed as the following direction cosine matrix differential equation [Eq. (5.9)]: C_ n ¼ Vn Cn e

e=n

e

The elements of this matrix were given in Chapter 3 for different definitions of the Cen matrix. As with the Cbn matrix, several methods are available to evolve positions.

106

INTEGRATED NAVIGATION SYSTEMS

5.3

Perturbation Form of Navigation System Error Equations

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

In this section, the nonlinear navigation state equations for velocity, attitude, and position are linearized to obtain linear error model forms. The linearization approach used in this section is to develop linear equations that represent perturbations about a nominal solution as developed earlier in Chapter 2. The resulting equations are further specialized to wander-azimuth frames, with the vertical z axis directed up, as presented in Chapter 3.

5.3.1 Velocity Error Equations The navigation frame velocity equation (5.15) is rewritten using the vector cross-product form for the rotation rates as v˙ n ¼ f n  (vne=n þ 2vni=e )  vn þ gn The computed velocity is maintained within the navigation system and is implemented as the same equation. As a result of initialization, inertial sensor, and other errors, this computed velocity is imperfect—corrupted. This corruption is represented with overbars over the variables in this equation as v¯˙ n ¼ f¯ n  (v¯ ne=n þ 2v¯ ni=e )  v¯ n þ g¯ n

(5:18)

The computed velocity is defined for the perturbation approach as the true value plus a linear perturbation velocity error dvn in the navigation frame as v¯ n ; vn þ dvn

(5:19)

Likewise, for the other terms in Eq. (5.18), the following are expressions for their computed values: f¯ n ¼ C bn f¯ b ¼ ½I  (f)Cbn ( f b þ df b )  ½I  (f) f n þ d f n ¯ ne=n ¼ vne=n þ dvne=n v

(5:20) (5:21)

¯ ni=e ¼ vni=e þ dvni=e v

(5:22)

g¯ n ¼ gn þ dgn

(5:23)

Substituting these equations into the corrupted form in Eq. (5.18), subtracting the uncorrupted equation from this result, and eliminating products of error quantities yield

dv˙ n ¼ (dvne=n þ 2dvni=e )  vn  (vne=n þ 2vni=e )  dvn þ f n  f þ df n þ dgn (5:24) The error rotation vectors dvne=n and dvni=e are functions of velocity and position perturbations, respectively, and are required to complete the expression for the velocity error equation. The result in Eq. (5.24) is in general applicable

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

TERRESTRIAL NAVIGATION

107

to any three-axis navigation system; however, at this point it is specialized to a wander-azimuth referenced system with the z axis up. In formulating the rotation vector vne=n for the purposes of linearization, the Earth radii are approximated by a nominal radius R 2 3 vny 6 R 7 6 7 vne=n  6 vnx 7 (5:25) 4 5 R rz The variation of this equation with respect to velocity and Earth radius yields 2 3 dvny rx 6  R  R dh 7 6 7 n n 7 dve=n  6 (5:26) 6 dvx  ry dh 7 4 R 5 R drz where dR  dh and the notation vne=m ; r has been retained from Example 3.2. The last element in Eq. (5.25) represents the vehicle transport velocity about the vertical axis. It was shown in Chapter 3 that, for wander-azimuth mechanizations, this term is specified as zero; however, for geodetic mechanizations, this is not true. The error in the Earth rotation vector vni=e ; V is expressed in vector crossproduct form (see Problem 4.8).

dvni=e ¼ vni=e  du

(5:27)

Substituting Eqs. (5.25 – 5.27) into Eq. (5.24), the perturbation velocity error equation form for wander-azimuth mechanizations is obtained.

5.3.2

Attitude Error Equations

Attitude error equations are derived as the f or tilt error formulation. The computed, or corrupted, body-to-navigation frame transformation matrix is represented as (see Problem 3.4) C bn ; bI  (f )cCbn

(5:28)

dCbn ¼ C bn  Cbn ¼ (f )Cbn

(5:29)

The error in this matrix is

The dynamic equation for f is obtained by taking the derivative of both lines of this equation, then equating the result and solving for f_ from the second row

108

INTEGRATED NAVIGATION SYSTEMS

expression in Eq. (5.29): n

dC_ b ¼ (f˙ )Cbn  (f)C_ bn

(5:30)

The derivative of the body-to-navigation frame transformation matrix was given in Eq. (5.16) as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

C_ bn ¼ Vnb=n Cbn Substituting this equation into Eq. (5.30) results in

dC_ bn ¼ b(f˙ )  (f )Vnb=n cCbn

(5:31)

From the first row and using the corresponding matrix derivative equation, as in Eq. (5.16), yields n dC_ bn ¼ C_ b  C_ bn

 n C n þ Vn Cn ¼ V b=n b b=n b  n ½I  (f )C n þ Vn C n ¼ V b=n b b=n b h i  n  Vn  V  n (f ) C n ¼ V b=n b=n b=n b h i  n  Vn  Vn (f ) C n  V b=n b b=n b=n

(5:32)

Equating Eq. (5.31) and this equation yields  n  Vn  Vn (f) (f˙ )  (f )Vnb=n ¼ V b=n b=n b=n or  n  Vn ) (f˙ ) ¼ Vnb=n (f ) þ (f )Vnb=n þ (V b=n b=n

(5:33)

Equivalently, in vector form,

f˙ ¼ f  vnb=n þ (v¯ nb=n  vnb=n )

(5:34)

Expanding angular rates using the angular velocity addition theorem to account for gyro error, the first of the terms in parentheses in Eq. (5.34) is expressed as ¯ bi=b ¯ ni=n  C bn v v¯ nb=n ¼ v ¯ ni=n  C nb vbi=b þ 1n ;v

(5:35)

TERRESTRIAL NAVIGATION

109

The second term in Eq. (5.34) is the same rate without error or corruption:

vnb=n ¼ vni=n  Cbn vbi=b

(5:36)

Subtracting this equation from Eq. (5.35) yields ¯ ni=n  vni=n  ½I  (f )Cbn vbi=b þ 1n þ Cbn vbi=b ¯ nb=n  vnb=n ¼ v v

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¯ ni=n  vni=n þ (f )Cbn vbi=b þ 1n ;v

(5:37)

Define, temporarily, a rotation rate error vector as ¯ ni=n  vni=n dv ¼ v

(5:38)

Then, Eq. (5.37) can be rewritten as

v¯ nb=n  vnb=n ¼ dv þ (f )Cbn vbi=b þ 1n

(5:39)

v¯ nb=n  vnb=n ¼ dv þ f  Cbn vbi=b þ 1n

(5:40)

or, in vector form, Substituting this equation into Eq. (5.34) yields

f˙ ¼ f  vnb=n þ dv þ f  Cbn vbi=b þ 1n ¼ dv þ f  (vnb=n þ vnb=n ) þ 1n

(5:41)

Recognizing that, from the angular velocity addition theorem,

vnb=n þ Cbn vbi=b ¼ vnb=n þ vni=b ¼ vni=n

(5:42)

then, Eq. (5.41) becomes

f˙ ¼ dv þ f  vni=n þ 1n

(5:43)

It remains to determine the rotation error vector dv. From its definition in Eq. (5.38), ¯ ni=e )  (vne=n þ vni=e ) dv ¼ (v¯ ne=n þ v ¯ ne=n  vne=n )  (v ¯ ni=e  vni=e ) ¼ (v ¼ dr þ vni=e  du

(5:44)

Substituting this equation into Eq. (5.43) yields the following attitude error equation:

f˙ ¼ dr þ vni=e  du þ f  vni=n þ 1n

(5:45)

110

INTEGRATED NAVIGATION SYSTEMS

In obtaining this equation for the tilt error, assumptions used to obtain the velocity error equation were not required; thus, it is generally applicable to any three-axis navigation system error model. 5.3.3

Position Error Equations

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Position errors are obtained from the Earth-to-navigation frame direction cosine matrix. The computed or corrupted direction cosine matrix is represented as C en ; ½I  (du )Cen

(5:46)

dCen ¼ C en  Cen ¼ (du )Cen

(5:47)

or the error in this matrix is

The dynamic equation for du is obtained in a similar fashion to that used for the tilt equation above. Taking the time derivative of the second row of this equation yields

dC_ en ¼ (du˙ )Cen  (du )C_ en h i ¼  (du˙ )  (du )Vne=n Cen

(5:48)

Returning to Eq. (5.47) and taking the time derivative of the first row and using the corresponding definition for the derivative of a direction cosine matrix yields n dC_ en ¼ C_ e  C_ en

 n C n þ Vn Cn ¼ V e=n e e=n e  n ½I  (du )C n þ Vn Cn ¼ V e e=n e=n e h i n n n  V V  (du ) Cn ¼ V e=n e=n e=n e h i  n  Vn  Vn (du ) Cn  V e=n e=n e=n e

(5:49)

Equating Eq. (5.48) and this equation, n

  Vn  Vn (du ) (du˙ )  (du )Vne=n ¼ V e=n e=n e=n or (du˙ ) ¼ (dr )  Vne=n (du ) þ (du )Vne=n

(5:50)

Equivalently, in vector form,

du˙ ¼ dr  vne=n  du

(5:51)

As with the tilt error equation, assumptions used to obtain the velocity error equation were not required; thus, this angular position error dynamic equation is generally applicable to any error model with three angular errors. However,

TERRESTRIAL NAVIGATION

111

this form is not generally applicable to angular position error with only two angular errors, that is, latitude and longitude errors (see Exercise 5.14). 5.3.4 Perturbation Error Summary In the following, it will be assumed that the variation of the transport rate is zero:

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

drz ; 0

(5:52)

thus specializing the following to the perturbation form for velocity error representation given in Eq. (5.19) and wander-azimuth mechanizations, where the vertical transport rate rz is defined as zero. This assumption is necessary to obtain a closed set of equations, that is, as many equations as unknown error variables. Figure 5.2 presents a summary of this perturbation form of navigation system error equations and is found in the literature [6]. The assumption drz ; 0 results in an error model with four attitude error states. This is an additional error state compared to a form based on an alternate velocity error representation presented later in this chapter. This additional state requires more processing time and storage for its implementation in a Kalman filter algorithm (see Chapter 8 problems). Whether or not to implement a model of this form requires consideration of the impact of its implementation on computer capability. Example 5.1

Approximations, Navigation Error, and Schuler Period

Consider a nonaccelerating case such that the following approximations hold:

dv_ y ¼ gfx þ dfy 1 f˙ x ¼  dvy þ 1x R These equations are decoupled from x velocity component equations and can be examined separately. Reducing these equations to a single secondorder differential equation, assuming that accelerometer and gyro errors are constant, yields

dv€ y ¼ gf˙ x g ¼  dvy þ g1x R or g dv€ y þ dvy ¼ g1x R Assuming a solution of the form

dvy ¼ A sin v t þ B cos v t þ C

Fig. 5.2

⎡ 0 ⎤ ⎡ 0 ⎤ ⎡0⎤ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢0⎥ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢0⎥ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎢δf x ⎥ ⎢δg x ⎥ ⎢ 0 ⎥ ⎢δf y ⎥ ⎢δg g ⎥ ⎢ 0 ⎥ ⎥+⎢ ⎥ ⎢ ⎥+⎢ ⎢δf z ⎥ ⎢δg z ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ε ⎥ ⎥ ⎢ x⎥ ⎢ ⎥ ⎢ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ε y ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ε ⎥ ⎥ ⎢ z⎥ ⎢ ⎥ ⎢ ⎣⎢ 0 ⎦⎥ ⎣⎢ 0 ⎦⎥ ⎣⎢ 0 ⎦⎥

1 R 0 0

0

2(ρ + Ω )y

− 2Ω z

1 R 0 v − z R

0

0 0 0

0 0

0

1 − R 0

0

− 2(ρ + Ω )x

ωx 0 0

− ωx 0

ωy 0

− ωy

0

− fx

0

Ωz

fx

0

fy

0

0

0

− Ωz

0

− fy

fz

(ρ + 2Ω )x

vz R

−

− fz

0

0 0

1

0

0

0

0

− (ρ + 2Ω )y

0

0

0

0

1 R

2Ω z

−

Perturbation form of navigation system error equations (dr2 5 0).

vy ⎡ 0 0 ⎢ 2 R ⎢ v 0 0 − x2 ⎡δθ x ⎤ ⎢ R ⎢δθ ⎥ ⎢ 0 0 0 ⎢ y⎥ ⎢ ⎢ v v ⎢ δh ⎥ 2v y Ω x − 2(v y Ω y + v x Ω x ) − z 2x ⎢ n⎥ ⎢ R ⎢δv x ⎥ ⎢ v y vz ⎢ n 2v x Ω y − 2(v x Ω x + v z Ω z ) − 2 d ⎢δv y ⎥ ⎢ R ⎢ n⎥ = dt ⎢δv x ⎥ ⎢ v x2 + v y2 2 g 2v x Ω z 2v y Ω z + ⎢φ ⎥ ⎢ R2 R ⎢ x ⎥ ⎢ v ⎢ y ⎢ φy ⎥ 0 − Ωz ⎢φ ⎥ ⎢ R2 ⎢ z ⎥ ⎢ v ⎢ 0 Ωz − x2 ⎣⎢δθ z ⎦⎥ ⎢ R 0 − Ωy Ωx ⎢ ⎢ 0 − ρx ρy ⎣

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

⎤ ⎥ ⎥ ⎥ ρx ⎥ ⎥ 0 ⎥ 2v z Ω x ⎥ ⎥ ⎥ 2v z Ωy ⎥ ⎥ − 2(v x Ω x + v y Ω y )⎥ ⎥ ⎥ Ωy ⎥ ⎥ ⎥ − Ωx ⎥ 0 ⎥ ⎥ 0 ⎦ − ρy ⎡δθ x ⎤ ⎢δθ ⎥ ⎢ y⎥ ⎢ δh ⎥ ⎢ n⎥ ⎢δv x ⎥ ⎢δv yn ⎥ ⎢ n⎥ + ⎢δv x ⎥ ⎢φ ⎥ ⎢ x ⎥ ⎢ φy ⎥ ⎢φ ⎥ ⎢ z ⎥ ⎣⎢δθ z ⎦⎥

112 INTEGRATED NAVIGATION SYSTEMS

TERRESTRIAL NAVIGATION

113

yields sin v t (1  cos v t) þ g1x v v2 From this equation, it is seen that the initial tilt error fx (0) and the y component of accelerometer bias dfy contribute as a sum to the velocity error. Returning to the dv€ y equation, velocity errors are governed by the frequency v. Using nominal values for gravity and Earth radius, this frequency is approximated by rffiffiffi rffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi g 9:798 2 p rad vs ¼   R 6,378,000 84 (60) s

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

dvy ¼ dvy (0) cos v t þ ½gfx (0) þ dfx 

This 84-min characteristic frequency is termed the Schuler period. 5.4

Navigation System Attitude Error Equations: c Formulation

In Sec. 5.3, mechanization equations were linearized using the perturbation approach. This approach produced error dynamic equations in the f formulation. Another useful form is the c formulation, which yields simpler attitude error dynamic equations and offers the potential for reduced computational demands of the onboard computer. The c formulation defines attitude error as

c ; f  du

(5:53)

This error angle vector represents the combination or local-level tilt angular error and an angular position error relative to Earth center. The error dynamic equation can be obtained by taking the derivative of the preceding equation, which yields

c˙ ¼ f˙  du˙

(5:54)

Substituting results from Eqs. (5.46) and (5.52) into this equation yields

c˙ ¼ dr þ vni=e  du þ f  vni=n þ 1n  dr þ vne=n  du ¼ (vni=e þ vne=n )  du þ f  vni=n þ 1n ¼ du  vni=n þ f  vni=n þ 1n ¼ (f  du)  vni=n þ 1n or

c˙ ¼ c  vni=n þ 1n

(5:55)

This c formulation requires fewer terms than the tilt error equation in Eq. (5.45) to form attitude error dynamics. This equation is generally applicable for any three-axis navigation frame mechanization.

114

INTEGRATED NAVIGATION SYSTEMS

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

5.5 Navigation System Error Equations Using Alternative Velocity Error In this section, navigation system equations developed in Sec. 5.1 are linearized using an alternative form for velocity error. The velocity error model that results from using this alternate velocity error representation, combined with the results of the preceding section, produces an error model that contains fewer elements and thus is a candidate for easier implementation. A summary of the resulting navigation system error equations is presented.

5.5.1

Alternative Velocity Error

The velocity given in Eq. (5.15) is again assumed for true and computed velocities. The computed velocity in this case is represented as v¯ n ; ½I  (du)vn þ dv1

(5:56)

Note, the superscript 1 in this equation is used to distinguish this velocity error from that used in Eq. (5.19). The computed values for specific force, rates, and gravity, expressed in terms of true values and perturbation errors, remain unchanged from those given in Eqs. (5.20 – 5.23). Substituting this equation and Eqs. (5.20 – 5.23) into Eq. (5.18) and then subtracting the uncorrupted form of this equation from the result yield the following equation for the velocity error:

dv˙ 1  du˙  vn  du  v˙ n ¼ f  f n þ df n  (vne=n þ vni=e )  (dv1  du  vn ) (dvne=n þ 2dvni=e )  vn þ dgn

(5:57)

This equation is simplified by using Eq. (5.53) and the velocity equation given in Eq. (5.15). Substituting these equations into this equation yields

dv˙ 1  (dvne=n  vne=n  du)  vn  du  b f n  (vne=n þ 2vni=e )  vn þ gn c ¼ f  f n þ df n  (vne=n þ vni=e )  (dv1  du  vn )  (dvne=n þ 2dvni=e )  vn þ dgn

(5:58)

Cancelling terms and using Eq. (5.27) with the following vector identities (vne=n  du)  vn þ du  (vne=n  vn ) ¼ vne=n  (du  vn )

du 

(vni=e

n

v )¼

vni=e

n

 (du  v ) 

(vni=e

 du)  v

n

(5:59) (5:60)

TERRESTRIAL NAVIGATION

115

gives the following form of the velocity error equation:

dv˙ 1 ¼  (du  f)  f n þ df n  (vne=n þ 2vni=e )  dv1 þ dgn þ du  gn

(5:61)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Using the definition of the c attitude error in Eq. (5.53) and regrouping terms, this equation becomes

dv˙ 1 ¼ gn  du  (vne=n þ 2vni=e )  dv1 þ f n  c þ df n þ dgn

(5:62)

By momentarily redefining the gravity error as

dg1 ; dgn  g  du

(5:63)

dv˙ 1 ¼ (vne=n þ 2vni=e )  dv1 þ f n  c þ df n þ dg1

(5:64)

Eq. (5.62) can be written as

Comparing this equation to Eq. (5.24) suggests the rotation rates vne=n and vni=e are known and without error (dvne=n and dvni=e are zero), and the tilt error vector f is replaced by the c form for attitude error. 5.5.2

Attitude Error for Alternate Velocity Error

The attitude error is given in Eq. (5.55) for the c form. 5.5.3 Position Error for Alternate Velocity Error The angular position error is given in Eq. (5.51). Using the velocity error representation given in Eq. (5.56), position error dynamics become 2

(dv1y þ vz dux  vx duz )=R þ (vy =R2 )dh

3

du˙ ¼ 4 (dv1x  vz duy þ vy duz )=R  (vx =R2 )dh 5 drz 3 2 32 rz ry 0 dux 0 rx 54 duy 5 þ 4  rz ry  rx 0 duz

(5:65)

Assuming a wander-azimuth frame implementation with the vertical transport rate rz being zero and using Eq. (5.25) for transport rates as functions of velocity

116

INTEGRATED NAVIGATION SYSTEMS

components, the horizontal components of the position error equations become vz vy 1 dux þ 2 dh  dv1y R R R vz vx 1 du˙ y ¼  duy  2 dh  dv1x R R R

du˙ x ¼ 

(5:66) (5:67)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

The altitude rate error is same as the vertical velocity error

dh_ ¼ dvnz

(5:68)

which, from Eq. (5.56), allows the vertical component of position error to be expressed as dh_ ¼ vy dux þ vx duy þ dv1z (5:69) 5.5.4

Error Equations for Alternate Velocity Error Summary

Figure 5.3 presents a summary of navigation system error equations using the alternate velocity error representation given in Eq. (5.56). The vertical transport rate rz is assumed to be zero. At this point in the development of these equations, no assumptions concerning duz , drz , etc. have been made. This error model, using the alternate velocity error representation, appears in the literature [7]. A similar formulation, using the f equivalent attitude error representation instead of the c form presented here, is used for a case study on transfer alignment presented in Chapter 13. The model summarized in Fig. 5.3 requires one less error state than that presented in Fig. 5.2 which required four attitude error states. Therefore, if computational constraints require an implementation with fewer states, this model might warrant further consideration. 5.6 Vertical Channel Current inertial navigation systems utilize barometric pressure to stabilize and control the vertical channel—altitude, but inaccurate vertical velocity, etc. Barometric pressure is considered to be relatively stable, but inaccurate measure of absolute altitude above sea level. A feedback control system is implemented that uses proportional/derivative and integral feedback of the difference between system and barometric altitudes. The integral feedback eliminates steady-state error between system and barometric altitudes. A representative vertical channel mechanization is illustrated in Fig. 5.4. This mechanization is simplified to include those elements common to the various mechanizations available. Differences between this and other mechanizations that exist address different approaches to dynamically adjusting the feedback coefficients Ci . During rapid altitude changes, the barometric altitude might be in error, and the coefficients are adjusted to rely more on the pure inertial solution for altitude. The delta values in the figure, that is, DhB , indicate corrections that are added to adjust the loop’s values for the vertical channel variables. These corrections are provided from another source, that is, a Kalman filter.

0 0 0

0

−g 0

v − z R vx

0 R v − x R 0 0 0 2g R 0 0 0

2

vy

Fig. 5.3

0 0 0

0 0 0

0

− ωx

ωy

0 − Ωz

fx

− fz 0

0

0

0

Ωz 0

− fy

fz

0 0

0

0

⎤ 0 ⎥ δθ ⎡ ⎤ ⎥⎢ x ⎥ 0 ⎥ ⎢δθ y ⎥ ⎥ ⎢ δh ⎥ 0 ⎥⎢ 1 ⎥ ⎥ δv f y ⎥ ⎢ 1x ⎥ ⎢δv ⎥ + − f x ⎥ ⎢ 1y ⎥ ⎥ ⎢ δv z ⎥ 0 ⎥⎢ ⎥ ⎥ ⎢ψ x ⎥ − ω y ⎥ ⎢ψ ⎥ y ω x ⎥⎢ψ ⎥ ⎥⎣ z ⎦ 0 ⎥⎦

Navigation system error equations using alternative form for velocity error.

0 0 0

1

0 − (ρ + 2Ω )y (ρ + 2Ω )x

0

0

0

− (ω + Ω )x

1 R

(ω + Ω )y

−

2Ω z 0

0 1 R 0 0 − 2Ω z

⎡ 0 ⎤ ⎡ 0 ⎤ ⎡0⎤ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢0⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢0⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢δf x ⎥ ⎢δg x ⎥ ⎢ 0 ⎥ ⎢δf y ⎥ + ⎢δg y ⎥ + ⎢ 0 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢δf z ⎥ ⎢δg z ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ε ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ x⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ε y ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ε ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ z⎦

⎡ vz − ⎡δθ x ⎤ ⎢ R ⎢δθ ⎥ ⎢ ⎢ y⎥ ⎢ 0 ⎢ δh ⎥ ⎢ ⎢ 1 ⎥ ⎢− vy δv x ⎢ d ⎢⎢ 1 ⎥⎥ ⎢ 0 δv y = dt ⎢ 1 ⎥ ⎢ g ⎢ δv z ⎥ ⎢ ⎢ψ ⎥ ⎢ 0 ⎢ x⎥ ⎢ 0 ⎢ψ y ⎥ ⎢ ⎢ψ ⎥ ⎢ 0 ⎣ z⎦ ⎢ ⎢⎣ 0

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

TERRESTRIAL NAVIGATION 117

118

INTEGRATED NAVIGATION SYSTEMS gz

fz

vz Δvz

−

( ρ +2Ω)yvx− ( ρ +2Ω)yvx

Δh

∫

−

∫

ΔhB −

C1 Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

h

∫

hB

C2

C3

Fig. 5.4

Representative vertical channel mechanization.

From this figure, the navigation frame baro-inertial vertical channel dynamic equations are written as h_ ¼ vnz  C1 (h  hB ) v_ nz

¼ fz þ gz þ (r þ

(5:70)

2V)y vnx

 (r þ

ð

2V)x vny

 C2 (h  hB )  C3 (h  hB ) dt

(5:71)

5.6.1 Feedback Coefficients The feedback coefficients, Ci in Eqs. (5.70) and (5.71), are determined from a specification associated with the altitude dynamics characteristic equation. This equation is a linearized form of these equations and is developed in the following. In the following, the barometric altitude error dhB is assumed to be a constant with zero time derivative. A perturbation form of the altitude equation becomes

dh_ ¼ dvnz  C1 (dh  dhB )

(5:72)

Differentiating this perturbation form with respect to time yields

dh€ ¼ dv_ nz  C1 dh_

(5:73)

TERRESTRIAL NAVIGATION

119

Differentiating this equation again yields :::

d h ¼ dv€ nz  C1 dh€ or :::

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

d h þC1 dh€ ¼ dv€ nz

(5:74)

To complete this equation, an equation for the second derivative of velocity perturbation dvnz is required. Neglecting the Earth and transport rate terms in Eq. (5.71), the following linear form of the velocity perturbation is obtained: ð dv_ nz ¼ dfz þ dgz  C2 (dh  dhB )  C3 (dh  dhB ) dt (5:75) Gravity is approximated by the following inverse-square expression:  2 Rh¼0 gz ¼ gh¼0 Rh¼0 þ h The corresponding gravity perturbation is written as   gz 2g dgz ¼ 2 dh  dh Rh¼0 þ h R

(5:76)

(5:77)

Gravity in Eq. (5.76) is defined for the navigation frames shown in Figs. 3.2 and 3.3. In Problem 4.7, the direction of gravity is reversed from that defined in this equation—hence the different sign in that exercise. Differentiating the velocity perturbation equation with respect to time, assuming that the specific force error dfz is a constant, and substituting the equation for gravity perturbation yield the following for the second derivative of the velocity perturbation:

dv€ nz 

2g _ dh  C2 d˙ h  C3 (dh  dhB ) R

(5:78)

Substituting this equation into the altitude perturbation differential equation and collecting terms yields the following:   ::: 2g _ € d h þC1 dh þ C2  dh þ C 3 dh ¼ C 3 dhB (5:79) R This equation describes the altitude error (perturbation) dynamics. This error dynamic equation can be written as the altitude dynamic characteristic equation, expressed in Laplace transform variable form, as   2g (5:80) s3 þ C1 s2 þ C2  s þ C3 ¼ 0 R

120

INTEGRATED NAVIGATION SYSTEMS

The roots of this equation are usually specified to have one real root and a complex pair. The real part of the complex pair has the same magnitude as the pair’s imaginary parts, and the single real root also has this value. The complex pair is specified to have a damping ratio of 0.707. That specification is expressed as the following factor of the characteristic equation: (s þ l)½s þ (l þ jl)½s þ (l  jl) ¼ 0

(5:81)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

and, when multiplied out, results in the following polynomial equation: s3 þ 3ls2 þ 4l2 s þ 2l3 ¼ 0

(5:82)

Equating coefficients of powers of s in Eqs. (5.80) and (5.82) results in the following expressions for the coefficients, Ci in the preceding altitude perturbation equation, being obtained: C1 ¼ 3l,

C2 ¼ 4l2 þ

2g , R

and

C3 ¼ 2l3

(5:83)

The value of l is usually specified as 0.01.

5.6.2

Instability

The instability in the vertical channel can be seen from the altitude perturbation equation (5.79) with the Ci coefficients set to zero. In this case, pffiffiffiffiffiffiffiffiffiffiffi the roots of this equation are zero and a real pair with roots + 2 g=R. The positive root is a source of instability for the vertical channel.

5.7 Chapter Summary The development of equations for a strap-down navigation system, describing the evolution of the navigation states of position, velocity, and attitude, were presented. In developing these equations, the objective was to establish these equations in terms of sensed accelerations and rates, as provided by corresponding sets of next three orthogonal sensors. Examples of inertial rate sensors are described next in Chapter 6. Different forms of linearized error equations were developed as design alternatives, each with their advantages and disadvantages. These illustrations of different forms to describe navigation system errors should reinforce the notion that an assumed error model needs to be verified prior to attempting to integrate other elements within an existing navigation system. All of the error models presented in this chapter can be found in the literature. Examples of these different linearized forms are used in several integrated navigation system applications to be presented later in Part 2.

TERRESTRIAL NAVIGATION

121

Problems 5.1

This exercise concerns inertial navigation frame mechanization equations. Paralleling the developments in this chapter, obtain the following equations describing position and velocity in an inertial frame:

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Position: r¨ i ¼ Cbi f b þ Gi Velocity: vi ¼ r˙i and find that the attitude equations simplify to the following for a strapdown inertial sensor configuration: Attitude: i C_ b ¼ Cbi Vbi=b

where the skew-symmetric matrix Vbi=b is made up of the gyro-sensed angular rotation vector in the body frame. 5.2

This exercise concerns inertial navigation frame error equations. Assume that equations in the preceding problem hold for both true and computed position, velocity, and attitude. Use the error representations r¯ i ¼ ri þ dri i b i f¯ ¼ C b f¯ ¼ ½I  (f )Cbi ( f b þ d f b )  ½I  (f ) f i þ d f i

¯ i ¼ G i þ dG i G to obtain the following error equation for position/velocity:

d r¨ i ¼ f i  f þ df i þ dGi where the gravitation error is a function of position error and was shown earlier in Problem 4.6 to be

dGi ¼ v2 b3e eT  Icdri to yield the following for the position/velocity error equation:

d r¨i ¼ v2 b3e eT  Icdri þ f i  f þ df i

122

INTEGRATED NAVIGATION SYSTEMS Show that tilt error dynamics are given as

f˙ ¼ 1i and that position/velocity and attitude error equations can be expressed in the following summary form:

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2

3 2 i I33 0 d r d 4 i 5 4 2 33 d v ¼ v ½3e eT  I 033 dt 033 033 f 5.3

32 i 3 2 3 033 0 dr ( f i ) 54 d vi 5 þ 4 d f i 5 033 1i f

Use alternate velocity error representation and linear position errors to obtain navigation system error equations, summarized next, for a wanderazimuth terrestrial navigation system (assume duz ; 0). The linear position error is related to the angular position error by the following: 2 6 6 du ; 6 4

dr y 3 R 7 drx 7 7 5 R duz



Hint: From Eq. (5.66),

du˙ x ¼ 

vz vy 1 dux þ 2 dh  dv1y R R R

With the x component of angular position error defined as just shown and taking its derivative, the following also holds: 1 vz du˙ x ¼  dr˙ y  dux R R Equating these two equations for du˙ x yields the following equation for the y component of linear position error:

dr_ y ¼ 

vy dh þ dv1y R

Or, using the transport rate vector as defined in Eq. (5.25),

dr_ y ¼ rx dh þ dv1y The remaining terms can be obtained similarly and are summarized as follows:

0 0

0 0

 ry rx

1 0

2

3

6 6 6 dr x 6 ry  rx 0 0 6 dr 7 6 6 y 7 6 ( fz  g) 6 7 6 0 0 0 6 dh 7 6 R 6 7 6 6 dv n 7 6 ( fz  g) 6 x7 6 0 0 2Vz 7 6 d6 R 6 dvny 7 ¼6 7 6 fy 2g dt 6 6 n 7 6  fx  (v þ V)y 6 dv x 7 6 R R R 6 7 6 6 fx 7 6 vz r 6 7 6  Vz 0  x 6 7 6 2 R R R 4 fy 5 6 6 ry Vz 1 6  vz  fz  6 2 R R R R 4 v vy x 0 0 R R 2 3 2 3 2 3 0 0 0 6 0 7 6 0 7 607 6 7 6 7 6 7 6 7 6 7 6 7 6 0 7 6 0 7 607 6 7 6 7 6 7 6 7 6 7 6 7 6 df x 7 6 dgx 7 6 0 7 6 7 6 7 6 7 7 6 7 6 7 þ6 6 df y 7 þ 6 dgg 7 þ 6 0 7 6 7 6 7 6 7 6 df z 7 6 dg z 7 6 0 7 6 7 6 7 6 7 6 0 7 6 0 7 6 1x 7 6 7 6 7 6 7 6 7 6 7 6 7 4 0 5 4 0 5 4 1y 5 0 0 1z

2

(r þ 2V)y (r þ 2V)x 0 0 0 0

2Vz vz R

(v þ V)x 1 R

0 0

 0

vx

vy

Vz

fx

0

fz

0

0 0

Vz

0

fy

fz

0

1

0



0

0 0

0 1

0 0

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

3 0 0 7 7 2 3 7 dr x 0 7 7 6 dr 7 7 6 y7 6 7 fy 7 7 6 dh 7 7 6 7 7 6 dv n 7 7 6 7 x fx 7 6 7 7  6 dvny 7 7 6 7 6 7 0 7 7 6 dvnx 7 7 6 7 7 6 fx 7 7 6 7 vy 7 6 7 7 4 fy 5 7 vx 7 fz 7 5 0

TERRESTRIAL NAVIGATION 123

124 5.4

INTEGRATED NAVIGATION SYSTEMS This exercise concerns attitude error equations for ECEF frame. Parallel the developments in this section to obtain the following equations describing attitude error dynamics for the ECEF frame. Assume the computed body to ECEF frame transformation matrix is represented as C eb ; ½I  (C)Cbe

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Then the error in this matrix is C eb  Cbe ¼ dCbe ¼ (C)Cbe Using the following expression for the dynamics of this matrix: C_ eb ¼ Veb=e Cbe obtain the following differential equation for C attitude error dynamics: ˙ ¼ C  ve þ (v¯ e  ve ) C b=e b=e b=e Then, using the following expressions for the rotation vector veb=e :

v¯ eb=e ¼ v¯ ei=e  C eb vbi=b þ 1e veb=e ¼ vei=e  Cbe vbi=b obtain the following final form: ˙ ¼ C  ve þ 1e C i=e where Earth rate error is assumed zero

dvei=e ¼ 0 5.5

Obtain Eq. (5.1) by considering the following. First, transform the position vector from an inertial frame to an ECEF frame as re ¼ Cie ri Take the time derivative to obtain r˙e ¼ (vei=e )re þ Cie r˙ i or because r˙ i ; V i and V e ¼ Cie V i , r˙e ¼ (vei=e )re þ V e

TERRESTRIAL NAVIGATION

125

Define a velocity of the local position as a result of the Earth’s rotation V ep ¼ vei=e  re then the velocity relative to this position ve ¼ V e  V ep

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¼ V e  (vei=e )re The relative velocity is identical to the right-hand side of the preceding position derivative equation: r˙ e ¼ ve ¼ Cne vn or vn ¼ Cen r˙ e 5.6

Use the angular velocity addition theorem from Sec. 2.5 to obtain an expression for the body-referenced body-to-geographic frame rotation rate vector vbg=b in terms of Euler angle rates w˙ , u˙ , and c˙ for a geographic north –east –down (N – E – D) navigation frame and a body frame whose x– y–z axes are parallel to the N– E –D directions when Euler angles are all zero. Hint: The following transformation matrix transforms geographic frame vector components to the body frame following a yaw –pitch –roll sequence of rotations: 3 2 32 32 1 0 0 cu 0 su cc sc 0 7 6 76 76 Cgb ¼ 4 0 cw sw 54 0 1 0 54 sc cc 0 5 su 0 cu 0 0 1 0 sw cw 2 3 cu cc c u sc su 6 7 ¼ 4 sw su cc  cw sc sw su sc þ cw cc sw cu 5 cw su cc þ sw sc

c w su sc  sw c c

cw cu

Show the following: 32 3 2 32 3 2 3 2 3 2 w˙ 0 su P 0 0 cu 1 0 0 76 ˙ 7 6 76 7 6 7 6 7 6 b vg=b ¼ 4 Q 5 ¼ 4 0 5 þ 4 0 cw sw 54 u 5 þ 4 sw su cw sw cu 54 0 5 0 sw cw 0 cw su sw cw cu c˙ R 0 2 32 3 w˙ 1 0 su 6 76 ˙ 7 ¼ 4 0 cw sw cu 54 u 5 0 sw cw cu c˙

126

INTEGRATED NAVIGATION SYSTEMS Then, the equation for Euler angle rates is obtained as 2 3 su su 2 3 w c w 1 s w˙ 6 cu cu 7 7 4 u˙ 5 ¼ R1 vb ¼ 6 6 0 cw sw 7vbg=b g=b 4 1 15 c˙ 0 sw cw cu cu

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

5.7

Use the result in Problem 5.6 to obtain equations for Euler-angle rates in terms of navigation frame and gyro rates. Hint: The body-frame-referenced body-to-local-geographic frame rotation rate vector vbg=b can be expressed, using the angular velocity addition theorem, as

vbg=b ¼ vbg=i þ vbi=b ¼ vbi=b  vbi=g ¼ vbi=b  Cgb vgi=g In this equation, the preceding body-frame-referenced body-to-inertial frame rotation rate vector vbi=b (gyro output rates) is denoted as 2 3 p vbi=b ; 4 q 5 r and the local geographic-frame-referenced geographic-to-inertial frame rotation rate vector is composed of “Earth” and “transport” rates and rewritten in the following notation:

vgi=g ¼ vgi=e þ vge=g ;Vþr Euler-angle rates can be expressed as 2 3 2 3 w˙ p 4 u˙ 5 ¼ R1 4 q 5  R1 C b (V þ r) g c˙ r Express the matrix R 21 as the following product: 2 3 su 2 3 1 0 0 6 7 1 0 c u 6 7 R1 ¼ 6 0 1 0 74 0 cw sw 5 4 1 5 0 sw cw 0 0 cu

TERRESTRIAL NAVIGATION

127

demonstrate that the product R1 Cgb becomes 2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

cc 6 cu 6 R1 Cgb ¼ 6 sc 4 su cc cu

sc cu cc su sc cu

0

3

7 07 7 5 1

Returning to the last term in the preceding Euler-angle rates equation and using the result just obtained, obtain the following: 2

3 vw 6v 7 4 u 5 ; R1 Cgb (V þ r) vc 2 3 cc sc 2 3 0 V n þ rn 6 cu 7 cu 6 7 6 cc 07 ¼ 6 sc 7 4 re 5 4 su 5 su Vd þ rd cc sc 1 cu cu And, for each component,

vw ¼

cc sc (Vn þ rn ) þ re cu cu

vu ¼ sc(Vn þ rn ) þ ccre vw ¼ 5.8

su su cc(Vn þ rn ) þ scre þ (Vd þ rd ) cu cu

This exercise concerns position difference observation and linearization. In the following, an observation is formed from vector dot products of columns of the wander-azimuth frame direction cosine matrix Cne (x), containing error, and another column vector formed from the geodetic referenced position, that is, latitude and longitude, without error. The results of the vector dot products are the wander-azimuth angular position difference that can be used as position observations in a navigation Kalman filter. Consider the following form for the computed Cne (x) direction cosine matrix: 2

ca¯ cf¯ e C n (x) ¼ 4 ca¯ sf¯ sl¯ þ sa¯ cl¯ ca¯ sf¯ cl¯ þ sa¯ sl¯

sa¯ cf¯ sa¯ sf¯ sl¯ þ ca¯ cl¯ sa¯ sf¯ cl¯ þ ca¯ sl¯

3 sf¯ cf¯ sl¯ 5 cf¯ cl¯

128

INTEGRATED NAVIGATION SYSTEMS where each of the elements are assumed to be composed of the true value plus an error as

f¯ ¼ f þ df l¯ ¼ l þ dl a¯ ¼ a þ da This matrix can be rewritten as Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

e C n ; ½¯s1 j¯s2 j¯s3 

where the unit vectors si represent the columns indicated. Because the direction cosine matrix is orthonormal, these unit vectors are normal to each other. That is, the vector dot products formed from these column vectors are zero. The last column of Cen (x), expressed in terms of the true values (without error), is 2

3 sf t3 ¼ 4 cf sl 5 cf cl Taking the dot product of this vector with the first two column vectors of C en yields the following results:

 

s¯1 t3 ¼ cadf þ sa cfdl s¯2 t3 ¼ sadf þ ca cfdl It was shown in Example 3.3 that the right-hand sides of these equations are equivalent to the y and x angular position errors, respectively. Therefore,



dux ¼ s¯2 t3 duy ¼ ¯s1 t3



The first few elements corresponding to the system errors of the linearized measurement matrix become H¼ 5.9

 @h(x, t) 1 ¼ 0 @du, . . .

0 1

 



In the following, express the difference between the navigation solution’s velocity (in the wander-azimuth frame), as provided by an inertial navigation unit (INU), and the geodetic-frame-referenced GPS velocity, and show that this difference depends on position errors.

TERRESTRIAL NAVIGATION

129

Consider the difference n

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Dvn ¼ v¯ nINU  C g vgGPS 2

ca¯

6 ¼ vn þ dvn  4 sa¯ 0 2 n 3 vy da 6 n 7 n ¼ dv þ 4 vx da 5 0

3

sa¯

0

ca¯ 0

7 0 5 vg 1

The wander-azimuth error da can be related to the angular position error vector components as

da ¼ duz  Dlsf   ¼ duz  tf caDux  saDuy Substituting this expression into the preceding equation yields the following: 2 3 ½duz  tf(cadux  saduy )vny 6 7 Dvn ¼ dvn þ 4 ½duz  tf(cadux  saduy )vnx 5 0 2

tf ca vny 6 n ¼ dv þ 4 tf ca vnx

tf sa vny tf sa vnx

0 5.10

0

3 32 vny dux 7 76 vnx 54 duy 5 0

duz

A comparison of velocities from two noncollocated sources fixed to a rigid body must consider their relative displacements and the body angular motion that induces additional velocity. In this exercise, GPS velocities referenced at the antenna and INU velocities will be used to establish their navigation-frame velocity difference. The INU’s position vector rINU is related to the GPS antenna position vector ramt , and lever arm d position vector by rINU ¼ rant þ d Representing this vector in an Earth-fixed reference frame, reINU ¼ reant þ de ¼ reant þ Cne Cbn db Use the prior relationships between the time derivative of these ECEF position vectors and velocity to form navigation-frame-referenced velocity differences. Take the time derivative of this equation,

130

INTEGRATED NAVIGATION SYSTEMS recognizing that the lever arm is fixed (a constant) in the body frame, to show e n r˙ eINU ¼ r˙eant þ C_ n Cbn db þ Cne C_ b db

¼ r˙eant þ Cne Vne=n Cbn db þ Cne Vnn=b Cbn db

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Recall that the velocity in the local-level navigation frame is defined as vn ; Cen r˙e and, after premultiplying the r˙e -equation by Cne , show that vnINU ¼ vnant þ (Vne=n þ Vbn=b ) Cbn db h i ¼ vnant þ (vne=n þ vbn=b )  Cbn db This equation shows the additional velocity at the antenna location. 5.11

This exercise establishes the state vector form of the vertical channel error dynamics suitable for a Kalman filter implementation. Rewrite Eq. (5.75) as

dv_ nz  dfz þ dgz  C2 (dh  dhB ) þ da where the equation for da is given by

da_ ¼ C3 (dh  dhB ) Express these error equations in the following state vector form: 2

3

2

 C1  dh 6 6 dvnz 7 6  C2  2g 7 6 d6 R 6 da 7 ¼ 6 6 7 6 dt 4 C3 6 5 dhB 4 0 dfz 0

1

0 C1

0

1 C2

0 0 0

0 C3 0 0 0 0

3 3 3 2 0 2 0 7 dhn 6 7 7 6 17 76 dvz 7 6 wvz 7 7 6 da 7 þ 6 0 7 7 6 7 6 07 7 4 dhB 5 4 0 5 5 0 dfz 0 0

In Sec. 5.6, there are modeling idealizations in the vertical velocity error dynamics, for example, transport and Earth rate terms and the attitude error terms are neglected. To recognize that these contributors are not included in the system error dynamics, process noise wvz is included in the velocity error state to account for these missing errors. 5.12

Obtaining the vector form in Eq. (5.34) from Eq. (5.33) [and Eq. (5.51) from Eq. (5.50)] is the subject of this exercise. Consider the expanded

TERRESTRIAL NAVIGATION

131

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

skew-symmetric differential equation from Eq. (5.33): 2 2 32 3 3 0 fz fy 0 fz fy 0 vz vy d6 6 6 0 fx 7 0 f x 7 0 vx 7 4 fz 54 fz 5 ¼ 4 vz 5 dt fy fx 0 fy fx 0 vy vx 0 3 2 32 0 fz fy 0 vz vy 6 6 0 fx 7 0 vx 7 þ 4 fz 5 54 vz fy fx 0 vy vx 0 2 3 0 dvz dvy 6 dv 0 dvx 7 þ4 5 z dvy dvx 0 2 3 2 3    vy fx vz fx    fy vx fz vx 6 7 6 7 ¼ 4       vz fy 5 þ 4       fz vy 5       2 3 0 dvz dvy 6 0 dvx 7 þ 4 dvz 5 dvy

dvx

0

Collect terms by matrix element: 1:2

 f˙ z ¼ vy fx þ vx fy  dvz

1:3

f˙ y ¼ vz fx þ vx fz þ dvy

2:3

 f˙ x ¼ vz fy þ vy fz  dvx

And rearrange to form the vector equation 2 32 3 2 2 3 3 fx fx 0 v z v y dvx d4 5 0 vx 54 fy 5 þ 4 dvy 5 fy ¼ 4 vz dt vy vx 0 fz fz dvz Rewrite this as a vector differential equation

f˙ ¼ vxf þ dv ¼ fxv þ dv This is Eq. (5.34). 5.13

Next is an exception to the statement regarding the general application of the position error dynamic equation. The N – E – D navigation (geographic) frame requires only two error angles to express error in the

132

INTEGRATED NAVIGATION SYSTEMS DCM as shown next. The DCM from Chapter 3 is 2 3 swcl swsl cw cl 0 5 Ceg (z) ¼ 4 sl cwcl cwsl sw Defining errors in latitude and longitude by

w¯ ¼ w þ dw

and

l¯ ¼ l þ dl

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

and using small-angle approximations sw¯  sw þ cwdw,

cw¯  cw  swdw

etc:

then, the corrupted form of the DCM can be related to the uncorrupted form as follows: 2 3 cw¯ sw¯ cl¯ sw¯ sl¯ g 6 7 C e ¼ 4 sl¯ cl¯ 0 5 cw¯ cl¯ cw¯ sl¯ sw¯ 2 3  (cw  swdw) (sw þ cwdw)(cl  sldl) 6 7 (sl þ cldl) (cl  sldl) 0 ¼4 5  (sw þ cwdw) (cw  swdw)(cl  sldl) 2 3  cw  swdw swcl  cwcldw þ swsldl 6 7 sl  cldl cl  sldl 0 4 5 cwcl þ swcldw þ cwsldl 3 swcl swsl cw 6 7 cl 0 5 ¼ 4 sl



sw  cwdw

2

cwcl cwsl sw 2 32 0 swdl dw swcl 6 76 0 cwdl 54 sl þ 4 swdl dw

cwdl

swsl cl

cwcl cwsl

0

3 cw 7 0 5 sw

; Ceg  (du g x) Ceg ¼ ½I  (du g x) Ceg where the angular position error vector from this approach would be defined as 2

3 cwdl du g ¼ 4 dw 5 swdl Demonstrate that the problem in using this form in the position dynamics equations developed in the text arises from the fact that the elements in

0 6 6 6 ve t w 3 6 2 6 Rcw dw 6 0 6 dl 7 6 7 6 6 v2e 7 6 6 6 6 dh 7 6  2vi=e cwve  7 6 Rc2 w 6 dv 7 6 6 6 n 7 6 vn ve 7 d6 þ 2vi=e (cwvn  swve ) 6 dve 7 ¼ 6 Rc2 w 7 6 6 dt 6 7 6 6 dvd 7 6 2vi=e swve 7 6 6 6 fn 7 6 7 6 6 6 7 6 vi=e sw 4 fe 5 6 6 6 6 fd 6 0 6 6 4 ve vi=e cw  Rc2 w 3 2 3 2 3 2 0 0 0 6 0 7 6 0 7 607 7 6 7 6 7 6 7 6 7 6 7 6 6 0 7 6 0 7 607 7 6 7 6 7 6 6 df 7 6 dg 7 6 0 7 6 n7 6 n7 6 7 7 6 7 6 7 6 7 6 7 6 7 þ6 6 dfe 7 þ 6 dge 7 þ 6 0 7 7 6 7 6 7 6 6 dfd 7 6 dgd 7 6 0 7 7 6 7 6 7 6 6 0 7 6 0 7 6 1n 7 7 6 7 6 7 6 7 6 7 6 7 6 4 0 5 4 0 5 4 1e 5 0 0 1d

2

0

0

0

0

0

0

0

0

0

vn 1 R R2 ve  0 2 R cw 0 0 vn vd v2e tw vd  þ R R2 R2 ve vd vn ve tw   (r þ 2v)d R2 R2 (v2n þ v2e ) 2re R2 ve  0 R2 vn 1  R R2 ve tw 0 R2  0 1 Rcw 0 2(r þ v)d vd vn tw þ R R 2(r þ v)n 1 R 0 tw  R 0 (r þ v)n

re

0

(r þ v)d

(r þ v)d

0

0

0 fn

0

fe fe

0

0 fd

0 0

1 re (r þ 2v)n

0

0

0

0

0

0

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

3 3 2 7 dw 0 76 76 dl 7 7 76 0 76 dh 7 7 76 7 76 fe 76 dvn 7 7 76 7 fn 7 7 d v 76 e7 6 76 76 dvd 7 0 7 76 7 76 7 f re 7 n 7 76 7 6 7 (r þ v)n 74 fe 5 5 fd 0 0

TERRESTRIAL NAVIGATION 133

134

INTEGRATED NAVIGATION SYSTEMS this vector are linearly related: swdung þ cwdudg ¼ 0 Therefore, only two error angles are required to describe error in the DCM above which are the two original angles for latitude and longitude error.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

5.14.

Establish the following “f” form error dynamic model based on latitude and longitude position errors starting with the position equations in Example 4.1 and using Eqs. (5.24) and (5.43).

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

6 Navigation Sensor Models

In integrated navigation systems analysis, inertial sensors are described by their performance characteristics and error models. In most integrated navigation system implementations the gyro’s characteristics are of primary importance. This presentation of navigation sensor models includes the following: gyro performance characterizations and sensor error models. Inertial sensors commonly used in navigation units are rate/rate integrating gyros, dynamically tuned rotor gyros, ring laser gyros (RLG), fiber-optic gyros (FOG), and various types of accelerometers. The most recent developments are in RLG and FOG gyro technologies. Both RLG and FOG sensors employ the Sagnac effect. This effect is described at a simplified level in this chapter. These sensors are presented to illustrate how the gyro’s design, that is, RLG path length, influences its performance. A vital part of the design and evaluation of integrated navigation systems is the ability to model and simulate errors associated with gyros and accelerometers. Models allow computer-based evaluations prior to fabricating hardware, thus saving time and money. General mathematical models representing gyro and accelerometer errors are presented. Terms included within these representations are long-term stable errors, which can be described as random constants, and shortterm random errors, represented as time-correlated and random walk errors. Dynamic models for time-correlated and random walk errors are developed, with examples from simulations illustrating the model’s time characteristics. Problems are included that expand upon the material presented.

6.1 Gyro Performance Characterizations 6.1.1 Ring Laser Gyro The RLG has recently seen increased usage in strap-down navigation system mechanizations. Most current RLG sensors are single degree-of-freedom sensors requiring three of these for an inertial navigation system implementation, as described in Chapter 5. A single degree-of-freedom RLG is shown schematically in Fig. 6.1. This figure illustrates a triangular version on the RLG. Other versions exist with four sides. The gyro includes a laser, a closed-path cavity, mirrors at each intermediate corner in the path, and an interferometer/photodetector. The operation of the gyro is based on optical and electronic phenomena, rather 135

136

INTEGRATED NAVIGATION SYSTEMS mirror

cw

ccw

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

input rate

ccw

cw

mirror

mirror

laser cw beam

ccw beam

Fig. 6.1 Ring laser gyro.

than the mechanical phenomena. As a result, these gyros exhibit little of the error associated with acceleration, as with mechanical gyros. The optical phenomenon employed by this gyro is the Sagnac effect. Figure 6.2 is an illustration of Sagnac’s experiment [8]. Two laser beams, from the same source, propagate around the closed path in opposite directions: clockwise cw and counterclockwise ccw. Three mirrors, indicated with an S, with a beam splitter form a square enclosed in a circle of radius r. S

r

S

r

S ω

ω

beam splitter emitter *

Fig. 6.2

detector

Sagnac experiment.

NAVIGATION SENSOR MODELS

137

Δs

Δl

ω

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

r

r

r

Fig. 6.3

Sagnac experiment: one leg.

This effect can be described based on geometrical optics. Shown in Fig. 6.3 is one leg of the experimental arrangement. As a result of the rotation v, the effective path length traveled in the direction shown is shortened. Assuming small angles, the effective path of the leg shown is shortened by pffiffiffi 2 Dl ¼ Ds 2 pffiffiffi 2 r vt ¼ 2

(6:1)

for a rotation duration t. The time to propagate around the closed path in one direction is

tþ ¼

(l þ Dl) c

(6:2)

(l  Dl) c

(6:3)

and, in the opposite direction, it is

t ¼

where c is the light propagation speed. The difference in time to propagate in the two directions is D t ¼ tþ  t Dl c pffiffiffi r vt ¼ 2 c

¼2

(6:4)

138

INTEGRATED NAVIGATION SYSTEMS

Define the preceding rotation period as the time to transient around the closed path without rotation: pffiffiffi r t¼4 2 c

(6:5)

Then, the time difference between the two propagation directions becomes

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Dt ¼ 8

r2 v c2

(6:6)

The time difference can be related to a change in wavelength l by Dl Dt ¼ l t

(6:7)

and the corresponding wavelength difference becomes Dl ¼

pffiffiffi r vl 2 c

(6:8)

For the geometry illustrated, the enclosed area A formed by the path and its perimeter P are A ¼ 2r 2 pffiffiffi P ¼ 2 2r such that the change in wavelength can be expressed as Dl ¼

pffiffiffi r vA 2 Pc=l

(6:9)

The frequency is related to the wavelength by c l

(6:10)

nl ¼ c

(6:11)

n¼ or

Because c is a constant, the variation of Eq. (6.11) yields Dl n l 4vA n ¼ Pc

Dn ¼ 

(6:12)

NAVIGATION SENSOR MODELS

139

or, from Eq. (6.10), jDnj ¼

4vA Pl

(6:13)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

This result shows that the frequency change as a result of the gyro’s rotation is more sensitive with the higher ratio of the area to the perimeter. This equation holds for other gyro configurations, triangular, and circular fiber-optic gyros.

6.1.2

Fiber-Optic Gyros

The FOG is a maturing technology. It is similar to the RLG in that it is a single-degree-of-freedom sensor and its principle of operation is based on optical and electronic phenomena. This device is shown schematically in Fig. 6.4. Optical fiber is wound around a spool that is the closed optical path for the gyro. The Sagnac effect is also exploited in this gyro’s mechanization. The interferometric FOG’s phase shift output can be determined from the frequency shift given in Eq. (6.13). The phase shift is related to frequency shift by Df ¼

2p DnNL c

(6:14)

where N is the number of windings around spool and L is the length of optical fiber. Substituting from Eq. (6.13), the phase shift becomes Df ¼

2p (pD2 =4) 4 NL c pDN l

Fiber-optic coil

laser

* beam splitter detector

Fig. 6.4

Fiber-optic gyro.

140

INTEGRATED NAVIGATION SYSTEMS

or Df ¼

2pLD v cl

(6:15)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

This result shows that the phase change as a result of the gyro’s rotation is more sensitive with the longer length fiber.

6.2 Sensor Error Models Inertial sensor component (instrument) errors create error in the navigation system’s computed position, velocity, and attitude, as indicated in the linearized equations developed in Chapter 5. These instrument errors can be represented in a general form, including some significant environment-dependent errors, as shown in Fig. 6.5 for accelerometers and in Fig. 6.6 for gyros. These errors are expressed in a “case” frame, which is usually the platform frame for gimballed inertial navigation units and the body frame for strap-down inertial navigation units. The errors presented in these figures do not include other environment-sensitive terms, for example, gyro g-sensitive drift, scale-factor asymmetry, etc. The specific form for these errors can depend on the design of the instrument, and therefore, they are specialized for a specific design. 6.2.1 Random Constant Errors Both the accelerometer and gyro models contain time-correlated random errors. The other terms, for example, biases and scale-factor/misalignments, ⎡δ fx ⎤ ⎡ aBx ⎤ ⎢δ f ⎥ = ⎢aB ⎥ ⎢ y⎥ ⎢ y⎥ ⎢⎣δ fz ⎥⎦ ⎢⎣ aBz ⎥⎦ ⎡ aSFx ⎢ + ⎢aMAyx ⎢ aMAzx ⎣ +[

Bias –long term stable error

aMAxy aSFy aMAzy

]

⎡ a 2 Bx ⎤ + ⎢⎢a2 B y ⎥⎥ ⎣⎢ a2 Bz ⎦⎥ ⎡ aWx ⎤ + ⎢⎢aWy ⎥⎥ ⎢⎣ aWz ⎥⎦

aMAxz ⎤ ⎡ f x ⎤ ⎥ aMAyz ⎥ ⎢⎢ f y ⎥⎥ aSFz ⎥⎦ ⎢⎣ f z ⎥⎦

Scale-Factor and MisAlignments

Other environment-sensitive errors:- scale factor asymmetry -g-squared sensitivity

Short-term error (time correlated)

White noise

Fig. 6.5

General accelerometer error model.

NAVIGATION SENSOR MODELS ⎡ε x ⎤ ⎡ gBx ⎤ ⎢ε ⎥ = ⎢ gB ⎥ ⎢ y⎥ ⎢ y⎥ ⎢⎣ε z ⎥⎦ ⎢⎣ gBz ⎥⎦ ⎡ gSFx ⎢ + ⎢ gMAyx ⎢ gMAzx ⎣

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

+[

141

Bias –long term stable error

gMAxy gSFy gMAzy

gMAxz ⎤ ⎡ωib/ b x ⎤ ⎥ ⎥⎢ gMAyz ⎥ ⎢ωib/ b y ⎥ gSFz ⎥⎦ ⎢⎣ωib/ b z ⎥⎦

Scale-Factor and MisAlignments

Other environment-sensitive errors:-scale factor asymmetry -g-sensitivity

]

⎡ g 2 Bx ⎤ + ⎢⎢ g 2 B y ⎥⎥ ⎣⎢ g 2 Bz ⎦⎥

Short-term error (time correlated)

⎡ gWx ⎤ + ⎢⎢ gWy ⎥⎥ ⎣⎢ gWz ⎦⎥

White noise

Fig. 6.6

General gyro model.

can be considered random constants whose dynamic model is simply c_ ¼ 0

(6:16)

Models for time-correlated error terms are presented next. 6.2.2

Random Process Dynamic Models

In the following, discrete model forms suitable for computer modeling and simulation of time-correlated and random walk models are developed. The statistical properties of random processes are also developed. These properties are useful in evaluating the computer model’s outputs to ensure that the model implements the process correctly. Continuous time: Continuous systems approach. In this section, dynamic models are assumed to be given in a continuous-time form. Consider the following scalar random variable, modeled as a continuous process, whose differential equation is x_ (t) ¼ bx(t) þ w(t)

(6:17)

The solution of this equation is ðt x(t) ¼ exp½b(t  t0 )x(t0 ) þ

exp½b(t  l)w(l) dl

(6:18)

t0

The initial condition and driving noise are assumed to be zero mean, that is, E½x(t0 ) ¼ E½w(l) ¼ 0

(6:19)

142

INTEGRATED NAVIGATION SYSTEMS

and the driving noise is assumed to be a white-noise process such that E½w(t)w(t) ¼ N d(t  t)

(6:20)

and the initial value and driving noise are assumed to be uncorrelated such that

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

E½x(t0 )w(t) ¼ 0

(6:21)

Statistical properties of this process are determined next. It can be readily shown that x(t), based on the preceding assumptions, is a zero mean process: ðt E½x(t) ¼ eb(tt0 ) E½x(t0 ) þ eb(tl) E½w(l) dl (6:22) t0

The variance of x(t) is determined from the square of x(t): ðt x2 (t) ¼ exp½2b(t  t0 )x2 (t0 ) þ 2 exp½b(t  t0 ) x(t0 ) exp½b(t  l)w(l) dl t0

 1  !   2   ! ðt ðt þ exp½b(t  l)w(l) dl exp½b(t  m)w(m) dm t0

t0

  3   !

(6:23)

Examining each of the indicated terms in Eq. (6.23), corresponding variances are rewritten as follows:   1Þ exp½2b(t  t0 )E x2 (t0 ) ðt 2Þ

2 exp½b(t  t0 )

exp½b(t  l)E½ x(t0 )w(l) dl ¼ 0 t0

ð t 3Þ

ðt exp½b(t  l)w(l) dl

E t0

ðt

ðt exp½b(t  l)

¼ t0 ðt

t0

exp½b(t  m)E½w(l)w(m) dm dl t0 ðt

exp½b(t  l)

¼ t0

ðt ðt

¼ N exp (2bt)

exp½b(t  m)N d(t  m) dm dl t0

exp½b(l þ m)d(t  m) dm dl t0

ðt ¼ N exp (2bt)

t0

exp½2bl dl t0

¼

 exp½b(t  m)w(m) dm

N ½1  exp½2b(t  t0 ) 2b

NAVIGATION SENSOR MODELS

143

Or, summarizing this result,     N E x2 (t) ¼ exp½2b(t  t0 )E x2 (t0 ) þ ½1  exp½2b(t  t0 ) 2b

(6:24)

The steady-state variance of this equation is evaluated in the following, redefining the times t and t0 as tþt

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

t t0

t

The expected value of x 2(t) at time t þ t becomes      N  E x2 (t þ t) ¼ e2bt E x2 (t) þ 1  e2bt 2b

(6:25)

Steady state is defined when     E x2 (t þ t) ¼ E x2 (t) ¼ s2 where

s2 ¼

N 2b

(6:26)

The specified steady-state value of E[x 2(t)] is determined by the value of N. A discrete-time form of the continuous process is developed next. Equation (6.18) is expressed as the following discrete process: xkþ1 ¼ ebDt xk þ wkþ1

(6:27)

where ð tkþ1 exp½b(t  l)w(l) dl

wkþ1 ¼

(6:28)

tk

and Dt ¼ tkþ1  tk

8

tk

The variance of xkþ1 is determined by first writing out several terms of Eq. (6.27): k ¼ 0: x1 ¼ ebDt x0 þ w1

144

INTEGRATED NAVIGATION SYSTEMS

k ¼ 1: x2 ¼ ebDt x1 þ w2 ¼ e2bDt x0 þ ebDt w1 þ w2 k ¼ 2:

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

x3 ¼ ebDt x2 þ w3 ¼ e3bDt x0 þ e2bDt w1 þ ebXDt w2 þ w3 The accumulated sum at time kþ1 becomes xkþ1 ¼ exp½b(k þ 1)Dtx0 þ exp½b(k)Dtw1 þ exp½b(k  1)Dtw2 þ    þ exp ( bDt)wk þ wkþ1

(6:29)

Assuming, as before, that the initial condition x0 and the random sequence wk are independent, and that variances E [w2i ] are equal and equal to v 2, the variance of Eq. (6.29) becomes   E ½x2kþ1  ¼ exp½2b(k þ 1)DtE x20 þ n2 ½1 þ exp (2bDt) þ    þ exp½2b(k  1)Dt þ exp½2b(k)Dt   ½1  exp½2b(k þ 1)Dt ¼ exp½2b(k þ 1)DtE x20 þ n2 ½1  exp (2bDt)

(6:30)

Equation (6.24) can be rewritten in discrete form as     N E x2kþ1 ¼ exp½2b(k þ 1)DtE x20 þ ½1  exp½2b(k þ 1)Dt 2b

(6:31)

Because Eqs. (6.30) and (6.31) are equivalent, the following relationship between continuous and discrete driving noise characteristics is obtained:  N  1  e2bDt 2b   ¼ s2 1  e2bDt

n2 ¼

(6:32)

The discrete form of this process, expressed in terms of the continuous process’s steady-state variance s2, becomes xkþ1 ¼ ebDt xk þ wkþ1 ¼ ebDt xk þ nukþ1   1=2 ¼ ebDt xk þ s2 1  e2bDt ukþ1

(6:33)

NAVIGATION SENSOR MODELS

145

where ui ¼ unit variance samples from a Gaussian random number generator. Assume that the following approximation is true: ebDt  1  bDt

(6:34)

Then, Eq. (6.33) can be rewritten as   1=2 xkþ1 ¼ (1  bDt)xk þ s2 1  e2bDt ukþ1 Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¼ xk  bxk Dt þ s(2bDt)1=2 ukþ1

(6:35)

Defining the differential Dxkþ1 ¼ xkþ1  xk

(6:36)

then the following stochastic differential equation form is obtained for the time-correlated process: pffiffiffiffiffi Dxkþ1 ¼ bxk Dt þ s(2b)1=2 ukþ1 Dt (6:37) As the preceding approximation suggests, ß being small, Eq. (6.37) describes a random walk process. Using Eq. (6.26) in Eq. (6.37) yields pffiffiffiffi pffiffiffiffiffi N ukþ1 Dt Dxkþ1 ¼ bxk Dt þ s s pffiffiffiffiffi pffiffiffiffi ¼ bxk Dt þ N ukþ1 Dt (6:38) which, after setting ß to zero, yields the following for the random walk process: pffiffiffiffiffi pffiffiffiffi Dxkþ1 ðb ¼ 0Þ ¼ N ukþ1 Dt (6:39) Next, these results will be shown to be the same as that obtained by using a stochastic differential equation approach.

Stochastic differential equation approach. Following Astrom [9], consider the following vector stochastic differential equation: dx(t) ¼ A(t) x(t) dt þ dm(t)

(6:40)

where dm(t) is a Brownian motion (independent increment) process. In a continuous form, Eq. (6.40) is often written as x_ (t) ¼ A(t) x(t) þ w(t)

(6:41)

which implies w(t) ¼

dm(t) dt

(6:42)

146

INTEGRATED NAVIGATION SYSTEMS

The derivative in Eq. (6.42) represents white noise; however, the continuous Gaussian process w(t) is not differentiable. The covariance of Eq. (6.40) is found from the following:

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

  dP(t) ¼ E ½x(t) þ dx(t)½x(t) þ dx(t)T x(t)xT (t)        ¼ E x(t)xT (t) AT (t) þ A(t)E x(t)xT (t) dt þ E dm d mT þ OðdtÞ   (6:43) ¼ P(t)AT (t) þ A(t)P(t) þ Q(t) dt This result is obtained assuming that the instantaneous state value x(t) and dm(t) are uncorrelated and Q(t) is defined by   E dm d mT ; Q(t) dt

(6:44)

pffiffiffiffi dm(t)  u(t) dt

(6:45)

  Q(t) ¼ E u(t)u(t)T

(6:46)

Equation (6.44) implies

where

Therefore, Eq. (6.40) can be expressed as pffiffiffiffi dx(t) ¼ A(t)x(t) dt þ u(t) dt

(6:47)

Equation (6.47) will be specialized for the two scalar processes, random walk and first-order correlated process, in the following. For a random walk process A(t) is zero. The random walk can be expressed by the following scalar stochastic differential equation: pffiffiffiffi dxRW (t) ¼ uRW (t) dt

(6:48)

Or, if implemented as a time derivative in a simulation, then uRW (t) x_ RW (t) ¼ pffiffiffiffi dt

(6:49)

The covariance of this process becomes dpRW (t) ¼ qRW (t) dt

(6:50)

p_ RW (t) ¼ qRW (t)

(6:51)

or

NAVIGATION SENSOR MODELS

147

0.02

Value

0.01 0.00 –0.01

–0.03

0.0

2.0

4.0

6.0

8.0

10.0

Time

Fig. 6.7

Monte Carlo simulation of random walk.

If, for example, this process is the model p for the RLG random drift, uRW is specified in angle error per root time (deg/ h), and then the differential covariance dpRW has units of angle squared. Monte Carlo simulation results implementing Eq. (6.48) are presented in Fig. 6.7. Shown are several sample paths (time histories) of the random walk process. Shown in Fig. 6.8 are the corresponding computed standard deviations (one-sigma values) and the results of integrating Eq. (6.51). Standard deviations and variances in Fig. 6.8 are approximately the same. As indicated by Fig. 6.8 and expressed in Eq. (6.51), the variance for the random walk continues to increase with time. A time-correlated process is specified by the process time constant t and the driving noise w(t) in Eq. (6.41). It is desirable to have the variance of stationary process reach a known steady-state value by specifying the magnitude of the process noise Q (Pss ¼ Q). This is achieved by the following time-invariant

0.02

mean std dev

-std dev

sqrt p_rw

sqrt p_rw

6.0

8.0

0.01 Value

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

–0.02

0.00 –0.01 –0.02 –0.03 0.0

Fig. 6.8

2.0

4.0

Time

10.0

Standard deviation and variance for random walk.

148

INTEGRATED NAVIGATION SYSTEMS

scalar stochastic differential equation: pffiffiffi 1 2 pffiffiffiffi dxc ¼  xc dt þ pffiffiffi wc dt t t

(6:52)

pffiffiffi pffiffiffiffi 2 1 x_ c ¼  xc þ pffiffiffi wc = dt t t

(6:53)

The corresponding variance differential equation is 2 2 p_ c ¼  pc þ qc t t

(6:54)

The steady-state condition, p˙c ¼ 0, for this process is pss ; qc

(6:55)

Monte Carlo simulation results implementing Eq. (6.52) are presented in Fig. 6.9. Shown are several sample paths (time histories) of the time-correlated process. Figure 6.10 shows the corresponding computed standard deviations (one-sigma values) and the results of integrating Eq. (6.54). Standard deviations and variance results in Fig. 6.10 agree. The steady-state condition described by Eq. (6.55) can be seen in Fig. 6.10. Simulation results presented here and those presented for random walk exhibit a small nonzero mean. This is an artifice of the simulation’s uniform random number generator. Caution should be exercised in using results such as these 3.00 2.00 1.00 Value

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

This equation is identical to Eq. (6.37) if wc is replaced with s and 1/t is replaced by ß. If Eq. (6.52) is implemented at the time-derivative level in a simulation, then

0.00 –1.00 –2.00 –3.00 0.0

2.0

4.0

6.0

8.0

10.0

Time

Fig. 6.9

Monte Carlo simulation of correlated error.

NAVIGATION SENSOR MODELS mean

std dev

-std dev

sqrt p_c

149

sqrt p_c

3.00 2.00

Value

1.00 0.00

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

–1.00 –2.00 –3.00

0.0

2.0

4.0

6.0

8.0

10.0

Time

Fig. 6.10

Standard deviation and variance for correlated error.

in evaluating a filter’s estimates of similarly modeled inertial instrument errors, that is, accelerometer and gyro random errors. This nonzero mean characteristic can be incorrectly estimated by a filter as one of these instrument’s bias values.

6.3

Chapter Summary

The Sagnac effect that governs the operation of both RLGs and FOGs was described at a simplified level. Simple performance characterizations of these sensors were presented to illustrate how the gyro’s design, that is, RLG path length, influences the gyro’s performance. General mathematical models representing gyro and accelerometer errors were presented, including long-term stable errors and short-term random errors. Dynamic models for time-correlated random processes and random walk errors were developed, and examples of these errors’ time-varying and statistical characteristics were illustrated using numerical simulations of these processes. Selected elements of these models are included within the error state structure describing the navigation system’s errors. The resulting system error model is implemented in the Kalman filter algorithm (see Sec. 11.2). The gyro and accelerometer error models are used in a case study examining the calibration of these sensors in a laboratory (see Chapter 9).

Problems 6.1

This exercise presents an illustration of including sensor errors as additional error states in a system error dynamics matrix.

150

INTEGRATED NAVIGATION SYSTEMS

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Consider the body-referenced instrument (accelerometer or gyro) errors 3b 2 3b 2 3 2 32 SFx MAxy MAxz d Bx x 4 d 5 ¼ 4 By 5 þ 4 MAyx SFy MAzx 54 y 5 þ    MAzx MAzy SFz d Bz z The preceding underline “_” can be replaced by accelerometer (specific force) or gyro symbols as in Figs. 6.5 and 6.6. The environmental stimulation   x b y for the scale-factor and misalignment errors are body-referenced z specific forces or angular rates. This body-referenced error is transformed to the navigation frame for the filter dynamics model as 2 3n 2 3b d d 4 d 5 ¼ Cbn 4 d 5 d d It is desirable to estimate the body-referenced errors by including these in the filter error state vector, because, for strap-down navigation systems, only the body-referenced sensor errors are relatively “constant” and lend themselves to a dynamic model, modeling these errors as random constants. The state vector partition, corresponding to these errors, is written as (transposed) Bx

½:

SFy

By

Bz

MAyz

SFx MAzx

MAxy MAzy

MAxz

S Fz

MAyx

:

The transformation of these errors from the body frame to the navigation frame is accomplished by the following expression. This form for the relevant errors is inserted into the velocity error states for accelerometer errors and into the attitude error states for gyro errors. The following result is obtained from a sequence of steps. 2 3n 2 3 Cxx x Cxx y Cxx z Cxy x Cxy y Cxy z Cxz x Cxz y Cxz z d 6 7 6 nC 7 4 d 5 ¼ 4Cb j yx x Cyx x Cyx x Cyy x Cyy y Cyy z Cyz x Cyz y Cyz z 5

d 2 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 4

Czx x Czx 3 B SFx 7 7 7 MAxy 7 7 MAxz 7 7 7 MAyx 7 7 SFy 7 7 7 MAyz 7 7 MAzx 7 7 7 MAzy 5 SFz

x

Czx

x

Czy

x

Czy

x

Czy

x

Czz

x

Czz

y

Czz

z

NAVIGATION SENSOR MODELS

151

First, expand the error model to separate the environment-sensitive errors from the bias errors: 2

3n 2 3 2 SFx d Bx 4 d 5 ¼ Cbn 4 By 5 þ Cbn 4 MAyx MAzx d Bz

MAxy SFy MAzy

32 MAxz MAzx 54 SFz

x y

3b 5

z

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Next, multiply through by the environment stimuli 2

3n 2 3 2 SFx x þ MAxy y þ MAxz d Bx 4 d 5 ¼ Cbn 4 By 5 þ Cbn 4 MAyx x þ SFy y þ MAyz MAzx x þ MAzy y þ SFz d Bz

z z

3 5

z

Define an error state vector composed of the sensor errors and configure the multiplying terms to reproduce the preceding result: 2 3 3n 2 3 d Bx x y z 0 0 0 0 0 0 4 d 5 ¼ Cbn 4 By 5 þ Cbn 4 0 0 0 x y z 0 0 0 5 d Bz 0 0 0 0 0 0 x y z 2 3 SFx 6 MAxy 7 6 7 6 MAxz 7 6 7 6 MAyx 7 6 7 7 6 6 SFy 7 6 MAyz 7 6 7 6 MAzx 7 6 7 4 MAzy 5 SFz 2

Finally, multiply through by the Cnb matrix to obtain the preceding result. 6.2

This exercise concerns the dual-axis sensor error model. Error models for dual-axis sensors contain common errors that are the result of common effects for the two axes. For example, a tuned rotor gyro has common acceleration and rate sensitive errors in the axes normal to the spinning rotor shaft. Consider the following gyro error model for a dual-axis sensor whose spin axis is parallel to its z axis: 2 3 2 32 3 3 gMAxy gMAxz gSFx 1x gBx vx 6 7 6 gMA 76 7 6 7 gSF gMA 1 ¼ gB þ 4 y5 4 4 y5 xy x yz 5 4 vy 5 gMAxz gMAyz sSFz  z gBz z vz z 2 32 3 gGSxy 0 fx gGSx 6 76 7 þ 4 gGSxy gGSx 0 5 4 fy 5 0 0 0 z fz 2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

152

INTEGRATED NAVIGATION SYSTEMS where the gyro misalignments, gMA, result from small-angle approximations to the transformation matrix transforming from the sensor’s case to the navigation system’s reference axes. Rearrange this dual-axis model into the following form: 2 3 gBx 6 gBy 7 6 7 6 gBz 7 6 7 36 gSFx 7 2 3 2 7 1 0 0 vx 0 vy vz 0 fx fy 6 1x 6 gSFz 7 4 1y 5 ¼ 4 0 1 0 vy 0 vx 7 0 vz fy fx 56 6 gMAxy 7 7 0 0 1 0 vz 0 vx vy 0 0 6  z 6 gMAxz 7 6 7 6 gMAyz 7 6 7 4 gGS 5 x gGSxy z

6.3

This exercise concerns redundant dual-axis sensors’ error model. If an additional dual-axis sensor is used, one of its axes is redundant. Consider an error model with the sensor’s spin axis parallel to its x axis, and show that the combined error model for the two dual-axis sensors becomes 2

gBx,z

3

6 gBy,z 7 6 7 6 7 6 gBz, x 7 6 7 6 gSFx,z 7 6 7 6 7 6 gSFz,x 7 6 7 2 3 2 36 gMAxy,z 7 6 7 1 0 0 vx 0 vy vz 0 0 0 fx fy 0 0 6 1x,z 7 6 7 gMA xz,z 6 7 6 7 7 4 1y,z 5 ¼ 4 0 1 0 vy 0 vx 0 vz 0 0 fy fx 0 0 56 6 gMA 7 yz,z 7 6 1z,x 0 0 1 0 vz 0 0 0 vx vy 0 0 fz fy 6 7 6 gMAxz,x 7 6 7 6 gMA 7 xy,x 7 6 6 7 6 gGSx,z 7 6 7 6 gGS 7 xy,z 7 6 6 7 4 gGSy,x 5 gGSyx,x

z

where the “,( )” subscript denotes the corresponding sensor’s spin axis direction.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

7 Navigation Aids

Descriptions of aids to navigation presented in this chapter include the following: Doppler velocity sensors (DVS), tactical-air-navigation (TACAN) range, global positioning systems (GPS), and forward-looking infrared (FLIR) and other line-of-sight (LOS) systems. These systems provide independent redundant navigation information that can aid an integrated navigation system via the Kalman filter algorithm. Doppler velocity sensors are used widely in aircraft, seacraft, and land vehicles. While operating with different mechanizations, for example, radar, acoustic, laser, etc., most employ the Doppler shift principle. A simple characterization of a Doppler system’s operation, performance characteristics, and development of error modeling approaches, assuming the navigation system error model representations defined in Chapter 5, is presented. GPS operates on a satellite-to-receiver/antenna ranging principle. This is functionally similar to the principle of operation for TACAN. TACAN ranging is used with respect to a fixed ground station, whereas GPS ranging is used with respect to a satellite whose position is known relatively accurately. In this presentation, TACAN is used to illustrate a single ranging operation that is extended to multiple-satellite ranges for GPS. Presented are Kepler’s equations that describe satellite orbital motion and are used in the GPS system’s processing to determine the satellite positions in an Earth-centered, Earth-fixed (ECEF) frame. Quality measures associated with GPS data are used to determine which satellite’s ranging data are to be used. The satellite elevation and geometrical-dilution-of-precision (GDOP) quality measures are developed. Airborne line-of-sight systems include radar, FLIR, television (TV), etc. These systems serve many functions, and they can provide aiding information to a navigation system. Error models are developed, using navigation system error models in Chapter 5, for incorporating this system’s data to a Kalman filter algorithm. Because the airborne LOS systems are closely related, only the FLIR system is presented. Problems are included to expand on the material presented.

153

154 7.1

INTEGRATED NAVIGATION SYSTEMS Doppler Velocity Sensors

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

7.1.1 Doppler Velocity Sensor Functionality The Doppler effect can be explained by first considering a stationary emitting source that emits waves with period t. The number of oscillations meeting a stationary observer over a time interval Dt is Dt N¼ (7:1) t If the observer moves toward the stationary source with velocity V, the number of additional oscillations DN is (stationary source with moving observer) VDt DN ¼ (7:2) l and, if the source is emitted from the observer and reflected back toward the observer, then these additional oscillations are doubled as (reflected moving source at observer) 2VDt DN ¼ (7:3) l The frequency change is the number of additional oscillations during the time interval DN Dn ¼ Dt (7:4) 2V ¼ l The equation describing the Doppler shift becomes 2nV Dn ¼ c

(7:5)

If the emitter/observer’s velocity is not parallel to the reflecting source but is at a slant angle u, as shown in Fig. 7.1, then the frequency change is modified by Dn ¼

2nV cos u c

V θ

Fig. 7.1 Doppler at angle with respect to ground surface.

(7:6)

NAVIGATION AIDS

155

4

1

v bx v by

V

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

3

2

Fig. 7.2

Janus Doppler configuration.

The frequency change is a function of the velocity relative to the reflecting surface. A commonly used Doppler mechanization is the “Janus” configuration, which uses four beams, two directed forward and two directed rearward, depressed downward and canted left and right, as shown in Fig. 7.2. Using the frequency changes from the four beams, velocity components referenced to the aircraft body axis can be formed as Vx / Dn1 þ Dn2  Dn3  Dn4

(7:7)

Vy / Dn2 þ Dn3  Dn1  Dn4

(7:8)

Vz / Dn1 þ Dn2 þ Dn3 þ Dn4

(7:9)

and

The mechanization shown in Fig. 7.2 is relatively insensitive to terrain variations. Doppler systems exhibit sensitivity while operating over water as a result of changes in surface reflectivity. This is dependent on the sea’s Beaufort state for over-ocean operations. Operations over a moving surface, such as water, corrupt the Doppler sensor’s measurements of velocity.

7.1.2 Doppler Velocity Sensor Outputs for Navigation Filter Implementation The Doppler unit is assumed to be a body-fixed unit. (There are some Doppler units that are stabilized and isolated from the vehicle’s motion via a mechanical gimbal arrangement; to analyze these units, the knowledge of the gimbal configuration is required.) The Doppler velocity errors include the unit’s inherent errors, for example, scale factor, bias and noise, and errors resulting from the unit’s installation, that is, mechanical misalignment (bore-sight). Shown in Fig. 7.3 is a generalized error model for the DVS. Bias and scalefactor/bore-sight errors can be considered as random constants. For Doppler units based on a radar principle, the noise can be a function of the vehicle’s velocity and is usually referred to as fluctuation noise.

156

INTEGRATED NAVIGATION SYSTEMS ⎡δ v xb DOP ⎤ ⎡ dBx ⎤ ⎢ b ⎥ ⎢ ⎥ ⎢δ v y DOP ⎥ = ⎢dB y ⎥ b ⎢δ v z DOP ⎥ ⎢⎣ dBz ⎥⎦ ⎦ ⎣

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

⎡ dSFx ⎢ + ⎢dBS yx ⎢ dBS zx ⎣

dBS xy dSFy dBS zy

Bias – long term stable error

dBS xz ⎤ ⎡v xb ⎤ ⎥⎢ ⎥ dBS yz ⎥ ⎢v by ⎥ dSFz ⎥⎦ ⎢⎣v zb ⎥⎦

⎡ dWx ⎤ + ⎢⎢dWy ⎥⎥ ⎣⎢ dWz ⎦⎥

Fig. 7.3

Scale-factor and misalignments

White noise

General Doppler velocity error model.

The off-axis, velocity-dependent terms in this model represent misalignment or bore-sight errors. The dBSyz and dBSzx terms represent azimuth and pitch boresight errors, respectively. For fixed-wing aircraft applications, products of Doppler errors with the aircraft velocity components vby and vbz are usually small enough to neglect relative to the Doppler’s bias terms. However, for helicopters, the products with vby might not be small enough to neglect because helicopters can develop significant lateral velocities. Outputs from the DVS are combined with inertial navigation system velocities to form a measurement for processing in an onboard Kalman filter algorithm. The difference between the Doppler velocity and the inertial navigation system velocity is termed the Doppler divergence and is used as the observation in the Kalman filter. This is expressed as DvDOP ¼ vINU  vDOP

(7:10)

In Example 7.1, the specialization of this difference to the inertial navigation system perturbation error model presented in Chapter 5 will be presented. Example 7.1 Perturbation Velocity Error Representation Expressing Eq. (7.10) in the navigation frame n and expanding about a nominal velocity vn and neglecting products of error quantities yield DvnDOP ¼ v¯ nINU  v¯ nDOP

 n ¼ vnINU þ dvnINU  C b vbDOP þ dvbDOP   ¼ vnINU þ dvnINU  ½I  (f)Cbn vbDOP þ dvbDOP  vnINU  Cbn vbDOP þ dvnINU  Cbn dvbDOP þ (f)Cbn vbDOP ¼ dvnINU þ (f)Cbn vbDOP  Cbn dvbDOP ¼ dvnINU  (vn )f  Cbn dvbDOP

NAVIGATION AIDS

157

The first terms corresponding to the navigation system and then the Doppler error terms of the linearized measurement matrix become @Dv @du, dv, f, . . . , dvbDOP   ¼ 033 I33 (vn )    Cbn

HDOP ¼

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

7.2 7.2.1

Tactical-Air-Navigation Range Tactical-Air-Navigation Functionality

TACAN provides a range measurement from the unit’s position to a fixed ground station, as illustrated in Fig. 7.4. Other data provided by some TACAN units include magnetic bearing and range rate; however, these data might not be of sufficient quality for use as navigation aids. Magnetic bearing is corrupted by local electromagnetic fields, and range rate is a derived value. Some TACAN units provide two channels for two sets of range, bearing and range rate. 7.2.2 Tactical-Air-Navigation Range for Navigation Filter Implementation TACAN range is the magnitude of the relative position vector and is formed next by the sum of the squares of these components:

rTACAN ¼

pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi Dx2 þ Dy2 þ Dz2 þ bTACAN þ vTACAN

(7:11)

where corrupting bias and measurement noise have been added to the measurement model. The range observation is formed as the difference between this measured range and that computed based on the navigation systems’ position TACAN Station N

Δ re ρ

Aircraft E

r es

D r ea

Fig. 7.4 TACAN ranging.

158

INTEGRATED NAVIGATION SYSTEMS

relative to the TACAN station’s coordinates (see Problem 2.7). Linearizing this range difference with respect to the relative position vector elements and biases results in the following linearized measurement matrix: @Dr @dr, dv, . . . , b h Dy Dz ¼ Dx r r r 

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

H Dr ¼

1

i

(7:12)

If Kalman filter state vector definitions consist of a linear position error as in Eq. (7.11), the result in Eq. (7.12) suffices; however, other filter state vector definitions consisting of a different description of position error, that is, angular position error, then the measurement matrix is formed using the chain differentiation rule as @Dr @Dr @Dr ¼ @du @Dr @du

(7:13)

The first of these partial derivatives is given in Eq. (7.12). Different Kalman filter state vector definitions result in different measurement matrix contents. The use of TACAN range is functionally similar to GPS, which will be presented in the next section. An example illustrating the integration of TACAN and an inertial navigation unit (INU) via a Kalman filter algorithm will be presented in Chapter 10. The following example illustrates the change of variable indicated in Eq. (7.13).

Example 7.2 TACAN Range Measurement Matrix The TACAN range measurement model is to be formulated. The relative position vector between the TACAN station and the aircraft position, formed using the inertial navigation system’s outputs, is expressed as 82 2 3e 3 2 39 Dx (Rn þ h) cos f¯ cos l¯ = < (Rn þ hs ) cos fs cos ls Dre ¼ 4 Dy 5 ¼ Czx 4 (Rn þ hs ) cos fs sin ls 5  4 (Rn þ h) cos f¯ sin l¯ 5 ; : Dz ½Rn (1  12 ) þ hs  sin fs ½Rn (1  12 ) þ h sin f¯ where the matrix 2

3 0 0 1 Czx ¼ 4 0 1 0 5 1 0 0 transforms from the ECEF frame, with the Earth rotation vector parallel to the z axis 2Cen(z), to the ECEF frame, with the Earth rotation vector parallel to the x axis 2Cen(x) (see Sec. 3.1). The overbar represents corrupted latitude and longitude as computed by the navigation system.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

NAVIGATION AIDS

159

In the preceding relative position equation, latitude error in the normal radius of curvature Rn is not included. It was shown in Problem 4.1 that the sensitivity of this value is of order 12. This error’s contribution is considered to be second-order and is neglected. Also neglected are altitude error affects because at the longer TACAN range, a large variation in altitude would be required for a significant effect on range. TACAN range observation is linearized with respect to angular position state vector elements as illustrated in Eq. (7.13) using the chain differential rule. The first of these partial derivatives is presented in Eq. (7.12). The second derivative is applicable to this example, as well as other measurements to be formed later in this chapter, and will be developed next. The computed latitude and longitude provided by the navigation system is imperfect. Representing the errors in latitude and longitude are the following:

f¯ ¼ f þ df l¯ ¼ l þ dl Substituting these expressions into the preceding relative position range equation, expanding the sums of these angles using appropriate trigonometric identities, and neglecting products of error angles result in the following (see expressions of latitude and longitude error defined in terms of wander angle a and angular position errors du developed in Example 3.3): 82 3 as cfs cls > >

> : bs sfs 2 39 a½cfcl  sfcl(sadux þ caduy )  sl(cadux  saduy ) > > 6 7= 7 f s l  s f s l (s adu þ c adu ) þ c l (c adu  s adu ) a½c 6 x y x y 5 4 > > ; b½sf þ cf(sadux þ caduy ) 82 3 2 39 as cfs cls > acfcl > > >

> > > : bsf ; bs sfs 2 3 a½(sfclsa  slca)dux þ (sfclca þ slsa)duy  6 7 7  Czx 6 4 a½(sfslsa þ clca)dux þ (sfslca  clsa)duy  5 b½(cfsa)dux þ (cfca)duy  2 e 3 bCn xy bCne xx 0 2 du 3 x 6 e 76 7 n 7 aC 0 aC du ¼ Dre þ 6 4 y5 n yx 4 n yy 5 duz aCne zy aCne zx 0

160

INTEGRATED NAVIGATION SYSTEMS

where as ; R n þ h s bs ; Rn (1  12 ) þ hs a ; Rn þ h

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

b ; Rn (1  12 ) þ h and Dre is the relative position vector without error. For this illustration, the second partial derivative in Eq. (7.13) is obtained directly from the preceding relative range difference equation: 2 e bC @Dre 4 ne xy ¼ aCn yy @du aC e n zy

bCne xx aCne yx aCne zx

3 0 05 0

The first few terms of the linearized measurement matrix corresponding to the navigation system angular position errors become HDr

7.3 7.3.1

 @Dr @Dr ¼ ¼ @ dux @du, . . .

@Dr @duy

 

Global-Positioning-System Range Global-Positioning-System Functionality

GPS data are provided in a variety of forms. In their most basic form, these data are a transit time difference and Doppler effect—pseudorange and delta range—between a receiver’s antenna and GPS satellites. In some GPS receiver units, these basic data are preprocessed to provide refined position and velocity data in either local geodetic coordinates (latitude and longitude) or ECEF coordinates. The range provided by GPS is functionally similar to that provided by TACAN, except that the TACAN station is fixed to the Earth’s surface and GPS satellite positions must be determined in orbit. Satellite positions are assumed to be known relatively accurately. The GPS system assumes the satellite orbital motion is described by Kepler’s equations. Corrections are applied to the parameters describing this motion to provide the user with accurate satellite positioning information. A description of this motion is presented, based on Newton’s laws of motion. Consider the central force attraction illustrated in Fig. 7.5. The gravitational attraction is between the two bodies, M and m, whose common mass center is centered in the larger of the two masses. Accelerations act on the smaller mass in radial and tangential directions along the orbit’s arc. Expressions for this acceleration are developed using polar coordinates.

NAVIGATION AIDS

161

eθ

er m

r θ

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

M

Fig. 7.5 Central force attraction.

Small displacements along the orbit line’s arc are shown in Fig. 7.6. From this figure, the rate of change of unit vectors er and eu, along and normal to the position vector r, are formed. The differential change in the radial er and its normal eu unit vectors resulting from a differential angle rotation du from position (1) to position (2) is (1) der ¼ e(2) r  e r ¼ d ue u (1) deu ¼ e(2) u  eu ¼ d uer

Dividing by differential time dt yields the time rate of change of these unit vectors: e˙ r ¼ u˙ eu e˙ u ¼ u˙ er

(7:14) (7:15) (2)

er

(2)

eθ

(1)

eθ

dr r dθ r(2)

dθ

r(1)

Fig. 7.6

θ

Position change along an arc.

(1)

er

162

INTEGRATED NAVIGATION SYSTEMS

Returning to Fig. 7.5, the position vector is expressed in polar coordinates as r ¼ rer

(7:16)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

The velocity is obtained from the time derivative of Eq. (7.16) using Eqs. (7.14) and (7.15): v ¼ r˙ ¼ r_ er þ r˙er ¼ r_ er þ r u˙ eu

(7:17)

Taking a second derivative, the acceleration is obtained: a ¼ v˙ ¼ r€ er þ r_ e˙ r þ r_ u˙ eu þ r u¨ eu þ r u˙ e˙ u 2 ¼ r€ er þ r_ u˙ eu þ r˙ u˙ eu þ r u¨ eu  r u˙ er 2

¼ (€r  r u˙ )er þ (2_r u˙ þ r u¨ )eu

(7:18)

It is assumed that the acceleration a is the result of only central force attraction acting along the radius vector, as shown in Fig. 7.5, from the two-body system’s common center of mass. This force is expressed as F¼

mm er r2

(7:19)

where m is the gravitational constant of the attracting mass M and m the satellite mass. The force per unit mass, specific force (acceleration), from Eq. (7.19) is set equal to Eq. (7.18), and common unit vector components are grouped resulting in the following equations of motion:

m r2

(7:20)

r u¨ þ 2˙r u˙ ¼ 0

(7:21)

2

r€  r u˙ ¼  and

The solution to Eq. (7.20) can be obtained with the change of variable 21/u ¼ r. This solution, position in an elliptical orbit and the corresponding velocity, is presented in Appendix B. With reference to definitions of the satellite orbit presented in Figs. 7.7 and 7.8, Table 7.1 summarizes the solution to Eqs. (7.20) and (7.21) for an elliptical orbit. The GPS satellite position is defined in terms of the orbital elements defined in Figs. 7.7 and 7.8. In the primed axis system defined in the orbital plane,

NAVIGATION AIDS

163

f

a a (1– ε 2)½

r E

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

a (1– ε )

ellipse

auxiliary circle

Fig. 7.7

Orbital plane elements.

the x0 and y0 positions are x0 ¼ r cos u

(7:22)

y0 ¼ r sin u

(7:23)

These positions are transformed into an Earth-centered frame, but, first, it is easier to form the transformation from Earth-centered to the orbital plane, and then transpose this result to obtain the transformation from orbital plane to Earth-centered. Referring to Fig. B.1, the rotation sequence follows first with ze h orbital plane m

r Ω

ω

ye i reference plane

xe

Fig. 7.8

n

Satellite ECEF positions.

164

INTEGRATED NAVIGATION SYSTEMS Table 7.1

Satellite orbit equations

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Orbit element

Equation pffiffiffiffiffiffiffiffiffiffi n ¼ 2p=t ¼ m=a3 M ¼ (2p=t)t E ¼ 1 sin E þ M cos f ¼ cos E  1=1  1 cos E r ¼ a(1 2 1 cos E) u¼ vþf _ V _ i=e )t V ¼ V0 þ (V ——

Mean motion Mean anomaly Eccentric anomaly E True anomaly f Orbit radius r Latitude u Longitude V Orbital elements (see Figs. 7.7 and 7.8) Semimajor axis a Eccentricity 1 Inclination with respect to equator I Longitude of ascending node V Argument of perigee from the reference v

the longitude, then the inclination as 2

3 2 x0 1 4 y0 5 ¼ 4 0 0 0

0 ci si

32 0 cV si 54 sV ci 0

32 e 3 sV 0 x cV 0 54 ye 5 0 1 ze

(7:24)

which, when the product of the matrices are transposed, yields the transformation from orbital plane to Earth-centered as 2

3 2 cV xe 6 e7 6 4 y 5 ¼ 4 sV ze

sVci cVci

0 2

32 0 3 sVsi x 76 0 7 cVsi 54 y 5

si

ci

0

3 0

cVx0  cisV y 6 7 ¼ 4 sVx0 þ cicV y0 5 si y0

(7:25)

Or, combining Eqs. (7.22), (7.23), and (7.25), xECEF ¼ r½cos u cos V  sin u cos i sin V y

ECEF

¼ r½cos u sin V þ sin u cos i cos V

zECEF ¼ r sin u sin i

(7:26) (7:27) (7:28)

Orbital parameters, including the longitude of the ascending node V, vary as a result of the Earth’s gravitational variation. The mean rate of change of this parameter, assuming the gravitational terms including the first nonzero term in the

NAVIGATION AIDS

165

potential function, is found as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2 dV 3 re n cos i ¼  J2 p dt 2

(7:29)

Because this is an idealization, the actual gravitational variation will cause change to the orbital parameters in addition to that predicted by this expression, thus requiring updates to the simplified equations in Table 7.1. The longitude of the ascending node is predicted by the following expression _ V _ i=e )t V ¼ V 0 þ (V (7:30) Corrections to this simple description of satellite motion are provided by GPS ground-based satellite monitoring. Updates to the satellite orbital elements, in the form of corrections, are provided for mean motion n, argument of latitude u, radius r, inclination i, and longitude of ascending node V. 7.3.2

Satellite Selection

Normally, several satellites are within view of the GPS receiver’s antenna. However, better performance can be obtained by properly selecting which of the satellites is to be used. Satellites are selected based on their elevation above the local horizon and their geometrical relationship to the user. Elevation angle. Of the satellites available, some are not visible or sufficiently high above the horizon to be used. Range data from satellites that are near the horizon are corrupted by atmospheric refraction and should be excluded from use. To select which satellites will be used, the elevation angle, shown in Fig. 7.9, from the receiver’s antenna position to each candidate satellite is determined.

Δ re

θ los

pe

Fig. 7.9

Satellite LOS elevation angle.

166

INTEGRATED NAVIGATION SYSTEMS

The LOS unit vector in ECEF from the receiver’s antenna position to the satellite position is eelos ¼

Dre r

(7:31)

The local vertical unit vector can be approximated as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

eeup 

pea R

(7:32)

where the magnitude of the receiver’s antenna ECEF position vector is qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi jpea j ; R ¼ p21 þ p22 þ p23 (7:33) Using the dot-product relationship for two unit vectors, the elevation angle relative to the local horizontal is determined from



sin ulos ¼ eelos eeup

(7:34)

Satellites that are too near the horizon are usually excluded from use in the measurement processing because of the atmosphere distorting the range information. Geometric dilution of precision. In addition to selecting a satellite based in its elevation angle, satellites can be selected based on their geometrical relationship to the GPS receiver’s antenna, as shown in Fig. 7.10. The geometry of the satellite positions relative to the antenna determines the GDOP or loss of precision resulting from this geometry. The minimum GDOP value is considered

ze S1 e

Δ r1

e

Δ r2

e r1

S2

e

Δ

r2

e r4

Δ

e

r4

ye e r3

e r3

S4 S3 xe

Fig. 7.10

Multiple-satellite geometry.

NAVIGATION AIDS

167

the best. For a selected group of satellites, the minimum GDOP is used to determine which group of satellites to use in the measurement processing. The ith satellite’s LOS unit vector eLlosi with respect to the local horizontal navigation frame, is obtained from

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

eLlos ¼ CeL eelos

(7:35)

where the sub- and superscripts have been momentarily dropped. Consider the case where four satellites are within view: i ¼ 1, 2, 3, 4. A Kalman filter’s use of satellite-range and/or delta-range updates can accommodate as few as one satellite, and, of course, more than four. The pseudoranges from the receiver’s antenna to the four satellites are computed from the component differences of the position vector p and each of the satellite’s position vectors s. Assuming all ranges are biased by the same receiver clock error, the ith pseudorange is expressed (neglecting additive noise) as  1=2 ri ¼ (xi  p1 )2 þ (yi  p2 )2 þ (zi  p3 )2 þb

(7:36)

where x, y, and z represent the vector components of the ith satellite’s position vector s. If the receiver’s antenna position is in error by dp, then the linearized form of this equation becomes

dri  

b(xi  p1 )dp1 þ (yi  p2 )dp2 þ (zi  p3 )dp3 c þ db ri

(7:37)

The differences in vector components divided by the magnitude is the unit vector 2

3 x i  p1 1 e i ¼ 4 y i  p2 5 ri z i  p3 For four satellites, the variation in pseudorange can be expressed as 2 3 3 2 T dr1 e1 1 6 dr2 7 6 eT 1 7 6 7 7 6 2 4 dr3 5 ¼ 4 eT 1 5 3 dr4 eT4 1

(7:38)

(7:39)

This linear vector equation can be redefined in the following form: z ¼ Hx

(7:40)

where the vector x contains the corrections to position and time. If the matrix H is nonsingular, then this equation can be solved for x as x ¼ H 1 z

(7:41)

168

INTEGRATED NAVIGATION SYSTEMS

Assuming that unknown corrections to position and time are random, resulting from variable satellite positions, the expected error in position and time is given as its covariance

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Cov½x ; EbxxT c   ¼ E (H 1 z)(H 1 z)T   ¼ H 1 E zzT H T

(7:42)

If the pseudorange error variance is the same for the four satellites and is equal to s2psuedo, then   E zzT ¼ s2psuedo I44 (7:43) Using this in Eq. (7.42), the covariance of position and time corrections becomes  1 Cov½x ¼ H T H s2psuedo (7:44) where it is assumed that pseudorange error statistics are the same. The error associated with the position and time corrections is governed by the geometry contained in the matrix H. Reinstating sub- and superscripts again and defining the matrix (g), Eq. (7.44) is rewritten as 2 T 3 eLlos 1 1 6 LT 7 6 elos 2 1 7 6 7 (g) ¼ 6 LT (7:45) 7 4 elos 3 1 5 T eLlos 4 1 From the matrix  1 J ¼ (g)T (g)

(7:46)

Geometric dilution of precision is computed from its diagonals as GDOP ¼

pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi J1,1 þ J2,2 þ J3,3 þ J4,4

(7:47)

position dilution of precision (PDOP) as PDOP ¼

pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi J1,1 þ J2,2 þ J3,3

(7:48)

and finally, horizontal dilution of precision (HDOP) as HDOP ¼

pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi J1,1 þ J2,2

(7:49)

GPS pseudorange and delta range measurements and their linearizations suitable for filter implementation are summarized in Fig. 7.11.

NAVIGATION AIDS

169

Pseudorange: r

b e eLOST

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Hpseudo

01×3 01×3 0 1 0

Delta Range/Pseudo range Rate: rf rb d t T Hdelt 01×3 e eLOS t 01×3 0 0 t where range from vehicle to satellite r unit line of sight vector from vehicle to satellite ee LOS t difference between final time tf and beginning time tb for Doppler integration interval

Fig. 7.11

7.4 7.4.1

GPS receiver measurement model.

Forward-Looking Infrared Line-of-Sight Systems Line-of-Sight System Functionality

Line-of-sight systems include airborne radar, FLIR, TV, laser ranging, etc. These systems establish LOS to a feature relative to the vehicle’s axis system (assumed to be parallel with the system body axis), as illustrated in Fig. 7.12. Generally, these systems employ two or more mechanical gimbals, within which the sensor moves to establish the LOS. The outer gimbal (first movement relative to the aircraft body axis) is usually azimuth Az, and the inner gimbal (mounted within the outer gimbal) is usually elevation El. The gimbal ordering, and therefore the sequence of angle rotations, is important to establishing the transformation matrix that transforms vector components from the LOS to the aircraft axis system and visa versa. This then forms the basis of the mathematical model for these measurements.

Az −El

xb ρ

Aircraft yb zb

Fig. 7.12

LOS to surface feature.

surface feature

170 7.4.2

INTEGRATED NAVIGATION SYSTEMS LOS System Measurements

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

If LOS is established by the sensor, all components of the relative position vector are zero except for the first, which is the magnitude of the relative range between the aircraft and the ground surface feature 2 3los 2 3 Dx r 4 Dy 5 ; 4 0 5 (7:50) Dz 0 In the following, it is assumed that the gimbal arrangement for LOS systems is such that the gimbal mounted to the aircraft is azimuth. The next inner gimbal is elevation. Transforming from the body axis to the LOS axis proceeds as follows: 32 32 3b 2 3los 2 cEl 0 sEl cAz sAz 0 Dx Dx 6 76 76 7 6 7 0 54 sAz cAz 0 54 Dy 5 4 Dy 5 ¼ 4 0 1 Dz

sEl 2

0

cElcAz 6 ¼ 4 sAz sElcAz

cEl cElsAz cAz sElsAz

0

0 32

1 3b

sEl Dx 76 7 0 54 Dy 5 cEl

Dz

(7:51)

Dz

Equations for azimuth and elevation in terms of the body-referenced relative position vector components are obtained by equating the right-hand sides of Eqs. (7.50) and (7.51). The resulting measurement equations and their corresponding linearizations for the matrix, that is, @DAz @Drb

(7:52)

@DEl @Drb

(7:53)

and

are presented in Table 7.2 for this LOS system. Example 7.3

Complete Line-of-Sight Measurement Formulation

The use of LOS measurements couples the navigation system position error, as examined earlier for TACAN measurements and navigation system attitude errors, into the measurement process. As with TACAN, the relative position vector between the navigation system’s position, and the ground reference point is expressed as 82 3 2 39 (Rn þ h) cos f¯ cos l¯ = < (Rn þ hs ) cos fs cos ls b n D¯rb ¼ C n C e Czx 4 (Rn þ hs ) cos fs sin ls 5  4 (Rn þ h) cos f¯ sin l¯ 5 ; : ½Rn (1  12 ) þ hs  sin fs ½Rn (1  12 ) þ h sin f¯

NAVIGATION AIDS

171

Table 7.2 LOS system measurements and linearizations Partial derivative with respect to ( ) Dx

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Measurement

Dy Az ¼ tan1 Dx El ¼ tan

r¼

1

Dy þ Dy2

Dz pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi Dx2 þ Dy2

pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi Dx2 þ Dy2 þ Dz2

!

Dy

Dz

Dx þ Dy2

Dx2

Dx2

DxDz pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi 2 r Dx2 þ Dy2

DyDz pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi 2 r Dx2 þ Dy2

(Dx2 þ Dy2 ) pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi r2 Dx2 þ Dy2

Dx r

Dy r

Dz r

where  e n T b n C n C e ¼ C n C b T  ¼ Cne ½I þ (du)½I  (fx)Cbn  Cnb ½I þ (f  du)Cen The navigation system computed latitude and longitude terms in the brackets in the preceding equation were expanded in Example 7.2. Using these results, the following is obtained for the body-frame relative position vector: 2 e 3 bCn xy bCne xx 0 DDrb ¼ Drb þ Cnb ½(f  du)Cen Dre þ Cnb Cen 4 aCne yy aCne yx 0 5du aCne zy aCne zx 0 Recognizing that Drn ¼ Cen Dre and exchanging the order of the vector cross-product terms in the second term in this equation, the following is obtained for the relative position vector: 82 e < bCn xy DDrb ¼ Drb  Cnb (Drn )f þ Cnb Cen 4 aCne yy : aC e n zy

bCne xx aCne yx aCne zx

9 3 0 = 0 5 þ (Drn ) du ; 0

The partial derivatives to accompany those in Eqs. (7.52) and (7.53), with respect to the angular position error and the attitude error, are 82 e 9 3 e < bCn xy bCn xx 0 = @Drb ¼ Cnb Cen 4 aCne yy aCne yx 0 5 þ (Drn ) : aC e ; @du aCne zx 0 n zy

172

INTEGRATED NAVIGATION SYSTEMS

and @Drb ¼ Cnb (Drn ) @f

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

To complete the measurement linearization, partial derivatives of azimuth (and similarly for elevation) LOS measurement, with respect to angular position and attitude errors, are obtained from the following: @DAz @DAz @Drb ¼ @du @Drb @du @DAz @DAz @Drb ¼ @f @Drb @f From this example, it can be seen that errors in LOS navigation aids include errors in body attitudes, in addition to position errors. 7.5

Chapter Summary

In this chapter, several navigation aids providing independent and/or redundant navigation state data, operation, and performance characteristics were presented. Their error characterizations and examples of combining outputs as measurements into an integrated navigation system were presented using several navigation state error representations from Chapter 5. Examples presented were DVS, TACAN, GPS, and FLIR. Additionally, TACAN was used to illustrate a single ranging operation and associated error modeling. This error model is applied later in Chapter 10 for postflight trajectory reconstruction. TACAN also functions as an example of how single-satellite ranging can be used in GPS applications. Single-satellite GPS operations are used in a case study presented in Chapter 12 for a GPS/inertial integrated navigation system. Extensions for multiple-satellite ranging for GPS were presented, along with GPS quality measures of satellite elevation and GDOP, which are also illustrated in this later case study. Mathematical methods presented in this chapter can be applied to other navigation aids and other navigation state error representations. It is the methodology, not specific examples presented, that is important because other navigation aids or different navigation state error representations can confront the integrator/ designer applications.

Problems 7.1

Doppler divergence (alternative velocity error representation) is dealt with in this exercise. Express Eq. (7.10) in the navigation frame n and expand about a nominal velocity vn, but this time use the alternate representation for INU velocity error as DvnDOP ¼ v¯ nINU  v¯ nDOP  n ¼ vnINU þ dv1INU  (du)vnINU  C b vbDOP þ dvbDOP

NAVIGATION AIDS

173

  ¼ vnINU þ dv1INU  (du)vnINU  ½I  (f)Cbn vbDOP þ dvbDOP  vnINU  Cbn vbDOP þ dv1INU  (du)vnINU  Cbn dvbDOP þ (f)Cbn vbDOP ¼ dv1 þ ½(f  du)Cbn vb  Cbn dvbDOP

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¼ dv1  (vnINU )c  Cbn dvbDOP Form the first few terms of the linearized measurement matrix corresponding to the navigation system errors and Doppler errors for the computer frame referenced velocity error as

HDOP ¼

dv 1 ,

@du,  ¼ 033

7.2

@Dv c, . . . , dvbDOP

I33

(vn )

   Cbn



Consider the alternate velocity doppler/INU measurement f formulation. Specialize the INU/Doppler difference in Sec. 7.1 for the alternative velocity error representation to the following. Assume the position error is linear position and expressed as 2 6 6 6 du ; 6 6 4

dr y 3 R 7 7 dr x 7 7 7 R 5



duz The attitude error is the f form. Show that the velocity difference can be expressed as 2

3 dr x 6 dr y 7 6 7 6 7 6 dh 7 7 2 v 36 6 dv 1 7 z x7 0 0 1 0 0 0 vz vy vy 6  7 6 R 76 dv1y 7 6 76 n v 6 z Dv ¼ 6 0  0 0 1 0 vz 0 vx vx 76 1 7  Cbn dvbDOP 4 5 6 dv z 7 7 R 7 ry rx 0 0 0 1 vy vx 0 0 6 6 fx 7 6 7 6f 7 6 y7 6 7 4 fz 5 duz

174 7.3

INTEGRATED NAVIGATION SYSTEMS Consider the alternate velocity doppler/INU measurement c formulation. Again, specialize the INU/Doppler difference in Sec. 7.1 to the following. Assume the position error is the linear position as just shown, the velocity error is the alternate error form, and the attitude error is the c form. Show that the velocity difference can be expressed as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

2

2

0 Dvn ¼ 4 0 0

0 0 0 0 0 0

1 0 0

0 1 0

0 0 1

0 vz vy

vz 0 vx

3 dr x 6 dr y 7 6 7 6 dh 7 36 1 7 dv 7 vy 6 6 x1 7 n b 5 6 v x 6 dv y 7 7  Cb dvDOP 6 7 1 0 6 dv z 7 6c 7 6 x 7 4c 5 y cz

7.4

For the preceding two specializations of the INU/Doppler velocity difference, the Doppler errors were not specialized. Assume that the vehicle is a fixed-wing aircraft whose velocity is primarily the x-axis body component and that the other components, when multiplied by Doppler errors, are assumed negligible. Show that the Doppler contribution to these two specializations is given as 2 3 dSFx 2 b 36 dMAyx 7 7 vx 0 0 1 0 0 6 6 dMAzx 7 7 Cbn dvbDOP ¼ Cbn 4 0 vbx 0 0 1 0 56 6 dBx 7 7 0 0 vbx 0 0 1 6 4 dBy 5 dBz

7.5

The Doppler velocity difference in Eq. (7.10) can also be formed in body axes. Reformulate the Doppler divergence in a body-frame using the perturbation form of velocity error representation. DvbDOP ¼ v¯ bINU  v¯ bDOP   b ¼ C n v¯ nINU  vbDOP þ dvbDOP     ¼ ½I þ (f)Cnb vnINU þ dvnINU  vbDOP þ dvbDOP  Cnb vnINU  vbDOP þ Cnb dvnINU þ (f)Cnb vnINU  dvbDOP ¼ Cnb dvnINU þ (f)Cnb vnINU  dvbDOP ¼ Cnb dvnINU  (vbINU )f  Cbn dvbDOP

7.6

Consider GPS/INU angular position differences from latitude/ longitude differences, and reexamine GPS position differences in Problem 5.8. Assume that latitude and longitude differences Df and Dl

NAVIGATION AIDS

175

are obtained from Df ¼ fINU  fGPS

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Dl ¼ lINU  lGPS and the INU wander angle a is used to transform these differences into the navigation frame. Show the following results: 2 3 daduy Dun ¼ dun þ 4 dadux 5 0 7.7

Review GPS/INU navigation frame velocity differences: alternate velocity. Reexamine GPS velocity differences. Assume GPS velocity available in the ECEF frame without error. For perturbation and alternative velocity error representations, show that the difference n

Dv ¼ v¯ nINU  C e veGPS yields for the following: Perturbation: Dv ¼ dvn  (vn )du Alternative: Dv ¼ dv1 7.8

Consider the difference between velocity referenced at a GPS antenna and the navigation system velocity during maneuvers. Specialize the GPS velocity at the antenna by again considering a fixed-wing aircraft. Also assume that the aircraft’s body-to-navigation frame rotation is much greater than its transport rate, yielding the following approximation: DvnINU=ant ¼ vnINU  vnant ¼ vbn=b  (Cbn db ) For a planar horizontal turn, such that all but the z component of the preceding rotation vector elements are zero, the velocity difference becomes 2

0

vz

0

32

Cxx

6 76 DvnINU=ant ¼ 4 vz 0 0 54 Cyx 0 0 0 Czx 2 3 vz Cyx d 6 7 ¼ 4 vz Cxx d 5 0

32 3 d 76 7   54 0 5   0  

176

INTEGRATED NAVIGATION SYSTEMS With the aircraft maintaining constant speed, V  vbx and constant bank angle f, the turn rate is

s˙ ¼

g tan f V

which is approximately equal to vz. Then the magnitude of the velocity difference is approximately

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

jDvINU=ant j ¼ s˙ d 7.9

Specialize the line-of-sight position difference assuming a linear position error and a c attitude error. Express the results in the form of the following linearized measurement matrix: HDr ¼ to obtain 2 HDv

7.10

6 6 6 ¼6 6 4

1þ

Drz R

0 

Drx R

0

@Dr @dr(x,y),dv, c, duz

0 0 0 0

0

Drz

Drz 0 0 0 0 Drz 1þ 0 R Dry  1 0 0 0 Dry Drx R

Dry Dry Drx 0

3

7 7 7 Drx 7 7 5 0

Terrestrial navigation systems also use stellar observations for update. This exercise forms the difference between a body-fixed (not gimbaled) star tracker sensor’s outputs and known stellar reference positions to be used as a measurement. Consider the difference vector between onboard computed position, using system variables, and the sensor’s outputs (ST): s

Dss ¼ C i si  s¯sST From star charts or similar information, the inertial position of the star si is assumed known. Expand this difference equation as s b n Dss ¼ C b C n C e Cie si  s¯ sST

The ST alignment with respect to the body axis is assumed to be represented by s C b ; ½I þ (d)Cbs

where the vector d includes misalignments between the ST and body axes. The other matrices in the vector difference equation were given earlier.

NAVIGATION AIDS

177

Expand the preceding difference vector to obtain the following: Dss  ss  ssST  (ss )½Cns c þ d

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

7.11

The mean rate of change of an elliptical orbit’s longitude of the ascending node is considered in this exercise [10]. Consider the vector form of the angular momentum (see Appendix B). 3 2 32 3 2 rz vy þ ry vz 0 rz ry vx 6 7 6 7 6 0 rx 7 h ¼ r  v ¼ 4 rz 54 vy 5 ¼ 4 rz vx  rx vz 5 ry rx 0 ry vx þ rx vy vz 2 rm 3 2 3 sin V sin i(1 þ e cos f ) h sin V sin i 6 h 7 6 rm 7 6 7 7 ¼6 6  h cos V sin i(1 þ e cos f ) 7 ¼ 4 h cos V sin i 5 4 5 rm h cos i cos i(1 þ e cos f ) h The rate of change of the angular momentum is obtained as d d dr dv dv h ¼ (r  v) ¼  v þ r  ¼ r  dt dt dt dt dt Because, in an inertial frame, the vectors dr/dt and v are the same vector and parallel, and the cross-product zero. The specific forces are resolved into component directions that are radial, tangential, and normal to the orbit dv ; f ; R^eR þ T e^ T þ N e^ N dt defining the orbital position vector as r ; r^eR Obtain the following for the terms on the right-hand side of the angular momentum derivative equation: r

dv ¼ rN e^ T þ rT e^ N dt

Next, obtain the following equation for the derivative of longitude of the ascending node from the first two elements of the preceding angular momentum equation: _ _ _ ¼ hx hy  hy hx V h2x þ h2y

178

INTEGRATED NAVIGATION SYSTEMS Equate the rate of change of angular momentum and specific forces to obtain the following equations: h_ x ¼ rT(sin V sin i)  rN( cos V sin u  sin V cos u cos i) ¼ r½(sin V sin i)T þ (cos V sin u þ sin V cos u cos i)N

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

h_ y ¼ rT( cos V sin i)  rN( sin V sin u þ cos V cos u cos i) ¼ r½(cos V sin i)T þ (sin V sin u  cos V cos u cos i)N Substitute these expressions into the longitude rate equation, and use h2x þ h2y ¼ h2 sin2 i to obtain the following rh{sVsi½(cVsi)T þ (sVsu  cVcuci)N _ ¼ þcVsi½(sVsi)T þ (cVsu þ sVcuci)N} V h2 s2 i r ½sVcVsiT þ (s2 Vsu  sVcVcuci)N ¼ hsi þ sVcVsiT þ (c2 Vsu þ sVcVcuci)N r ½(s2 Vsu)N þ (c2 Vsu)N ¼ hsi r sin u N ¼ h sin i Or _ ¼ V

 1=2 h sin u a(1  e2 ) sin u N¼ N m (1 þ e cos f ) sin i m(1 þ e cos f ) sin i

Now consider the specific forces. The gravitational potential, including the J2 term is m  re 2 UJ2 ¼  J2 P2 (cos w) r r m  re 2 1 J2 (2  3 sin2 w) ¼ r r 2 Define the orbital latitude as sin2 w ¼

x 2 þ y 2 r 2  z2 z2 ¼ ¼ 1  r2 r2 r2

NAVIGATION AIDS

179

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Then obtain the following for the preceding potential function: 

 m re 2 1 z2 U J2 ¼  J2 2  3 1  2 r r r 2 2

2 mr 1 z ¼  e J2 3 2  1 2 r r Operate on this potential with the gradient to yield the specific forces resolved into radial and vertical components as @U @U e^ R þ e^ z @r @z



2

 mr 2 1 z 1 z2 mr2 h z i ¼  e J2 3 4 3 2  1 þ 3 6 3 e^ R  e J2 6 2 e^ z 2 r r 2 r r r 



 3 15z2 3z ¼ mJ2 re2  6 e^ R þ 5 e^ z 4 2r 2r r

f J2 ¼ rUJ2 ¼

Express the vertical unit vector in terms of the radial, tangential, and normal vectors as e^ z ¼ sin i sin ue^ R þ sin i cos ue^ T þ cos i^eN and use the following for z: z ¼ r cos w ¼ r sin i sin u to obtain the following expression for the specific force: f J2 ¼ mJ2 re2







3 15r 2 s2 is2 u 3rsi su ^ ^ ^ e e e  þ u þ sic u þ ci^ e ) (si s R R T N 2r 6 r5 2r 4

Or f J2

3mJ2 re2 ¼ r4





1 3s2 is2 u 2  e^ R þ s isu cue^ T þ si ci sue^ N 2 2

Return to the longitude rate equation, and use the normal component of specific force, to obtain the following for the longitude rate:   3mJ2 re2 _ ¼ h sin (v þ f ) V sin i cos i sin ( v þ f )  m (1 þ e cos f ) sin i r4 ¼

3J2 re2 h cos i 2 sin (v þ f )(1 þ e cos f )3 p4

180

INTEGRATED NAVIGATION SYSTEMS This rate is continuously varying along the orbit. Of interest is the mean rate of change over an orbital period. Form the mean as ð dV 1 2p dV ¼ dM dt 2p 0 dt where the differential mean and true anomaly are

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

dM ¼ ndt df ¼

h dt r2

Change the integration variable as dM ¼

nr 2 np2 df ¼ df h h(1 þ e cos f )2

and substitute this into the preceding integral and evaluate over an orbital period as ð dV 3J2 re2 h cos i 1 2p 2 np2 3 sin ( v þ f )(1 þ e cos f ) df ¼ dt p4 2p 0 h(1 þ e cos f )2 ð 3J2 re2 n cos i 1 2p 2 sin (v þ f )(1 þ e cos f )df ¼ p2 2p 0 Or

2 dV 3 re ¼  J2 n cos i: p dt 2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

8 Kalman Filtering

The Kalman filter algorithm is used extensively in integrated navigation systems. The algorithm integrates independent redundant sources of navigation information with a reference navigation solution to obtain an optimal estimate of navigation states—position, velocity, and attitude—and other variables that contribute to navigation solution error. In this chapter, the Kalman filter algorithm is derived in incremental stages and extended for applications. This development and its extensions proceed as follows: recursive weighted least squares for constant systems; recursive weighted least squares for dynamic systems; discrete linear minimum variance estimator; U –D factored form; summed measurements; combined estimate from two (or more) Kalman filters; and derivative free estimation. Following the approach of Sage [2], the minimization of a weighted least-squares (WLS) cost function provides an estimator for a system of constants. This result is reformulated into a recursive (RWLS) estimator, again, for a constant system. Structurally, this estimator has the same form as the measurement update portion of the Kalman filter algorithm. The constant system is modified to incorporate dynamics associated with a linear time-invariant system. This model is incorporated into the cost function structure used for the constant system allowing the same minimization approach to be used. This final least-squares step results in a linear discrete estimation algorithm form. With the form of an optimal linear estimator established, the algorithm is derived again by establishing a minimum variance linear estimator. The linear estimator form is assumed; however, in this derivation, the system model is expanded to include random dynamic model error. The resulting estimator is the linear discrete Kalman filter algorithm. The two approaches, least-squares and minimum variance estimators, result in the same algorithm form. The standard Kalman filter algorithm has been shown to exhibit poor numerical accuracy for ill-conditioned measurements. This problem is mitigated with the U –D factored form developed by Bierman [3]. This form of the algorithm is developed for measurement update equations. The U – D measurement update algorithm has been applied by itself in current integrated navigation systems, not accompanied with the time update portion of the U – D factored algorithm. Additional topics are presented next. In an attempt to reduce the effect of noisy measurements, some system integrators sum measurements prior to their being 181

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

182

INTEGRATED NAVIGATION SYSTEMS

processed in the Kalman filter algorithm. Modifications to the algorithm modifications to incorporate this pre-processing stage are presented. If outputs from more than one Kalman filter are available, providing estimates of the same or linearly related variables, then individual filters’ estimates can be optimally combined. A method for combining filter estimates is presented. Recent advances in nonlinear estimation have yielded a filtering methodology that does not require the system and measurement linearizations that make up much of this book. Two of these algorithms, unscented Kalman filter (UKF) and divided difference Kalman filter (DDKF), are presented in the final section of this chapter. Problems that expand upon the material presented are included. 8.1 8.1.1

Recursive Weighted Least Squares: Constant Systems Weighted Least Squares

The least-squares estimation algorithm for constant nondynamic systems was presented in Example 2.3. In Problem 2.4, this algorithm was extended by modifying the cost function to include a symmetric weighting matrix W forming the WLS cost function as JWLS ¼ (z  Hx)T W(z  Hx) Minimizing this cost function yielded the estimator for the vector x  1 xˆ ¼ H T WH H T Wz

(8:1)

(8:2)

If the measurement errors are zero mean random processes, such that E½v ; 0 and E ½vvT  ; R, then the estimation error covariance becomes, replacing the weighting matrix W with the inverse of the measurement error covariance matrix R1 ,    1 E ðx  xˆ Þðx  xˆ ÞT ; P ¼ H T R1 H 8.1.2

Recursive Weighted Least Squares

If additional data are to be processed, it is desirable to not recompute the estimate using the estimator in Eq. (8.2). It is more efficient to use previously computed estimates in an estimator form that can incorporate an addition data point. To accomplish this, a recursive form of the estimator is to be developed. Consider again the measurement model from Example 2.3. z ¼ Hx þ v Expanding this equation in component form yields 2

3 2 z1 h11 6 z2 7 6 h21 6 7 6 6 .. 7 ¼ 6 .. 4 . 5 4 . hm1 zm

h12 h22 .. . hm2

32 3 2 3 x1    h1n v1 6 x2 7 6 v2 7    h2n 7 76 7 6 7 6 . 7þ6 . 7 .. 7 .. . 54 .. 5 4 .. 5 . xn vm    hmn

(8:3)

KALMAN FILTERING

183

Consider the estimate in Eq. (8.2), as based on m data points or measurements and designating this estimate as xˆ m . With a single additional measurement zmþ1 , the measurement vector is modified to include this measurement as 



z



zmþ1

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

   H v x þ vmþ1 hT

¼

(8:4)

The new estimate of x, based on incorporating a single new data point, is specified to be of the form xˆ mþ1 ; xˆ m þ Dx

(8:5)

where the correction Dx to the current estimate is to be obtained from the next single measurement processed. The cost function in Eq. (8.1) is modified again as  JRWLS ¼



z



 T  1 H R x mþ1 0T hT



zmþ1

0



r 1

z

zmþ1



 

 H x (8:6) mþ1 hT

In partitioning the inverse of the measurement error covariance matrix with zeros in the last row and column, except for the additional measurement’s error variance, it is assumed that the error associated with the additional measurement is independent (uncorrelated) with previous measurement errors. (See later discussions on uncorrelated errors and Table 8.1.) For notational convenience, the following temporary definitions are used: 

j¼



z

zmþ1



,

 H H¼ T , h



z ¼ xmþ1

and

R L¼ 0

0 r



In terms of these temporary variable definitions, the cost function in Eq. (8.6) is rewritten as JRWLS ¼ ðj  Hz ÞT Lðj  Hz Þ In this form, earlier minimizations give the form of the estimator without the need to take derivatives. The result is  1 zˆ ¼ HT L1 H HL1 j Returning to the original definitions allows this estimate to be written as

zˆ ¼



H hT

T 

R1 0T

0 r 1



H hT

!1 

H hT

T 

R1 0T

0 r 1



z zmþ1



184

INTEGRATED NAVIGATION SYSTEMS Table 8.1

Assumptions

Linear discrete Kalman filtering equations h i xkþ1 ¼ Fkþ1,k xk þ Gk wk E wi wTj ¼ ½0 8i = j h i E wi wTj ¼ Qk 8i ¼ j ¼ k h i E vi vTj ¼ ½0 8i = j zkþ1 ¼ Hkþ1 xkþ1 þ vkþ1 h i E vi vTj ¼ Rk 8i ¼ j ¼ k   E½xðt ¼ 0Þ ¼ xˆ 0 E ðx0  xˆ 0 Þðx0  xˆ 0 ÞT ¼ P0 h i {considered here} E wi vTj ¼ ½0 8i, j

Propagate stage

x˜ kþ1 ¼ Fkþ1,k xˆ k

System model

Measurement model

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Initial conditions

——

Propagate covariance P~ kþ1 ¼ Fkþ1,k P^ k FTkþ1,k þ Gk Qk GTk

——

Measurement update

——

xˆ kþ1 ¼ x˜ kþ1 þ Kkþ1  ½zkþ1  Hkþ1 x˜ kþ1  P^ kþ1 ¼ ½I  Kkþ1 Hkþ1 P~ kþ1 T Kkþ1 ¼ P~ kþ1 Hkþ1  (Hkþ1 P~ kþ1 HTkþ1 þ Rkþ1 )1

or, in expanded form,  1  T 1  zˆ ¼ H T R1 H þ hr1 hT H R z þ hr 1 zmþ1

(8:7)

Before proceeding with the estimator development, a recursive form for the error covariance matrix, the uncertainty in the estimate, is developed in the following. Recall from Problem 2.4 that the error covariance matrix, processing m data points, resulting from the WLS estimator is T 1 P1 m ¼ H R H

then using the temporary variable definitions and expanding terms yield T 1 P1 mþ1 ¼ H L H

¼ H T R1 H þ hr 1 hT 1 T ; P1 m þ hr h

(8:8)

The matrix inversion lemma given in Example 2.2 is used to express the error covariance matrix as itself rather than its inverse. Using this lemma, the following

KALMAN FILTERING

185

form for the error covariance matrix is obtained:  1 Pmþ1 ¼ Pm  Pm h hT Pm h þ r hT Pm

(8:9)

or grouping terms in this equation that reoccur later and writing it as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

  Pmþ1 ¼ I  k hT Pm

(8:10)

where the grouped terms form the vector k as  1 k m ¼ P m h hT P m h þ r

(8:11)

Returning to the estimator in Eq. (8.7), first using the expression for Pmþ1 in the second line in Eq. (8.8) and then Eq. (8.9), the estimate is written as   xˆ mþ1 ¼ Pmþ1 H T R1 z þ hr 1 zmþ1 ¼ Pm H T R1 z þ Pm hr 1 zmþ1    1  Pm h hT P h þ r hT Pm H T R1 z þ hr 1 zmþ1 Recognizing terms in this expression as the estimate from the preceding step,  1 xˆ m ¼ Pm H T R1 z ¼ H T R1 H H T R1 z After grouping terms, this equation can be rewritten as h i   1 xˆ mþ1 ¼ xˆ m þ Pm  Pm h hT Pm h þ r hT Pm hr 1 zmþ1  1  Pm h hT P h þ r hT xˆ m Factoring Pm from the left, this equation becomes h i   1 xˆ mþ1 ¼ xˆ m þ Pm I  h hT Pm h þ r hT Pm hr 1 zmþ1  1  Pm h hT P h þ r hT xˆ m Multiplying the square bracket term from the right by h and factoring h out of this bracket from the left yields h i  1 xˆ mþ1 ¼ xˆ m þ Pm h 1  hT Pm h þ r hT Pm h r 1 zmþ1  1  Pm h hT P h þ r hT xˆ m

186

INTEGRATED NAVIGATION SYSTEMS

The terms in the square brackets become scalars and are simplified as h i  1 1  hT P m h þ r h T P m h ¼ 1 

hT Pm h r ¼ T h Pm h þ r h P m h þ r T

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Using this result, the estimate equation is rewritten as  1  1 xˆ mþ1 ¼ xˆ m þ Pm h hT Pm h þ r zmþ1  Pm h hT P h þ r hT xˆ m Using the definition of the gain km from Eq. (8.11), the estimate equation becomes   (8:12) xˆ mþ1 ¼ xˆ m þ km zmþ1  hT xˆ m Equations (8.10 –8.12) form the recursive weighted least-squares (RWLS) estimator and are functionally similar to the measurement update equations of the Kalman filter algorithm. This development has assumed a constant nondynamic state vector x. These equations were obtained for a single new measurement update. This form also holds for vector updates. For the vector update case, the measurement zmþ1 and vector h become zmþ1 and Hmþ1 , respectively. The equations also are valid for measurement step depend variances, that is, rm, and forms of the measurement vector represented by hm . Example 8.1

Data Trends

If the state vector is a scalar constant, the covariance update equation (8.9) can be written as   pm h2 pmþ1 ¼ 1  pm ð pm h2 þ r Þ Because both the measurement variance and the error variance are positive, the quantity in the bracket is less than one, and the new error variance is always less than the previous variance. This leads to the continual reduction of the estimation error with each new measurement until the error variance is reduced to an arbitrarily small value 0. This would reflect improved knowledge of the state’s value by its estimate. With small error variance, the gain k would also be small. New values of the state produced from Eq. (8.12) would change little from previous values. This is related to the divergence problem in some applications of Kalman filtering. In this example, suppose that the measurement variance is very large. This corresponds to large measurement error magnitudes compared to the states to be estimated. From the preceding equation, it is seen that the updated error variance would also be changed little from the earlier value. Because the gain k can also be expressed as (see Problem 8.1) km ¼ Pmþ1 hr 1

KALMAN FILTERING

187

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

the correction of the prior estimate value would also be very small. New values from the state update in Eq. (8.12) would be unchanged. Therefore, for the case of large measurement variances, the estimation error and state would remain unchanged from their initial values.

8.2 Recursive Weighted Least Squares: Dynamic Systems A dynamic system is in general nonlinear as discussed in Chapter 2. This system can also be driven or disturbed by additive error, which in most applications in navigation systems is the result of unmodeled dynamical elements. The applications of Kalman filtering in integrated navigation systems assume a linearized form of state dynamics. This linearized form is used next in the development of a recursive weighted least-squares estimator for a dynamic system. At this point, it is assumed that there are no additive errors to the dynamics. This approach will continue the preceding process by using the same cost function structure. Using this approach, the dynamics are incorporated into the measurement matrix resulting in a time-dependent measurement, and, in effect, a constant state vector as assumed in the preceding. The linear state dynamics, with no additive disturbances, is represented by the following discrete-time equation: xiþ1 ¼ Fiþ1,i xi

(8:13)

where the state is propagated in time, from the previous time instant i to the next instant i þ 1, by the state transition matrix Fiþ1,i . The state is observed by measurements at the instant i by zi ¼ Hi xi þ vi

(8:14)

where the allowance for time dependence of the measurement, represented by the Hi matrix, and measurement error vi , are indicated. A sequence of measurements, up to the kth time instant, is related to the state vector at the kth time instant by incorporating into the measurement matrix at each time instant the state transition matrix to propagate the state from that time instant to the kth time instant 2

3 2 3 2 3 H1 F1,k v1 z1 6 z2 7 6 H2 F2,k 7 6 v2 7 6 7 6 7 6 7 6 .. 7 ¼ 6 . 7xk þ 6 .. 7 4 . 5 4 .. 5 4 . 5 zk vk Hk Fk,k

(8:15)

Writing this in a composite form as

jk ¼ Hk xk þ vk

(8:16)

Recalling the WLS cost function [see Eq. (8.1)]  T   JWLS ¼ jk  Hk x L1 k jk  H k x

(8:17)

188

INTEGRATED NAVIGATION SYSTEMS

and minimizing with respect to xk [see Eq. (8.2)] yield  1 T 1 Hk Lk jk xˆ k ¼ HTk L1 k Hk

(8:18)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

As with RWLS for constant systems, consider an additional vector measurement at time instant k þ 1: zkþ1 ¼ Hkþ1 xkþ1 þ vkþ1

(8:19)

The measurement represented by Eq. (8.16) is modified to include this additional vector measurement at time instant k þ 1 as 



jk

zkþ1

¼

    Hk Fk,kþ1 vk xkþ1 þ Hkþ1 vkþ1

(8:20)

The modified cost function becomes 

  T Hk Fk,kþ1 xkþ1 zkþ1 Hkþ1 " #    L1 ½ 0 jk Hk Fk,kþ1 k   x kþ1 zkþ1 Hkþ1 ½0 R1 kþ1  T   ¼ jk  Hk Fk,kþ1 xkþ1 L1 k jk  Hk Fk,kþ1 xkþ1

JRLWS ¼

jk





þ ðzkþ1  Hkþ1 xkþ1 ÞT R1 kþ1 ðzkþ1  H kþ1 xkþ1 Þ

(8:21)

The new estimate of x, based on incorporating the new vector measurement, is specified to be the sum of the propagated earlier time-step estimate and an incremental change as a result of the new vector measurement. This is expressed as xˆ kþ1 ¼ Fkþ1,k xˆ k þ D xˆ kþ1 which implies Fk,kþ1 xˆ kþ1 ¼ xˆ k þ Fk,kþ1 D xˆ kþ1

(8:22)

KALMAN FILTERING

189

Using these relationships, the cost function in Eq. (8.21) becomes (momentarily dropping the ^)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

  T    JRLWS ¼ jk  Hk xk þ Fk,kþ1 D xkþ1 L1 k jk  Hk xk þ Fk,kþ1 D xkþ1   T þ zkþ1  Hkþ1 Fkþ1,k xk þ D xkþ1     R1 kþ1 zkþ1  Hkþ1 Fkþ1,k xk þ D xkþ1 T T 1 T T 1 T T 1 ¼ jTk L1 k jk  xk Hk Lk jk  D xkþ1 Fk,kþ1 Hk Lk jk  jk Lk Hk xk T T 1  jTk L1 k Hk Fk,kþ1 D xkþ1 þ xk Hk Lk Hk xk T T T 1 þ xTk HTk L1 k Hk Fk,kþ1 D xkþ1 þ D xkþ1 Fk,kþ1 Hk Lk Hk xk

þ D xTkþ1 FTk,kþ1 HTk L1 k Hk Fk,kþ1 D xkþ1   T þ zkþ1  H kþ1 Fkþ1,k xk þ D xkþ1     R1 kþ1 zkþ1  Hkþ1 Fkþ1,k xk þ D xkþ1

(8:23)

From Eq. (8.18) T 1 xTk HTk L1 k Hk ¼ jk Lk Hk

(8:24)

Using Eq. (8.24), several terms in Eq. (8.23) cancel, leaving JRLWS ¼ DxTkþ1 FTk,kþ1 HTk L1 k Hk Fk,kþ1 D xkþ1   T þ zkþ1  H kþ1 Fkþ1,k xk þ D xkþ1     R1 kþ1 zkþ1  Hkþ1 Fkþ1,k xk þ D xkþ1

(8:25)

Minimizing JRWLS with respect to D xkþ1 yields  1 T 1 D xˆ kþ1 ¼ FTk,kþ1 HTk L1 k Hk Fk,kþ1 þ Hkþ1 Rkþ1 Hkþ1   T ˆk  Hkþ1 R1 kþ1 zkþ1  Hkþ1 Fkþ1,k x

(8:26)

As before, defining T 1 P1 k ; Hk Lk Hk

(8:27)

T 1 T 1 P1 kþ1 ¼ Fk,kþ1 Pk Fkþ1,k þ Hkþ1 Rkþ1 Hkþ1

(8:28)

and

190

INTEGRATED NAVIGATION SYSTEMS

allows the incremental state vector estimate, given in Eq. (8.26), to be written as   T ˆk R1 D xˆ kþ1 ¼ Pkþ1 Hkþ1 kþ1 zkþ1  Hkþ1 Fkþ1,k x

(8:29)

Substituting this equation into Eq. (8.22) yields for the state vector estimate at time instant k þ 1

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

  T ˆk xˆ kþ1 ¼ Fkþ1,k xˆ k þ Pkþ1 Hkþ1 R1 kþ1 zkþ1  Hkþ1 Fkþ1,k x

(8:30)

Finally, using the matrix inversion lemma for Eq. (8.28) yields the update for the error covariance matrix Pkþ1 ¼ Fkþ1,k Pk FTkþ1,k  1 T T  Fkþ1,k Pk FTkþ1,k Hkþ1 Hkþ1 Fkþ1,k Pk FTkþ1,k Hkþ1 þ Rkþ1  Hkþ1 Fkþ1,k Pk FTkþ1,k

(8:31)

This equation can be simplified by adopting the nomenclature of the caret as the estimate after a measurement update and the tilde as the estimate after a time update as P~ kþ1 ; Fkþ1,k P^ k FTkþ1,k

(8:32)

then Eq. (8.31) becomes  1 T T P^ kþ1 ¼ P~ kþ1  P~ kþ1 Hkþ1 Hkþ1 P~ kþ1 Hkþ1 þ Rkþ1 Hkþ1 P~ kþ1

(8:33)

The model of the process given in Eq. (8.13) assumes no disturbing noise in state dynamics. If system dynamics are governed by the following discrete-time equation xkþ1 ¼ Fkþ1,k xk þ Gk wk

(8:34)

the resulting estimation algorithm is summarized in Table 8.1. Equations in this table are the standard Kalman filter form for discrete-time measurement update form. The filter algorithm processing flow is illustrated in Fig. 8.1. Beginning at some prior time instant, that is, k 2 5, the state vector and covariance matrix are propagated in time (time update—open arrow) to the k 2 4 time instant, at which measurements are available for use in updating the state and covariance matrix. At this k 2 4 time instant, one or more measurements can be processed by the state vector and covariance matrix measurement update equations (measurement update—vertical arrow). With the measurement update completed, the state vector and covariance matrix are again propagated in time (time update—open arrow) to the k 2 3 time, at which measurements are

KALMAN FILTERING

191

–time update –measurement update

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

event sequence

tk–4

Fig. 8.1

tk–3 tk–2

tk–1

tk

tk+1

Kalman filter algorithm processing flow.

available for updating the state and covariance matrix. This alternating event sequence of time and measurement updates is repeated until stopped. Figure 8.1 shows irregular time intervals and variable numbers of measurements allowed by the algorithm. Examining the equations in Table 8.1, certain observations are useful. If the system model, measurement model, and disturbance error characteristics are linear and constant with time, then the covariance matrix does not depend on the states. For this constant system, the covariance matrix propagation and updates can be performed offline. Once computed, the gain matrix K can be stored as a function of time within the onboard computer. This approach is termed the scheduled gain filter implementation. As such, it is not a dynamic Kalman filter but a recursive estimator. Applications of this approach have been used for inertial navigation system ground alignment and integrated navigation systems for quasi-steady dynamic conditions.

8.3

Discrete Linear Minimum Variance Estimator

Linear discrete Kalman filter equations are derived in the following by a different approach: that of minimizing the estimation error variance. The objective of the derivation is to obtain a linear unbiased estimator for the state vector x. Consider again the dynamical process described by Eq. (8.34): xkþ1 ¼ Fkþ1,k xk þ Gk wk where the noise wk is a zero-mean uncorrelated process. A conditional expectation is considered as an operator whose result is based on whether the object of the operator, that is, 1k has zero mean or is correlated. Assume the estimate (expectation) of this process can be propagated in time by x˜ kþ1 ; Fkþ1,k xˆ k

(8:35)

192

INTEGRATED NAVIGATION SYSTEMS

The state estimate’s error, from the measurement update, is defined as

1ˆ k ¼ xˆ k  xk

(8:36)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

The error at the next time step k þ 1 is obtained from these equations as

1˜ kþ1 ¼ x˜ kþ1  xkþ1   ¼ Fkþ1,k xˆ k  Fkþ1,k xk þ Gk wk ¼ Fkþ1,k 1ˆ k  Gk wk

(8:37)

The estimate/expectation of this error is E½1˜ kþ1  ¼ E½x˜ kþ1  xkþ1  ¼ Fkþ1,k E½1ˆ k   Gk E½wk  ¼0

(8:38)

where it is assumed E½1ˆ k  ¼ 0

(8:39)

E½wk  ¼ 0

(8:40)

and

That is, if initially unbiased, the linear system’s estimate remains unbiased. The covariance of the estimation error at the next time step k þ 1 is   P~ kþ1 ; E 1˜ kþ1 1˜ Tkþ1 h  T i ¼ E Fkþ1,k 1ˆ k  Gk wk Fkþ1,k 1ˆ k  Gk wk     ¼ Fkþ1,k E 1ˆ k 1ˆ Tk FTkþ1,k  Gk E wk 1ˆ Tk FTkþ1,k      Fkþ1,k E 1ˆ k wTk GTk þ Gk E wk wTk GTk

(8:41)

P~ kþ1 ¼ Fkþ1,k P^ k FTkþ1,k þ Gk Qk GTk

(8:42)

or

KALMAN FILTERING

193

The two middle terms in Eq. (8.41) are assumed zero because h    T i E wk 1ˆ Tk ¼ E wk Fk,k1 1ˆ k1  Gk1 wk1     ¼ E wk 1ˆ Tk1 FTk,k1  E wk wTk1 GTk

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¼ ½0

(8:43)

The first term in this equation is zero because the noise w at the current time step k is independent of the error 1ˆ at the preceding time step k 2 1 (see Problem 8.5), and the second term is zero because it is assumed that the process noise is uncorrelated (see Table 8.1). Equations (8.35) and (8.42) define the estimate and the error in the estimate, both propagated in time. With insight gained from the preceding section, assume a linear form for the state estimator given by xˆ kþ1 ¼ x˜ k þ Kkþ1 ½zkþ1  Hkþ1 x˜ k 

(8:44)

and, from the definition of the estimate error in Eq. (8.36), and rewritten as xˆ k ¼ xk þ 1ˆ k

(8:45)

Equation (8.44) can be rewritten as   xkþ1 þ 1ˆ kþ1 ¼ Fkþ1,k ðxk þ 1ˆ k Þ þ Kkþ1 zkþ1  Hkþ1 Fkþ1,k ðxk þ 1ˆ k Þ ¼ Fkþ1,k xk þ Fkþ1,k 1ˆ k     þ Kkþ1 Hkþ1 xkþ1  Fkþ1,k xk þ vkþ1  Hkþ1 Fkþ1,k 1ˆ k (8:46) Using Eq. (8.34), this equation becomes

1ˆ kþ1 ¼ ½I  Kkþ1 Hkþ1 Fkþ1,k 1ˆ k  ½I  Kkþ1 Hkþ1 Gk wk þ Kkþ1 vkþ1

(8:47)

The corresponding estimation error covariance matrix is   P^ kþ1 ; E 1ˆ kþ1 1ˆ Tkþ1 ¼ ½I  Kkþ1 Hkþ1 Fkþ1,k P^ k FTkþ1,k ½I  Kkþ1 Hkþ1 T

(8:48)

T þ ½I  Kkþ1 Hkþ1 Gk Qk GTk ½I  Kkþ1 Hkþ1 T þ Kkþ1 Rkþ1 Kkþ1

Using Eq. (8.42), the first and second terms are combined, resulting in T P^ kþ1 ¼ ½I  Kkþ1 Hkþ1 P~ kþ1 ½I  Kkþ1 Hkþ1 T þ Kkþ1 Rkþ1 Kkþ1

(8:49)

Thus far in this development there have been no assumptions concerning the gain matrix Kkþ1 , other than its use in the linear estimator given in Eq. (8.44).

194

INTEGRATED NAVIGATION SYSTEMS

Therefore, it can be assumed that Eq. (8.49) is true for any gain matrix Kkþ1 . This form, also known as Joseph’s form, has been used in covariance simulations to evaluate reduced state filter implementations with truth models that include more errors than the reduced state Kalman filter implementation. The gain matrix Kkþ1 that minimizes Eq. (8.49) yields a minimum variance estimator. Taking the variance of this equation and setting equal to the null matrix yields for the optimal gain

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

 1 T T Kkþ1 ¼ P~ kþ1 Hkþ1 Hkþ1 P~ kþ1 Hkþ1 þ Rkþ1

(8:50)

This gain is the same as derived earlier using the recursive WLS approach. When this gain matrix is substituted into Eq. (8.49), the earlier form of the error covariance matrix measurement update equation is obtained.

Example 8.2

From Joseph’s Form [Eq. (8.49)] to Standard Kalman Form Rewrite Eqs. (8.49) and (8.50) without subscripts: P^ ¼ ½I  KH P~ ½I  KH T þ KRK T and   ~ T H PH ~ T þ R 1 K ¼ PH Substituting the second equation into the first, and expanding the measurement update for the error covariance becomes ~ T K T þ KRK T ~ T K T þ KH PH P^ ¼ P~  KH P~  PH   ~ T H PH ~ T þ R 1 H P~  PH ~ T ð Þ1 H P~ ¼ P~  PH ~ T ð Þ1 H PH ~ T ð Þ1 H P~ þ PH ~ T ð Þ1 Rð Þ1 H P~ þ PH ~ T ð Þ1 H P~  PH ~ T ð Þ1 H P~ þ PH ~ T ð Þ1 ð Þð Þ1 H P~ ¼ P~  PH ~ T ð Þ1 H P~ þ PH ~ T ð Þ1 H P~ ~ T ð Þ1 H P~  PH ¼ P~  PH   ~ T H PH ~ T þ R 1 H P~ ¼ P~  PH ¼ ½I  KH P~

8.4

U –D Factored Form

Equations summarized in Table 8.1 represent one of many forms of the Kalman filtering algorithm. This algorithm has been modified into an U –D

KALMAN FILTERING

195

factored form by Bierman [3]. The U – D algorithm provides enhanced numerical precision, reduced storage requirements, and a reduced number of numerical operations. This algorithm processes measurements sequentially (single scalar measurements), thus eliminating the need for matrix inversions. In the following, the nomenclature associated with this algorithm is presented, and, in an example, the form of the algorithm presented by Bierman is developed. The P matrix is factored as (see Example 2.1)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

P ¼ UDU T

(8:51)

where the U matrix is a upper triangular matrix with unity diagonal values and zeros below the diagonal. The D matrix is a diagonal matrix with zeros off the diagonal.

8.4.1

Measurement Update

The P matrix, from Table 8.1, can be rewritten for a scalar update as 1 ~ T~ P^ ¼ P~  Phh P a

(8:52)

where, for the scalar update, the measurement residual variance is defined as ~ þr a ¼ hT Ph

(8:53)

Expressing the covariance matrix measurement update in this factored form, ~ U~ T P~ ¼ U~ D

(8:54)

Then, substituting Eq. (8.54) into Eq. (8.52) yields ~ U~ T  1 U~ D ~ U~ T hhT U~ D ~ U~ T P^ ¼ U~ D a   ~ U~ T hhT U~ D ~ U~ T ~ 1D ¼ U~ D a

(8:55)

defining the repeated terms in this equation as T f ; U~ h

(8:56)

~ g ; Df

(8:57)

and

Then, Eq. (8.55) can then be rewritten as   ~  1 ggT U~ T P^ ¼ U~ D a

(8:58)

196

INTEGRATED NAVIGATION SYSTEMS

The gain K in Table 8.1 can be written in terms of the factors as 1~ Ph a 1 ~ ~T U h ¼ U~ D a

k¼

(8:59)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

or, using the definitions in Eqs. (8.56) and (8.57), k¼

1 ~ Ug a

(8:60)

Using the preceding notation, the U –D measurement update algorithm is presented next. It is assumed that the measurement variance is nonsingular, that is, a = 0. Initially, form the vectors f and g, as given in Eqs. (8.56) and (8.57). f ¼) fi , i ¼ 1, 2, . . . , n and g ¼) gi , i ¼ 1, 2, . . . , n ~ 1 f1 g1 ¼ D a 1 ; r þ f 1 g1 ^1 ¼ r D ~1 D a1 for j ¼ 2, . . . , n

aj ¼ a j1 þ fj gj ^ j ¼ a j1 D ~j D aj lj ¼ 

fj a j1

for i ¼ 1, . . . , j  1 U^ ij ¼ U~ ij þ gi lj gi ¼ gi1 þ gj U~ ij gi ki ¼ a The algorithm’s initialization is accomplished from the initial factorization of the initial covariance matrix P0, as in Example 2.1. The estimation covariance matrix P can be reformed from the updated U – D factors, as given in Eq. (8.54). 8.4.2 Time Update This section introduces the nomenclature of the U – D factored form’s time update. The P matrix time update (propagation) from Table 8.1 can be rewritten as ^ T þ GQGT P~ ¼ FPF

(8:61)

KALMAN FILTERING

197

or ^ U^ T FT þ GQGT P~ ¼ FU^ D # " #" ^ ½0   D U^ T FT r ¼ FU^ G GT ½ 0 Q

(8:62)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Identifying the matrices  W ¼ FU^



G

(8:63)

and 

^ D D¼ ½ 0

½0 Q

 (8:64)

the covariance time update can be expressed as P~ ¼ WDW T

(8:65)

The covariance time update is accomplished by applying Gram – Schmidt orthogonalization to determine the row vectors of the matrix W. The algorithm for the time update can be found in Bierman [3]. In many applications, the U – D factor time update is not used. After its initialization and propagation to the measurement time using Eq. (8.61), the error covariance P matrix is factored for the measurement update. After completing the measurement update in factored form, the factors are recombined into the P matrix and again propagated to the next measurement time, whereupon the cycle is repeated. Example 8.3

U –D Measurement Update Algorithm

The U –D measurement update algorithm is developed by illustration, using the notation as just defined, for the 3  3 example problem presented in Example 2.1. The terms within the brackets in Eq. (8.58) become 2 3 1 1 1 d 1  g1 g1  g1 g 2  g1 g3 7 a a a   6 6 7 1 1 1 1 6 7 ~  ggT ¼ 6  g2 g1 D d2  g2 g2  g2 g3 7 6 7 a a a a 4 5 1 1 1  g3 g1  g3 g 2 d3  g3 g 3 a a a The product of this matrix with the U factors, as shown in Eq. (8.58), yields the updated factors. Equating the measurement update form of the U –D factors with (8.58) yields   ~  1 ggT U~ T ^ U^ T ¼ U~ D U^ D a

198

INTEGRATED NAVIGATION SYSTEMS

Equating the corresponding elements in the bottom rows on the left- and righthand sides in this equation yields the following equations:

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

d^3 u^ 13

1 d^3 ¼ d~3  g3 g3 a 1 1 d^3 u^ 23 ¼  g3 g2 þ d~3  g3 g3 u~ 23 a a  1 1 1 ¼  g3 g1  g3 g2 u~ 12 þ d~3  g3 g3 u~ 13 a a a

The nomenclature defined earlier allows the expression for the measurement covariance as

a ¼ fTg þ r or

aj ¼

j X

fi gi þ r

i¼1

where the subscripted a is an incremental sum and a0 ¼ r. Using the definitions in Eqs. (8.56) and (8.57), the first of the preceding equations becomes n o g3 ¼ d~3 f3

1 d^3 ¼ d~3  g3 g3 a d~3 ¼ d~3  f3 g3 a  a  f3 g3 ¼ d~3 a a 2 ¼ d~3 a



a ¼ a3 ¼ r þ f1 g1 þ f2 g2 þ f3 g3

Continuing with the other two equations and using the result just obtained:  1 1 d^3 u^ 23 ¼  g3 g2 þ d~3  g3 g3 u~ 23 a a ( ) ~3 1 a d ¼  g3 g2 þ d^ 3 u~ 23 ¼ a a2 d^3 or, u^ 23 ¼ u~ 23 

f3 g2 a2



KALMAN FILTERING And

199

 1 1 1 d^3 u^ 13 ¼  g3 g1  g3 g2 u~ 12 þ d~3  g2 g3 u~ 13 a a a 1 ¼  g3 ðg1 þ g2 u~ 12 Þ þ d^3 u~ 13 a

or Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

u^ 13 ¼ u~ 13 

f3 ðg1 þ g2 u~ 12 Þ a2

The 2:2 element of the resulting product in the preceding second equation is  1 1 d^2 þ u^ 23 d^3 u^ 23 ¼ d~2  g2 g2  g2 g3 u~ 23 a a  1 1 ~  g2 g3 u~ 23 þ d3  g3 g3 u~ 223 a a  1 2 ¼ d~2  g2 g2  g2 g3 u~ 23 þ u~ 23 d^3 u~ 23 a a The second term on the left-hand side (LHS) of this equation, using the results obtained before, becomes u^ 23 d^3 u^ 23 ¼ u~ 23 d^3 u~ 23 

2 ðg3 g2 =aÞðg3 g2 =aÞ g2 g3 u~ 23 þ a d^3

Canceling applicable terms, the following equation is obtained:  n o 1 ðg2 g3 =aÞðg2 g3 =aÞ g2 ¼ d~2 f2 d^2 ¼ d~2  g2 g2  a d^3



 d~2 f2 g2 =a d~3 f3 g3 =a a  f g 2 2  ¼ d~2 a d^3   

 f2 g2 1 a  f2 g2 ¼ a1 þ f3 g3 ¼ d~2 a  f2 g2  f3 g3 a2 a ¼ d~2 ½ða1 þ f2 g2 Þða1 þ f3 g3 Þ  f2 g2 f3 g3 

a1 ¼ d~2 ða1 þ f2 g2 þ f3 g3 Þ a2 a a 1 ¼ d~2 a2 By induction, a j1 d^j ¼ d~j aj

1 a2 a

a ¼ a1 þ f2 g2 þ f3 g3



200

INTEGRATED NAVIGATION SYSTEMS

Finally, the 1:2 element of the resulting product in the preceding second equation is

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

 1 1 1 d^2 u^ 12 þ u^ 23 d^3 u^ 13 ¼  g2 g1 þ d~2  g2 g2 u~ 12  g2 g3 u~ 13 a a a    1 1 1 ~ þ  g3 g1  g3 g2 u~ 12 þ d 3  g3 g3 u~ 13 u~ 23 a a a The second term on the LHS is 1 1 u^ 23 d^3 u^ 13 ¼ u~ 23 d^3 u~ 13  g3 g2 u~ 13  g3 g1 u~ 23 a a 1 1 2 ðg1 þ g2 u~ 12 Þ  g3 g2 u~ 12 u~ 23 þ 2 g3 g2 a a d^3 Canceling applicable terms, the following equation is obtained:  1 1 1 ðg1 þ g2 u~ 13 Þ ~ ^ d2 u^ 12 ¼  g2 g1 þ d2  g2 g2 u~ 12  2 g23 g2 a a a d^3 1 1 g2 g2 g1 ¼ d^2 u~ 12  g2 g1  2 3 a a d^3  1 1 g23 ^ ¼ d2 u~ 12  g2 g1 1 þ a a d^3 h i ^3 þ ð1=aÞg2 d ~ 3 d2 ¼ d^2 u~ 12  d~1 f1 f2 a d^3 d~2 d~3 ¼ d^2 u~ 12  d~1 f1 f2 a d^3

(

d~2 ¼ d^2 u~ 12  d~1 f1 f2 a2 ! ~2 d ¼ d^2 u~ 12  f1 f2 a1 or, u^ 12 ¼ u~ 12 

n o g1 ¼ d~1 f1

f2 g1 a1

a2 d~2 ¼ a1 d^2

)

KALMAN FILTERING

201

The results in this example are summarized here:

a0 d^1 ¼ d~1 a1 a1 d^2 ¼ d~2 a2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

u^ 12 ¼ u~ 12 

f2 g1 a1

a2 d^3 ¼ d~3 a3 u^ 13 ¼ u~ 13 

f3 ðg1 þ g2 u~ 12 Þ a2

u^ 23 ¼ u~ 23 

f3 g2 a2

8.5 Summed Measurements It was illustrated in Problem 2.1 that data averaging can improve the estimate of a constant quantity by reducing the estimation error covariance. This serves as motivation to consider using this method to reduce errors in noisy measurements; thus, this concept is extended in this section to allow for time-varying states. This concept is also useful in changing a measurement’s state relationship, for example, using change in position over a time interval as measurements, which is then related to an accumulation of velocity error states over that same interval. Both of these concepts can be formulated into a common structure of summed measurements. Developments that follow address the use of summed measurements over a given time interval prior to their being processed in the Kalman filter measurement update equation. It is shown that the use of summed measurements requires modification to the algorithm’s measurement matrix and measurement covariances. The time dependence of measurements is indicated next by using the subscript k: zk ¼ Hk xk þ vk Summing m of these measurements in between measurement update cycles yields m X k¼1

zk D ¼

m X k¼1

H k xk D þ

m X

vk D

(8:66)

k¼1

where a temporary time-independent scalar quantity D has been multiplied to each of the terms in the measurement equation. The definition of this scalar

202

INTEGRATED NAVIGATION SYSTEMS

quantity will be specified later depending on which of the preceding two cases is to be illustrated. It is assumed that the measurement matrix Hk does not have significant time dependence during this summation process. The first term on the RHS of Eq. (8.66) is rewritten as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

X

Hk xk D ¼ HD

X

xk

¼ HD½xðt1 Þ þ xðt2 Þ þ    þ xðtm Þ ¼ HD½Fðt1 , t0 Þ xðt0 Þ þ Fðt2 , t0 Þ xðt0 Þ þ    þ Fðtm , t0 Þ xðt0 Þ ¼ HD½Fðt1 , t0 Þ þ Fðt2 , t0 Þ þ    þ Fðtm , t0 Þ xðt0 Þ

(8:67)

where xðt0 Þ is the state vector prior to beginning of the summation interval. The system model dynamics are assumed to be time invariant, and the time intervals are equal length such that the preceding state transition matrices can be expressed as

Fðt1 , t0 Þ ¼ eFDt ; Dt ¼ t1  t0 Fðt2 , t0 Þ ¼ e2FDt ; 2Dt ¼ t2  t0 Fðtm , t0 Þ ¼ emFDt ; mDt ¼ tm  t0

Substituting these expressions into the preceding equation yields X

  Hk xk D ¼ HD eFDt þ e2FDt þ    þ emFDt xðt0 Þ

The terms in the brackets in this equation represent a geometric series whose ratio of the current term to the preceding term is eFDt . Rewriting this equation using the series summation results in X

 1 Hk xk D ¼ HDF 1 emFDt  I xðt0 Þ Dt

(8:68)

With equal time intervals, the total time interval, referenced to the start time, used for the measurement summation can be defined as DT ¼ mDt. Assuming that the exponential in Eq. (8.68) can be approximated by a truncated series with three

KALMAN FILTERING

203

terms, this equation is rewritten as

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

X

8.5.1

  DT 1 þ   I xðt0 Þ I þ FDT þ F 2 2! Dt   2 1 1 2 DT xðt0 Þ FDT þ F  HDF 2! Dt   DT 2 1 xðt0 Þ  HD DT þ F 2! Dt   DT DT  HD I þ F xð t 0 Þ 2! Dt   DT  HDm I þ F xðt0 Þ 2!

Hk xk D ¼ HDF 1

(8:69)

Data Averaging

Returning to Eq. (8.66), using the results in Eq. (8.69) and defining D as D¼

1 m

and redefining the start time as the current time, yields   1X DT 1X zk ¼ H I  F xðtÞ þ vk m 2! m Or defining the summations as a new measurement and equivalent noise, 

 DT jk ¼ H I  F xðtÞ þ nk 2!

(8:70)

where

jk ¼

1X zk m

(8:71)

nk ¼

1X vk m

(8:72)

and

204

INTEGRATED NAVIGATION SYSTEMS

The covariance of the measurement noise v is adjusted by Rn ¼ 8.5.2

1 Rv m

(8:73)

Numerical Integration

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Again returning to Eq. (8.66), using the results in Eq. (8.69) and now defining D as D ¼ Dt yield X

  X DT zk Dt ¼ HDtm I  F xðtÞ þ vk Dt 2!

Or defining the summations as a new measurement and equivalent noise,   DT jk ¼ HDT I  F xðtÞ þ nk 2!

(8:74)

where

jk ¼ Dt

X

zk

(8:75)

vk

(8:76)

and nk ¼ Dt

X

The covariance of the measurement noise v is adjusted by Rn ¼

DT 2 Rv m

(8:77)

8.6 Combined Estimate from Two (or More) Kalman Filters If two or more Kalman filters are providing estimates of related navigation states, not necessarily using the same state vector structure, these estimates can be combined to establish an unbiased estimate. An example of the application of this concept is when there are two navigation systems, for example, either two inertial systems or one inertial and one dead-reckoning system located on a vehicle and separate Kalman filter algorithms are implemented for each system. If the two sets of navigation states are not the same but can be related by transformation, then the concept developed in this section can be applied. This concept can also be extended for combining estimates from more than two Kalman filters.

KALMAN FILTERING

205

Define an estimate from two Kalman filters as the following weighted combination: xˆ 1=2 ; W1 xˆ 1 þ W2 xˆ 2

(8:78)

where the estimate from the ith filter is related to the true state value by

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

xˆ i ¼ x þ 1 xi

(8:79)

and where each filter’s estimate is assumed to be unbiased—E½1xi  ¼ 0. The relationship between the weights can be determined by requiring the combined estimation error to be unbiased—zero expected value. This combined estimation error is written as 1 x1=2 ¼ xˆ 1=2  x

(8:80)

and the expectation of this error becomes   E 1x1=2 ¼ E½W1 ðx þ 1 x1 Þ þ W2 ðx þ 1 x2 Þ  x ¼ W 1 E ½ x þ W 2 E ½ x  E ½ x which implies W2 ¼ I  W1

8

E ½ x = 0

(8:81)

for an unbiased estimate. The specific relationship for the preceding weights is determined by minimizing the covariance, thus establishing a minimum variance estimate. The error covariance of the combined estimate is expressed as h i P1=2 ¼ E 1 x1=2 1 xT1=2

¼ E ½W1 ðx þ 1 x1 Þ þ W2 ðx þ 1 x2 Þ  x ½W1 ðx þ 1 x1 Þ  þ W2 ðx þ 1 x2 Þ  xT     ¼ W1 E 1 x1 1 xT1 W1T þ W2 E 1 x2 1 xT2 W2T     þ W2 E 1 x2 1 xT1 W1T þ W1 E 1 x1 1 xT2 W2T þ O½Eð1 xi Þ Assuming that individual filter errors are uncorrelated, j k E 1 xi 1 xTj ¼ ½0 8

i=j

(8:82)

206

INTEGRATED NAVIGATION SYSTEMS

Then the combined covariance becomes P1=2 ¼ W1 P1 W1T þ W2 P2 W2T ¼ W1 P1 W1T þ ðI  W1 ÞP2 ðI  W1 ÞT

(8:83)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Weighting matrices are determined by taking the variation of this equation and setting the result to the null matrix. The variation with respect to the weighting matrix becomes   dP1=2 ¼ d W1 P1 W1T þ P2  W1 P2  P2 W1T þ W1 P2 W1T ¼ dW1 P1 W1T þ W1 P1 dW1T  dW1 P2  P2 dW1T þ dW1 P2 W1T þ W1 P2 dW1T   ¼ dW1 P1 W1T  P2 þ P2 W1T þ ½W1 P1  P2 þ W1 P2 dW1T setting this result to the null yields, for any dW1 = ½0 

 P1 W1T  P2 þ P2 W1T ¼ ½0 ¼ ½W1 P1  P2 þ W1 P2 

or W1 ¼ P2 ðP1 þ P2 Þ1

(8:84)

and, using the relationship between the preceding weighing matrices W2 ¼ P1 ðP1 þ P2 Þ1

(8:85)

Substituting Eqs. (8.84) and (8.85) into the combined covariance matrix equation (8.83) yields P1=2 ¼ W1 P1 W1T þ P2  W1 P2  P2 W1T þ W1 P2 W1T ¼ P2 ðP1 þ P2 Þ1 P1 ðP1 þ P2 Þ1 P2 þ P2  P2 ðP1 þ P2 Þ1 P2  P2 ðP1 þ P2 Þ1 P2 þ P2 ðP1 þ P2 Þ1 P2 ðP1 þ P2 Þ1 P2 ¼ P2 ðP1 þ P2 Þ1 ðP1 þ P2 ÞðP1 þ P2 Þ1 P2 þ P2  P2 ðP1 þ P2 Þ1 P2  P2 ðP1 þ P2 Þ1 P2 ¼ P2 ðP1 þ P2 Þ1 P2 þ P2  2P2 ðP1 þ P2 Þ1 P2   1 ¼ P2 ðP1 þ P2 Þ1 þP1 P2 2  2 ð P 1 þ P2 Þ  1  ¼ P2 P2  ðP1 þ P2 Þ1 P2 ¼ P2  P2 ðP1 þ P2 Þ1 P2

KALMAN FILTERING

207

or, using the matrix inversion lemma from Chapter 2 (see Example 2.2), the combined estimation error covariance becomes 1 1 P1 1=2 ¼ P1 þ P2

(8:86)

The combined estimate in Eq. (8.78) becomes

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

xˆ 1=2 ¼ P2 ðP1 þ P2 Þ1 xˆ 1 þ P1 ðP1 þ P2 Þ1 xˆ 2

(8:87)

This equation can be rewritten in a more compact form. Premultiplying this equation with Eq. (8.86) yields    1 ˆ 1=2 ¼ P1 P2 ðP1 þ P2 Þ1 xˆ 1 þ P1 ðP1 þ P2 Þ1 xˆ 2 P1 1=2 x 1 þ P2   1 ˆ 1 þ P1 ðP1 þ P2 Þ1 xˆ 2 ¼ P1 1 P2 ð P1 þ P 2 Þ x   1 ˆ 1 þ P1 ðP1 þ P2 Þ1 xˆ 2 þ P1 2 P2 ð P1 þ P2 Þ x 1 ˆ 1 þ ðP1 þ P2 Þ1 xˆ 2 ¼ P1 1 P2 ð P1 þ P 2 Þ x 1 ˆ2 þ ðP1 þ P2 Þ1 xˆ 1 þ P1 2 P 1 ð P 1 þ P2 Þ x  1    1 ˆ2 ¼ P1 P2 þ I ðP1 þ P2 Þ1 xˆ 1 þ P1 2 P1 þ I ðP1 þ P2 Þ x 1 1 ˆ 1 þ P1 ˆ2 ¼ P1 1 ðP2 þ P1 ÞðP1 þ P2 Þ x 2 ðP1 þ P2 ÞðP1 þ P2 Þ x

Or, ˆ 1=2 ¼ P1 ˆ 1 þ P1 ˆ2 P1 1=2 x 1 x 2 x

(8:88)

Extending this concept to more than two filters can be easily followed from the form of Eqs. (8.86) and (8.88).

8.7 Derivative-Free Estimation 8.7.1 Unscented Kalman Filter Table 8.2 summarizes the unscented Kalman filter algorithm [11 – 13]. The algorithm begins with forming additional vectors about the state vector that are displaced, or dispersed, using column vectors form by the error covariance pffi matrix P factor (see Problem 2.15). In this table, the symbol i represents the scaled i th column/row vector of the covariance matrix factor. Once established, these new vectors are instantiated using a discrete nonlinear form of the system dynamics model. From this result, a mean of the vectors is computed based on a weighted sum, and a covariance about this mean is computed, again using a weighted sum. Each of the vectors instantiated via the system model is instantiated using a nonlinear form of the measurement model. In similar fashion with the dynamics instantiation, the results from the measurement model instantiation,

208

INTEGRATED NAVIGATION SYSTEMS Unscented/Kalman filter [11 – 14]a

Table 8.2

Establish symmetric xˆ 0 ; x^ sigma points about pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi xˆ i ¼ x^ þ ðn þ kÞPi 8i ¼ 1, . . . , n state estimate pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi xˆ i ¼ x^  ðn þ kÞPi

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

8i ¼ n þ 1, . . . , 2n

Instantiate sigma points through process model Predict mean

  x˜ i ¼ f xˆ i

x ¼

2n P

Discrete form of nonlinear dynamics equation for time propagation

Wi x˜ i

i¼0

Predict covariance

P xx ¼

2n X

  Wi x˜ i  x¯

i¼0

 T  x˜ i  x¯ fþQg Instantiate sigma points through measurement model Predict observation

  Yi ¼ h x˜ i

y ¼

2n P

Wi Yi

i¼0

Innovation covariance

Pyy ¼

Cross covariance

Pxy ¼

2n P

Wi ðYi  y ÞðYi  y ÞT fþRg

i¼0

2n P

  Wi x˜ i  x¯ ðYi  y ÞT

i¼0

Kalman filter equations

W0 ¼ k=n þ k and 1 Wi ¼ 2ðn þ kÞ k ¼ constant, n number p¼ ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ffi of states ðn þ kÞPi ¼ scaled ith P matrix row/column of square-root factor

K ¼ Pxy P1 yy

Weighted linear combination Weighted linear combination of deviations from predicted mean Nonlinear measurement model

Weighted linear combination Weighted linear combination of deviations from mean observation Weighted linear combination of deviations ——

x^ ¼ x þ K ðy  y Þ P ¼ P xx  KPyy K T

a

This form of the UKF algorithm reflects the weights defined in the earliest of the references cited.

for each of the dispersed state vectors, are used to compute a mean and covariance. A cross covariance is also computed. The weighted means and covariances thus computed are used in a form of the Kalman filter equations (see Problem 8.9). The process noise and measurement covariance matrices are inserted at the appropriate computational stage in the table.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

KALMAN FILTERING

209

The authors of this algorithm have demonstrated that the covariance matrix formed with the weighted sum, using the weights presented in Table 8.2, reproduces the error covariance matrix. This is demonstrated in the sequence of steps in the following equation. Note, the caret or tilde symbol has been temporarily ignored.   T P ¼ W0 x0  x x0  x   T   T þ W1 x1  x x1  x þ W2 x2  x x2  x þ      T þ Wn xn  x xn  x   T   T þ Wnþ1 xnþ1  x xnþ1  x þ Wnþ2 xnþ2  x xnþ2  x þ      T þ W2n x2n  x x2n  x ¼0



T pffi pffi þ W1 x þ 1  x x þ 1  x



T pffi pffi þ W2 x þ 2  x x þ 2  x þ   



T pffi pffi þ Wn x þ n  x x þ n  x



T pffi pffi þ Wnþ1 x  nþ1  x x  nþ1  x



T pffi pffi þ Wnþ2 x  nþ2  x x  nþ2  x þ   



T pffi pffi þ W2n x  2n  x x  2n  x ¼ W1

T T pffi pffi 1

1





þ Wnþ1

pffi

nþ1

þ    þ W2n

þW2

pffi

T pffi pffi 2

T nþ1

þWnþ2

þ    þ Wn

2

pffi

nþ2

pffi

T pffi pffi n

n

T

nþ2





pffi pffi T 2n

2n

n hpffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ihpffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi iT X ¼ 2Wi ðn þ kÞPi ðn þ kÞPi i¼1

¼ 2Wi ðn þ kÞ

n pffiffiffi pffiffiffi T X Pi Pi i¼1

¼P where the symbol matrix factor.

(8:89) pffi

i

represents the scaled i th column/row vector of the covariance

210

INTEGRATED NAVIGATION SYSTEMS

The following example presents a motivation for the UKF to illustrate the limitations of a linear Kalman filter for nonlinear systems. Using a scalar nonlinear function, a variance is formed that leads to an infinite series that is used as “truth,” and matching the order of its terms relative to an approximation to this variance formed using differences rather than derivatives.

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

Example 8.4

Scalar Example for True Covariance Approximation

This example illustrates the formulation of the covariance of a nonlinear function by expanding this function in a Taylor series. The inputs to this function consist of a variable’s mean plus its random variation, and the expected value of this variation odd powers is assumed to be zero. Consider the nonlinear function y ¼ f ð xÞ The input to this function is the mean value plus random variable deviation. x ¼ x þ dx where it is assumed that the random variable deviation satisfies the following: E ½ dx  ; 0   E dx2 ; Pxx and all odd powers of higher moments are zero     E dx 3 ¼ E dx 5 ¼    ; 0 Expanding in Taylor series about the mean value yields y ¼ f ðx þ dxÞ ¼ f ðx Þ þ

@f 1 @2 f 2 1 @3 f 3 1 @4 f 4 dx þ dx þ dx þ dx þ    @x 2! @x2 3! @x3 4! @x4

Taking the expected value of this function, using the assumption concerning the random variable deviation’s odd powers, to form a mean of the function yields y ¼ E½ f ðx þ dxÞ   @f 1 @2 f 2 1 @3 f 3 1 @4 f 4 dx þ d x þ d x þ d x þ    ¼ E½ f ðx Þ þ E @x 2! @x2 3! @x3 4! @x4  2  1 @ f 2 1 @4 f 4 ¼ f ðx Þ þ E d x þ d x þ    2! @x2 4! @x4

KALMAN FILTERING

211

The difference between function and expected value becomes

y  y ¼ f ðx þ dxÞ  E½ f ðx þ dxÞ @f 1 @2 f 2 1 @3 f 3 1 @4 f 4 dx þ dx þ dx þ dx þ    @x 2! @x2 3! @x3 4! @x4  2  1 @ f 2 1 @4 f 4 dx þ dx þ     f ðx Þ  E 2! @x2 4! @x4

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¼ f ðx Þ þ

¼

@f 1 @2 f 2 1 @3 f 3 1 @4 f 4 dx þ dx þ dx þ dx þ    @x 2! @x2 3! @x3 4! @x4  2  1 @ f 2 1 @4 f 4 E dx þ dx þ    2! @x2 4! @x4

Then the variance of this difference is formed as

E½(y  y )2  ¼ E½{f (x þ dx)  E½ f (x þ dx)}2  20 12 3 @f 1 @2 f 2 1 @3 f 3 1 @4 f 4 dx þ dx þ dx þ    C 7 6B dx þ 2!@x2 3! @x3 4! @x4 6 @x C 7 ¼ E6B A 7 1 @2 f 2 1 @4 f 4 4@ 5 d x þ d x þ    E 2! @x2 4! @x4 2 3 1 1 fx2 dx2 þ fxx fx dx3 þ fxxx fx dx4 6 7 2! 3! 6 7 1 6 7 6 þ     fxx fx E½dx2 dx     7 6 7 2! 6 7 1 1 1 6 7 6 7 fxx2 dx4 þ fx fxx dx3 þ 6 7 2! 2!2! 6 7 11 2 6 7 ¼ E6 þ  fxx E½dx2 dx2     7 6 7 2!2! 6 7 1 6 7 þ fx fxxx dx4 þ        6 7 6 7 3! 6 7 1 6 7 2  fxx fx E½dx dx     6 7 6 7 2! 4 5 11 2 2 2 fxx E½dx dx      2!2! 1 11 2 1 ¼ fx2 E½dx2  þ fxxx fx E½dx4  þ fxx E½dx4  þ fx fxxx E½dx4  3! 2!2! 3! 11 2 f E½dx2 E½dx2  þ     2!2! xx

212

INTEGRATED NAVIGATION SYSTEMS

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

where the notation fx implies the operation @f =@x and so forth for fxx, etc.. The variance of the nonlinear function based on the distribution of the input’s random variable deviation becomes   1 1 1 2 4 2 4 4 Pyy ¼ fx Pxx þ E fxxx fx dx þ f dx þ fx fxxx dx 3! 2  2! xx 3!     1 1 fxx dx2 E fxx dx2 þ    E 2! 2! Linear system function would include only the first term. For a nonlinear system function, this expression for the true variance is finite only when the derivatives it contains become zero. Rationale for the formulation of the UKF algorithm is presented by way of another example. This example also considers a scalar nonlinear system and presents an expansion for the predicted variance listed in Table 8.2.

Example 8.5 Scalar Nonlinear System Example Consider the scalar system and measurement models xiþ1 ¼ f (xi ) þ wi

and

zj ¼ h(xj ) þ vj

In this example, the number of states is one (n ¼ 1), and the weights in Table 8.2 become W0 ¼

k , kþ1

W1 ¼

1 , 2(k þ 1)

and

W2 ¼

1 2(k þ 1)

The first step in the sequence of steps is to form the dispersed states pffiffiffiffiffiffiffiffiffiffiffi pffiffiffiffiffiffiffiffiffiffiffi x^ 0 ; x^ ; x0 , x^ 1 ¼ x^ þ k þ 1s, and xˆ 2 ¼ xˆ  k þ 1s where the state x^ and s are both dynamic variables. In the following, the root in pffi the preceding terms will be represented symbolically as simply . The predicted mean is formed by instantiating the dispersed state as x ¼ W0 x˜ 0 þ W1 x˜ 1 þ W2 x˜ 2

k 1 1 f (xˆ 0 ) þ f (xˆ 1 ) þ f (xˆ 2 ) kþ1 2(k þ 1) 2(k þ 1) 1 1 pffi pffi ¼ {k f (xˆ 0 ) þ ½ f (^x þ s) þ f (^x  s)} kþ1 2 ¼

Continuing with the variance about the predicted mean P xx ¼

2 X

Wi ½x˜ i  x¯ ½x˜ i  x¯ T

i¼0

¼ W0 (x˜ 0  x¯ )2 þ W1 (x˜ 1  x¯ )2 þ W2 (x˜ 2  x¯ )2

KALMAN FILTERING 



213  2

pffiffiffi pffiffiffi k 1 1 f (^x)  k f (^x) þ ½ f (^x þ s ) þ f (^x  s ) kþ1 kþ1 2    2 pffiffiffi pffiffiffi pffiffiffi 1 1 1 f (^x þ s)  þ k f (^x) þ ½ f (^x þ s ) þ f (^x  s ) kþ1 2(k þ 1) 2    2 pffiffiffi pffiffiffi pffiffiffi 1 1 1 f (^x  s)  þ k f (^x) þ ½ f (^x þ s ) þ f (^x  s ) kþ1 2(k þ 1) 2   pffiffiffi pffiffiffi 2 k 1 ½ f (^ x þ ) þ f (^ x  ) ¼ ( k þ 1)f (^ x )  k f (^ x )  s s 2 (k þ 1)2   pffiffiffi pffiffiffi pffiffiffi 2 1 1 þ (k þ 1)f (^x þ s )  k f (^x)  ½ f (^x þ s ) þ f (^x  s ) 2 2(k þ 1)2   pffiffiffi pffiffiffi pffiffiffi 2 1 1 þ (k þ 1)f (^x  s )  k f (^x)  ½ f (^x þ s ) þ f (^x  s ) 2 2(k þ 1)2

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

¼

This expression can be simplified further to yield

pffiffiffi pffiffiffi 2 k f (^x þ s )  2f (^x) þ f (^x  s ) P xx ¼ 2 2(k þ 1)   pffiffiffi pffiffiffi pffiffiffi 2 1 1 þ k½ f (^x þ s )  f (^x) þ ½ f (^x þ s )  f (^x  s ) 2 2(k þ 1)2   pffiffiffi pffiffiffi pffiffiffi 2 1 1 þ k½ f (^x)  f (^x  s ) þ ½ f (^x þ s )  f (^x  s ) 2 2(k þ 1)2 Examining the last terms in the second of the f g brackets, consider a difference between the following Taylor-series expansions about the state x^ : f (x þ

pffiffiffi @f pffiffiffi 1 @2 f pffiffiffi 2 1 @3 f pffiffiffi 3 s ) ¼ f (x) þ sþ ( s) þ ( s) þ  @x 2! @x2 3! @x3

f (x 

pffiffiffi @f pffiffiffi 1 @2 f pffiffiffi 2 1 @3 f pffiffiffi 3 s ) ¼ f (x)  sþ ( s)  ( s) þ  @x 2! @x2 3! @x3

and

therefore f (x þ

pffiffiffi pffiffiffi @f pffiffiffi 1 @3 f pffiffiffi 3 s )  f (x  s) ¼ 2 sþ2 ( s) þ  @x 3! @x3

Or pffiffiffi pffiffiffi 1 @f pffiffiffi 1 @3 f pffiffiffi 3 sþ ( s) ½ f (x þ s )  f (x  s )  2 @x 3! @x3

214

INTEGRATED NAVIGATION SYSTEMS

Also, from these expansions, the term in the first f g bracket can be expressed as ½ f (x þ

pffiffiffi pffiffiffi pffiffiffi ( s )2 @2 f s )  2f (x) þ f (x  s )  2 2! @x2

and the first terms in the second f g brackets become

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

f (x þ

pffiffiffi @f pffiffiffi 1 @2 f pffiffiffi 2 1 @3 f pffiffiffi 3 s )  f (x) ¼ sþ ( s) þ ( s) þ  @x 2! @x2 3! @x3

and f (x)  f (x 

pffiffiffi @f pffiffiffi 1 @2 f pffiffiffi 2 1 @3 f pffiffiffi 3 s) ¼ s ( s) þ ( s)   @x 2! @x2 3! @x3

Substituting these approximating expansions in the preceding variance equation yields  pffiffiffi 2 2 k ( s) fxx 2 2! 2(k þ 1)2    pffiffiffi pffiffiffi pffiffiffi 1 ( s )2 ( s )3 )f f f þ k ( s þ þ x xx xxx 2! 3! 2(k þ 1)2  2 pffiffiffi 1 pffiffiffi ( s )3 fxxx þ 2( s )fx þ 2 3! 2    pffiffiffi pffiffiffi pffiffiffi 1 ( s )2 ( s )3 fxx þ fxxx þ k ( s )fx  2! 3! 2(k þ 1)2  2 pffiffiffi 1 pffiffiffi ( s )3 fxxx þ 2( s )fx þ 2 3! 2

P xx 

Squaring these expression, retaining terms up to fourth order, and grouping terms in derivative order yield the following: pffiffiffi pffiffiffi pffiffiffi (k2 þ 2k) ( s )4 2 2( s )4 P xx  ( s )2 fx2 þ f fx fxxx þ xx 4 3! (k þ 1)2  2  (k þ 2k) 2 2(k þ 1)2 fx fxxx s4 ¼ (k þ 1)fx2 s2 þ fxx þ 3! 4 If the function f (x) were the instantiation of the state with the nonlinear system model, then this equation would represent the time propagation of the variance 2s2, the predicted variance. Following this process for the other

KALMAN FILTERING

215

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

terms, the predicated measurement variance and cross variance can be found and result in similar forms involving the measurement function 2h(x). Continuing this example, by using the results from the preceding example, yields insight into the errors associated with the preceding predicted variance approximation. Returning to the preceding example, assuming the function y represents the instantiated state for time propagation, and if the deviation about the mean were Gaussian such that the expectation operators in this equation are related to the variance as E½dx2  ¼ s2 ,

E½dx4  ¼ 3s4 , etc:

then the variance equation becomes 1 1 1 fxxx fx s4 þ 3 fxx2 s4  f 2 s4 þ    3! 2  2! 2!  2! xx 1 ¼ fx2 s2 þ fxxx fx s4 þ fxx2 s4 þ    2  1 2 2 2 ¼ fx s þ f þ fx fxxx s4 þ    2 xx

Pyy ¼ fx2 s2 þ 3  2

The difference between this form and that just given becomes approximately    1 2 2 2 4  1P ; Pyy  Pxx ¼ fx s þ f þ fx fxxxx s þ    2 xx    2  (k þ 2k) 2 2(k þ 1)2 2 2 4 fx fxxx s þ    fxx þ  (k þ 1)fx s þ 3! 4      1 (k2 þ 2k) 2 2(k þ 1)2  ¼ k fx2 s2 þ fx fxxx s4 fxx þ 1  3! 2 4 The constant k is chosen to minimize the magnitude of this difference. Mentioned in the literature [13] is the choice of setting this constant to zero. With this choice, the resulting variance difference becomes  1 2 2 1P  f þ fxxx fx s4 2 xx 3 The predicted variance would approximate the true variance with an error including at least those terms remaining in this equation. The authors [12] suggest a specific value for the constant k and/or additional constants to reduce this error. Current research is aimed at approximating this variance, using differences to approximate the derivatives, a selection of constants as in Table 8.2 plus others, or higher-order approximations for the derivatives in this expression.

216

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

8.7.2

INTEGRATED NAVIGATION SYSTEMS Divided-Difference Filter

The following table summarizes the divided difference filter [15]. The algorithm shown in this table is expressed in matrix form; however, most references to this algorithm are expressed in matrix factored form including the one cited. The algorithm’s development is based an application of Stirling’s interpolation formula [16]. A function is to be evaluated at an intermediate, interpolated, point between fixed points where its value has been already determined. The points where this function’s values are specified are separated by fixed interval given by h. An approximation to this function of an intermediate point is evaluated by using this interpolation formula, which is given as

1 f (x þ rh) ¼ f (x) þ r ½ f (x þ h)  f (x  h) 2 þ

r2 ½ f (x þ h)  2f (x) þ f (x  h) þ    2!

(8:90)

where for interpolation the variable r ranges between 21 , r , 1. For the divided-difference filter the variable r is defined by the ratio r ; dx/h. Substituting this expression into the first few terms in the interpolation formula yields  dx 1 ½ f (x þ h)  f (x  h) f (x þ dx) ¼ f (x) þ h 2  1 dx 2 þ ½ f (x þ h)  2f (x) þ f (x  h) þ    2! h

(8:91)

The fixed step intervals are redefined as dynamically scaled steps by replacing the step size variable h with hs, where s is a dynamic variable 

dx 1 ½ f (x þ hs)  f (x  hs) hs 2  1 dx 2 þ ½ f (x þ hs)  2f (x) þ (x  hs) þ    2! hs  dx 1 ¼ f (x) þ ½ f (x þ hs)  f (x  hs) s 2h  2 dx 1 ½ f (x þ hs)  2f (x) þ f (x  hs) þ    þ s 2h2

f (x þ dx) ¼ f (x) þ

(8:92)

KALMAN FILTERING

217

In the following, the variable s will assume the characteristics of the root of the variance of the random variable dx. The variable (dx/s) is the assumed to be a normally distributed Gaussian random variable whose expectations are defined as " # dx i E ¼0 s

8 i  odd powers

(8:93)

8 i  even powers

(8:94)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

and " # dx i E ; si s

where, for Gaussian random variables, si ¼ 1 3 . . . (i 2 1), for example,

s2 ¼ s2 ; 1,

s4 ¼ 3s2 ¼ 3,

etc:

(8:95)

The expectation of Eq. (8.92) becomes 2

3  dx 1 6 f (x) þ s 2h ½ f (x þ hs)  f (x  hs) 7 6 7 E½ y ¼ E6  2 7 4 5 1 dx þ 2 ½ f (x þ hs)  2f (x) þ f (x  hs) þ L 2h s   dx 1 ½ f (x þ hs)  f (x  hs) ¼ f (x) þ E s 2h " # dx 2 1 ½ f (x þ hs)  2f (x) þ f (x  hs) þ    þE s 2h2  f (x) þ

1 ½ f (x þ hs)  2f (x) þ f (x  hs) þ    2h2

(8:96)

To order, E[(dx/s)3]. This equation can also be written in the form in Table 8.3 as 1 E½ y ¼ f (x) þ 2 ½ f (x þ hs)  2f (x) þ f (x  hs) 2h   1 1 ¼ 1  2 f (x) þ 2 ½ f (x þ hs) þ f (x  hs) h 2h  2  h 1 1 ¼ f (x) þ 2 ½ f (x þ hs) þ f (x  hs) 2 h 2h

(8:97)

Kalman filter equations

Cross covariance

Innovation covariance a posteriori update

Measurement a posteriori update

Covariance a priori update

State a priori update

Factor state covariance matrix

Table 8.3

n pffiffiffiffiffi pffiffiffiffiffi (s4  1) X ½ f (xi þ h Pi )  2f (x0 ) þ f (xi  h Pi ) 4h4 i¼1 pffiffiffiffiffi pffiffiffiffiffi  ½f (xi þ h Pi )  2f (x0 ) þ f (xi  h Pi )T fþQg

P ¼ Pxx  KPyy K T

x^ ¼ x þ K(y  y )

K ¼ Pxy P1 yy

þ

n pffiffiffiffiffi pffiffiffiffiffi (s4  1) X ½g(xi þ h Pi )  2g(x0 ) þ g(xi  h Pi ) 4h4 i¼1 pffiffiffiffiffi pffiffiffiffiffi  ½g(xi þ h Pi )  2g(x0 ) þ g(xi  h Pi )T fþRg n pffiffiffiffiffi pffiffiffiffiffi pffiffiffiffiffi 1 X Pxy ¼ Pi ½g(xi þ h Pi )  g(xi  h Pi )T 2h i¼1

n pffiffiffiffiffi pffiffiffiffiffi 1 X ½g(xi þ h Pi )  g(xi  h Pi ) 4h2 t¼1 pffiffiffiffiffi pffiffiffiffiffi  ½g(xi þ h Pi )  g(xi  h Pi )T

n pffiffiffiffiffi pffiffiffiffiffi (h2  n) 1 X g(x ) þ ½g(xi þ h Pi ) þ g(xi  h Pi ) 0 h2 2h2 i¼1

Pyy ¼

y ¼

þ

n pffiffiffiffiffi pffiffiffiffiffi 1 X ½f (xi þ h Pi )  f (xi  h Pi ) 2 4h i¼1 pffiffiffiffiffi pffiffiffiffiffi  ½f (xi þ h Pi )  f (xi  h Pi )T

n pffiffiffiffiffi pffiffiffiffiffi (h2  n) 1 X f (x0 ) þ 2 ½ f (xi þ h Pi ) þ f (xi  h Pi ) 2 h 2h i¼1

Pxx ¼

x ¼

pffiffiffiffiffi Pi 8 i ¼ 1, 2, K, n

Return to top

Mean and measurement prediction variance

Same as predicted variance

Same as predicted mean

Predicted mean from function’s expectation based on Stirling interpolation formula to 2nd order Function error variance to 2nd order

ith row or column of square-root factor of P

Second-order divided-difference Kalman filter [15] (illustrated in matrix form)

Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

218 INTEGRATED NAVIGATION SYSTEMS

KALMAN FILTERING

219

The difference between the function and its expectation becomes  dx 1 y  E½ y ¼ f (x) þ ½ f (x þ hs)  f (x  hs) s 2h  1 dx 2 ½ f (x þ hs)  2f (x) þ f (x  hs) þ    þ 2 2h s Downloaded by Stanford University on September 28, 2012 | http://arc.aiaa.org | DOI: 10.2514/4.861598

 f (x) 

1 ½ f (x þ hs)  2f (x) þ f (x  hs) 2h2

or y  E½ y ¼

 dx 1 ½ f (x þ hs)  f (x  hs) s 2h " # dx 2 1 1 ½ f (x þ hs)  2f (x) þ f (x  hs) þ    þ s 2h2

(8:98)

Defining temporary variables a;

1 ½ f (x þ hs)  f (x  hs) 2h

and b;

1 ½ f (x þ hs)  2f (x) þ f (x  hs) 2h2

The preceding difference can be written, including the first two terms, as " #  dx dx 2 2 y  E½ y  1 b aþ s s The variance of this difference becomes 0(

" # )2 1  2 d x d x E½( y  E½ y2 ) ¼ E@ 1 b A aþ s s

¼E

8