INS/CNS/GNSS Integrated Navigation Technology

Wei Quan • Jianli Li • Xiaolin Gong

Jiancheng Fang

INS/CNS/GNSS Integrated
Navigation Technology
Wei Quan Xiaolin Gong
Beijing University of Aeronautics Beijing University of Aeronautics
and Astronautics and Astronautics
Beijing Beijing
China China
Jianli Li Jiancheng Fang
Beijing University of Aeronautics Beijing University of Aeronautics
and Astronautics and Astronautics
Beijing Beijing
China China

ISBN 978-3-662-45158-8 ISBN 978-3-662-45159-5 (eBook)

DOI 10.1007/978-3-662-45159-5

Library of Congress Control Number: 2014956092

Jointly published with National Defense Industry Press, Beijing

ISBN: 978-7-118-09765-8 National Defense Industry Press, Beijing

Springer Heidelberg New York Dordrecht London

© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015
This work is subject to copyright. All rights are reserved by the publishers, whether the whole or part of
the material is concerned, specifically the rights of translation, reprinting, reuse of illustrations, recitation,
broadcasting, reproduction on microfilms or in any other physical way, and transmission or information
storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar methodology
now known or hereafter developed. Exempted from this legal reservation are brief excerpts in connection
with reviews or scholarly analysis or material supplied specifically for the purpose of being entered and
executed on a computer system, for exclusive use by the purchaser of the work. Duplication of this
publication or parts thereof is permitted only under the provisions of the Copyright Law of the publishers
location, in its current version, and permission for use must always be obtained from Springer. Permissions
for use may be obtained through RightsLink at the Copyright Clearance Center. Violations are liable to
prosecution under the respective Copyright Law.
The use of general descriptive names, registered names, trademarks, service marks, etc. in this publication
does not imply, even in the absence of a specific statement, that such names are exempt from the relevant
protective laws and regulations and therefore free for general use.
While the advice and information in this book are believed to be true and accurate at the date of publication,
neither the authors nor the editors nor the publishers can accept any legal responsibility for any errors or
omissions that may be made. The publishers makes no warranty, express or implied, with respect to the
material contained herein.

Printed on acid-free paper

Springer is part of Springer Science+Business Media (www.springer.com)

Preface

Navigation system with high precision, high reliability, and strong autonomy will
provide various moving objects and carriers with high-precision motion parameter
information, which is the precondition to realize its accurate navigation and the ba-
sis to develop precise guidance technology. At present, navigation means commonly
used in the aerospace field mainly include inertial navigation, satellite navigation and
celestial navigation, which have respective advantages and disadvantages in applica-
tion. Full autonomy, possibility to continuously provide complete motion parameters
and high short-term high precision are the advantages of inertial navigation, and er-
ror accumulation with working time is its disadvantage; satellite navigation is able
to provide all-time and all-weather high-precision position and velocity information
with error not accumulating with time, but it is difficult to directly provide attitude
information and is easily disturbed; celestial navigation is able to provide attitude
and position information with error not accumulating with time, but it is restricted
by weather condition and has low position precision. Therefore, a single navigation
means is hard to meet the requirements of long-time and high-precision navigation
of long-range missiles, naval vessels, long-range bombers and HALE UAV. The in-
ertial navigation system (INS)/celestial navigation system (CNS)/global navigation
satellite system (GNSS integrated navigation system able to make full use of the
feature of complementary advantages among each navigation subsystem to greatly
improve the precision and reliability of navigation system has become an effective
means to realize precise positioning and navigation and has always been the research
focus and hotspot of the navigation technology.
This monograph consists of 11 chapters. Chapter 1 primarily introduces the history
and current situation of INS/CNS/GNSS integrated navigation. Chapter 2 introduces
the operating principles and analysis of error characteristics of inertial, satellite, and
celestial navigation systems. Chapter 3 mainly introduces some advanced filtering
methods such as unscented Kalman filter, unscented particle filter, and predictive
filter commonly used in a navigation system. Chapter 4 mainly introduces gyro
error modeling, test and compensation, calibration methods for strapdown inertial
measurement unit, and high-dynamic strapdown inertial navigation system algo-
rithm, etc. Chapter 5 primarily introduces the star map preprocessing method for
star sensor on which celestial navigation is based, quick and efficient star map
v
vi Preface

identification method and celestial navigation method based on star sensor, etc.
Chapters 6–9 are the key contents of this book. Among them, Chap. 6 introduces
the principle and modeling method of INS/GNSS integrated navigation system and
high-precision INS/GNSS integrated navigation method; Chap. 7 introduces the prin-
ciple and modeling method of INS/CNS integrated navigation system, new INS/CNS
integrated navigation method of guided missile and lunar vehicle, and INS/CNS inte-
grated attitude method of satellite; Chap. 8 introduces the basic principle, composite
mode, and modeling method for INS/CNS/GNSS integrated navigation system and
INS/CNS/GNSS integrated navigation method based on federal UKF and informa-
tion distribution factor of federal filtering optimization. Chapter 9 introduces PWCS
observable analysis theory and method, and the method of using improved observ-
able analysis theory to improve real-time performance of INS/CNS, INS/GNSS and
INS/CNS/GNSS integrated navigation systems. Chapter 10 mainly introduces the
principle, composition, realization, and experiment of semi-physical simulation sys-
tem for INS/CNS/GNSS integrated navigation. The last chapter forecasts the future
development trend of INS/CNS/GNSS integrated navigation technology.
This book is prepared by referring to the latest research achievements in integrated
navigation technology field at home and abroad on the basis of research achievements
made by the research team of the author for more than a decade on completion of
a dozen relevant scientific research tasks. This book strives for systematic contents,
novel viewpoints, and theory linked with practice, but deficiencies are inevitable
since it involves multiple academic forelands and relative original contents and ex-
pertise owned by the author is limited, so criticism of various experts and readers
are expected. This book may be regarded as a textbook or teaching reference book
of graduate students and senior undergraduates of relevant major in institutions of
higher education, or referred by engineering and technical staff engaged in navigation
and guidance technology researches.
Special thanks are given to Professor Fang Jiancheng for his meticulous guidance,
strong support, and enthusiastic assistance, who has provided many directional sug-
gestions and specific opinions for this book. I would also like to thank Associate
Professor Yu Wenbo and Professor Zhong Maiying for proofreading most chapters
of the book and a great number of good suggestions; Doctor Liu Baiqi, Doctor Xu
Fan, Doctor Cao Juanjuan, Doctor Ning Xiaolin, Doctor Ali Jamshaid, Doctor Geng
Yanrui, Doctor Yang Sheng, Doctor Zhang Xiao, Doctor Zhang Haipeng, and Doctor
Sun Hongwei for their successive participation in compiling and proofreading of
partial contents of this book; Li Yanhua, Wu Haixian, Lin Minmin, Zhang Yu, Wang
Ziliang, Yu Yanbo, Shen Zhong, Guo Enzhi, Tan Liwei, and other students; “Inertial
Technology” National Key Laboratory and “New Inertial Instrument and Navigation
System Technology” Key Discipline Laboratory of National Defense for their en-
ergetic support and assistance; and National Natural Science Foundation of China
and Ministry of Science and Technology for their support and assistance. In addi-
tion, partial contents of this book have been prepared by referring to latest research
achievements of experts and scholars of the same industry at home and abroad, for
whom sincere appreciation is hereby expressed!
Preface vii

Thanks also extended to Book Publishing Foundation of National Defense Science

and Technology and National Defense Industry Press for their strong support given
during the publication of this book, Springer Press for its energetic support given
during publication of this book, Beihang University for its support and assistance
given to scientific research, and all people who provided concern, support, and
assistance during the preparation of this book!

Beihang University, July 2014 Author

Brief Introductions

INS(Inertial Navigation System)/CNS(Celestial Navigation System)/GNSS(Global

Navigation Satellite System) integrated navigation technology can make full use of
the complementary characteristics of the different navigation sub-systems, greatly
improve the accuracy and reliability of the integrated navigation system, which has
become an effective integrated navigation tool for precise positioning navigation, and
is the hot topic in the field of navigation technology. The monograph is written on
the basis of the more than 20 years research results of the authors and his groups, and
the latest developments in aboard integrated navigation technology. This monograph
consists of eleven chapters. Chapter one primarily introduces the history and current
situation of INS/CNS/GNSS integrated navigation. Chapter two introduces the oper-
ating principles and analysis of error characteristics of INS, CNS and GNSS. Chapter
three mainly introduces some advanced filtering methods such as unscented kalman
filter, unscented particle filter and predictive filter commonly used in navigation sys-
tem. Chapter four mainly introduces gyro error modeling, test and compensation,
calibration methods for strapdown inertial measurement unit and high-dynamic strap-
down inertial navigation system algorithm, etc. Chapter five primarily introduces the
star map preprocessing method for star sensor on which celestial navigation is based,
quick and efficient star map identification method and celestial navigation method
based on star sensor, etc. Chapter six, seven, eight and nine are the key contents of
this book. Among them, chapter six introduces the principle and modeling method of
INS/GNSS integrated navigation system and high-precision INS/GNSS integrated
navigation method; chapter seven introduces the principle and modeling method of
INS/CNS integrated navigation system, new INS/CNS integrated navigation method
of guided missile and lunar vehicle, and INS/CNS integrated attitude method of
satellite; chapter eight introduces the basic principle, composite mode and modeling
method for INS/CNS/GNSS integrated navigation system and INS/CNS/GNSS inte-
grated navigation method based on federal UKF and information distribution factor
of federal filtering optimization. Chapter nine introduces PWCS observable analysis
theory and method, and the method of using improved observable analysis theory to
improve real-time performance of INS/CNS, INS/GNSS and INS/CNS/GNSS inte-
grated navigation systems. Chapter ten mainly introduces the principle, composition,
realization and experiment of semi-physical simulation system for INS/CNS/GNSS
ix
x Brief Introductions

integrated navigation. The last chapter forecasts the future development trend of
INS/CNS/GNSS integrated navigation technology.
The monograph is not only provided for the materials and reference books for
the related graduates and high-grade undergraduates in colleges and universities, but
also for the references for the engineering and technical researchers in the navigation
and control field.
Contents

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1 The History of INS/CNS/GNSS Navigation . . . . . . . . . . . . . . . . . . . . . 2
1.2 The Current Status of INS/CNS/GNSS Navigation Development . . . 4
1.2.1 INS/GNSS Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.2.2 INS/CNS Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.2.3 INS/CNS/GNSS Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2 Principle of INS/CNS/GNSS Navigation System . . . . . . . . . . . . . . . . . . . 9

2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Coordinate Frames and Earth Reference Model Commonly Used in
Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2.1 The Coordinate Frames Used in Navigation . . . . . . . . . . . . . . 9
2.2.2 The Conversion of Coordinate Systems . . . . . . . . . . . . . . . . . . 13
2.2.3 Earth Reference Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.3 Inertial Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.3.1 Work Principle of Inertial Navigation System . . . . . . . . . . . . . 21
2.3.2 SINS System Error Equation and Error Propagation
Characteristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.4 Satellite Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
2.4.1 Operating Principle of Satellite Navigation System . . . . . . . . 30
2.4.2 Analysis of Error Characteristics for Satellite Navigation
System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
2.5 Celestial Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
2.5.1 Autonomous Celestial Positioning Principle . . . . . . . . . . . . . . 36
2.5.2 Celestial Attitude Determination Principle . . . . . . . . . . . . . . . 44
2.5.3 Star Sensor in CNS and Analysis of Its Error Characteristics 46
2.6 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

xi
xii Contents

3 Filters in Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
3.2 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
3.3 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
3.3.1 Mathematical Description of Stochastic Nonlinear System . . 56
3.3.2 Discrete Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . 57
3.4 Unscented Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
3.5 Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
3.6 Unscented Particle Filter (UPF) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
3.7 Predictive Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
3.8 Federated Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
3.8.1 Structure of Federated Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
3.8.2 Fusion Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
3.9 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

4 Error Modeling, Calibration, and Compensation of Inertial

Measurement Unit (IMU) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
4.2 Error Modeling and Compensation of Inertial Sensors . . . . . . . . . . . . 76
4.2.1 Error Model of Gyroscopes . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
4.2.2 Scale Factor Error Modeling of Gyroscope . . . . . . . . . . . . . . . 78
4.2.3 Temperature Error Modeling of Gyroscope . . . . . . . . . . . . . . . 85
4.3 Design, Error Calibration, and Compensation of Inertial
Measurement Units . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
4.3.1 Design of Inertial Measurement Units . . . . . . . . . . . . . . . . . . . 90
4.3.2 The Optimization Six-Position Hybrid Calibration for SINS . 104
4.3.3 Integrated Calibration Method for RLG IMU Using a
Hybrid Analytic/Kalman Filter Approach . . . . . . . . . . . . . . . . 108
4.3.4 Temperature Error Modeling of IMU Based on Neural
Network . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117
4.4 High Dynamic Strapdown Inertial Algorithm . . . . . . . . . . . . . . . . . . . 123
4.4.1 Error Analysis and Gyro Biases Calibration of Analytic
Coarse Alignment for Airborne POS . . . . . . . . . . . . . . . . . . . . 124
4.4.2 Conical Motion Analysis and Evaluation Criteria for
Conical Error Compensation Algorithm . . . . . . . . . . . . . . . . . 131
4.4.3 An Improved Single-Subsample Rotating Vector Attitude
Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
4.5 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142

5 Star Map Processing Algorithm of Star Sensor and Autonomous

Celestial Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
5.2 Star Map Preprocessing Method for Star Sensors . . . . . . . . . . . . . . . . 145
Contents xiii

5.2.1 Problem Statements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146

5.2.2 Blurred Star Image De-noising . . . . . . . . . . . . . . . . . . . . . . . . . 148
5.2.3 Blurred Star Image Restoration . . . . . . . . . . . . . . . . . . . . . . . . . 150
5.2.4 Results and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152
5.2.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158
5.3 Star Map Identification Method of Star Sensor . . . . . . . . . . . . . . . . . . 159
5.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160
5.3.2 Star Recognition Method Based on AAC Algorithm . . . . . . . 161
5.3.3 Hybrid Simulation Result and Analysis . . . . . . . . . . . . . . . . . . 167
5.3.4 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169
5.4 Celestial Navigation Method Based on Star Sensor and
Semi-physical Simulation Verification . . . . . . . . . . . . . . . . . . . . . . . . . 170
5.4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 171
5.4.2 Celestial Navigation Measurements and Orbit Dynamic
Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172
5.4.3 UKF Information Fusion Algorithm . . . . . . . . . . . . . . . . . . . . . 176
5.4.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 178
5.4.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181
5.5 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181

6 INS/GNSS Integrated Navigation Method . . . . . . . . . . . . . . . . . . . . . . . . . 185

6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185
6.2 Principle of Inertial/Satellite Integrated Navigation . . . . . . . . . . . . . . 186
6.2.1 Combination Mode of Inertial/Satellite Integrated Navigation 186
6.2.2 Basic Principle for Inertial/Satellite Integrated Navigation . . 187
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System 189
6.3.1 Linear Modeling Method of Inertial/Satellite Integrated
Navigation System Based on the  Angle . . . . . . . . . . . . . . . 190
6.3.2 Nonlinear Modeling Method of the Inertial/Satellite
Integrated Navigation System Based on Quaternion Error . . 193
6.4 High-Precision Inertial/Satellite Integrated Navigation Method . . . . 199
6.4.1 Inertial/Satellite Integrated Navigation Method Based on
Mixed Correction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200
6.4.2 Self-Adaptive Feedback Correction Filter Method Based
on Observability Normalization Processing Method . . . . . . . 203
6.4.3 Inertial/Satellite Outlier-Resistant Integrated Navigation
Method Based on Kalman Filtering Innovation
Orthogonality . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 209
6.4.4 An Air Maneuvering Alignment Method Based on
Observability Analysis and Lever Arm Error Compensation 214
6.4.5 SINS/GPS Integrated Estimation Method Based on
Unscented R–T–S Smoothing . . . . . . . . . . . . . . . . . . . . . . . . . 217
6.5 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 231
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233
xiv Contents

7 INS/CNS Integrated Navigation Method . . . . . . . . . . . . . . . . . . . . . . . . . . 237

7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 237
7.2 Basic Principle of Inertial/Celestial Integrated Navigation . . . . . . . . . 238
7.2.1 Operating Mode of the Inertial/Celestial Integrated
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 238
7.2.2 Combination Mode of Inertial/Celestial Integrated
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 240
7.2.3 Principle of Inertial Component Error Correction Based on
Celestial Measurement Information . . . . . . . . . . . . . . . . . . . . 241
7.3 Modeling Method of Inertial/Celestial Integrated Navigation
System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 242
7.3.1 State Equation of Inertial/Celestial Integrated Navigation
System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243
7.3.2 Measurement Equation of Inertial/Celestial Integrated
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 245
7.4 New Inertial/Celestial Integrated Navigation Method of Ballistic
Missile . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 245
7.4.1 Principle for Initial Position Error Correction of Missile
Launching Point Based on Celestial Measurement
Information . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246
7.4.2 Inertial/Celestial Integrated Navigation Method of Ballistic
Missile Based on UKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246
7.5 Inertial/Celestial Integrated Navigation Method of Lunar Vehicle . . . 250
7.5.1 Strapdown Inertial Navigation Method of Lunar Vehicle . . . . 251
7.5.2 A Lunar Inertial/Celestial Integrated Navigation Method
Based on UPF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 252
7.6 Inertial/Celestial Integrated Attitude Determination Method of
Satellite . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 257
7.6.1 Satellite Attitude Determination System Equation . . . . . . . . . 257
7.6.2 An Inertia/Celestial Integrated Attitude Determination
Method of Piecewise Information Fusion Based on EKF . . . 259
7.6.3 Method of Minimum Parameter Attitude Matrix Estimation
of Satellite Based on UKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263
7.6.4 Interlaced Optimal-REQUEST and Unscented Kalman
Filtering for Attitude Determination . . . . . . . . . . . . . . . . . . . . 269
7.7 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 276

8 INS/CNS/GNSS Integrated Navigation Method . . . . . . . . . . . . . . . . . . . . 279

8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 279
8.2 Principle of INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . . 280
8.2.1 Basic Principle of INS/CNS/GNSS Integrated Navigation . . 280
8.2.2 Combination Mode of INS/CNS/GNSS
Integrated Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 280
8.2.3 Modeling of INS/CNS/GNSS Integrated Navigation System 285
Contents xv

8.3 INS/CNS/GNSS Integrated Navigation Method Based on

Federated UKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 287
8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation
Method Based on the Optimized Information Distribution
Factor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 291
8.4.1 Federated Filtering Equation and Information
Distribution Process . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 291
8.4.2 Federated Filtering INS/CNS/GNSS Integrated
Navigation Method Based On Information Distribution
Factor Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293
8.4.3 Research on FKF Method Based on an Improved
Genetic Algorithm for INS/CNS/GNSS Integrated
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 294
8.5 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304

9 Study for Real-Time Ability of INS/CNS/GNSS Integrated

Navigation Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 307
9.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 307
9.2 Piecewise Constant System (PWCS) Observability Analysis
Theory and Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 308
9.2.1 Observability Analysis Theory of the PWCS . . . . . . . . . . 308
9.2.2 An Improved System State Degree of Observability
Analysis Method Based on Singular Value
Decomposition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313
9.3 Dimensionality Reduction Filter Design of INS/CNS Integrated
Navigation System Based on the Improved Degree of
Observability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315
9.4 Dimensionality Reduction Filter Design of INS/GNSS Integrated
Navigation System Based on the Improved Degree of
Observability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 318
9.5 Federated Filter Design of the INS/CNS/GNSS Integrated
Navigation System Based on Dimensionality Reduction Filtering 322
9.6 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 326
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 328

10 Semi-physical Simulation Technology of INS/CNS/GNSS

Integrated Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 331
10.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 331
10.2 Principle and Composition of Semi-Physical Simulation System
of INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . . . . . . . . 332
10.2.1 Principle of Semi-Physical Simulation System of
INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . . 332
10.2.2 Composition of Semi-Physical Simulation System of
INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . . 334
xvi Contents

10.3 Realization and Test of Semi-Physical Simulation System of

INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . . . . . . . . . . 347
10.3.1 Realization of Semi-physical Simulation System of
SINS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . 350
10.3.2 Experiments of Semi-physical Simulation System of
INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . . 359
10.4 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 361
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 361

11 Prospects of INS/CNS/GNSS Integrated Navigation Technology . . . . 363

11.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363
11.2 Development and Prospect of Integrated Navigation Technology . 363
11.2.1 Accurate Modeling Techniques of the INS/CNS/GNSS
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363
11.2.2 Information Fusion of the INS/CNS/GNSS Navigation
System and the Advanced Filtering Method . . . . . . . . . . . 364
11.2.3 INS/CNS/GNSS Navigation Method Based on
Advanced Control Theory . . . . . . . . . . . . . . . . . . . . . . . . . 365
11.2.4 Integrated Inertial/Celestial/Satellite Navigation
System Technology Based on Integration . . . . . . . . . . . . . 368
11.2.5 Applications of the Inertial/Celestial/Satellite
Navigation Technology . . . . . . . . . . . . . . . . . . . . . . . . . . . . 369
11.3 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 370
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 370
Chapter 1
Introduction

Navigation is a technology to provide real-time position, velocity, and attitude in-

formation for the moving vehicles to guide them to the destination accurately. It
involves mathematics, mechanics, optics, electronics, instrumentation, automation,
and computer as well as other disciplines; thus, it becomes one of the key technolo-
gies for aircrafts, missiles, satellites, ships, vehicles and other moving vehicles to
complete its tasks [1; 2].
Currently, with the increase in the sources of information used for navigation,
more types of navigation systems are also emerging accordingly. In addition to
traditional navigation systems like the inertial navigation system, satellite naviga-
tion system, celestial navigation system, Doppler navigation system, ground-based
radio-navigation system, others like geomagnetic navigation system, terrain contour
matching system, scene matching system, gravitational field and visual navigation
system have also developed rapidly. These navigation systems have their own fea-
tures and applications. For spacecraft, the inertial, satellite, and celestial navigation
systems are still some of the most commonly used means of navigation.
Inertial navigation, as an autonomous navigation system, provides position, speed,
attitude and other motion parameters by strap-down solver based on measurement
data obtained from gyroscope and accelerometer without receiving any extra infor-
mation; thus, it has anti-interference ability. Meanwhile, the navigation information
is complete and can be updated thoroughly, etc. [3–5]; therefore, the inertial navi-
gation system is the most important navigation system for spacecraft. However, the
inertial navigation system also has its own shortcomings. Due to integral principle of
strap-down solver, error induced by inertial devices accumulates over time; thus, the
inertial navigation system alone is difficult to meet the requirements of long-range,
high-precision navigation for the moving vehicles [6–8].
Satellite navigation is a space-based radio-navigation system that develops with
the advancement of space technology. This radio-based technology has high precision
positioning, and the navigation error does not accumulate over time; thus, it can work
throughout the day. Therefore, it is called a breakthrough in the field of navigation
technology. However, the satellite navigation system is difficult to provide attitude
information directly, and it is susceptible to electromagnetic interference [9; 10].

© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 1
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_1
2 1 Introduction

Celestial navigation is a kind of autonomous navigation system based on celestial

observations by sun/star sensors, and it directly solves position and attitude param-
eters. The navigation error does not accumulate over time; thus, it is suitable for
aerospace vehicles that require attitude determination with high precision and long
flying hours during autonomous operation [11]. But there are also disadvantages: it
has low data update rate and it’s vulnerable to weather conditions [12].
In this regard, inertial, satellite, and celestial navigations have its own advantages
and disadvantages, so one of them alone is difficult to meet for the long time and
high-performance demand for navigation. Therefore, the integration of two or three of
them, namely, the INS/CNS/GNSS navigation system [13; 14] can complement each
other; thus, it is an effective means to achieve spacecraft/aerospace vehicle navigation
with high precision and high reliability. Currently, the INS/GNSS, INS/CNS/GNSS
navigation systems have been widely used in ballistic missiles, cruise missiles, high-
altitude long-endurance unmanned reconnaissance aircraft, long-range bombers and
reconnaissance satellites and other aerospace vehicles.
The specific advantages of such integrated systems are as follows:
1. Complementary and surpassing. The integrated navigation system combines the
navigation information of each navigation subsystem and complements with each
other, surpassing the performance and precision of each single subsystem and
improving environmental suitability as well.
2. Redundancy and reliability. The same navigation information can be measured
by multiple navigation systems to obtain redundant measurement information,
strengthening the redundancy rate of the system and improving system reliability.
3. Cost reduction. With integrated navigation technology, the precision of the
navigation system can be ensured and the device requirements of navigation sub-
systems can be reduced, particularly the requirements for inertial devices, thus
reducing the total cost of integrated navigation system.
In INS/CNS/GNSS navigation systems, the inertial navigation system is the only
one which is able to provide a complete and continuous parameter for the navigation
system, and is completely autonomous with high precision during short time span.
Therefore, in INS/CNS/GNSS navigation systems, the inertial navigation system
is at the core position. In this book, the INS/CNS/GNSS navigation technologies,
mainly focused on inertial navigation, will be studied.

1.1 The History of INS/CNS/GNSS Navigation

Since the 1950s, there have been studies on integrated navigation system technology.
At that time, due to the lack of advanced navigation algorithm, early integrated
navigation system basically adopted a simple combination of different functions of
each system that are completely independent of each other, and integrated navigation
is realized by supplements and amendments of output data from navigation sensors
[15]. In mid-1950s, the USA developed the U-2 high-altitude reconnaissance with the
1.1 The History of INS/CNS/GNSS Navigation 3

INS/CNS integrated navigation system, which, in a way, realizes a simple integrated

navigation functionally. In the early 1960s, Kalman filtering method was proposed
by RE Kalman, which greatly improved the development of integrated navigation
system technology. The Kalman filter method is applied in the integrated navigation
system, which can overcome the shortcomings of the navigation sensors; thus, the
performance of the integrated system can outrun any single navigation system with
complementary advantages [13].
With the development of ballistic missiles, high-altitude, long-endurance UAVs
and other weapon systems, integrated navigation technology has also made some
progress. In the 1970s, the INS/CNS integrated navigation technology was utilized
in America’s “Trident I type” submarine ballistic missiles, with the mission accuracy
of less than 100 m; in the former Soviet Union, the INS/CNS integrated navigation
technology has also been adopted in “Scalpel” and “Topol” intercontinental ballistic
missile [16]. In the late 1970s and early 1980s, GPS and GLONASS navigation
systems were developed with success by the USA and the former Soviet Union,
respectively, which brought about new vitality into the development of integrated
navigation technology. In the late 1980s, the USA, Britain, France and other countries
were also involved in working on the INS/GNSS navigation technology [10]. In 1986,
the INS/GNSS navigation system for aircraft with 0.3 m positioning accuracy was
first developed in the USA, clearly showing that the integrated navigation technology
has superiority in the field of aircraft navigation technology. Subsequently, the inte-
grated navigation system of laser gyro inertial navigation system, GPS and air data
sensors has been jointly developed by the US Honeywell Corporation and Stanford
Telecommunications Company and used in Boeing airplanes; besides, France and
Britain have also developed the INS/GNSS navigation systems, such as FIN-3110
system produced by GEC-Marconi Avionics.
To the late Twentieth century, with the rapid development of strap-down inertial
navigation technology, the emergence of small high-precision star sensors as well as
the gradual improvement of the global navigation satellite system, the inertial nav-
igation system, the celestial navigation system, and the satellite navigation system
are integrated to improve the precision of navigation systems; thus, it has become
an important direction for high-performance aerospace navigation. In recent years,
the integrated navigation technology has been further developed in the USA, Russia,
Britain and the other countries; meanwhile, the INS/CNS/GNSS navigation technol-
ogy has been used in high-altitude long-endurance unmanned aerial vehicles, ballistic
missiles, long-range transport aircraft as well as reconnaissance aircraft and other
major war weapon systems. With the advancement of filtering, information fusion
and other theoretical methods as well as the development of instrumentation tech-
nology and process manufacturing technology, INS/CNS/GNSS navigation system
technology will be researched continuously and thoroughly [17].
4 1 Introduction

1.2 The Current Status of INS/CNS/GNSS Navigation

Development

Since the successful application of Kalman filter in navigation, there has been con-
tinuous research on navigation technology by scholars at home and abroad. From
a technical aspect, due to the fact that the inertial navigation system has features
like strong autonomous ability, high precision within short time span, interference
immunity, and can be easily disguised, therefore in the integrated system, the inertial
navigation system mostly serves as a core navigation system, with other systems
assisting it to improve the accuracy and reliability of navigation. The current de-
velopment status of INS/GNSS, INS/CNS, and INS/CNS/GNSS navigation will be
described as follows.

1.2.1 INS/GNSS Navigation

The integrated system of the inertial navigation system and the satellite navigation
system can not only fix the accumulated error, but also calibrate the error of inertial
devices. Meanwhile, the use of inertial navigation system can help the satellite navi-
gation system improve its ability to track satellites, enhancing dynamic performance
and antidisturbance capability of the receivers [1]. Thus, the integrated INS/GNSS
navigation system is an ideal solution, which is an important development direc-
tion. The following paragraphs will be focused on the development of INS/GNSS
navigation system technology from the two aspects of integration objects and modes.
As for the advancement of integration objects, there has been very rapid develop-
ment of inertial navigation systems and satellite navigation systems due to military
demand. On the one hand, with the emergence of new inertial devices such as optical
gyroscope, MEMS gyro and the new advances in computer technology, the novel
strap-down inertial navigation has become a major trend of the inertial navigation
system. Currently, the strap-down inertial navigation system is mainly focused on
two directions, namely, precision fiber optic gyro strap-down inertial navigation sys-
tem and low-cost MEMS/MOEMS gyro strap-down inertial navigation system. At
the same time, the accuracy, data update rate, and dynamic performance of the satel-
lite navigation system is improving, showing a diversified development trend, such
as the GPS in the USA, the GLONASS in Russian, the Galileo in Europe as well as
China’s “Big Dipper”, etc.
The diversified development and technological progress of strap-down inertial
navigation systems and the satellite navigation system has promoted the development
of the INS/GNSS navigation system; thus, the early single INS/GNSS navigation
system has evolved into various new types of the strap-down inertial navigation
system, integrated with a variety of satellite navigation systems; therefore, all sorts
of INS/GNSS navigation system are coexisting today to meet different application
needs.
1.2 The Current Status of INS/CNS/GNSS Navigation Development 5

With regard to the integration mode, the development of the INS/GNSS navigation
system has undergone a step-by-step process [1], that is, from the initial simple mode
to shallow mode, and further developed into the in-depth mode.
Simple integration model is the direct reset of the inertial navigation system by the
use of position and velocity of satellite navigation system. This integration mode can
fix the position and velocity errors of the inertial navigation system, but the attitude
errors and inertial device errors cannot be corrected; thus, it is the initial and most
simple INS/GNSS integration mode.
Subsequently, shallow integration mode has been widely applied in the INS/GNSS
navigation system, which uses the difference of output position and velocity data from
inertial navigation systems and the satellite navigation system, respectively, as an
observed quantity; thus, the inertial navigation system errors can be estimated and
corrected by filter. The merit of this integration mode is that it is simple and easy to
be implemented in engineering, while the demerit is that it cannot correct errors in
the satellite navigation system, as a result, the advantages of both the systems cannot
be fully exploited to achieve surpassing performance.
In order to achieve a better complement of the inertial navigation system and the
satellite navigation system, the in-depth integration mode has been proposed. It is an
advanced integration mode in which the inertial navigation system and the satellite
navigation system can aid and correct each other to achieve surpassing performance.
As satellite navigation technology improves, a variety of different in-depth modes
are developed. It has evolved from the early pseudorange/pseudorange rate in-depth
integration mode, to the current carrier phase in-depth integration mode, and an
auxiliary mode for tracking loop of satellite navigation receiver by use of position
and velocity parameters of the inertial navigation system. With the improvement of
integration mode, the inertial navigation system and the satellite navigation system
can complement and aid each other, promoting the performance of the INS/GNSS
navigation system.

1.2.2 INS/CNS Navigation

The inertial navigation system and the celestial navigation system all have strong au-
tonomous ability. The former can output navigation information continuously, but the
errors accumulate over time; while the latter can output attitude and position informa-
tion without error accumulation, but the output is not continuous. Thus, the two are
highly complementary and the integration of them, that is, the INS/CNS integrated
navigation system can make full use of their complementary advantages; therefore,
the accuracy and reliability of navigation systems can be further improved. In this
way, it has become an important means of high-performance navigation for mis-
siles, aircrafts, satellites and other long-range, long-endurance aircrafts, especially
for maneuvering launch and underwater missile launch. In the following passage,
the INS/CNS navigation system will be briefly described from integration objects,
integration modes, and integration methods.
6 1 Introduction

In terms of the development of integration object, as we mentioned above, the

FOG- and MEMS/MOEMS-based gyro strap-down inertial navigation system has
gradually become the two main development direction of the inertial navigation
system. On celestial navigation technology, the development of the celestial sensor
technology is mainly focused. Celestial sensors include star sensor, sun sensor, earth
sensor, moon and planets sensitive sensors, etc., among which, star sensors have been
widely applied due to its high pointing accuracy. Currently, there are two main types
of star sensors: charge coupled device (CCD) star sensor and complementary metal-
oxide-semiconductor active pixel sensor (CMOS APS) star sensor; the former has a
high image quality and high accuracy, but it is not easy to be integrated; the latter has
a strong antiradiation ability, large dynamic range, low power consumption and low
cost, and it’s easy to be integrated with peripheral circuits, signal processing, and
large-scale circuits, etc., so it has wide applications. Thanks to the rapid development
of inertial sensor devices and celestial sensor technology, the INS/CNS integrated
navigation continues to progress, and a variety of systems have adopted the INS/CNS
integrated navigation system, such as the American B-2A bombers, the Russian
SS-N- 23 ballistic missiles, and the French M51 [18].
Concerning the integration mode, the INS/CNS integrated navigation mode has
evolved from the platform mode to full strap-down mode thanks to the development
of the star sensor technology, the inertial navigation systems technology, and the
integration technology. In full strap-down mode, the inertial navigation system and
star sensors are installed in way of strap-down, and it is of low cost and high reliability
compared with platform mode. Of course, this mode also asks for high performance
of the gyroscope, accelerometer sensor, and star sensors. However, the full strap-
down INS/CNS integrated navigation system is still the most promising modes of
operation considering the current status and development trend of the future.
As to the integration method, the INS/CNS navigation method can be divided
into simple INS/CNS integration and optimal estimation-based INS/CNS integra-
tion, considering the currently existing INS/CNS integrated systems. The former
integrates inertial, celestial navigation system with each other to realize real-time
calibration of attitude error angle in the inertial navigation system by the use of
celestial navigation observation information; the latter integrates inertial, celestial
navigation system to achieve real-time estimation and compensation of inertial errors
by the use of observation information from the celestial navigation system, combined
with the extended Kalman filter, unscented Kalman filter, unscented particle filtering
and other advanced filtering methods, in order to improve the accuracy and reliability
of the INS/CNS integrated navigation system.

1.2.3 INS/CNS/GNSS Navigation

As mentioned above, the integrated system of inertial, satellite, and celestial naviga-
tion can greatly satisfy the requirements of high-performance navigation for missiles,
1.2 The Current Status of INS/CNS/GNSS Navigation Development 7

aircrafts, satellites and other moving vehicles; therefore, it has become the most
effective means of high-performance navigation for spacecraft.
Currently, the INS/CNS/GNSS navigation system has developed towards a diver-
sified trend, and it has been widely applied for high-performance navigation in the
fields of aeronautics and astronautics. A typical application is the LN-120G naviga-
tion system in the USA. RC-135 reconnaissance aircraft developed by the Northrop
Company in 2005. This navigation system is an enhanced GPS and INS/CNS inte-
grated navigation system which can track the stars day and night with its position
accuracy up to 15 m, speed accuracy up to 0.15 m/s; meanwhile, the heading accuracy
is better than 20„ and the pitch/roll better than 0.05◦ .
To achieve high-performance navigation in integrated navigation system, the in-
formation fusion filter is indispensable. There are two ways for the INS/CNS/GNSS
navigation system to realize optimal information fusion by Kalman filtering tech-
nique: one is centralized Kalman filtering and the other is distributed Kalman filtering.
Centralized Kalman filter is to have a centralized processing of all the subsystem in-
formation using a single Kalman filter. In theory, a centralized Kalman filter provides
the optimal error estimation and the filtering equation is simple at the same time, so
the centralized Kalman filter is a common way to deal with optimal state estimation.
Distributed Kalman filter is to use the subfilters to process subsystem information
and achieve information fusion of all subsystems through overall filter. Among the
many distributed filtering methods, the federal filtering proposed by Carlson, has
been paid much attention due to its design flexibility, small amount of calculation,
good performance, and fault-tolerance ability [19]. Federal filtering can solve the
following three questions [13]: first, to improve fault tolerance of the system. When
one or several navigation subsystem fails, it can easily detect and isolate faults, re-
construct the left subsystems quickly and give the desired filtered solution; second,
to improve system accuracy. Federated filter is a suboptimal filtering in nature. It
depends on the measurement data of each subsystem to further improve its accuracy
as far as possible; third, to enhance real timeliness. The algorithm (fusion) from the
partial to the overall filter is simple which asks for only small amount of calculation,
so that the algorithm can be run in real time.
In addition, on the research of INS/GNSS, INS/CNS, INS/CNS/GNSS integrated
navigation filtering methods, it is mainly focused on how to further improve the
accuracy and real timeliness of the integrated navigation system. The optimal esti-
mation methods commonly used in existing integrated navigation are Kalman filter,
extended Kalman filter, unscented Kalman filter, particle filter, the unscented particle
filter method, and predictive filtering methods. Owning to the development of infor-
mation and intelligent technology, some of the advanced intelligent methods have
gradually been introduced into the integrated navigation system, such as the fuzzy
adaptive Kalman filtering method [20], the integrated multimodel adaptive federal
filter navigation method [21], genetic algorithms-based adaptive filtering methods
[22; 23], the neural network state estimation method [24], genetic algorithms-based
multimodel Kalman filter algorithm [25] and so on. These algorithms have produced
relatively good results under certain conditions.
8 1 Introduction

References

1. Yuan X, Yu JX, Chen Z (1993) Navigation system. Aviation industry, Beijing

2. Hu XP (2002) Theory and application of autonomous navigation. National University of defense
technology, Changsha
3. Wan DJ, Fang JC (1998) Initial alignment of inertial navigation system. Southeast University,
Nanjing
4. Qin YY (2006) Inertial navigation. Science, Beijing
5. Deng ZL (2006) Inertial technology. Harbin institute of technology, Harbin
6. Grewal MS, Weill LR, Andrews AP (2007) Global positioning systems, inertial navigation,
and integration. Wiley, Hoboken
7. Yi GQ (1987) Inertial navigation principle. Aviation industry, Beijing
8. Deng ZL (1994) Inertial navigation system. Harbin Institute of Technology, Harbin
9. Wan DJ, Fang JC, Wang Q (2000) Theory and method of GPS dynamic filter and its application.
Jiangsu Science and Technology, Nanjing
10. Dong ZR, Zhang SX, Hua ZC (1998) GPS/INS integrated navigation and its application.
National University of Defense Technology, Beijing
11. Fang JC, Ning XL, Tian YL (2006) The spacecraft autonomous celestial navigation principle
and method. National defense industry, Beijing
12. Fang JC, Ning XL (2006) Principle and application of celestial navigation. Beihang University,
Beijing
13. Qin YY, Zhang HY, Wang SH (1998) Kalman filter and principle of integrated navigation.
Northwestern polytechnical University, Hsian
14. Rogers RM (2007) Applied mathematics in integrated navigation systems. American institute
of aeronautics and astronautics, Inc., Reston
15. Yi X, He Y (2003) Comments on multisensor integrated navigation system. J Fire Control
Command Control 28(4):28–32
16. Revision committee of china aerospace corporation (1998) World missiles. Military Science,
Beijing
17. Liu QX, Liu YH (2001) Status quo and future of integrated navigation system. J Mech Electr
Equip (3):31–33
18. Tan HQ, Liu L (2008) Inertia/Star integrated navigation technology. J Cruise Missile (5):44–51
19. Fu MY, Deng ZH, Zhang JW (2003) Kalman filtering theory and its application in navigation
system. Science, Beijing
20. Bai J, Liu JY, Yuan X (2002) Study of fuzzy adaptive Kalman filtering technique. J Astronaut
control (1):193–197
21. Li YH, Fang JC (2003) A multi-model adaptive federated filter and it’s application in INS/
CNS/ GPS integrated navigation system. J Astronaut Control (2):33–38
22. Ostrowki T (1995) Genetic algorithm approach to nonlinear adaptive filtering. J Tech Phys
36(1):89–101
23. Ostrowski T (1994) Adaptive filter design using genetic algorithm, Proceedings of the
international society for optical engineering laser technology IV
24. Tham YK, Wang H, Teoh EK (1999) Multiple-sensor fusion for steerable four-wheeled
industrial vehicles. Control Eng Pract 7:1233–1248
25. Wan ZL, Fang JC, Quan W (2004) Multi-model Kalman filter algorism based on GA. J Beijing
Univ Aeronaut Astronaut 30(8):748–752
Chapter 2
Principle of INS/CNS/GNSS Navigation System

2.1 Introduction

Inertia/celestial/satellite navigation can provide high-precision, highly reliable posi-

tion, velocity, and attitude information with strong autonomous ability, thus it is the
most effective means of high-performance navigating for new generation aircraft.
To achieve high accuracy navigation for vehicles, i.e., to determine the position,
velocity and attitude information, we must first understand the motion information
of the vehicles to which the reference coordinate system relates; meanwhile, for
mastering the inertia/celestial/satellite navigation system technology, the basic prin-
ciple and knowledge of inertial, satellite, and celestial navigation also need to be
understood. For this reason, coordinate systems commonly used in navigation and
Earth reference model will be briefly introduced firstly in the following paragraphs,
and then, the working principle and error characteristics of inertial, satellite, and
celestial navigation system (CNS) will be further described.

2.2 Coordinate Frames and Earth Reference Model Commonly

Used in Navigation

2.2.1 The Coordinate Frames Used in Navigation

For the purpose of navigation, the vehicles need to be provided with the position,
velocity, and attitude information relative to space or an object. Due to the relativity
of movement, an appropriate coordinate system needs to be established to describe
the parameters of a vehicle. Coordinate systems used in navigation can be divided
into inertial (absolute) and noninertial (relative) coordinate system.

© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 9
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_2
10 2 Principle of INS/CNS/GNSS Navigation System

1. Inertial Coordinate Frame of Reference

Inertial coordinate system complies with Newton’s laws of mechanics, in which any
free motion has a constant magnitude and direction. Everything in the universe is
in motion, thus selection of the inertial coordinate system will be based on different
navigation needs of objects. The following paragraphs will describe two common
inertial coordinate systems.
1) Geocentric inertial coordinate system (Referred to as the i)
Geocentric inertial coordinate system is also known as celestial coordinate system.
It does not consider the orbital motion of the Earth around the Sun or the movement
of the Sun relative to space. The origin of the coordinate system is the center of the
Earth and axis xi , yi are in the Earth’s equatorial plane. The axis xi points to the
equinox, the axis zi points to the Earth’s polar, and the right-hand rule determines
the direction of axis yi .
2) Launch point inertial coordinate system (Referred to as the li)
Some vehicles (such as missiles) use launch point inertial coordinate system as the
reference for measuring flying position. The origin of the coordinate system is the
emission point; axis xli points to the launching direction, axis yli points to the sky,
and the direction of axis zli is determined by the right-hand rule. After launching, the
launch point inertial coordinate system will be constantly set in the inertial space.
2. Noninertial Coordinate System

1) Earth coordinate system (Referred to as the e)

Earth coordinate system is fixed with the Earth and rotates with the Earth. It
is approximated that the coordinate system revolves at the Earth’s rotation rate
ωie (ωie ≈ 15.04107◦ / h) relative to the inertial coordinate system. The origin of
the coordinate system is the center of the Earth; axis ze points to the Earth’s polar,
axis xe extends to zero meridian line, and direction of axis ye is determined by the
right-hand rule (pointing to 90◦ E).
2) Geographic coordinate system (Referred to as the t)
The geographic coordinate system commonly used is the east-north-up coordinate
system and the north-east-down coordinate system. The origin of east-north-up co-
ordinate system is the centroid of the vehicle; axis xt points toward the east along the
prime vertical of reference ellipsoid direction, axis yt points to the north along the
meridian of reference ellipsoid, and axis zt is determined by the right hand rule. The
origin of the north-east-down coordinate system is also the centroid of the vehicle;
axis xt points to the north along the meridian of reference ellipsoid, axis yt points to
the east along prime vertical of the reference ellipsoid, and axis zt is determined by
the right-hand rule.
2.2 Coordinate Frames and Earth Reference Model Commonly Used in Navigation 11

Fig. 2.1 Body coordinate

system for airplane zb

yb
Ob

xb
3) Body coordinate system (Referred to as the b)
This section will introduce the body coordinate system for aircraft, missiles, and
satellites.
1 Body Coordinate System for Airplane

The plane body coordinate system is a coordinate system fixed on the body of the
plane. Its origin is the centroid of the aircraft; axis xb points to right wing along
the horizontal axis of the carrier, axis yb points to the head of the plane along the
longitudinal direction of the carrier, and axis zb points upward to the carrier along
the vertical axis, as shown in Fig. 2.1.
2 Body Coordinate System for Missile

The missile body coordinate system is fixed with the missile body. Its origin is the
centroid of the missile; axis xb points to the head direction of the missile along the
longitudinal axis, axis yb is vertical to axis xb and points upward, and the direction
of axis zb is determined by the right-hand rule, as shown in Fig. 2.2.
3 Satellite Coordinate System

The satellite coordinate system is fixed with the satellite body. Its origin is the satellite
centroid; axes xb , yb , and zb are normally defined on the inertial principle axis of
the satellites, which is also known as principle axis coordinate system, as shown in
Fig. 2.3.
4) Platform coordinate system (Referred to as the p)
For platform inertial navigation system, the platform coordinate system is fixed with
the platform which describes the pointing direction of the platform with its origin
at the centroid of it. For strapdown inertial navigation system (SINS), the platform
coordinate system is realized by direction cosine matrix storing in the computer, thus
it is also called “math platform.”
12 2 Principle of INS/CNS/GNSS Navigation System

yb
Fig. 2.2 Body coordinate
system for missile

xb

Ob

zb
Fig. 2.3 Body coordinate
system for satellite

xb

yb

zb

5) Navigation coordinate system (Referred to as the n)

The navigation coordinate system is chosen while solving the navigation parameters
according to working needs. For SINS, the navigation parameters are not solved
within the body coordinate system, while the measurement values of inertial device
is generally in the body coordinate system. Therefore, the output of inertial devices
must be decomposed into different coordinate system for the convenience of solving
navigation parameters, then navigation calculation is carried out, and this coordinate
system is the navigation coordinate system. For platform inertial navigation system,
the platform coordinate system is the navigation coordinate system.
2.2 Coordinate Frames and Earth Reference Model Commonly Used in Navigation 13

2.2.2 The Conversion of Coordinate Systems

Currently, the methods commonly used to describe the relation between two
coordinate systems are the direction cosine matrix method and quaternion.
The following paragraphs will introduce coordinate conversion system from sys-
tem n to b by taking aircraft as an example, using the usual rotation sequence
Z → X → Y , i.e., first turn the heading angle ϕ, then the pitch angle θ , and
roll angle γ at last.
1. Direction Cosine Matrix Method
Direction cosine matrix is one of the most basic and direct conversion matrix methods
between two coordinate systems. According to the above order, the direction cosine
matrixes after three rotations are:
⎡ ⎤ ⎡ ⎤
cos ϕ sin ϕ 0 1 0 0
⎢ ⎥ ⎢ ⎥
Tϕ = ⎢ ⎥
⎣− sin ϕ cos ϕ 0⎦ Tθ = ⎣0 cos θ
⎢ sin θ ⎥
⎦
0 0 1 0 − sin θ cos θ
⎡ ⎤
cos γ 0 − sin γ
⎢ ⎥
Tγ = ⎢ ⎣ 0 1 0 ⎥ ⎦
sin γ 0 cos γ

Cnb = Tγ · Tθ · Tϕ
⎡ ⎤
cos γ cos ϕ − sin γ sin θ sin ϕ cos γ sin ϕ + sin γ sin θ cos ϕ − sin γ cos θ
⎢ ⎥
=⎣⎢ − cos θ sin ϕ cos θ cos ϕ sin θ ⎥.
⎦
sin γ cos ϕ + cos γ sin θ sin ϕ sin γ sin ϕ − cos γ sin θ cos ϕ cos γ cos θ
(2.1)

Among them, Cnb is the coordinate conversion matrix from system n to b.

2. Quaternion
Since the beginning of the 1960s, with the development of SINS, the observation
instrument itself translates and rotates with the vehicles in dynamic measurement,
which makes the description and solution of the problem very difficult. The quater-
nion theory classified such problem as the one of a rigid body rotating around a fixed
point, which effectively solves this problem, and thus has been widely used.
1) The definition of the quaternion
Definition of quaternion q:

q = q0 + q1 i + q2 j + q3 k
14 2 Principle of INS/CNS/GNSS Navigation System

where q0 is the quaternion scalar part, the latter is the quaternion vector part, denoted
as q, and the equation can be written as:

q = q0 + q

Define the quaternion mode as 1, and it is written as:


N(q) = q20 + q21 + q22 + q23 = 1.

2) Describing the vector rotation with quaternion

Quaternion can describe a rigid body rotating around a fixed point, which is to
describe the rotation of a coordinate or a vector relative to a coordinate system. For
quaternion q = q0 + q1 i + q2 j + q3 k, it can be written as follows:

θ θ θ θ
q = cos + sin cos αi + sin cos βj + sin cos γ k (2.2)
2 2 2 2
where θ is the rotation angle, cos α, cos β, cos γ is the direction cosine between
instantaneous rotation axis and the reference coordinate axis.
Compare the two equations:
θ θ θ θ
q0 = cos , q1 = sin cos α, q2 = sin cos β, q3 = sin cos γ (2.3)
2 2 2 2
Usually, this type of formula (2.2) is called feature quaternion, referred to as quater-
nion. The scalar part of the quaternion cos θ/2 represents the cosine of half the
rotation angle, and its vector part represents the direction of instantaneous rotation
axis. Therefore, a quaternion represents both the direction of the axis and the size of
the rotation angle; this relation can be realized by the following operation:

Rb = q ◦ Rn ◦ q ∗ (2.4)

Formula (2.4) indicates that the vector R n is rotated at θ relative to the reference
coordinate, instantaneous angle is determined by the quaternion q and becomes
vector R b after rotation. Vector q ∗ is the conjugate quaternion of quaternion q. This
is the rotating vector expression of the reference coordinate.
From the Formula (2.4), it can be obtained:
⎡ ⎤
q02 + q12 − q22 − q32 2 (q1 q2 − q0 q3 ) 2 (q1 q3 + q0 q2 )
⎢ ⎥
⎢ ⎥
⎢ ⎥
R = ⎢ 2 (q1 q2 + q0 q3 )
b
q02 − q12 + q22 − q32 2 (q2 q3 − q0 q1 ) ⎥ R n
⎢ ⎥
⎣ ⎦
2 (q1 q3 − q0 q2 ) 2 (q2 q3 + q0 q1 ) q0 − q1 − q2 + q3
2 2 2 2

(2.5)
2.2 Coordinate Frames and Earth Reference Model Commonly Used in Navigation 15

So,
⎡ ⎤
q02 + q12 − q22 − q32 2 (q1 q2 − q0 q3 ) 2 (q1 q3 + q0 q2 )
⎢ ⎥
⎢ ⎥
⎢ ⎥
Cnb = ⎢ 2 (q1 q2 + q0 q3 ) q02 − q12 + q22 − q32 2 (q2 q3 − q0 q1 ) ⎥. (2.6)
⎢ ⎥
⎣ ⎦
2 (q1 q3 − q0 q2 ) 2 (q2 q3 + q0 q1 ) q02 − q12 − q22 + q32

From formulas (2.1) and (2.6), it can be drawn that the direction cosine matrix and
the quaternion methods can both achieve conversion between coordinate systems, in
essence, they are equivalent.

2.2.3 Earth Reference Model

1. Main Radius of Curvature of the Earth

The Earth is not a homogeneous sphere, but a nonhomogeneous ellipsoid resulted
from the Earth’s gravity and centrifugal force. There exist mountains, deep valleys,
continents, and oceans on the Earth’s surface, thus it has an irregular curve surface
and the curvature radius of any point is different. As shown below in Fig. 2.4a, 2.4b,
the Plane G is the tangent plane of a point M on the Earth’s surface with the normal
n; AB is the tangent of point M along the south- north direction, CD is the tangent of
point M along the east- west direction. Plane AB and n determine the main radius of
curvature of the meridian plane and the Plane CD and n determine the main radius
of curvature of the prime plane [1].
From the elliptic equation:
z2 x2
+ =1 (2.7)
Rp2 Re2
Lt is the angle between the normal of point M and scale plane of reference ellipsoid,
we have:
dz Rp 2 x
= − 2 = − cot Lt (2.8)
dx Re z
By the
Rp 2
= 1 − k2 (2.9)
Re2
Substitute into Formula (2.8), we have:

z = (1 − k 2 )x tan Lt (2.10)
16 2 Principle of INS/CNS/GNSS Navigation System

N n
A
g G L
K M g D
Cg g
F
E
OE
g B
Qg

S
a
z

Rp
M

Lc Lt
x
OE N Re
b
Fig. 2.4 Reference ellipsoid. a Primary curvature radius of the Earth reference ellipsoid. b Partial
cutaway view of the reference ellipsoid

Then substitute the above formula into elliptic equations, we have:

2
(1 − k 2 ) x 2 tan2 Lt x2
+ =1
Rp2 Re2
2.2 Coordinate Frames and Earth Reference Model Commonly Used in Navigation 17

Then:
Re cos Lt
x= 1/2
(2.11)
(1 − k 2 sin2 Lt )
That is, the expression of the radius curvature can be written as follows:

2 3/2 2
dz d z
ρ = 1+ (2.12)
dx dx 2

From the Formula (2.8), we have:

d 2z 1 dLt
= (2.13)
dx 2 sin2 Lt dx
From Formula (2.11), we have:
dx 3/2
= −Re (1 − k 2 ) sin Lt /(1 − k 2 sin2 Lt ) (2.14)
dLt
Eventually, after finishing, the radius curvature of the Earth in meridian plane can
be obtained as:
Re (1 − k2 )
RM = 3/2
(2.15)
(1 − k2 sin2 Lt )
Re −Rp
As e = Re
, obviously, e < 0, k 2 = 2e − e2 , omitting e2 :

Re (1 − 2e)
RM = (2.16)
(1 − 2esin2 Lt )3/2
−3/2
Take (1 − 2esin2 Lt ) ≈ 1 + 3esin2 Lt , then:
−3/2
(1 − 2esin2 Lt ) ≈ 1 + 3esin2 Lt . (2.17)

Now we find the radius curvature RN perpendicular to meridian plane with the same
normal. According to the law of radius of curvature in any plane surfaces, the relation
between radius curvature RN of the curve at the point of M and radius of latitude
circle of the same point is as follows:

x = RN cos Lt (2.18)

Then:
Re
RN = (2.19)
(1 − 2k sin2 Lt )1/2
2

−1/2
Take k 2 = 2e − e2 and omit e2 . Then take (1 − 2esin2 Lt ) ≈ 1 + esin2 Lt
18 2 Principle of INS/CNS/GNSS Navigation System

Finally we have:
−1/2
(1 − 2esin2 Lt ) ≈ 1 + esin2 Lt . (2.20)

2. Several Vertical Lines

The vertical lines mentioned in this book are mainly astronomy, geographic, and
geocentric vertical line, shown in Fig. 2.5. Astronomy vertical line is the direction of
normal line at the horizontal plane of the Earth of any point on the Earth’s surface.
The angle between the equatorial plane, the astronomy vertical line is defined as
astronomical latitude Lg . Geographic vertical line is the normal direction of any
point on the reference ellipsoid. The angle between geography vertical line and the
equatorial plane is defined as geographic latitude Lt . Geocentric vertical line is the
connection of a point on the Earth’s surface with the center of the reference ellipsoid,
and the angle between the equatorial plane and geocentric vertical line is defined as
geocentric latitude Lc .
The three latitudes are interrelated with each other [2]. Set Lc as the deviation
angle between geographic and geocentric vertical line, then:

Lct = Lt − Lc (2.21)

Set the coordinate of M point on the ellipse as ( x, z), then the elliptic equation is:
z2 x2
+ =1 (2.22)
Rp2 Re2

The normal of ellipse (geographic vertical line) has a slope of:

dx Re 2 z2
tan Lt = − =− 2 2 (2.23)
dz Rp x

Fig. 2.5 Several vertical lines z

and latitude
Re

Rp
M

Lc Lg

Lt
OE x
2.2 Coordinate Frames and Earth Reference Model Commonly Used in Navigation 19

Fig. 2.6 Gravity vectors Ω

P
－Ω×（Ω×R）
R

G g

Geocenter line of the same point (geocentric vertical line) has a slope of:
x
tan Lc = (2.24)
z

tan Lt − tan Lc Re2 − Rp2

tan Lct = = xz (2.25)
1 + tan Lt tan Lc 2Re2 Rp2
√
Set R = x 2 + z2 as the length of geocentric line, we have:

Re2 − Rp2 x z Re2 − Rp2 2

tan Lct = R2 = R sin 2Lc
2Re2 Rp2 zR 2Re2 Rp2
(2.26)
Re − Rp Re + Rp R 2
= sin Lc ≈ E sin 2Lc
Re 2Rp Rp Re

Re + Rp R2 Re − Rp
where ≈ 1, ≈ 1, E = .
2Rp R p Re Re
In practice, Lct is very small, Lt is commonly used to replace Lc , the deviation
angle approximation between the geographic vertical line and geocenter line can be
obtained as:

Lct = E sin 2Lt (2.27)

From Formula (2.27), Lct gets maximum value about 10 when Lt = 45◦ .
3. Gravity Field
1) The expression of gravity field under local coordinate system
Set any point on the Earth’s surface as P, its gravitational force g is the combination
of gravity G and the negative direction of the Earth’s rotation centripetal acceleration
(the centrifugal force per unit mass) −ωie × (ωie × R) (Fig. 2.6), namely:
20 2 Principle of INS/CNS/GNSS Navigation System

g = G − ωie × (ωie × R) (2.28)

where R is the position vector of point P relative to the center of the Earth, ωie is the
Earth’s rotation rate:

ωie = 7292115.1467 × 10−11 rad/s ≈ 15.04107o / h

According to the reference ellipsoid parameters, gravity at different latitudes can

be calculated in theory [2, 3] According to the global surface coordinate frame in
WGS-84, the gravity can be expressed as:
1/2
g = ge (1 + ksin2 Lt )/(1 − e2 sin2 Lt ) (2.29)

In the above formula, k = [Rp gp /(Re ge )] − 1, gp and ge are reference ellipsoid

equator and theoretic gravity of the pole respectively, and e is the first eccentricity
of reference ellipsoid. The gravity numeric expression based on is:

978.03267714 × (1 + 0.00193185138639sin2 L)
g= 1/2
.
(1 − 0.00669437999013sin2 L)
Due to the irregular shape and uneven mass distribution of the Earth, the theoretical
value of gravity of a point on the Earth is different from that of the actual mea-
surement, and this difference is called gravity anomaly. The direction of gravity is
usually inconsistent with the normal direction of this point at the reference ellipsoid,
and this bias is called the vertical deflection. In general, gravity anomaly and vertical
deflection effects need to be considered in high-precision inertial navigation and it
can be achieved through ground observation.
2) Gravity field expressions under geocentric inertial coordinate system
Usually in the geocentric inertial coordinate system, the expression of gravitational
acceleration vector [1] is as follows:
⎡
2   ⎤
3 Re  r 2 
z
⎡ ⎤ ⎢ 1 + J2 1−5 rx ⎥
⎢ 2 r r ⎥
Gix ⎢ 
2    ⎥
⎢ ⎥ −μ ⎢ ⎢   ⎥
3 R r 2 ⎥
Gi = ⎢ ⎥= e z
⎣ G ⎦ ⎢ 1 + J 2 1 − 5 ry ⎥ (2.30)
iy
r3 ⎢ 2 r r ⎥
⎢
2   ⎥
Giz ⎢  r 2  ⎥
⎣ 3 Re z ⎦
1 + J2 3−5 rz
2 r r

where μ is the product of the mass of the Earth and the gravitational constant,
μ = 3.9860305 × 1014 m3 /s 2 ; Re is the Earth ’s equatorial radius, Re = 6378.165m;
J2 is the constant, J2 = 1.08230 × 10−3 ; r is the position vector of the vehicles in
geocentric inertial coordinate system. r = [rx , ry , rz ]T .
2.3 Inertial Navigation System 21

2.3 Inertial Navigation System

2.3.1 Work Principle of Inertial Navigation System

The basic operating principle of inertial navigation system is based on Newton’s laws
of mechanics. Gyroscopes are used to establish spatial coordinates baseline (navi-
gation coordinate system), and accelerometers are used to measure the acceleration
of the vehicles, then acceleration is switched to the navigation coordinate system,
after two integral operations, the position and velocity parameters of vehicles will be
ultimately determined. Inertial navigation system does not rely on any outside infor-
mation, or radiate energy to the outside world. Since it has obvious advantages like
high short-term accuracy, comprehensive motion information, and at the same time,
it is easily to be disguised and insusceptible to interference, it is the most important
means of navigation in the fields of aerospace, aviation, voyaging, and other areas.
According to the different ways of mounting for inertial devices on the vehicles
(gyroscopes and accelerometers), the inertial navigation systems can be classi-
fied into platform inertial navigation system (Gimbaled Inertial Navigation System,
GINS) and SINS [1]. In GINS, the inertial devices are installed on the platform iner-
tial navigation system, and the angular motion vectors and angular vibration can be
isolated by the inertial platform. In this way, the inertial devices have a good work-
ing environment, moreover, the platform can create navigation coordinates directly,
thus only small amount of calculation is needed, and the output of inertial devices
can be easily compensated and corrected. However, the GINS structure is complex,
bulky, and expensive. For SINS, there is no physical platform, and gyroscopes and
accelerometers are mounted directly on the vehicle, so the volume, weight, and cost
are greatly reduced. But due to the fact that the inertial devices are attached to the
vehicles, they have to withstand vibration and shock directly. This poor working
environment makes the reduction of measurement accuracy. Meanwhile, the ac-
celerometer output in SINS is the acceleration component along the body coordinate
system, and it needs to be converted to the navigation coordinate system, thereby
increasing the amount of computation. However, with the rapid development of iner-
tial devices and computer technology, this will not hinder the development of SINS,
and it has been widely applied in navigation field. Based on the data reported, all
the inertial navigation systems for the US military are all platform type in 1984, but
nearly half of them have changed to strapdown type in 1989. In 1994, the strap-
down type has accounted for about 90 %. Therefore, SINS has become the main
direction of development of inertial navigation system [4]. At present, the SINS has
been developed toward the direction of high accuracy and low-cost. According to the
forecast of US Draper Laboratory [5], until 2020, there will be two types of main-
stream SINSs worldwide, one is based on high-precision interferometric fiber-optic
gyroscope, and the other is based on low-cost MEMS/MOEMS gyroscope.
22 2 Principle of INS/CNS/GNSS Navigation System

f ibb Velocity n
ωen Position
Accelerometer Transformation C en L, λ , h
Calculating Calculating

Attitude ϕ, θ, γ
Attitude Matrix
Calculating
Error

Com- Quaternion
pen-
sation
Rotating
Vector n
ω en
+ +
Gyro C bn Cen ωiee
ωibb + b
- ωin ωinn ωien

Fig. 2.7 Principle of strapdown inertial navigation system (SINS)

During the work process of SINS, a series of navigation parameters, such as po-
sition (longitude, latitude, and altitude), velocity, and attitude need to be calculated.
Here, the principle of SINS will be briefly introduced [1].
The principle of SINS is shown in Fig. 2.7; for ease of illustration, vector and
matrix are used for description. The attitude angle for the vehicles is determined by
the three rotation angles from navigation coordinate (n) to body coordinate (b), which
are the heading angle ϕ, pitch angle θ and roll angle γ . Because of the changing
attitude of the vehicles, therefore, the elements of attitude matrix Cnb is a function of
time. To set the attitude of the vehicles at any time, a quaternion kinematic equation
should be solved when using the quaternion method to determine the attitude matrix
(if the direction cosine method is used, a direction cosine matrix equation needs to
be solved):
1
q̇ = q ◦ ωnb
b
(2.31)
2
where the angular velocity ωnbb b
is the attitude matrix update rate, ωnb = ωib
b
− ωin
b
b
the relationship between ωnb and the other angular velocity is:
 n 
b
ωnb = ωib
b
− Cnb ωin
n
= ωib
b
− Cnb ωie + ωen
n
(2.32)
b n
where ωib is the gyroscope output; ωie is angular rate of the Earth in the navigation
n
frame; ωen is displacement angular velocity, which can be determined by the relative
velocity of the vector; attitude angles can be obtained from the corresponding element
in the attitude matrix Cnb .
The accelerometer output is specific force fibb , the acceleration vector transforms
from system b to system n to get fibn through Cbn . The relative velocity Ven n
can be
n n
obtained by integrating the relative acceleration aen , and relative acceleration aen can
2.3 Inertial Navigation System 23

be obtained by the fibn after elimination of harmful acceleration, which is:

 n 
n
aen = fibn − 2ωie + ωen
n
× Ven
n
+ gn. (2.33)

The position calculating of the vehicles has something to do with the relative velocity
n
and the rate of the displacement angular ωen . On the basis of getting the relative
velocity and for reflecting this change correctly, we should solve a direction cosine
matrix equation due to the constantly changing position of the vehicles, which is:

Ċen = −
nen Cen (2.34)
⎡ ⎤
0 −ωenz
n n
ωeny
⎢ ⎥
where
nen = ⎢ n
⎣ ωenz 0 n ⎥, the position of the vehicle can be obtained
−ωenx ⎦
−ωeny ωenx
n n
0
from the corresponding element of Cne .

2.3.2 SINS System Error Equation and Error Propagation

Characteristics

For inertial navigation system, the establishment of an accurate error equation is

the basis for integrated navigation filter. Whether the inertial navigation system error
model is accurate or not directly affects the state and estimation accuracy as well as the
estimated time of the parameter. There are many sources of inertial navigation system
error, among which are inertial device error, initial alignment error, calculation error,
and error due to various disturbances. There are usually two error equations, one is
called  angle equation (real geographic coordinate method) and the other is called
the  angle equation (calculation geographic coordinate method). The two models
are equivalent to each other in essence [6, 7].
1. SINS Error Equation
In practical applications, ballistic missiles and aircraft chose different navigation
coordinates during flight. Due to the unstable vertical channel of inertial navigation
system, it is desirable to keep the directions of the navigation coordinate system as
the horizontal coordinate system, so that two of the three channels are insusceptible
to gravitational influence to secure the stability of the navigation calculation. The
aircraft whose navigation time is calculated by the hour, must meet this condition,
therefore geographic coordinate system or moving position coordinate system is often
used. For ballistic missiles, since navigation and control is only in the active section
(sometimes also includes modified section), and the flight time in active section is
usually less than 200 s, therefore, although the altitude channel is unstable, while the
divergence speed is very slow, altitude divergence can be ignored approximately in
the active section phase. In addition to be compatible with the calculation of guided
rate of ballistic missile in the coordinate system conversion, the inertial navigation
24 2 Principle of INS/CNS/GNSS Navigation System

system of ballistic missile uses a space stable manner, and the platform coordinate
system (including the physical platform of platform inertial navigation and “math
platform” of SINS) remains parallel to the launch point inertial coordinate system in
the missile flight process, thus, the navigation coordinate system of ballistic missile
usually adopts the launch point inertial coordinate system.
The following paragraphs will describe error equations of SINS under different
coordinate systems.
1) Error equation of inertial navigation system under geographic coordinate
Make the geographic coordinates of aircraft (east-north-up) as the basic coordinate
system for navigation solution [1], considering the altitude h and assuming that the
Earth is a spheroid.
Attitude error equation:
Attitude error equation of inertial navigation system is:


δvN vE
φ̇E = − + ωie sin L + tan L φN
RM + h RN + h


vE
− ωie cos L + φU + εE
RN + h


δvE vE vN
φ̇N = −ωie sin LδL− ωie sin L + tan L φE − φU + εN
RN + h RN + h RM + h


δvE vE
φ̇U = tan L + ωie cos L + sec2 L δL
RN + h RN + h


vE vN
+ ωie cos L + φE + φN + εU (2.35)
RN + h RM + h

where, E, N, U represent three directions of east, north, and sky of geographic coor-
dinates; RM is main radius of curvature of local meridian plane; RN is main radius
of main curvature of the plane vertical to meridian plane; L represents geographic
latitude.
Speed error equation


vN vU
δ v̇E = fibN
n
φU − fibU
n
φN + tan L − δvE
RM + h RM + h


vE
+ 2ωie sin L + δvN
RN + h


vE vN
+ 2ωie cos LvN + sec2 L + 2ωie sin LvU δL
RN + h


vE
− 2ωie cos L + δvU + ∇E
RN + h


vE vU
δ v̇N = fibU
n
φE − fibE
n
φU − 2 ωie sin L + tan L δvE − δvN
RN + h RM + h
2.3 Inertial Navigation System 25


vN vE
− δvU − 2ωie cos L + sec2 L vE δL + ∇N
RM + h RN + h


vE
δ v̇U = fibE
n
φN − fibN
n
φE − 2 ωie cos L + δvE
RN + h
vN
+2 δvU − 2ωie sin Lve δL + ∇U (2.36)
RM + h
Position error equation
δvN
δ L̇ =
RM + h
δvE vE (2.37)
δ λ̇ = sec L + sec L tan LδL
RN + h RN + h
δ ḣ = δvU

where λ represents a geographic longitude.

Inertial device error equation
Gyroscope and accelerometer errors are considered as a random constant, their
error equation is written as:
∇˙ x = 0; ∇˙ y = 0; ∇˙ z = 0; ε̇x = 0; ε̇y = 0; ε̇z = 0.
2) Error equation of the inertial navigation system under inertial coordinate
Take the inertial frame of reference for the missiles as the fundamental reference
frame for the navigation calculating and express the error equation below.
Attitude error equation
Ignoring the one order and two order errors of the gyroscope, the attitude error
equation can be expressed as below:
φ̇ = Cbli (ε + Wε ) (2.38)
where ε represents constant drift of the gyroscope, Wε is the Gaussian white noise
of the gyroscope.
The component form of the above equation is:
⎡ ⎤ ⎡ ⎤⎡ ⎤
φx C11 C12 C13 εx + Wzx
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢φy ⎥ = ⎢C21 C22 C23 ⎥ ⎢εy + Wz ⎥. (2.39)
⎣ ⎦ ⎣ ⎦⎣ y⎦

φz C31 C32 C33 εz + W zz

Acceleration error equation

Inertial coordinate system for missile differs from inertial coordinate system for
airplane. Under inertial frame of the missile, drift rate of the gyroscope equals to the
draft rate of the navigation system [8]. Ignoring the two-order error, the acceleration
error equation can be expressed as:
δa = φ × a + C lib (∇ + a∇ ) (2.40)
26 2 Principle of INS/CNS/GNSS Navigation System

where a represents the acceleration sensed by the accelerometer, φ is the misalign-

ment angle of the platform, a∇ is the Gaussian white noise of the accelerometer, C lib
is the transfer matrix for the inertial frame of the launching point.
⎡ ⎤
C11 C12 C13
⎢ ⎥
C lib = ⎢
⎣C21 C22 C23 ⎦
⎥

C31 C32 C33

⎡ ⎤
cos θ cos ϕ cos θ sin ϕ sin γ − sin θ cos γ cos θ sin ϕ cos γ + sin θ sin γ
⎢ ⎥
=⎢ ⎥
⎣ sin θ cos ϕ sin θ sin ϕ sin γ + cos θ cos γ sin θ sin ϕ cos γ − cos θ sin γ ⎦
− sin ϕ sin γ cos ϕ cos γ cos ϕ
(2.41)
Rewrite Formula (2.40) in component form:
⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤⎡ ⎤
δax 0 az −ay φ C C12 C13 ∇x + a∇x
⎢ ⎥ ⎢ ⎥ ⎢ x ⎥ ⎢ 11 ⎥⎢ ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ δay ⎥ = ⎢ −az 0 ax ⎥ ⎢ φy ⎥ + ⎢ C21 C22 C23 ⎥ ⎢ ∇y + a∇y ⎥.
⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥
⎣ ⎦ ⎣ ⎦⎣ ⎦ ⎣ ⎦⎣ ⎦
δaz ay −ax 0 φz C31 C32 C33 ∇z + a∇z
(2.42)
Velocity-position error equation
Simplify the gravitation field model, the velocity-position error equation is:
⎡ ⎤ ⎡ ⎤⎡ ⎤
δ V̇x 0 0 0 f14 f15 f16 δVx
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ δ V̇y ⎥ ⎢0 0 0 f24 f25 f26 ⎥ ⎢ δVy ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ δ V̇z ⎥ ⎢0 0 0 f34 f35 f36 ⎥ ⎢ δVz ⎥
⎢ ⎥=⎢ ⎥⎢ ⎥ (2.43)
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ δ ẋ ⎥ ⎢1 0 0 0 0 0 ⎥ ⎢ δx ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ δ ẏ ⎥ ⎢0 1 0 0 0⎥ ⎢ ⎥
⎣ ⎦ ⎣ 0 ⎦ ⎣ δy ⎦
δż 0 0 1 0 0 0 δz

where,


∂gx GM x2 ∂gx GM x(y + R0 )
f14 = − 3 1−3 2 ; f15 = =3 3 ;
∂x r r ∂y r r2

∂gx GM xz ∂gy ∂gx

f16 = =3 3 2 f24 = = = f15
∂z r r ∂x ∂y



∂gy GM (R0 + y)2 ∂gy GM (R0 + y) z
f25 = =− 3 1−3 f26 = =3 3
∂y r r2 ∂z r r2
2.3 Inertial Navigation System 27

∂gz ∂g ∂gz ∂gy

f34 = = x = f16 f35 = = = f26
∂x ∂z ∂y ∂z

 
∂gz GM z2
f36 = =− 3 1−3 2 r= x2 + (y + R0 )2 + z2
∂z r r

Coefficient f14 , f15 , f16 , f24 , f25 , f26 , f34 , f35 , f36 is the derivative of gravitational
acceleration, which varies from different missile position. x, y, z is the missile
position in the inertial frame of reference.
Inertial device error equation
Errors of gyroscope and accelerometer should be considered as random constant.
2. Error Propagation Characteristics of SINS
For simplicity, error propagation characteristic of inertial navigation system under
stationary base is studied. Since the vertical channel of inertial navigation is instable,
it can be omitted; longitude errors and other errors have no coupling correlation, so
it can be considered separately without affecting the other dynamic characteristics of
the system [8]. Thus, the other errors that have a coupling relation can be described
by matrix form as follows:
⎡ ⎤
0 2wie sin L 0 0 −g 0
⎢ ⎥
⎡ ⎤ ⎢ ⎢
⎥
⎥
δ̇Vx ⎢−2wie sin L 0 0 g 0 0 ⎥
⎢ ⎥ ⎢ ⎢ ⎥
⎢δ̇Vy ⎥ ⎢ ⎥
⎢ ⎥ ⎢ 1 ⎥
⎢ ⎥ ⎢ 0 0 0 0 0 ⎥
⎢δ L̇ ⎥ ⎢ ⎥
⎢ ⎥=⎢ R ⎥
⎢ ⎥ ⎢ ⎥
⎢φ̇x ⎥ ⎢ 1 ⎥
⎢ ⎥ ⎢ 0 0 0 w ie sin L −w ie cos L ⎥
⎢φ̇ ⎥ ⎢ R ⎥
⎣ y ⎦ ⎢ ⎥
1 ⎥
⎢ 0 −wie sin L −wie sin L 0 0 ⎥
φ̇ z ⎢ ⎥
⎢ R ⎥
⎣ tgL ⎦
0 wie cos L wie cos L 0 0
R
⎡ ⎤ ⎡ ⎤
δVx ∇x + adx
⎢ ⎥ ⎢ ⎥
⎢ δVy ⎥ ⎢ ∇y + ady ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ δL ⎥ ⎢ 0 ⎥
⎢ ⎥+⎢ ⎥ (2.44)
⎢ ⎥ ⎢ ⎥
⎢ φx ⎥ ⎢ εx + wdx ⎥
⎢ ⎥ ⎢ ⎥
⎢φ ⎥ ⎢ε + w ⎥
⎣ y ⎦ ⎣ y dy ⎦

φz εz + wdz
28 2 Principle of INS/CNS/GNSS Navigation System

To have Laplace transform for Formula (2.44):

⎡ ⎤
0 2wie sin L 0 0 −g 0
⎡ ⎤ ⎢ ⎥
sδVx (s) ⎢−2wie sin L 0 0 0 0 ⎥
⎢ g ⎥
⎢ ⎥ ⎢ 1 ⎥
⎢sδVy (s)⎥ ⎢ ⎥
⎢ ⎥ ⎢ 0 0 0 0 0 ⎥
⎢ ⎥ ⎢ R ⎥
⎢sδL(s) ⎥ ⎢ ⎥
⎢ ⎥=⎢ 1 ⎥
⎢ ⎥ ⎢ 0 0 0 wie sin L −wie cos L⎥
⎢sφx (s) ⎥ ⎢ R ⎥
⎢ ⎥ ⎢ ⎥
⎢sφ (s) ⎥ ⎢ 1 ⎥
⎣ y ⎦ ⎢ 0 −w sin −w sin 0 0 ⎥
⎢ ie L ie L ⎥
sφz (s) ⎢ R ⎥
⎣ tgL ⎦
0 wie cos L wie cos L 0 0
R
⎡ ⎤ ⎡ ⎤
δVx0 (s) ∇x (s) + adx (s)
⎢ ⎥ ⎢ ⎥
⎢δVy0 (s)⎥ ⎢∇y (s) + ady (s)⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢δL0 ⎥ ⎢0 ⎥
⎢ ⎥+⎢ ⎥ (2.45)
⎢ ⎥ ⎢ ⎥
⎢φx (s) ⎥ ⎢εx (s) + wdx (s) ⎥
⎢ ⎥ ⎢ ⎥
⎢φ (s) ⎥ ⎢ε (s) + w (s) ⎥
⎣ y ⎦ ⎣ y dy ⎦
φz (s) εz (s) + wdz (s)

X(t) represents error vectors in the error equation in Formula 2.44, F represents
coefficient matrix, W (t) represents the errors of gyroscopes and accelerometers,
then we can get (2.46):

Ẋ(t) = F X(t) + W (t) (2.46)

And the corresponding Laplace transform is:

s Ẋ(s) = F X(s) + X0 (s) + W (s) (2.47)

Thus, the solution of the Laplace transform is as follows:

X(s) = (sI − F )−1 [X0 (s) + W (s)] (2.48)

According to the inverse formula, we have:

N (s)
(sI − F )−1 = (2.49)
|sI − F |
where denominator is the determinant of the system characteristics matrix, molec-
ular N (s) is adjoin matrix of features array (sI − F ). Expand the characteristic
2.3 Inertial Navigation System 29

determinant, we have:
(s) = |sI − F | =
 
 
 s 2wie sin L 0 0 g 0 
 
−2wie sin L s 0 −g 0 0 
 
 1 
 − 
 0 s 0 0 0 
 R 
 1 
 0 0 s −wie sin L wie cos L
 R 
 1 
 − 0 wie sin L wie sin L 0 
 s 
 R 
 tgL 
 − 0 −wie cos L −wie cos L 0 s 
R
   2 2 
= s 2 + ωie
2
s + ωs2 + 4s 2 ωie2
sin2 L (2.50)

g
where ws2 = is Shula frequency.
R
The characteristic equation of the system is:

   2 
s 2 + ωie
2
s 2 + ωie
2
+ 4s 2 ωie
2
sin2 L = 0 (2.51)

From the first factor on the left side of the above equation, we can get S1,2 directly;
simplifying the second factor S3,4 , we can get S5,6 :
s1,2 = ±j wie
s3,4 = ±j (ws + wie sin L)
s5,6 = ±j (ws − wie sin L) (2.52)
As can be seen from the above equation, all the eigenvalues of the system are
imaginary numbers.
The system is undamped free oscillation system. The oscillation frequency is
classified into three categories:

w1 = wie
w2 = ws + wF (2.53)
w3 = ws − wF

where w1 is the angular frequency of the Earth; wF is Foucault frequency; oscillation

periods of ws and wF are called Shula cycle T, Foucault cycle TF ; and:
2π
Ts = = 84.4min
ws
30 2 Principle of INS/CNS/GNSS Navigation System

2π 2π
TF = = .
wF wie sin L
Since ws >> wF , the difference between w2 and w3 is small. In analytic expression,
there is linear combination of two sinusoidal components of close frequencies.

x(t) = x0 sin (ws + wie sin L)t + x0 sin (ws − wie sin L)t (2.54)

And perform difference of this formula, we have:

x(t) = 2x0 cos (wie sin L)t sin ws t (2.55)

As it can be seen from the above equation, the amplitude of the Shula oscillation
frequency is modulated by the Foucault frequency.

2.4 Satellite Navigation System

The satellite navigation system is a kind of widely used space-based radio naviga-
tion and positioning system, the essential thought of which is to place traditional
radio range station on artificial satellite and adopt the starry and ranging system with
medium and high apogee to realize high-precision determination of position and
velocity of the carrier through simultaneous ranging of multi-satellite. Satellite nav-
igation systems having been constructed currently mainly include GPS of the USA
and GLONASS of Russia, and those under construction mainly include Galileo of
Europe and the second generation of “Big Dipper” of China, etc. which are col-
lectively referred to as global navigation satellite system (GNSS) [9]. Since GNSS
has advantages such as all time, all weather, high-precision positioning and velocity
measurement, it has been widely used in high-precision navigation field of sea, land,
air, and sky carriers in motion; and since GNSS may realize high-precision posi-
tioning through differential measurement, it also plays an important role in geodetic
surveying. The operating principle and analysis of error characteristics of GNSS are
mainly introduced as follows.

2.4.1 Operating Principle of Satellite Navigation System

In general, GNSS mainly consists of ground monitoring network, space satellite con-
stellation, and user GNSS receiver. Ground monitoring network generally consists of
master control station, monitoring station and injection station, major responsibility
of which is to continuously track satellites in the constellation, collect operational
data of the whole system, calculate, prepare, and correct navigation message based
on the data, and finally send the navigation message to satellites in the constellation;
it also provides time reference of the whole system. The space satellite constellation
2.4 Satellite Navigation System 31

Fig. 2.8 GNSS Sat2 Sat3

three-dimensional positioning
Satn
sketch map ……
Sat1
ρ3
ρ2
ρn
ρ1

GNSS receiver

sends navigation signals to the user to ensure that at least four constellation satellite
are observed simultaneously everywhere on Earth. The user GNSS receiver mainly
receives and processes navigation signals sent by constellation satellites to determine
inherent position and velocity of the user.
Basic principle for GNSS navigation and positioning is to determine the position
of GNSS receiver after working out the distance between the receiver and satellite
through relevant processing to measure the propagation time delay from the satellite
to the receiver or weeks of phase position change of satellite carrier signal in the
propagation path upon reception by the user GNSS receiver of navigation signal sent
by GNSS constellation satellite.
During three-dimensional positioning, if position of certain point to the located
is to be determined solely, the distances between the point to be located and at least
three given points are required to be measured. After making three positioned spheres
with the three given points as the center of sphere and the observed three distances
as the radius, the three spherical surfaces will meet in one point, coordinate of which
(i.e., 3-D position of the point to be located in the space) may be worked out by
solving the three equation sets with unknown numbers, as shown in Fig. 2.8.
During GNSS navigation and positioning, the measured distance contains certain
error instead of truly reflecting the geometrical distance between the satellite and
GNSS receiver due to influences of various error sources. Such GNSS measuring
distance containing errors is called pseudorange. At present, observed quantities such
as code (also called code phase) and phase pseudorange (also called carrier phase)
are widely used during GNSS navigation and positioning measurement.
Both the two kinds of observed quantities have problems of GNSS constellation
satellite clock error and GNSS receiver clock error. Satellite clock generally adopts
high-precision atomic clock and is continuously measured and corrected by ground
monitoring network, so it has high precision and its errors may be ignored. If the clock
error between user clock and satellite clock is t, the propagation time delay τ ∗ and
corresponding distance measured at this moment are not true radio wave propagation
time delay τ and the distance r between the satellite and user. The pseudorange ρ by
32 2 Principle of INS/CNS/GNSS Navigation System

this time shall be:

ρ = r + c t (2.56)

So the pseudorange for the i satellite measured by the user shall be:

ρi = ri + c t (2.57)
1/2
wherein, ri = [(x − xsi )2 + (y − ysi )2 + (z − zsi )2 ] , and x, y, z and xsi , ysi , zsi
are respectively position coordinates of the user and satellite i in the terrestrial
coordinate system.
It is obvious that four unknown numbers are included in the measured pseudor-
ange, i.e. x, y, z and t. The pseudorange to at least four satellites shall be measured
to work out the four unknown numbers, so at least four independent equations are
obtained thereby, i.e.,:
 1/2
ρi = (x − xsi )2 + (y − ysi )2 + (z − zsi )2 + c t(i = 1,2, 3,4 . . .). (2.58)

The four unknown numbers may be worked out through simultaneous solution, and
therefore position coordinate of the user is worked out.
GNSS may also provide user velocity besides providing the user with three po-
sition coordinates and precise time. The pseudorange change rate may be worked at
this time by measuring Doppler frequency shift of the electromagnetic wave carrier
frequency, and another four equations are established thereby, i.e.,:
(x − xsi )(ẋ − ẋsi ) + (y − ysi )(ẏ − ẏsi ) + (z − zsi )(ż − żsi )
ρ̇i =  1/2
(x − xsi )2 + (y − ysi )2 + (y − ysi )2
+ c t˙ (i = 1,2, 3,4, . . .) (2.59)

wherein, ẋsi , ẏsi , żsi and xsi , ysi , zsi are respectively velocity and position of the
satellite, ρ̇i is worked out by measuring Doppler frequency shift, ẋ, ẏ, ż and t˙
are numbers to be worked out, and the user velocity may be worked out through
simultaneous solution.

2.4.2 Analysis of Error Characteristics for Satellite Navigation

System

Analysis of error characteristics for GNSS is mainly from aspects of inherent error
of constellation satellite, signal propagation error, and signal reception error [10].
During the research on influences of error on GNSS positioning, the error is generally
converted into the distance between GNSS constellation satellite and the monitoring
station to express with corresponding distance error.
2.4 Satellite Navigation System 33

1. GNSS Constellation Satellite Error

GNSS constellation satellite error mainly includes inherent orbital position error of
the satellite and clock error. The former is mainly caused by influences of various
perturbative forces, number of monitoring station and spatial distribution, number
and precision of orbital parameters, and non-real-time property of ephemeris, and
the range error caused thereby is approximately within 1.5 m–7.0 m; influences on
range error are tried to be weakened generally through simultaneous observation to
work out the difference or method of orbit improvement to limit it within the error
scope of 1 m. The latter refers to the nonsynchronous deviation between satellite
clock and GNSS standard time due to frequency offset and frequency drift between
clock of GNSS constellation satellite and GNSS standard time; total quantity of
such deviation may reach 1 ms, and equivalent range error caused thereby may
reach 300 km; the deviation may correct the clock error model through continuous
measurement of the ground station to limit the equivalent range error within 6 m; if
residual error of GNSS constellation satellite clock is further weakened, it may be
realized through differential technology.
2. GNSS Signal Propagation Error
GNSS signal propagation error mainly includes ionospheric refraction, tropospheric
refraction, multipath error, etc.
Ionospheric refraction error: Ionospheric refraction error is in direct proportion to
atmospheric electron density and inversely proportional to the penetrated electromag-
netic wave frequency. Atmospheric electron density varies with radiation intensity of
the Sun and other celestial bodies, season, time, geographic position, etc. For GNSS
constellation satellite signal, randomness of solar activity is large, so the difference
of impact of ionospheric refraction on GNSS navigation and positioning error is
large, and it is impossible to establish precise mathematical model for it. During
GNSS navigation and positioning, methods may be generally adopted to weaken the
impact of ionosphere on the precision of GNSS navigation and positioning include
(1) correction through double frequency observation and (2) correction through si-
multaneous observation value to work out the difference, i.e., relative positioning
method.
Tropospheric refraction error: Tropospheric refractive index is irrelevant to fre-
quency or wave length of electromagnetic wave, and reduces gradually with increase
of altitude; it is also closely related to atmospheric pressure, humidity, and temper-
ature. Since atmospheric convection is intensive and changes of pressure, humidity,
and temperature are complicated, it is difficult for precise modeling, and range error
generally caused thereby may reach nanometer. An effective method to reduce influ-
ence of tropospheric refraction on navigation and positioning error is to estimate the
propagation delay of electromagnetic wave in the troposphere. Measures commonly
taken at present mainly include: (1) working out the difference through simultaneous
observation to effectively reduce or even eliminate influences on delay of troposphere
and (2) improving atmospheric model of troposphere through measured data nearby
the monitoring station.
34 2 Principle of INS/CNS/GNSS Navigation System

Multipath error: Besides directly receiving signals sent by GNSS constellation

satellite, GNSS receiver also receives GNSS constellation satellite signals sent for
once or many times by buildings surrounding the receiver antenna, i.e., during actual
measurement, signals received by GNSS antenna are signals superimposed by direct
and reflected wave. Phase delay may exist between reflected and direct wave since
the length of path passed by the two signals is different, and multipath error is
thereby caused. It is difficult to establish accurate error model due to the uncertainty
of reflection coefficient of reflector and the distance between it and the receiver
antenna. Measures may be taken to reduce multipath error at present mainly include
(1) trying to make GNSS receiver antenna avoid object surface with large reflection
coefficient and (2) using GNSS receiver antenna with good directivity such as the
choking coil antenna.
3. Signal Reception Error
Such errors mainly include GNSS receiver measurement error, GNSS receiver clock
error, antenna phase center error, etc. Among them, besides related to measurement
resolution of software and hardware of GNSS receiver for constellation satellite
signals, GNSS receiver measurement error is also related to installation precision of
GNSS receiver antenna. GNSS receiver clock error is related to the receiver, so the
clock error may be taken as an unknown parameter to estimate such error by working
out the pseudorange equation; influences of such error may also be eliminated by
working out the differential treatment through observation quantity. Antenna phase
center error is caused by different incident angles, and equivalent range error may
reach a few millimeters or even a few centimeters according to conditions of antenna
performance.
Error subdivision and their influences on distance measurement are shown in
Table 2.1 [9].

2.5 Celestial Navigation System

Celestial navigation realizes positioning and attitude determination through angle

measurement in essence. It measures azimuth information of celestial bodies (such
as the fixed star, the Earth, Sun, other planets, etc.) by using of celestial sensor and
obtains position and attitude of carriers in motion through calculation. Before the
appearance of radio navigation in 1930s, celestial navigation has always been the only
available navigation technology [11]. It remains to be used widely up to now owing
to its advantages such as strong autonomy, high-precision attitude measurement and
error not accumulating with time.
At present, CNS is mainly applied in two fields:
First application: positioning of naval ship, aircraft, and spacecraft. Its basic
principle is to measure the vector direction of a celestial body through the celestial
sensor, obtain horizon information through other means to form elevating angle of
the carrier, and obtain position of the carrier according to geometrical relationship.
During positioning and navigation of naval ship and aircraft, information of elevating
2.5 Celestial Navigation System 35

Table 2.1 Error subdivision and their influences on distance measurement (taking GPS for instance)
Source of error Influence on distance measurement/m
Code P Code C/A

(1) Satellite
√
Ephemeris error and model error 4.2 4.2
√
Clock error and stability 3.0 3.0
√
Satellite perturbation 1.0 1.0
√
Uncertainty of phase position 0.5 0.5
√
Others 0.9 0.9
Subtotal 9.6 9.6
(2) Signal propagation
√
Ionospheric refraction 2.3 5.0 ∼ 10.0
√
Tropospheric refraction 2.0 2.0
√
Multipath error 1.2 1.2
√
Others 0.5 0.5
Subtotal 6.0 8.7 ∼ 13.7
(3) Signal reception
√
Receiver noise 1.0 0.5
√
Others 0.5 7.5
Subtotal 1.5 8.0
Total 17.1 26.3 ∼ 31.3

angle of celestial body relative to the horizon is generally used and real-time posi-
tioning of naval ship and aircraft is realized by combining contoured circle method,
etc.; during positioning of spacecraft, real-time and precise positioning of spacecraft
is realized by combining orbital dynamics and adopting filtering method to obtain
high-precision positioning information besides using aforesaid basic information.
Second application: attitude determination of guided missile, aircraft, and satel-
lite. Its basic principle is to provide high-precision attitude information for navigation
of guided missile, aircraft, and satellite through the attitude of the carrier relative to
geocentric inertial coordinate system measured through celestial sensor (such as the
star sensor whose attitude measurement precision may reach second of arc).
CNS is generally classified into autonomous celestial positioning system (used
for positioning of satellite and naval ship) and celestial attitude determination system
(used for navigation of guided missile and aircraft) according to the difference of its
application object. Operating principles of the two systems are mainly introduced as
follows.
36 2 Principle of INS/CNS/GNSS Navigation System

σ
P
h

H X

900 − h

Fig. 2.9 Contoured circle

2.5.1 Autonomous Celestial Positioning Principle

Autonomous celestial positioning system realizes positioning of the carrier mainly

relying on various observed information of celestial bodies, and its positioning
methods mainly include geometric method-based positioning method and orbital
dynamics-based positioning method.
1. Positioning Principle Based on Geometric Method
1) Positioning principle based on contoured circle
CNS was first applied in marine navigation, and positioning method based on con-
toured circle is mainly applied in positioning of naval ship and aircraft during marine
navigation and aviation [12]. Aircraft is hereby taken for instance to briefly introduce
its principle as follows:
If observing the star σ at a point P in the aircraft H away from the ground (the
vector direction may be approximately regarded as the same when observing the star
from the Earth’s core and the aircraft since the star is far away from us), the elevating
angle h may be obtained, and PX is the local horizon, as shown in Fig. 2.9.
2.5 Celestial Navigation System 37

Draw a cone with the Earth’s core O as the vertex and (90◦ − h) as cone angle; and
the circle formed by the cone and intersecting line of spherical surface with R + H
as the radius is defined as a contoured circle. Two elevating angles will be obtained
when observing two stars from point P and two contoured circles can be made. The
two circles generally have two points of intersection which are usually far apart from
each other. The true and false position may be excluded by using priori knowledge
or judged through approximate geographical position, or excluded by observing the
elevating angle of another star, and the point of intersection of the three contoured
circles will be the position of the observer.
2) Positioning principle based on pure celestial geometric analytical method
Since position of a celestial body in the inertial space at any time may be obtained by
searching the solar calendar, ephemeris, and other information, attitude information
of spacecraft at that time may be determined through azimuth information of celestial
body observed by the celestial sensor (star sensor, planet sensor, etc) in the spacecraft
[10]. For example, attitude of spacecraft in the inertial space may be determined
through observation data of three or more stars. However, observation data of nearby
celestial bodies with known position is required to be obtained to determine the
position of the spacecraft in the space.
Specific positioning principle based on pure celestial geometric analytical method
is as follows: First, work out the included angle α between one star vector and center-
of-mass vector of one known nearby celestial body through the celestial sensor; then,
draw a cone with center of mass of the observed nearby celestial body as the vertex,
vector direction of the star to be observed as the axis and included angle α as the
cone angle to determine that the spacecraft is bound to be located in the conical
surface. The second cone whose vertex coincides with the position of the nearby
celestial body may be obtained through the second measurement of a second star
and the same nearby celestial body. Two position lines will be determined through
intersection of the two conical surfaces, as shown in Fig. 2.10. The spacecraft is
located in one of the two position lines, and the other position line may be excluded
by observing a third star to get a third cone; it may also be excluded by utilizing the
known position of the spacecraft a moment before.
Two methods are usually adopted to determine specific position of the spacecraft in
the position line by obtaining the third piece of observation information: (1) Calculate
and get the distance between the spacecraft and the nearby celestial body through
viewing angle of the body, and determine the position of the spacecraft through the
distance and known position line; (2) select another known nearby celestial body
and get another position line, and the point of intersection of the two position lines
is just the position of the spacecraft.
2. Basic Principle of Celestial Navigation and Positioning Based on Orbital
Dynamics Equation
Such method is mainly specific to celestial positioning and navigation of spacecraft.
Celestial navigation methods based on orbital dynamics equation mainly include ce-
lestial navigation methods of direct sensitive horizon and indirect sensitive horizon
38 2 Principle of INS/CNS/GNSS Navigation System

Sun i1
恒星1
Star 1
Orbit of nearby celestial body 1
近天体1轨道

ir

Spacecraft
航天器
2
Orbit of nearby celestial body 2

i2

恒星2
Star 2

Fig. 2.10 Basic principle of pure celestial navigation

by utilizing stellar refraction [13, 14]. Its basic principle is to use filtering method to
precisely estimate position of the carrier based on spacecraft orbital dynamics equa-
tion and measurement information of celestial body [10, 15, 16]. Difference of the
two methods is that the measurement information of celestial body and corresponding
measurement equations used are different.
1) Basic principle for autonomous celestial navigation method of direct sensitive
horizon
Basic principle for autonomous celestial navigation of direct sensitive horizon is sim-
ple, using star sensor to observe the navigation star and get direction of the starlight
in measurement coordinate system of the star sensor and working out direction of the
starlight in coordinate system of the spacecraft by installing matrix transformation
through the star sensor; and then measuring the direction of geocentric vector in the
carrier coordinate system by using the infrared earth sensor or spatial sextant; after
angular distance of starlight (as shown in Fig. 2.11 and Fig. 2.12) and other mea-
sured celestial information has be obtained, position information of the carrier can
be estimated by combining orbital dynamics equation and advanced filtering method
[16, 17].
System state equation:
State equation for autonomous CNS of direct sensitive horizon is just the satel-
lite orbital dynamics equation which has many expression forms. Perturbed motion
equation and Newton perturbed motion equation expressed in the form of rectangular
coordinates are most commonly used in CNS[10].
2.5 Celestial Navigation System 39

Fig. 2.11 Observation model

of direct sensitive horizon
s
Angular distance of starlight

Spacecraft α
r

The earth

Fig. 2.12 Elevating angle of

starlight

1 Satellite Orbital Dynamical Equation Based on Rectangular Coordinate

System
Epoch (J2000.0) geocentric inertial coordinate system is selected during research on
motion of spacecraft. CNS state model (orbital dynamical model) usually selected
40 2 Principle of INS/CNS/GNSS Navigation System

in such case is:

⎧
⎪
⎪
dx
= vx
⎪
⎪
⎪
⎪ dt
⎪
⎪
⎪
⎪ dy
⎪
⎪ = vy
⎪
⎪ dt
⎪
⎪
⎪
⎪ dz
⎪
⎪ = vz
⎨
dt 

 (2.60)
⎪ dvx = −μ x 1 − J2 Re z2
⎪
⎪ 7.5 − 1.5 + FX
⎪
⎪ dt r3 r r2
⎪
⎪ 


⎪
⎪ dvy y Re z2
⎪
⎪ = −μ − − + Fy
⎪
⎪ 1 J 2 7.5 1.5
⎪
⎪ dt r3 r r2
⎪
⎪ 


⎪
⎪ dvz z Re z2
⎪
⎩ = −μ 3 1 − J2 7.5 2 − 4.5 + Fz
dt r r r

r= x 2 + y 2 + z2
Short for:
Ẋ(t) = f (X, t) + w(t)
T
wherein, the state vector X = [x y z vx vy vz ] and x, y, z, vx, vy , vz
are respectively positions and velocities of the spacecraft in the directions of X, Y,
and Z; μ is the geocentric gravitational constant; r is positional parameter vec-
tor of the spacecraft; J2 is the terrestrial gravitational coefficient; Fx , Fy , Fz
are respectively perturbation acceleration of the terrestrial nonspherical high-order
perturbation and solar–lunar perturbation, solar radiation pressure perturbation and
atmospheric perturbation, etc.

2 Newton Perturbed Motion Equation

Newton perturbed motion equation is:

⎧
⎪
⎪
da 2
⎪ dt = n√1 − e2 [e(S sin f + T cos f ) + T ]
⎪
⎪
⎪
⎪
⎪
⎪ √
⎪
⎪ de 1 − e2
⎪
⎪ = [(S sin f + T cos f ) + T cos E]
⎪
⎪ dt
⎪
⎪ na
⎪
⎪
⎪
⎪ di r cos (w + f )
⎪
⎨ dt = na 2 √1 − e2 W
⎪
(2.61)
⎪
⎪ d
r sin (w + f )
⎪
⎪ = √ W
⎪
⎪ dt na 2 1 − e2 sin i
⎪
⎪ √
⎪
⎪

⎪
⎪ dw 1 − e2  r  d

⎪
⎪ = −cos f · S + 1 + sin f · T − cos i
⎪
⎪ dt nae p dt
⎪
⎪
⎪
⎪
  
⎪
⎪ dM 1−e 2
r 
⎪
⎩ =n− − cos f − 2e S + 1 + pr sin f · T
dt nae p
2.5 Celestial Navigation System 41

wherein, p = a(1 − e2 ); S is the perturbative force along the direction of vector

r; T is the perturbative force perpendicular to r and pointing to the satellite motion
direction inside the orbital plane; W is the perturbative force along normal direction
of the orbital plane and constituting right-handed system orientation with S and T
[10].
For convenience, the three components S, T, and W of perturbative force are
usually converted into U, N, and W, and the perturbed motion equation in such case
is:
⎧ da 2
⎪
1
⎪
⎪ = √ (1 + 2e cos f + e2 ) 2 U
⎪ dt
⎪ n 1 − e2
⎪
⎪
⎪
⎪ √
⎪ √
⎪ de = 1 − e (1 + 2e cos f + e2 ) 2 [2( cos f + e)U − 1 − e2 sin E · N ]
⎪ 2 1
⎪
⎪
⎪
⎪ dt na
⎪
⎪
⎪
⎪ di
⎪
⎪ r cos (w + f )
⎪
⎪ = √ W
⎨ dt na 2 1 − e2
⎪
⎪ d
r sin (w + f )
⎪
⎪ = √ W
⎪
⎪ dt na 2 1 − e2 sin i
⎪
⎪
⎪
⎪ √
⎪
⎪ 1 − e2
⎪
⎪ dw 1 d

⎪
⎪ = (1 + 2e cos f + e2 ) 2 [2 sin f · U + ( cos E + e)N ] − cos i
⎪
⎪ dt nae dt
⎪
⎪
⎪
⎪ dM
⎪
⎪ 1 − e2 1 2e2
⎩ = n− [(1 + 2e cos f +e2 ) 2 (2 sin f + √ sin E)U +( cos E −e)N ]
dt nae 1 − e2
(2.62)
wherein, U is the perturbative force along the tangential direction of spacecraft
motional orbit, and the velocity direction of the pointing motion is positive; N is
the perturbative force along the main normal direction of the orbit, and the normal
direction inside is positive.
System measurement equation:
Angular distance and elevation angle of starlight are usually used as observed
quantities during celestial navigation of direct sensitive horizon of spacecraft, and
introduction to corresponding measurement equations is as follows.
1 Measurement Equation Based on Angular Distance of Starlight

As an observed quantity usually used during celestial navigation, angular distance

of starlight refers to the included angle between the vector direction of navigation
starlight observed from the spacecraft and the geocentric vector direction [18].
Expression of angular distance of starlight α and corresponding measurement
equation obtained according to the geometric relationship shown in Fig. 2.11 are
respectively as follows:
 r · s
α = arccos −
r
 r · s
Z(k) = α + vα = arccos − + vα
r
42 2 Principle of INS/CNS/GNSS Navigation System

wherein, r is the position vector of the spacecraft in the geocentric inertial spherical
coordinate system and is obtained through horizon sensor; s is the unit vector of the
navigation starlight direction and is identified by star sensor.
2 Measurement Equation Based on Elevation Angle of Starlight

Elevation angle of starlight refers to the included angle between the navigation star
observed from the spacecraft and the tangential direction on the edge of the Earth.
Expression of elevation angle of starlight γ and corresponding measurement equation
obtained according to the geometric relationship shown in Fig. 2.12 are respectively
as follows:
 s · r

Re
γ = arccos − − arcsin
r r
 s · r

Re
Z(k) = γ + vγ = arccos − − arcsin + vγ
r r
wherein, r is the position vector of the spacecraft in the geocentric inertial spherical
coordinate system; s is the unit vector of the navigation starlight direction; Re is the
Earth radius.
1) Basic principle for autonomous celestial navigation of indirect sensitive horizon
by utilizing starlight refraction
Basic principle for autonomous celestial navigation of indirect sensitive horizon by
utilizing starlight refraction is: When observing two stars simultaneously by using
the star sensor, starlight altitude of one star is far more larger than altitude of the
atmospheric layer and the starlight is not subject to refraction while starlight of
the other star is subject to atmospheric refraction, so angular distance between the
starlight of the two stars will differ from the nominal value, and variation of the
angular distance is just the angle of star refraction. There is relatively precise function
relationship between the angle of starlight refraction and atmospheric density, which
has accurate model with altitude change, therefore it is possible to precisely determine
the altitude of refracted starlight in the atmospheric layer, and this observed quantity
reflects the geometric relationship between the carrier and Earth, from which indirect
horizon information may be obtained, as shown in Fig. 2.13. Position information
of the carrier can be estimated by further combing the orbital dynamics equation
and advanced filtering method [19–21]. Since precision of star sensor is far much
higher than that of horizon sensor, more precise carrier position information may be
obtained by using starlight refraction method.
System state equation:
State equation for autonomous CNS of indirect sensitive horizon through starlight
refraction is the same as that of autonomous CNS of direct sensitive horizon.
System measurement equation:
There are several measurement equations for indirect sensitive horizon of space-
craft, and measurement equation with apparent altitude of starlight retraction ha and
projection of spacecraft position vector rs in the vertical direction rs · uup as observed
quantities is mainly introduced hereby.
2.5 Celestial Navigation System 43

Navigation star

s u

Spacecraft ß α rk Refraction star

γ

rh

r Re
Spacecraft orbit plane
Terrestrial average level plane

Fig. 2.13 Observation model of sensitive horizon by utilizing starlight refraction

1 Measurement Equation with Apparent Altitude of Starlight Retraction ha as

the Observed Quantity

Since measurement equation shall reflect the relationship between observed and
state quantity, apparent altitude of starlight retraction ha is usually selected as the
observed quantity during celestial navigation of indirect sensitive horizon through
starlight retraction, as shown in Fig. 2.14 and referring to Reference [10] for details;
the measurement equation is:


ha = rs2 − u2 + u tan (R) − Re − a + ν (2.63)

wherein, ν is Gaussian measurement noise.

2 Measurement Equation with Projection of Spacecraft Position Vector rs in

the Vertical Direction rs · uup as the Observed Quantity

Starlight retraction sketch map is shown in Fig. 2.14. In the Figure, rs is the position
vector; s is the starlight direction vector, where the star is observed from the geo-
centric direction; vector uup is the unit vector perpendicular to the starlight inside
the plane comprising starlight and spacecraft position vector, i.e., uup = |(s×r(s×rs )×s
s )×s|
.
Obviously, the spacecraft position vector rs may be expressed as the sum of two
orthogonal vectors:

rs = (rs · s)s + (rs · uup )uup

with rs · uup as the observed quantity, when the star sensor observes an angle of
retraction R, corresponding starlight retraction altitude hg can be worked out, and
44 2 Principle of INS/CNS/GNSS Navigation System

Apparent direction of the star

R

Refracted ray ua

us B
a
A
ha hg ha
R
Z

S
·
Bottom radius b
u up
Spacecraft Re

rs
( rs . u up ) u up

u
s
O The earth’s core

Fig. 2.14 Geometrical relationship of starlight retraction

the system measurement equation obtained thereby is:

rs · uup = Re + hg − |rs · s| tan R

Refer to references [10; 12] for details of specific autonomous celestial navigation
method of direct sensitive horizon and autonomous celestial navigation method of
indirect sensitive horizon by utilizing starlight retraction.

2.5.2 Celestial Attitude Determination Principle

The previous section mainly introduces principle of autonomous celestial positioning

navigation specific to naval ship and satellite. In fact, since celestial navigation
may directly obtain high-precision attitude information by measuring celestial body
with celestial sensor, it is widely used as aided navigation means in high-precision
navigation of carriers such as high-altitude long-endurance spacecraft and ballistic
missile, etc.
As a navigation subsystem of high-performance navigation system, celestial atti-
tude determination system provides carriers with high-precision attitude information
to be used for estimation and correction of attitude error of carriers. Acquisition of
attitude information of carriers is in essence to determine attitude parameter of the
carrier coordinate system in the navigation coordinate system. Methods to determine
2.5 Celestial Navigation System 45

attitude of carriers by using information of celestial body mainly include single refer-
ence vector method, double reference vector method, and multiple reference vector
method [22].
1. Single Reference Vector Method
Single reference vector method refers to the method of using only one observed
reference vector to determine attitude of the carrier at each moment. Due to the
existence of only one reference vector, it is impossible to fully determine the attitude
of the carrier in the navigation coordinate system with the method, i.e., attitude of the
carrier coordinate system rotating around the reference vector is uncertain. If attitude
angles ϕ and ψ can be determined according to attitude sensor measurement on the
carrier, since the three attitude angles ϕ, ψ, and γ satisfy the constraint equation:

cos2 ϕ + cos2 ψ + cos2 γ = 1

γ can be worked out. Generally, γ has two values, γ1 and γ2 , and γ1 + γ2 = π. If we

discuss attitude determination from the perspective of geometry, the two-value per-
formance (or multiple-value performance under more complicated circumstances)
determined from the equation above shall be considered. When discussing and
solving from the perspective of algebra, multiple value performance of solution
of quadratic function constraint equation shall be noted [22].
2. Double Reference Vector Method
Double reference vector method refers to the method of using two noncollinear ref-
erence vectors observed to determine attitude of the carrier at each moment. Attitude
of the carrier in the navigation coordinate system can be fully determined with the
method. Assume the two noncollinear reference vectors are respectively expressed as
S1 and S2 in the navigation coordinate system, and S1∗ and S2∗ in the carrier coordinate
system.
Record S3 = S1 × S 2 and S3∗ = S1∗ × S2∗ . Assume the attitude matrix from the
navigation system to the carrier system is A, then

Si∗ = ASi (i = 1, 2, 3).

Make: B = (S1 , S2 , S3 ) and B ∗ = (S1∗ , S2∗ , S3∗ )

Then: B ∗ = AB, i.e., A = B ∗ B −1 , and the matrix A may be worked out since
B is known and B ∗ is obtained from measurement. The attitude matrix A may be
determined solely since the reference vectors are noncollinear.
3. Multiple Reference Vector Method
Multiple reference vector method refers to the method of using more than two non-
collinear reference vectors observed to determine attitude of the carrier at each
moment. Both attitudes of the carrier in the navigation coordinate system can be fully
determined and problem of low-precision attitude matrix caused by measurement
error can be effectively solved with the method.
46 2 Principle of INS/CNS/GNSS Navigation System

If attitude sensor on the carrier has obtained multiple noncollinear reference vec-
tors, the attitude matrix may be determined by using the combination of certain
measured quantities. However, different combinations of measured quantities will
have different attitude matrices since there is error in measured quantities of reference
vectors. Therefore, when there is redundancy in measurement, there is a problem of
how to obtain the optimal estimated value of attitude matrix. Least square method
is a common method used to solve the problem. Another commonly used multiple
vector attitude determination method will be introduced below [22].
Assume expressions of multiple noncollinear reference vectors in the navigation
coordinate system are S1 , S2 , . . ., SN , and S1∗ , S2∗ , . . ., SN∗ in the carrier coordinate
system. It can be obtained according to double reference vector principle that:

B ∗ = AB

wherein, B = (S1 , S2 , . . ., SN ), B ∗ = (S1 , S2 , . . ., SN ).

The equation above is false if there is measurement error. If Bi∗ = GBi , a pseudo-
inverse approximate solution of G can be worked out:
−1
G∗ = B(B ∗ )T (B ∗ (B ∗ )T )

wherein, G∗ is not necessarily the orthogonal matrix. It is possible to adopt:

 ∗ 2
f (G) = B − (G)T Bi  = min
i

and combine constraint condition of orthogonal matrix: GGT = I to work out an

approximate optimal solution Gopt of G, i.e., the optimal attitude matrix Aopt :

1 ∗
Aopt = Gopt = G ((3I − (G∗ )T G∗ ).
2

2.5.3 Star Sensor in CNS and Analysis of Its Error Characteristics

Celestial sensor usually used in CNS mainly includes star, sun, earth, and other
planet sensors according different tasks and flight regions. Among them, sun, earth
and other planet sensors may give only one vector direction, so it is impossible to
fully determine attitude of the carrier in motion; but star sensor can give multiple
reference vectors by sensing multiple stars and fully determine attitude of the carrier
in motion through calculation [23]. Since position change of star in inertial space is
very small every year, only several seconds of arc, high precision may be reached to
determined attitude of the carrier by using it, and compared with inertial gyro, the
attitude error will not accumulate with time, so it is one of celestial sensors widely
used at present, and an indispensable key part especially for high-precision attitude
determination of a new generation of spacecraft.
2.5 Celestial Navigation System 47

Fig. 2.15 Operating principle Star

of star sensor

Sensory system
Lens Lens hood
Sensitive
surface

Sensitive star map data

Star map preprocessing

Data processing system

Star map matching
identification

Extraction of center
of mass for star

Attitude
determination

General star sensor mainly includes sensory system and data processing system.
The former consists of lens hood, optical lenses, and sensitive array and mainly
realizes acquisition of star-map data in the sky; the latter realizes processing of star
map data acquired and determination of attitude, including the four processing steps
of star-map preprocessing, star-map matching identification, extraction of center of
mass for star point and attitude determination. Its operating principle is shown in
Fig. 2.15.
As a key part of astrometry in CNS, star sensor mainly includes errors such as
position error of star image, focal length error, principal optic axis error, calibration
error, electronic circuit error, and software processing error, etc. Among them, po-
sition error of star image mainly depends on star image drift, optical system design
noise, image processing, etc., and the two parameter errors of focal length f and
position of principal optic axis (x0 , y0 ) are mainly caused by mechanical structure
design, machining, installation, etc.; the three errors are main factors affecting atti-
tude measurement precision of star sensor. Brief analysis specific to the three errors
is as follows.
1. Location Error of the Center of Mass for Star Image Points
Modern star sensor optical system generally adopts sub-pixel technology regardless
of influences of optical aberration. Optical design satisfies normal distribution of fac-
ula energy of star image, and point spread function expressed with two-dimensional
48 2 Principle of INS/CNS/GNSS Navigation System

Gauss function is as follows:

   
 2   2 
I0 − (x−xc )  − (y−yc ) 
 2   2 
l (x, y) = 2
e 2σP SF e 2σP SF (2.64)
2πσP SF

wherein, (xc , yc ) represents the true central position of star image; σP SF refers to
radius of Gauss, representing energy concentration ratio of the point spread function.
In accordance with the point spread function, assume a star point whose location
of the center of mass is in the center of the grid is generated inside a grid of p × p,
and its energy is distributed into a small region; also assume optical axis of the star
sensor optical system has passed the calibration and is pointing to the grid center;
so it can be obtained through sensing of the star sensor that a star image facula of
2 × 2 m is surrounded by a star map of n × n; center of the facula coincides with
the sensed image center, and it can be obtained from calculation through moments
method of center of mass that the estimated value (x̄, ȳ) of position of center of mass
of the star point (m, m) under the star sensor coordinate system is:

!
2m !
2m
xij Iij yij Iij
i,j i,j
x̄ = , ȳ = (2.65)
!
2m !
2m
Iij Iij
i,j i,j

wherein, xij = i, yij = j is the pixel position (i, j ) sampled from the star sensor; Iij
represents energy value (gray level of pixel) of the pixel point (i, j ).
In such case, position
 error of center of mass of the star point may be simply
calculated as: d = x̄ 2 + ȳ 2 and measurement error of corresponding attitude
angle caused by it can be roughly estimated as:

α ≤ arctan (d × δ/f )

where δ represents the pixel size, and f is the focal length of star sensor optical
system.
2. Errors of Focal Length f and Optical Axis Position (x0 , y0 )
Factors such as machining and system installation inevitably cause errors of focal
length f and optical axis position (x0 , y0 ). Such errors have great influences on
precision of attitude measurement of CNS, and are even likely to cause impossibility
to implement correct attitude determination. They may only be compensated through
parameter correction of the system. Primary analysis of influences of such parameters
and correction methods is as follows. It is obtained hereby by ignoring position error
of center of mass of the star image point that diagonal distance of the star is:
N  
ŵiT ŵj = = Gij x̂0 , ŷ0 , fˆ (2.66)
D1 D2
2.5 Celestial Navigation System 49

wherein,
     
N = xi − x̂0 xj − x̂0 + yi − ŷ0 yj − ŷ0 + fˆ2


D1 = (xi − x̂0 )2 + (yi − ŷ0 )2 + fˆ2


D2 = (xj − x̂0 )2 + (yj − y0 )2 + fˆ2

Since the error is small, it is obtained subject to linearization of above equation in

the estimated value (x0 , y0 , f ) that:
 
 
 x0 
 
   ∂G ∂G ∂G   
viT vj = Gij x̂0 , ŷ0 , fˆ − 
ij
,
ij
,
ij   y0 

(2.67)
∂x0 ∂y0 ∂f   
 
 
 f 
 
 
 x0 
  
    
Rij = viT vj − Gij x̂0 , ŷ0 , fˆ = − 
∂Gij
,
∂Gij
,
∂Gij 
  y0  (2.68)
∂x0 ∂y0 ∂f   
 
 
 f 

Therefore for i = 1,2 . . . n − 1, following equation can be obtained:

R = A Z (2.69)

wherein,
 
 ∂G12 ∂G12 ∂G12 
 
 ∂x0 ∂y0 ∂f 
 
 ∂G13 ∂G13 ∂G13 
 
 
 ∂x0 ∂y0 ∂f 
A = − 
 .. .. .. 
 . . . 
 
 
 ∂Gn−1,n ∂Gn−1,n ∂Gn−1,n 
 
 
∂x0 ∂y0 ∂f

 T
R = R12 , R13 · · · R23 · · · Rn−1,n

 T
Z = x0 y0 f
50 2 Principle of INS/CNS/GNSS Navigation System

Fig. 2.16 Two-dimensional

conversion sketch map Sky
Star b Star a

. Lens
f
Y-axis

p . o
q X-axis

Sensitive array

For convenience, origin of the coordinate system is established in (x0 , y0 ), and cor-
∂G ∂G ∂G
responding relation with ∂x0ij , ∂yij , ∂fij , (xi , yi ) and (xj , yj ) can be obtained in such
0
case. Map the system into 2-D space according to symmetry of the system, details
as follows:
For any two points mapped to the celestial sphere within the field of view [24],
establish a 2-D planar coordinate system by virtue of the great circle the two points
located in for analysis, where the sketch map is shown in Fig. 2.16.
In accordance with 2-D conversion, the system differential equation is corre-
spondingly converted into:
 
 
   ∂G ∂G   x0 
 
Rij = viT vj − Gij x̂0 , fˆ =  
ij ij
,  (2.70)
∂x0 ∂f   
 f 

wherein, domain of definition of variable quantity also changes correspondingly, and

it can be obtained in such case that:
⎧
⎪
⎪ N = xi xj + f
2
⎪
⎪
⎪
⎨ 
D1 = xi2 + f 2 (2.71)
⎪
⎪
⎪
⎪ 
⎪
⎩ D = x 2 + f 2
2 j

If xi = xj , it represents that image points of star i and j are the same point in star
sensor, i.e., star i and j are the same star; there is no angle difference in such case,
References 51

∂G ∂G
so it can be obtained that ∂x0ij = ∂fij = 0, and correctness of above derivation is
thereby proved.
Without loss of generality, if xi = −xj , it can be obtained that:
⎧
⎪ ∂Gij
⎪
⎪ =0
⎨ ∂x0
4f xi2 (2.72)
⎪
⎪ ∂Gij
⎪
⎩ ∂f =  2 2
x + f2 i
∂G ∂G
It indicates that when xi = ±xj , ∂x0ij = 0, then xi = −xj , ∂fij = max.
Focal length f is generally one order of magnitude larger than xi during star sensor
optical system design, so it can be obtained that calibrated precision of focal length
f is an important factor affecting precision of attitude measurement of star sensor.

2.6 Chapter Conclusion

Inertial/celestial/satellite integrated navigation technology is an effective means and

important development direction for high-performance navigation of a new gener-
ation of flying machine such as the guided missile, aircraft, and satellite. Starting
with basic knowledge of inertial/celestial/satellite integrated navigation, this chap-
ter first introduces coordinate systems commonly used during navigation and their
mutual relations as well as reference model of the Earth. Then, it outlines operat-
ing principles of inertial, satellite, and celestial navigation systems commonly used
in integrated navigation system and their error characteristics, which has provided
theoretical basis for introduction of subsequent chapters.

References

1. Yi GQ (1987) Inertial navigation principle. Aviation Industry press, Beijing

2. Deng ZL (1994) Inertial navigation system. Harbin Institute of Technology press, Harbin
3. Yuan X (1995) Navigation system. Aviation Industry press, Beijing
4. Gai E (2000) The century of inertial navigation technology. IEEE proceedings of aerospace
conference. Big sky: IEEE, 3(1), pp. 59–60
5. Barbour N, Schmidt G (2001) Inertial sensor technology trends. IEEE Sens J 1(4):332–339
6. Benson DO Jr (1975) A comparison of two approaches to pure-inertial and Doppler-inertial
error analysis. IEEE Trans Aerosp Electron Syst 11:447–455
7. Bar-Itzhack IY, Goshen-Meskin D (1992) Unified approach to inertial navigation system error
modeling. J Guid Control Dyn1 15(3):648–653
8. Xu YW (1991) Analysis and design of ballistic missile and launch vehicle system. China
astronautic publishing house, Beijing
9. Wang HN (2003) Principle and application of GPS navigation. Beijing Science & Technology
press, Beijing
10. Fang JC, Ning XL, Tian YL (2006) The principle and method of the spaceraft autonomous
celestial navigation. National Defence Industry press, Beijing
52 2 Principle of INS/CNS/GNSS Navigation System

11. Zhang YL, Wu JJ (2006) Spacecraft. National Defence Industry press, Beijing.
12. Fang JC, Ning XL (2006) Principle and application of celestial navigation. Beihang University
press, Beijing
13. Song LF, Fang JC (2005) Spacecraft autonomous celestial navigation based on the unscented
particle filter. Space Control 23(6):31–35
14. Ning XL, Fang JC (2005) An autonomous celestial navigation method for lunar probe based
on RJMCMC algorithm particle filter. J Astronaut 26 (S1):39–44
15. Yang B, Fang JC, Wu XJ (2001) Comparisons of autonomous orbit determination methods for
spacecraft using starlight refraction. Aerosp Control (3):1–7
16. Yang B, Fang JC, Wu XJ, Zhao JH (2000) A method of celestial autonomous orbit determination
for spacecraft. J China Inertial Technol 8(3):33–37
17. Ning XL, Fang JC (2006) A method of autonomous celestial navigation for lunar rover based
on UPF. J Astronautics 27(4):648–654
18. Fang JC, Ning XL (2006) Autonomous celestial navigation for lunar explorer based on genetic
particle filter. J Beijing Univ Aeronaut Astronaut 32 (11):1273–1276
19. Zhang Y, Fang JC (2003) Study of the satellite autonomous celestial navigation based on the
unscented kalman filter. J Astronaut 24(6):649–650
20. Ning XL, Fang JC (2003) A new method of autonomous celestial navigation for spacecraft
based on information fusion. J Astronaut 24(6):579–583
21. Fang JC, Ning XL (2003) Autonomous celestial orbit determination using bayesian bootstrap
filtering and EKF. Fifth international symposium on instrumentation and control technology,
pp 219–222
22. Tu SC (2005) Attitude dynamics and control of satellites, vol.2. China astronautic publishing
house, Beijing
23. Quan W (2008) Study on the technology on spacecraft integrated navigation based on star
sensor. Beijing: a dissertation submitted for the degree of philosophy doctor of Behang
University
24. Quan W, Fang JC (2005) High-precision simulation of star map and its validity check. Cpto-
electron Eng 32(7):22–26
Chapter 3
Filters in Navigation System

3.1 Introduction

For integrated navigation system, when the system hardware performance is given,
advanced filter estimation method will be an effective way to improve the precision,
real-time performance, and reliability of integrated navigation system, and realize
collaborative transcendence [1, 2]. This chapter gives a brief introduction to research
results of relevant scholars at home and abroad during recent years on filter estimation
method to provide basic theoretical knowledge for research contents of subsequent
chapters of this book.
In the 1960s, R. E. Kalman proposed Kalman filter theory, which successfully
resolved integrated navigation problem of multiple navigation sensors during Apollo
moon landing and attracted extensive attention of the engineering sector. Afterward,
Kalman filter theory began to be universally applied in navigation system. Its appli-
cation principle was to estimate error of the navigation system through Kalman filter
and correct to realize higher-precision navigation based on output information of two
or two nonsimilar navigation systems. However, Kalman filter theory only applied
to linear system with noise of Gaussian distribution [3], where systems were mostly
nonlinear systems without Gaussian noise. On the basis of Kalman filter theory,
many scholars at home and abroad began to show a stirring of interest in research on
optimal estimation method to solve the practical problem, and many advanced filter
methods appeared successively.
At the beginning of 1970s, in order to solve the optimal estimation problem of
nonlinear system, A. H. Jazwinski proposed extended Kalman filter (EKF) method
[4–6], principle of which was to linearize the nonlinear system before obtaining
approximate estimates of quantity of state and its variance through Kalman filter
method. EKF might be applied to nonlinear system, but the linearization error would
reduce precision of the filter, and Jacobian matrix was required to be calculated.
In 1997, S. J. Juliear and J. K. Uhlman proposed a new nonlinear filter method
without the need to linearize nonlinear systems—unscented Kalman filter (UKF)
[7, 8], which was free of truncation error of higher order term and had performance
superior to EKF.

© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 53
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_3
54 3 Filters in Navigation System

For non-Gaussian noise, the particle filter (PF) [9, 10] proposed at the beginning
of this century was a recursive Bayes filter method expressing posterior probabil-
ity distribution of system state variable with random sample (particle), where the
sequential important sampling method is the most fundamental PF method widely
used at present [11–13].
Recent researches show that filter method combining PF and EKF or UKF (such as
EKPF, UPF) [14], i.e., PF methods using EKF or UKF to generate important density
function, can give play to respective advantages of the two methods, which solve not
only non-Gaussian noise problem, but also nonlinear system problem [15, 16]. There
also are model predicative filters that solve unstable model problem and federated
filters that improve fault tolerance and reliability of integrated navigation system, etc.
Researches on advanced filter methods will develop deeper and deeper driven by
demands of high-precision navigation of the aerospace field. The following is an
introduction to several common filter methods [17].

3.2 Kalman Filter

In 1960, Kalman filter firstly proposed by R. E. Kalman was a kind of minimum linear
variance estimation. Kalman filter theory attracted great attention of engineering
application immediately after proposed, and Apollo moon landing flying and C-5 A
aircraft navigation were successful examples during earlier application. At present,
Kalman filter theory remains to be an optimal estimation method most widely applied
in engineering. A navigation system will be taken for instance below to give a brief
introduction to basic equation of discrete Kalman filter.
If the estimated state Xk is driven by system noise sequence Wk−1 at the time of
tk , the drive shall be described in the following state equation:

Xk = k,k−1 Xk−1 + k−1 Wk−1 (3.1)

Measurement of Xk meets the linear relation and the measurement equation shall be:

Zk = Hk Xk + Vk , (3.2)

wherein, k,k−1 is the single-step transfer matrix from the time of tk−1 to tk , k−1
is the system noise drive matrix, Wk is system noise sequence, Hk is measurement
matrix, and Vk is measurement noise sequence. Meanwhile, Wk and Vk meet the
following equation:
    ⎫
E[Wk ] = 0, Cov Wk , Wj = E Wk Wj T = Qk δkj ⎪ ⎪
    ⎬
E[Vk ] = 0, Cov Vk , Vj = E Vk Vj T = Rk δkj , (3.3)
    ⎪
⎪
⎭
Cov Wk , Vj = E Wk Vj T = 0
wherein, Qk is variance matrix of the system noise sequence; Rk is variance matrix
of the measurement noise sequence.
3.2 Kalman Filter 55

If the estimated quantity Xk meets Eq. (3.1), measurement Zk of Xk meets

Eq. (3.2), system noise Wk and measurement noise Vk meet Eq. (3.3), system noise
variance matrix Qk is nonnegative definite, variance matrix Rk of measurement noise
sequence is positive definite, and measurement at the time of k is Zk , estimation X̂k
of Xk shall be worked out as per the following equation:
Single-step state prediction:

X̂k/k−1 = k,k−1 X̂k−1 (3.4)

State estimation

X̂k = Xk/k−1 + Kk (Zk − Hk X̂k/k−1 ) (3.5)

Filtering gain:

Kk = Pk/k−1 Hk T (Hk Pk/k−1 Hk T + Rk )−1 (3.6)

Or

Kk = Pk/k−1 Hk T Rk −1 (3.7)

Mean square error of single-step prediction:

Pk/k−1 = k,k−1 Pk−1 k,k−1 T + k−1 Qk  T k−1 (3.8)

Mean square error of estimation:

Pk = (I − Kk Hk )Pk/k−1 (I − Kk Hk )T + Kk Rk KkT (3.9)

Or

Pk = (I − Kk Hk )Pk/k−1 (3.10)

Or
−1
Pk−1 = Pk/k−1 + HkT Rk−1 Hk (3.11)

Equations (3.4)–(3.11) are just basic equations of discrete Kalman filter. As long as
staring values and P0 are given, state estimation X̂k (k = 1,2, · · · ) at the time of k
will be obtained through recursive calculation according to measurement Zk at the
time of k.
It can be observed from algorithm shown in Eqs. (3.4)–(3.11) that Kalman filter
has two counter circuits: gain counter circuit and filter counter circuit, where the
former is an independent counter circuit and the latter relies on the former.
Within a filtering cycle, it can be observed from precedence order of system in-
formation and measurement information application by Kalman filter that Kalman
filter has two obvious information updating processes: time-updating process and
measurement-updating process. Equation (3.4) shows the method to predict state
56 3 Filters in Navigation System

estimation at the time of k according to state estimation at the time of k − 1, and

Eq. (3.8) conducts quantitative description for quality of such prediction. Only infor-
mation related to system dynamic characteristics is used in Eq. (3.4) and (3.8), such
as single-step transfer matrix, noise drive matrix, and variance matrix of drive noise.
It can be observed from the process of time lapse that the two equations advance
the time from k − 1 to k. Therefore, the two equations have described the time-
updating process of Kalman filter. Other equations are used to calculate correction
of time-updating value, and the correction is determined by quality of time updating
(Pk/k−1 ), quality of measurement information (Rk ), relation between measurement
and state (Hk ), and specific measurement value Zk . All these equations focus on one
purpose, i. e., correct and reasonable application of measurement Zk , therefore this
process has described the measurement-updating process of Kalman filter.
During the promotion of Kalman filter method, application to high-precision
integrated navigation of carriers is one of the most successful engineering applica-
tions [2]. However, Kalman filter theory only applies to linear system with noise
of Gaussian distribution, and the measurement is also required to be linear, but
actual navigation system is nonlinear system without Gaussian noise. Bucy, Suna-
hara, Jazwinski, et al. have conducted further researches on improved Kalman filter
method applying to nonlinear system and non-Gaussian noise to solve nonlinear and
non-Gaussian problems on the basis of Kalman filter theory, which has EKF theory.

3.3 Extended Kalman Filter

Mathematical model of systems is assumed to be linear in optimal state estimation

issue discussed in the previous section. However, mathematical model of systems
in engineering practice is often nonlinear, such as the guidance and control sys-
tem of rocket, inertial navigation system of aircraft and vassal ship, and attitude
determination and positioning system of satellite, etc. Therefore, researches on
nonlinear optimal state estimation problem have significant engineering applica-
tion value. This section will give a brief introduction to a filter method applying to
nonlinear systems—EKF method.

3.3.1 Mathematical Description of Stochastic Nonlinear System

Stochastic nonlinear system may generally be described through following nonlinear

differential equations or nonlinear difference equations.
For discrete stochastic nonlinear system, we have:

X(k + 1) = f [X(k), W (k), k]
(3.12)
Y (k + 1) = h[X(k + 1), V (k + 1), k + 1],

wherein, f [•] is nonlinear function of n-dimensional vector, h[•] is nonlinear function

of m-dimensional vector, {W (k); k ≥ 0} is p-dimensional system interfering noise
3.3 Extended Kalman Filter 57

vector, {V (k); k ≥ 0} is m-dimensional observation noise vector, and X(t0 ) or X(0)

is arbitrary n-dimensional stochastic or nonstochastic vector.
If probability distribution of noise {W (k); k ≥ 0} and {V (k); k ≥ 0} is arbitrary,
Eq. (3.12) has described a pretty general stochastic nonlinear system, which is hard
for solving the problem of optimal state estimation. Therefore, appropriate restric-
tions are usually imposed on above model, and several assumptions complying with
the reality and convenient for mathematical treatment are provided for statistical char-
acteristics of noise. That is to say, the optimal state estimation problem of nonlinear
system to be discussed below is based on following stochastic nonlinear model.

X(k + 1) = f [X(k), k] + [X(k), k]W (k)
(3.13)
Y (k + 1) = h[X(k + 1), k + 1] + V (k + 1),

wherein, {W (k); k ≥ 0} and {V (k); k ≥ 0} are both zero-mean Gaussian white noise
process or sequence irrelevant to each other, and they are also irrelevant to X(0).
That is for k ≥ 0, we have:
% &
E {W (k)} = 0, E W (k)W T (j ) = Qk δkj
% &
E {V (k)} = 0, E V (k)V T (j ) = Rk δkj
% & % & % &
E W (k)V T (j ) = 0, E X(0)W T (k) = E X(0)V T (k) = 0

And the original state is Gaussian distribution random vector with following mean
value and variance matrix:

E {X(0)} = μx (0), Var {X(0)} = Px (0)

X(t) or f [•] is n-dimensional state vector, f [•] is n-dimensional nonlinear vector

function, h[ • ] is m-dimensional nonlinear vector function, and F [ • ] or [ • ] is
n × p-dimensional matrix function.

3.3.2 Discrete Extended Kalman Filter

EKF firstly linearizes nonlinear vector functions f [•] and h[•] in stochastic nonlinear
system model around the filtering value line to get linear system model, and then
applies basic equation of Kalman filter to realize optimal estimation of nonlinear
system.
After expanding nonlinear vector function f [ • ] in state Eq. (3.13) of discrete
stochastic nonlinear system into Taylor series around the state estimation X̂(k|k) and
leaving out terms above quadratic, we get:
 
X(k + 1) = f X̂(k|k), k
 
∂f X(k), k     
+  X(k) − X̂(k|k) +  X(k), k W (k) (3.14)
∂X(k) X(k)=X̂(k|k)
58 3 Filters in Navigation System

After expanding nonlinear vector function h[•] in measurement Eq. (3.13) into Taylor
series around the state estimation X̂(k + 1/k) and leaving out terms above quadratic,
we get:
 
Y (k + 1) = h X̂(k + 1|k), k + 1
 
∂h X̂(k + 1), k + 1   
+  X(k + 1) − X̂(k + 1|k) + V (k + 1)
∂X(k + 1) X(k)=X̂(k+1|k)
(3.15)
If:
∂f   
 =  k + 1, k
∂X X(k)=X̂(k|k)
  ∂f 
 X̂(k|k), k −  X̂(k|k) = U (k)
∂X X(k)=X̂(k|k)
   
 X(k), k =  X̂(k|k), k
∂h 
 = H (k + 1)
∂X X(k+1)=X̂(k+1|k)
  ∂h 
h X̂(k + 1|k), k + 1 −  X̂(k + 1|k) = Z(k + 1)
∂X X(k+1)=X̂(k|k)
Equations (3.14) and (3.15) may be written as:
⎧    
⎨X(k + 1) =  k + 1, k X(k) + U (k) +  X̂(k|k), k W (k)
⎩Y (k + 1) = H k + 1X(k) + Z(k + 1) + V (k + 1),
(3.16)
   
wherein,  k + 1, k and H k + 1 are “Jacobian matrix.”
Obviously, Eq. (3.16) is a circumstance with nonrandom external effect U (k)
and nonrandom observation error term Z(k + 1). In accordance with corresponding
equation of discrete Kalman filter, we can get following discrete EKF equation:

X̂(k + 1|k + 1) = X̂(k + 1|k)

 
+ K(k + 1) Y (k + 1) − Z(k + 1) − H (k + 1)X̂(k + 1|k) ,
(3.17)
i.e.:
X̂(k + 1|k + 1) = X̂(k + 1|k)
'  (
+ K(k + 1) Y (k + 1) − h X̂(k + 1|k), k + 1 ,

wherein,
   
X̂(k + 1|k) =  k + 1, k X̂(k|k) + U (k) =  X̂(k|k), k . (3.18)
3.4 Unscented Kalman Filter 59

So
 
X̂(k + 1|k + 1) = ϕ X̂(k|k), k
'  (
+ K(k + 1) Y (k + 1) − h X̂(k + 1|k), k + 1

Recurrence equation of discrete EKF is:

⎧  
⎪
⎪X̂(k + 1|k + 1) = ϕ X̂(k|k), k
⎪
⎪ '  (
⎪
⎪
⎪
⎪ + K(k + 1) Y (k + 1) − h X̂(k + 1|k), k + 1
⎪
⎪  
⎪
⎨K(k + 1) = P (k + 1|k)H T (k + 1) H (k + 1)P (k + 1|k)H T (k + 1) + Rk+1 −1
⎪
⎪  
⎪
⎪ P (k + 1|k) = F k + 1, k P (k|k)F T [k + 1, k]
⎪
⎪    
⎪
⎪ + G X̂(k|k), k Qk GT X̂(k|k), k
⎪
⎪
⎩P (k + 1|k + 1) = I − K(k + 1)H (k + 1)P (k + 1|k)
⎪

(3.19)

The original value is:

X̂(0|0) = E {X(0)} = μx (0)
P (0|0) = Var {X(0)} = Px (0)

wherein,
∂f [x(k), k]
[k + 1, k] = | ,
∂X(k) X(k)=X̂(k|k)
∂h[X(k + 1), k + 1]
H (k + 1) = |X(k)=X̂(k+1|k) .
∂X(k + 1)
EKF has been widely applied in navigation fields of guided missile and aircraft,
etc. However, it should be pointed out that above discrete EKF equation can only be
applied when the filter error X̃(k|k) = X(k) − X̂(k|k)and single-step prediction error
X̃(k + 1|k) = X(k + 1) − X̂(k + 1|k) are both small; and there are restrictions on
precision since that linearization of nonlinear systems and calculation of “Jacobian”
matrix are required during application.

3.4 Unscented Kalman Filter

Performance of EKF is affected by system linearization and Jacobian matrix calcu-

lation. S. J. Juliear and J. K. Uhlman proposed a new nonlinear filter method called
UKF [7] in 1997 to solve the problem. For linear system, its filter performance is
comparable to Kalman filter, but for nonlinear system, its performance is obviously
superior to EKF, i.e., it is unnecessary to calculate Jacobian matrix, nor to linearize
the state equation or measurement equation; therefore, there is no truncation error
for higher order term [18].
60 3 Filters in Navigation System

Basic principle of such algorithm is to assume a discrete nonlinear system first:

xk+1 = f (xk , uk , k) + ωκ (3.20)

yk = h(xk , uk , k) + ν κ (3.21)

Wherein, xk is the system state vector, uk is input control vector, ωk is system noise
vector, yk is observation vector, and vk is measurement noise vector.
Select a series of sampling points nearby x̂k , and make mean value and covari-
ance of such sampling points be x̂k and Pk . Select a series of sampling points nearby
X̂(k|k), and make mean value and covariance of such sampling points be X̂(k|k)
and P (k|k). Such sampling points will generate corresponding transformed sam-
pling points when passing through the nonlinear system. Calculate such transformed
sampling points to get predicted mean value and covariance.
If the state variable is n-dimension, 2n + 1 sampling points and their weights are
respectively as follows:
⎧
⎪
⎪ χ 0,k = x̂ k W0 = τ/(n + τ )
⎪
⎪
⎪
⎨ √ √ 
χ i,k = x̂ k + n + τ P (k|k) i Wi = 1/[2(n + τ )] (3.22)
⎪
⎪
⎪
⎪
⎪
⎩ χ √ √ 
i+n,k = x̂ k − n + τ P (k|k) i Wi+n = 1/[2(n + τ )]
√ 
wherein, τ ∈R; when P (k|k) = AT A, P (k |k ) i is row i of A, and when
√ 
P (k|k) = AAT , P (k |k ) i is column i of A. Standard UKF algorithm is as follows:
1. Initialization
  T 
x̂ 0 = E[x 0 ], P0 = E x 0 − x̂ 0 x 0 − x̂ 0 , for k ≥ 1 (3.23)

2. Sampling Points Calculation

 √ √  √ √  
χ k−1 = x̂k−1 x̂k−1 + n+τ Pk−1 i
x̂ k−1 − n+τ Pk−1 i
(3.24)
i = 1,2, · · ··, n

3. Time Updating

χ k|k−1 = f (χ k−1 , uk−1 , k − 1) (3.25)

2n
x̂ −
k = Wi χ i,k|k−1 (3.26)
i=0
  T
Pk− = Wi χ i,k|k−1 − x̂ k̄ χ i,k|k−1 − x̂ k̄ + Qk (3.27)
3.5 Particle Filter 61

y k|k−1 = h(χ k|k−1 , uk , k) (3.28)

2n
ŷ k̄ = Wi y i,k|k−1 (3.29)
i=0

4. Measurement Updating

2n
  T
Pỹk ỹk = Wi y i,k|k−1 − ŷ k̄ y i,k|k−1 − ŷ k̄ + Rk (3.30)
i=0

2n
  T
Pxk yk = Wi χ i,k|k−1 − x̂ k̄ y i,k|k−1 − ŷ −
k (3.31)
i=0

Kk = Pxk yk Pŷ−1
k ŷk
(3.32)
 
x̂k = x̂k̄ + Kk yk − ŷk̄ (3.33)

Pk = Pk̄ − Kk Pŷk ŷk KkT , (3.34)

wherein, Qk and Rk are respectively covariances of system and measurement noise.

When x(k) is assumed to be Gaussian distribution, n + τ = 3 is often selected. When
τ < 0, calculated estimation error covariance P (k + 1|k) is likely to be negative
definite. A correctional prediction algorithm is usually adopted in such case, while
mean value of predication value still adopts Eq. (3.24) to calculate and covariance
of predication value changes to be around χ 0,k+1/k .
For nonlinear system, UKF will reach good filtering effect and has extensive
application prospect in satellite GNC system; but it has shortcomings, too, i.e., it
has more calculations than Kalman Filter, low real-time performance, and requires
noise to have statistical characteristic of Gaussian distribution.

3.5 Particle Filter

PF is a recursive Bayes filter method using several random samples (particles) to

express posterior probability distribution [19] of system state variable and available
to be applied to any noise distribution system. Basic thought of PF is to approximate
posterior probability density function of state variable by using a batch of discrete
random sampling points with corresponding weights, where the batch of sampling
points is called particle, according to which and weights of which the state esti-
mation is calculated. When number of particles is enough, the method is close to
optimal Bayes estimation [20, 21]. Probability density function of state variable in
PF provides all information related to state variable distribution, and it is possible to
calculate and get the maximum likelihood estimation, minimum variance estimation,
62 3 Filters in Navigation System

and maximum posterior estimation of state variable according to different' criterion

(
(i)
functions once the probability density function is obtained. If N particles x0:t can
be obtained from posterior probability density p(x0:t |y1:t ) through sampling, it is
possible to approximate and estimate posterior probability density by using following
empirical probability distribution:
N
1
p̂(x0:t |y1:t ) = δx (i) (dx0:t ), (3.35)
N i=1
0:t

wherein, δ is Dirac function; when N is large enough, the posterior estimation proba-
bility converges to posterior probability density according to law of large numbers. In
accordance with bibliography [22], it is possible to get a group of weighted samples
by using importance sampling density function q easy for sampling, and estimate
approximately the distributed sample p by using the group of weighted samples;
therefore, selection of importance sampling density is one of the most important
steps to design PF. Calculation formula of optimal significance sampling density is
given as follows [13]:
     
 (i)  (i)
q xt(i) x0:t−1 , y1:t = p xt(i) x0:t−1 , y1:t (3.36)

However, under practical conditions, it is hard to get optimal sampling distribution.

At present, a basic PF method used widely is SIR filter, which adopts prior probability
density p(xt |xt−1) as importance sampling function and has features of being simple
and easy to achieve; under circumstances with low measurement precision, SIR
filter may obtain good effect, but its estimation precision is low. Since importance
sampling density function of standard PF ignores latest measurement information,
there is large deviation between samples obtained from it and samples generated from
true posterior probability density, especially when the likelihood function is located
in the tail of prior probability density function of system state or the measurement
model has very high precision, such deviation is particularly obvious, and many
samples become invalid due to small normalized weight.
Complete SIR filter algorithm is as follows (except for initialization, other steps
take [tk , tk+1 ] measurement sampling period for instance):
1. Initialization, sample p(x0 ) when t = 0 to generate N particles x0(i) , i = 1, . . . , N
conforming to p(x0 ) distribution;
2. When t ≥ 1, steps are as follows:
– Sequence importance sampling:  generate  N random samples
 (i)
{x̂k(i) , i = 1, . . . , N }conforming to q xk x0:k−1 , y1:k distribution;
– Weight calculation:
       
(i) (i)  (i) (i)
w̃t x0:t = p y1:t | x0:t p x0:t /q x0:t |y1:t (3.37)

Normalize the weight and we get:

   ) N  
(i) (i) (i)
wt x0:t = w̃t x0:t w̃t x0:t (3.38)
i=1
3.5 Particle Filter 63

Calculate valid particle size Neff :

1
Neff =  2 (3.39)
!
N
(i)
wt x0:t
i=1

If Neff is less than the threshold value, conduct resample; the threshold shall
generally be 2N/3.
– Resample: '  (
Conduct resample from discretely distributed xk(i) , wk xk(i) i = 1, ..., N
' ∗ (
for N times to get a group of new particles xk(i ) , 1/N , remaining to be
approximate representation of p(xk |y0:k ).
– MCMC (optional) [23]:
Since diversity of particles may reduce subject to resample, i.e., particles
with large weight are selected for many times, which will make the sampling
result contain many repeated points, diversity of particles lost, and lead to
impossibility to reflect probability distribution of state variable effectively or
even cause filter divergence. MCMC method is the primary method to solve
unscented particle problem at present, which may effectively increase diversity
of particles by adding a MCMC-moving step steadily distributed as posterior
probability density to each particle [24]. Its specific calculation method is as
follows:
Generate a random number  i u to make u ∼ U[0,1] ;
Sample xk∗(i) ∼ p(xk xk−1 ) from Markov chain;

  ∗(i)  
p yk xk
If u ≤ min 1,   (i∗)  :
p yk xk
 ∗ 
Adopt MCMC movement, x0:k i (i )
= xk−1 , xk∗(i) ,
(i ) ∗
i
Otherwise: x0:k = x0:k
– Output:
In accordance with minimum variance principle, the optimal estimation is just
mean value of conditional distribution:
N
x̂k = wki xki (3.40)
i=1

N
  T
pk = wki xki − x̂k xki − x̂k (3.41)
i=1

As recursive Bayes filter based on posterior probability distribution, PF may

effectively solve optimal estimation problem of integrated navigation sys-
tem with noise of non-Gaussian distribution. However, it is difficult to select
64 3 Filters in Navigation System

suitable importance sampling density, and it is impossible to consider measure-

ment information if the selected importance sampling density is not so good,
which will make many particles invalid and cause low filtering precision.

3.6 Unscented Particle Filter (UPF)

To overcome shortcomings of above standard PF, Eric Wan et al. proposed UPF
method in 2000, which used UKF to obtain importance sampling density of PF, i.e.,
using UKF to generate next prediction particle, and sampling density of each particle
is obtained through following equation:
    
 (i)
q xk x0:k−1 , y1:k = N x̄k(i) , Pk(i) , i = 1, · · · , N , (3.42)

wherein, x̄k(i) and Pk(i) are respectively mean value and covariance calculated by using
UKF. Though posterior probability density may not be Gaussian distribution, it is
feasible to approximate distribution of each particle through Gaussian distribution,
and since posterior probability density is estimated to the second-order term through
UKF, nonlinearity of the system is maintained well. After substituting step and
formula of UKF to standard PF algorithm, complete UPF algorithm will be obtained
[25]. Algorithm steps of UPF are given below [14].
1. Initialize when t = 0:
Sample p(x0 ) to generate N particles x0(i) , i = 1, · · · , N conforming to p(x0 )
distribution, and their mean value and variance meet:
  ⎫
x̄0(i) = E x0(i) ⎪
⎬
 
T ⎪
(3.43)
P0(i) = E (x0(i) − x̄0(i) )(x0(i) − x̄0(i) ) ⎭

2. Steps Are as Follows When t ≥ 1:

– Sampling: ' ( ' (
(i) (i)
Update particle xk−1 , Pk−1 by using UKF to get x̄k(i) , Pk(i) , and sample

 (i)
x̂k(i) ∼ q(xk(i) x0:k−1 , y1:k ) = N (x̄k(i) , Pk(i) ), i = 1, · · · , N .
– Weight calculation:
  
 (i) 
p yk x̂k(i) )p(x̂k(i) xk−1
w̃k(i) = w̃k−1
(i)
   (3.44)
 (i)
q x̂k(i) x0:k−1 , y1:k

Normalize the weight and we get:

) N
wk(i) = w̃k(i) w̃k(i) (3.45)
i=1
3.7 Predictive Filtering 65

– Resample: ' (
Conduct resample from discretely distributed xk(i) , wk (xk(i) ) i = 1, · · · , N
' ∗ (
for N times to get a group of new particles xk(i ) , 1/N , remaining to be
approximate representation of P (xk |y0:k ). Since diversity of particles may
reduce subject to resample, the particles shall be subject to rugged operation
through MCMC method to solve this problem.
– Output:
In accordance with minimum variance principle, the optimal estimation is just
mean value of conditional distribution:
N
x̂k = wki xki (3.46)
i=1

N
  T
pk = wki xki − x̂k xki − x̂k (3.47)
i=1

Applying to nonlinear and non-Gaussian noise system, UPF is an advanced

nonlinear non-Gaussian filter method which can greatly meet filtering de-
mands of practical engineering application system. Shortcomings of UPF lie
in that its precision is in direct proportion to number of particles, the more the
number of particles, the higher the precision, but the real-time performance
will decline with the increase of number of particles. However, with develop-
ment of computer technology, UPF will have extensive application prospect
in high-precision navigation field.

3.7 Predictive Filtering

Filter methods above focus on precision of filter. The following is an introduction to

fast filter of arbitrary nonlinear system model—model predictive filtering (MPF).
Called predictive filtering for short, MPF is a real-time filter method based on
nonlinear system model [26]. Thought of such estimation method was derived from
predictive tracking scheme in system control, and Crassidis has further developed it
to filtering estimator with random measurement process [26]. Operating principle of
such new real-time nonlinear filter is to track measurement output by using prediction
output to estimate model error of the system [27, 28]. Compared with other nonlinear
filter methods, predicative filter can estimate model error of any form on line and
correct it effectively, so it has developed rapidly [29, 30].
Assume a nonlinear system is as follows:

ẋ(t) = f (x(t), t) + G(x(t), t)D(t) (3.48)

y(t) = h(x(t), t) + v(t), (3.49)

66 3 Filters in Navigation System

wherein, f ∈ Rn is continuously differentiable nonlinear function, x(t) ∈ Rn is

state variable, D(t) ∈ Rq is model error vector, and G ∈ Rn×q is model error
distribution matrix. y(t) ∈ Rm is measurement vector, v(t) is measurement noise
vector
% and assumed
& to be zero-mean Gaussian white noise, and the covariance is
E v(t)vT (t) = R.
The relation between state estimation and predictive output estimation is as
follows:

˙ = f (x̂(t), t) + G(x̂(t), t)D(t)

x̂(t) (3.50)

ŷ(t) = h(x̂(t), t) (3.51)

 
Record the i component of h x̂ (t), t as hi , where i = 1, . . . m. Conduct continu-
˙ in Eq. (3.50) to right
ously differential for Eq. (3.51), substitute expression of x̂(t)
end of the equation, and record minimum order of any component of D(t) first ap-
pearing in hi differential as pi . After taking the smaller time interval t and expand
approximately the i component of Eq. (3.51) to pi -order Taylor series while ignoring
higher order terms, we get:

∂ ŷi (t) t 2 ∂ 2 ŷi (t) t pi ∂ pi ŷi (t)

ŷi (t + t) ≈ ŷi (t) + t + + · · · +
∂t 2! ∂t 2 pi ! ∂t pi



∂hi ∂ x̂ t 2 ∂ ∂hi t pi ∂ ∂ pi −1 hi
= ŷi (t) + t + + ··· +
∂ x̂ ∂t 2! ∂t ∂t pi ! ∂t ∂t pi −1
(3.52)

In accordance with k -order Lie derivative Lkf (hi ) in Lie derivative definition:
L0f (hi ) = hi
∂Lk−1 (3.53)
f (hi )
Lkf (hi ) = f (x̂(t), t) k≥1
∂ x̂
Since the minimum order of any component of D(t) first appearing in hi differential
is pi , when the differential order is less than pi , ∂h i
∂ x̂
G(x̂(t), t)D(t) = 0, therefore in
accordance with Lie derivative definition, Eq. (3.52) may be written as:

t 2 2
ŷi (t + t) ≈ ŷi (t) + tL1f (hi ) + L (hi ) + · · ·
2! f
i p −1
t pi pi t pi ∂Lf (hi )
+ Lf (hi ) + G(x̂(t), t)D(t) (3.54)
pi ! pi ! ∂ x̂

Work out Taylor expansion of ẑi when i = 1, . . . m respectively and write it

collectively as matrix form, so we get:

ŷ(t + t) ≈ ŷ(t) + S(x̂(t), t) + ( t)U (x̂(t))D(t), (3.55)

3.7 Predictive Filtering 67

wherein, ( t) ∈ R m×m is the diagonal matrix, and its diagonal element is:
t pi
λii = , i = 1, . . . , m (3.56)
pi !
Record first-order Lie derivative of scalar function Lkf (hi ) relevant to the vector field
 
gj x̂ (t), t as Lgj Lkf (hi ):

∂Lkf (hi )
Lgj Lkf (hi ) = gj (x̂(t), t) (3.57)
∂ x̂
 
U (x̂(t)) ∈ R m×q is sensitivity matrix, and according to Eq. (3.57), U x̂ (t) can be
expressed as:
⎡ ⎤
p −1 p −1
Lg1 Lf 1 (h1 ) · · · Lgq Lf 1 (h1 )
⎢ ⎥
  ⎢ p2 −1 p2 −1 ⎥
⎢ Lg1 Lf (h2 ) · · · Lgq Lf (h2 ) ⎥
U x̂ (t) = ⎢ ⎥, (3.58)
⎢ ··· ··· ··· ⎥
⎣ ⎦
p −1 p −1
Lg1 Lf m (hm ) · · · Lgq Lf m (hm )

wherein, gj is column j of G, and j = 1, . . . q.

S(x̂(t), Δt) is q-dimension column vector, and its components are:
pi
t k k
Si (x̂(t), t) = L (hi ), i = 1, . . . q (3.59)
k=1
k! f

Define the performance index function according to operating principle of predictive

filter:
1 T  
J (D(t)) = y(t + t) − ŷ(t + t) R −1 y(t + t) − ŷ(t + t)
2
1
+ D T (t)W D(t), (3.60)
2
wherein, W ∈ R q×q is model error weighting matrix.
The performance index function consists of residual weighted sum square between
measurement output and prediction output as well as weighted sum square of model
correction item.
If the smaller time interval is a constant value, then y(t) = yk , y(t + t) = yk+1 .
To make J minimum, it is necessary to meet the condition of ∂J /∂Dk = 0, and thus
model error estimation within the time interval of [tk , tk+1 ] can be obtained:
'  T    (−1
D̂k = −  ( t) U x̂k R −1  ( t) U x̂k + W
  T     (3.61)
·  ( t) U x̂k R −1 S x̂k , t − zk+1 + ẑk

'  T    (−1

Mk =  ( t) U x̂k R −1  ( t) U x̂k + W
  T
·  ( t) U x̂k R −1 ,
68 3 Filters in Navigation System

If above equation can be written as:

   
D̂k = −Mk S x̂k , t − yk+1 + ŷk (3.62)

In conclusion, the flow of predictive filter is briefly summarized as follows: (1)

calculate predictive output vector ŷk according to Eq. (3.59) based on state estimation
x̂k at the time of tk ; (2) Estimate model error D̂k within the time interval [tk , tk+1 ]
by using Eq. (3.62); (3) Estimate quantity of state x̂k+1 at the time of tk+1 by using
the state Eq. (3.50) based on x̂k and D̂k . The filter method tracks measurement
output through prediction output. It can effectively estimate model error of nonlinear
system, and compared with other nonlinear filter methods such as UKF and UPF, it
has good real-time performance.

3.8 Federated Filter

Several common advanced filter methods have been introduced previously, and there
are usually two ways when applying these filtering methods to integrated navigation
of multiple navigation sensors: one is centralized filtering and the other is decentral-
ized filtering. Centralized filtering carries out centralized processing of information
of all navigation systems by using a filter; it has simple structure, easy to realize
in engineering, but it has high state dimension, heavy computational burden, and
poor fault-tolerance performance. Therefore, Pearson proposed decentralized filter-
ing based on two-stage structure of dynamic decomposition and state estimation in
1971. Thereafter, Speyer, Willsky, Bierman, Kerr, Carlson, et al. conducted intensive
studies on decentralized filtering technology and proposed many methods to improve
decentralized filtering. Among them, federated filter proposed by Carlon received
widespread attention due to its flexible design, small computational quantity, and
good fault-tolerance performance [2].
Federated filter is used to solve following two problems:
1. Good fault tolerance of filter. In case of fault of one or several navigation sub-
systems, it must be easy to detect and isolate the fault and combine (reconstruct)
remaining normal subsystems rapidly to continue to give required filtering result.
2. Algorithm of synthesis (fusion) from local filtering to global filtering must be
simple with small computational quantity and less data communication for the
sake of real-time implementation of algorithm.

3.8.1 Structure of Federated Filter

Ordinary federated filter has one public reference system and many several
subsystems, as shown in Fig. 3.1
3.8 Federated Filter 69

Reference system

−1
Xˆ g , β1 Pg
Master filter
Xˆ 1 , P1
Subsystem 1 Z1 Local filter 1

−1 Time updating Xˆ g , Pg
Xˆ g , β 2 Pg
Xˆ 2 , P2 Xˆ g
Subsystem 2 Z2 Local filter 2
Xˆ m , Pm β m −1 Pg
−1
M M Xˆ g , β N Pg
Optimal fusion
Xˆ N , PN
Subsystem 1 ZN Local filter M

Fig. 3.1 General structure of federated filter

3.8.2 Fusion Algorithm

Federated filter is a decentralized filter method with two-stage data processing and
consists of numerous subfilters and one master filter. Circumstance of only two
existing local filters is considered hereby for simplexes. Local states are assumed to
be X̂1 and X̂2 , and variance matrix of corresponding estimation error is P11 and P22 .
The global state estimation X̂g subject to fusion is linear combination of local state
estimation, i.e.:
X̂g = W1 X̂1 + W2 X̂2 , (3.63)
wherein, W1 and W2 are undetermined weighted matrix.
Global estimation shall meet the following conditions: (1) If X̂1 and X̂2 are
unbiased estimations, X̂g is also unbiased estimation; (2) Covariance matrix for
estimation error of X̂g is the least.
When the system is observed independently by n observation sensors, there are
n local filters correspondingly, where each filter can complete filtering computation
independently, and the principle of such federated filter is the same as that with only
two local filters.
Typical federated Kalman-filtering algorithm is divided into four processes [31]:
(1) Information distribution process
Information distribution process refers to distribute the system information among
various subfilters and the master filter. Process information Q−1 (k) and P −1 (k) of
the system is distributed among various subfilters and the master filter as per the
following information distribution principle:
Qi (k) = βi−1 Qf (k)
Pi (k) = βi−1 Pf (k) i = 1,2, · · · , n, m, (3.64)
X̂i (k) = X̂f (k)
70 3 Filters in Navigation System

wherein, m is the subscript of the master filter, and βi > 0 is information distribution
factor complying with information distribution principle:
n
βi + β m = 1 (3.65)
i=1

Variance upper bond technique is adopted during system noise distribution, so the
estimation of each subfilter is suboptimal estimation.
(2) Time updating of information
Time-updating process of federated Kalman filtering is implemented independently
among various master filters and subfilters, filtering algorithm of which is:

X̂i (k + 1/k) = (k + 1, k)X̂i (k/k)

P̂i (k + 1/k) = (k + 1, k)Pi (k/k)T (k + 1, k) + (k + 1, k)Qi (k) T (k + 1, k)
(i = 1,2, · · · , n, m) (3.66)

(3) Measurement updating of information

The master filter will not be subject to measurement updating since it does not have
measurement quantity. Measurement updating is only implemented among various
subfilters and completed through the following equation:

Pi−1 (k/k) = Pi−1 (k/k − 1) + Hi T Ri−1 (k)Hi (k)

Pi−1 (k/k)X̂i (k/k) = Pi−1 (k/k − 1)X̂i (k/k) + Hi T (k)Ri−1 Zi (k)
(i = 1,2, · · · , n) (3.67)

(4) Information fusion

Fusion algorithm of federated filter is to fuse local estimation information of various
local filters as per the following equation to get the global optimal estimation.

Pf−1 = P1−1 + P2−1 + · · · + Pn−1 + Pm−1 (3.68)

Pf−1 X̂f = P1−1 X̂1 + P2−1 X̂2 + · · · + Pn−1 X̂n + Pm−1 X̂m

Though information loss is caused by variance upped bond technique in local filters
through above information distribution, time updating, measurement updating, and
information fusion processes, the information quantity is recovered to the original
value during fusion and the global optimal result is obtained finally.

3.9 Chapter Conclusion

Advanced filtering estimation methods can effectively improve precision, real-time

performance, and reliability of navigation system, and especially for random error
processing integrated navigation system, filter method is a more effective solution.
References 71

This chapter mainly introduces several common advanced filter methods used in
integrated navigation system.
The classical Kalman filter method applying to linear and Gaussian noise system
is introduced first. On this basis, specific to nonlinear system, non-Gaussian noise
and unstable system model problems, several effective advanced filter methods are
introduced respectively. (1) Specific to nonlinear problem of navigation system, EKF
method and UKF method are introduced. The former is widely used in guided missile,
aircraft, and other navigation fields, but it requires linearization of nonlinear system
and calculation of Jacobian matrix, so there are restrictions imposed on precision; the
latter does not required linearization of nonlinear system, it can achieve good filtering
effect, and has good application prospect in satellite GNC system, but its real-time
performance is low. (2) Specific to system with non-Gaussian distribution noise, PF
method and UPF method are introduced; PF is very effective for the circumstance
of non-Gaussian distribution noise, but it is hard to select importance sampling
density during application, and the filtering precision is affected; but UPF applies to
both nonlinear system circumstance and non-Gaussian noise circumstance, so it will
have extensive application prospect in high-precision navigation field. (3) Specific to
problem of uncertain system model, a real-time model predictive filter method based
on nonlinear system model with good filtering stability is introduced; it is possible
for real-time and rapid estimation of unknown model error and correction of system
model to get the optimal state estimation. Finally, specific to system fault tolerance
and reliability problem, federated filter method is introduced. Since it has two-
stage filtering structure and feedback mechanism, in case of fault of one or several
navigation subsystems in the integrated navigation system, it is possible to effectively
and rapidly detect and isolate the fault and reconstruct the system by using it. The
method has advantages such as simple algorithm, small computational quantity and
high fault tolerance and reliability, so it has been widely used in high-performance
integrated navigation system at present.
Implementation of information fusion for the integrated navigation system by
using advanced filter methods introduced in this chapter may effectively improve
precision, real-time performance and reliability of integrated navigation system,
achieve performance superior to any single navigation system, and realize collabora-
tive transcendence. Advanced filter methods introduced in this chapter have provided
prerequisite knowledge for subsequent chapters. These advanced filter methods will
be verified and applied in subsequent chapters.

References

1. Rogers RM (2007) Applied mathematics in integrated navigation systems. American institute

of aeronautics and astronautics, Inc
2. Qin YY, Zhang HY, Wang SH (1998) Kalman filter and principle of integrated navigation.
Northwestern Polytechnical University, Hsian
3. Kalman RE, Bucy RS (1961) New results in linear filtering and prediction theory. J Basic Eng
(ASME)
72 3 Filters in Navigation System

4. Farina A, Benvenuti D (2002) Tracking a ballistic target: Comparison of several nonlinear

filters. IEEE Trans Aerosp Electron Syst 38(3):854–867
5. Jazwinski AH (1970) Stochastic processes and filtering theory. Academic, New York
6. Jazwinski AH (1973) Nonlinear and adaptive estimation in re-entry. AIAA J 11(7):922–926
7. Julier SJ, Uhlmann JK (1997) A new extension of the Kalman filter to nonlinear systems. The
11th international symposium on aerospace/defense sensing. Simulation and controls, Orlando,
FL
8. Julier SJ, Uhlmann JK (2002) Reduced sigma point filters for the propagation of means and co-
variances through nonlinear transformations. Proceedings of the American Control Conference
Anchorage, AK, pp 887–892
9. Arulampalam SM, Simon M, Neil G, Tim C (2002) A tutorial on particle filter for online
nonlinear/non-Gaussian Bayesian tracking. IEEE Trans Signal Process 50(2):1–15
10. Kwok C, Fox D, Meila M (2004) Real-time particle filters. Proc IEEE, 92(3):469–484
11. Michael AH (1998) Sequential importance sampling algorithms for dynamic stochastic pro-
gramming. dempster centre for financial research, judge institute of management studies.
University of Cambridge, Cambridge, England and Cambridge systems associates limited
12. Fearnhead P (1998) Sequential Monte Carlo methods in filter theory. PhD thesis, University
of Oxford
13. Doucet A, Gordon NJ, Krishnamurthy V (2001) Particle filters for state estimation of jump
Markov linear systems. IEEE Trans Signal Process 49(3):613–624
14. Merwe R, Doucet A, Freitas N, Wan E (2000) The unscented particle filter. Technical report
CUED/F-INFENG/TR 380, Cambridge University Engineering Department
15. Lu P (1994) Nonlinear predictive controllers for continuous systems. J Guidance Control Dyn
17(3):553–560
16. Lu P (1995) Optimal predictive control of continuous nonlinear systems. Int J Control
62(3):633–649
17. Fang JC, Ning XL (2006) Principle and application of celestial navigation. Beihang University,
Beijing
18. Zhang Y, Fang JC (2003) Study of the satellite autonomous celestial navigation based on the
unscented Kalman filter. J Astronaut 24(6):646–650
19. Handschin JE, Mayne DQ (1969) Monte Carlo techniques to estimate the conditional
expectation in multi-stage non-linear filtering. Int J Control
20. Chopin N (2004) Central limit theorem for sequential Monte Carlo methods and its application
to Bayesian inference. Ann Statist 32(6):2385–2411
21. Andrieu C, De Freitas N, Doucet A (1999) Sequential MCMC for Bayesian model selection.
Higher-order statistics. Proceedings of the IEEE signal processing workshop on, 14–16 June
1999, pp 130–134
22. Long A, Leung D, Folta D, Gramling C (2000) Autonomous navigation of high-earth satel-
lites using celestial objects and doppler measurements. AIAA/AAS Astrodynamis Specialist
Conference, Denver, CO, Aug, pp 14–17
23. Hastings W (1970) Monte Carlo sampling methods using Markov chains and their applications.
Biometrika 57:97–109
24. Liu J, Chen R (1998) Sequential Monte Carlo methods for dynamic systems, J Am Stat Assoc
93:1031–1041
25. Leven WF, Lanterman AD (2004) Multiple target tracking with symmetric measurement equa-
tions using unscented kalman and particle filters. System Theory, 2004 Proceedings of the
thirty-sixth southeastern symposium, March 14–16, pp 195–199
26. Crassidis JL, Markley FL (1997) Predictive filtering for nonlinear systems. J Guidance Control
Dyn 20(3):566–572
27. Yang J, Zhang HY (2001) Attitude determination with GPS based on predictive filter. J Aviat
22(5):415–419
28. LinYR, Deng ZL (2001) Model-error-based predictive filter for satellite attitude determination.
J Astronaut 22(1):79–88
References 73

29. Yang J, Zhang HY, Li J (2003) INS nonlinear alignment with large Azimuth misalignment
angle using predictive filter. J Chin Inert Technol 11(6):44–52
30. Li J, Zhang HY (2005) Predictive filters with model error estimations using neural networks.
J Control Decis 20(2):183–186
31. Broatch SA, Henley AJ (1991) An integrated navigation system manager using federated
Kalman filtering. Aerospace and electronics conference, Proceedings of the IEEE 1, pp 422–426
Chapter 4
Error Modeling, Calibration, and Compensation
of Inertial Measurement Unit (IMU)

4.1 Introduction

Strapdown inertial navigation system (SINS) determines all motion parameters such
as position, velocity, and attitude of the carrier through integral operation by us-
ing measurement information and initial system parameter of inertial components.
Neither relying on any outside information during operation nor radiating energy
outside, it is a completely autonomous navigation system. However, since various
errors of navigation system have also been subject to integral operation, navigation
calculation error of SINS accumulates continuously with the time, and oscillates
and diverges constantly. Therefore, it is a must to understand error characteristics of
SINS and conduct necessary system modeling, calibration, and compensation.
In accordance with the composition and structure of SINS, SINS error can be
classified as inertial component, inertial measurement unit (IMU), and system er-
rors. Inertial component error is just an error of the gyro and accelerometer; for
instance, for electromechanical gyro, such error is mainly caused by texture struc-
ture, processing technique, and unequal-elastic deformation, etc. IMU error is also
called as component error, which includes inertial component and installation errors.
Though the gyro and accelerometer have been subject to separate component error
calibration before constituting IMU, installation error is introduced on one side after
IMU constitution and working environment and data collection mode have changed
on the other side, which will make error of inertial components change, so repeated
calibration is necessary. System error is formed during navigation calculation, and
is system position error, velocity error, and attitude error formed from error of IMU
subject to integral operation and oscillating and diverging with time.
Most of the errors mentioned above are determinate errors and leading error
source, so they are required for modeling, calibration, and compensation. Though
other random errors occupy a small proportion in overall error, they still have great
influence on precision of SINS, which will be discussed extensively in subsequent
chapters.
This chapter will introduce research achievements of the author in terms of
strapdown inertial navigation technology during recent years, and introduce SINS
modeling and calibration methods as well as high-dynamic strapdown algorithm
© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 75
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_4
76 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

from three levels of component, part, and system to lay theoretical foundation for
extensive research of subsequent chapters.

4.2 Error Modeling and Compensation of Inertial Sensors

Inertial component is the core of SINS, and its precision directly determines perfor-
mance of SINS. However, inertial components with perfect principle and structure
also have errors caused by various interferences, which has great influence on
precision of SINS.
Errors of inertial components can be classified as determinate errors and random
errors. Also called as system error, determinate error is the main body and leading
error source of inertial component error, so it is necessary to establish determinate
error model of inertial components, determine various error coefficients in the model
through testing, and compensate within SINS. Though occupying only a small part
of inertial component error, random error also has a great influence on precision of
SINS. Random error includes random error with and without statistical regulation.
Specific to random error with statistical regulation, optimal filtering method is often
adopted for estimation, and compensation is implemented within the system. Specific
to random error without statistical regulation, there is no good solution yet at present,
so it requires further study.
Gyro is a very important angular rate-sensitive apparatus among inertial compo-
nents. This chapter will mainly introduce determinate modeling, measurement, and
compensation methods for gyro, and optimal filtering method specific to random
error will be introduced in subsequent chapters.

4.2.1 Error Model of Gyroscopes

There are two factors causing gyro error: one is the internal reason, i.e., various errors
formed from imperfect internal principle, structure and fabrication of gyro; the other
is external reason, i.e., various errors formed from linear- and angular motion of
the carrier. Mathematical expression of the relation between gyro error and relevant
physical quantity is called as gyro error model, which includes mathematical model
of static and dynamic errors.
1. Mathematical Model of Gyro Static Error
Mathematical model of gyro static error is a mathematical expression of gyro error
under linear motion conditions; it has determined the functional relationship between
gyro error and specific force. Taking rotor gyro for instance, it will form disturbance
torque under linear motion conditions due to its internal fabrication error, mass un-
balance, elastic deformation of structure, and unequal elasticity, etc. When carrying
out static error modeling for gyro under normal temperature, the error model is as
4.2 Error Modeling and Compensation of Inertial Sensors 77

shown in Eq. (4.1):

δGsx = Dx + Dxx fx + Dxy fy + Dxz fz + Dxxy fx fy

+Dxyz fy fz + Dxzx fz fx + Dxxx fx2 + Dxyy fy2 + Dxzz fz2 (4.1)

wherein, δGsx is static error of gyro; Dx is constant drift of gyro; Dxi is relevant term
coefficient of gyro and specific force of axis i; Dxij is quadratic term error coefficient
of gyro and specific force of axis i and j; fi is specific force of axis i; x is input axis,
y is output axis, and z is drive axis; wherein i = x, y, z; j = x, y, z.
Equation (4.1) is a relatively complete mathematical model of static error for gyro.
The error model includes ten error terms, where the first term is the drift error term
not sensitive to specific force; the second, third, and fourth terms are drift error terms
sensitive to first power of specific force; the fifth, sixth, seventh, eighth, ninth, and
tenth terms are drift error terms sensitive to square of specific force.
Precision and service conditions are mainly depended on, during practical opera-
tion to select and simplify error model to describe gyro drift and analyze measurement
result.
Components of specific force along each axis of gyro during gyro drift test
conducted in the laboratory are as follows:

fx = −gx , fy = −gy , fz = −gz . (4.2)

Gyro error model under the condition of 1g can be obtained thereby. Taking error
model of ten error terms for instance, complete mathematical model of static error
for gyro used during drift measurement can be written as:

δGx = Dx + Dxx gx + Dxy gy + Dxz gz + Dxxy gx gy

+Dxyz gy gz + Dxzx gz gx + Dxxx gx2 + Dxyy gy2 + Dxzz gz2 . (4.3)

2. Mathematical Model of Gyro Dynamic Error

Mathematical model of gyro dynamic error is a mathematical expression of gyro
error under angular motion conditions; it has determined the functional relationship
between gyro error and angular velocity, angular acceleration. Taking rotor gyro for
instance, it will form disturbance torque under angular motion conditions due to its
internal unequal inertial moment, frame inertia accumulated error, unequal inertia
coupling error, and cross-coupling error. When carrying out dynamic error modeling
for gyro under normal temperature, the error model is as shown in Eq. (4.4):
78 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

δGmx = Hx ω̇x + Hy ω̇y + Hz ω̇z + Hxy ωx ωy + Hxz ωx ωz + Hyz ωy ωz

+Hxx ωx2 + Hyy ωy2 + Hzz ωz2 + Hxxx ωx3 + Hxzz ωx ωz2 (4.4)

wherein, δGmx is dynamic error of gyro; Hi is relevant term coefficient of gyro

and angular acceleration of axis i; Hij is quadratic term error coefficient of gyro
and angular velocity of axis i and j; ωi is angular velocity of axis i; ω̇i is angular
acceleration of axis i; wherein i = x, y, z; j = x, y, z.

4.2.2 Scale Factor Error Modeling of Gyroscope

The MEMS gyroscopes, an alternative to the conventional rate gyroscope based on a

high-speed rotor supported in gimbals, can be used to measure the spacecraft angular
rate with respect to an inertial reference frame 1. The benefits of MEMS gyroscope
compared to conventional gyroscopes are well-known and include robustness, low-
power consumption, potential for miniature dimensions, and hence, for low cost [2].
Despite the potential of the MEMS gyroscope over conventional rate gyroscope, its
performance is degraded due to significant scale factor error, misalignment, noise,
and temperature-bias drift. To decrease the error and improve the performance of
MEMS gyroscopes, the misalignment and scale factor errors have been considered.
In these disturbances, the scale factor error is the main error in dynamic maneuvering
[3]. The typical output characteristic of MEMS gyroscope includes offset, nonlinear,
and asymmetry error of scale factor. The error of scale factor is severe and therefore
vital to analyze and compensate the scale factor error to improve the performance of
the gyroscope to an appropriate level. The error of scale factor has been investigated
and mainly compensated by employing the 1th order curve fitting in least squares
[4]. However, the approach cannot eliminate the nonlinear and asymmetrical error
of scale factor. However, these methods are currently faulty in either performance or
CPU efficiency.
In this section, our attention is focused on the modeling and compensation of the
scale factor errors of dual gimbaled (DG) MEMS gyroscope, which originate from
a number of adverse sources including manufacturing tolerance, material inhomo-
geneity, and inevitable mechanical characteristic variation of sensor with rotational
rate. Based on the operational principle of the MEMS gyroscope, the physical origin
of offset, nonlinear, and asymmetry error of scale factor are analyzed. The model
of scale factor is proposed. Motivated by the capability of fuzzy logic in managing
nonlinearity, the error compensation scheme of scale factor is represented by the
fuzzy model via experimental data and is online compensated by fuzzy logic. The
main idea of this approach is to derive several piecewise linear models for some
intervals of rotational rate by using a linear interpolation process. The fuzzy model
is saved into memory and used to compensate for the dynamic error of scale factor.
4.2 Error Modeling and Compensation of Inertial Sensors 79

Fig. 4.1 Schematic structure

of DG-MEMS gyroscope

Fig. 4.2 Operating principle

of DG-MEMS gyroscope

1. Operating Principle of DG-MEMS Gyroscope

The DG-MEMS gyroscope that had been developed in inertial science was first de-
signed by Draper Lab in 1988. The device is a monolithic-silicon gyroscope and
consists of a vibrating mass, electrostatic drive electrodes, electrostatic pickoff elec-
trodes, two anchors, supporting beams, and inner and outer gimbals as shown in
Fig. 4.1. The operating principle of DG-MEMS gyroscope is well-understood, but a
brief summary is given in Fig. 4.2.
The device is a two-gimbal structure supported by supporting beams. In operation,
the inner gimbal is driven to vibrate at constant amplitude (θx ) by electrostatic torque
using electrostatic drive electrodes placed in close proximity. In the presence of a
rotational rate (
) normal to the plane of the device, the Coriolis force will cause
the outer gimbal to oscillate about its output axis with a frequency equal to the
drive frequency and with an amplitude (θy ) proportional to the input rotational rate
(
). Maximum response is obtained when the inner gimbal is driven at the resonant
frequency of the outer gimbal. The readout of device is accomplished by sensing
the capacitance difference ( C) between the outer gimbal and a pair of electrostatic
pickoff electrodes. When operated open loop, the torsion displacement of the outer
gimbal about the output axis is proportional to the input rotational rate (
).
80 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

2. Errors Analysis and Compensation of Scale Factor

When gyroscope rotates (externally input rate,
) in input axis, the vibrating mass
experiences a Coriolis inertial torque (MG ). The MG can be expressed as:

MG = I1 θ̇x
(4.5)

where I1 is the equivalent moment of inertia, I1 = Ix + Iy − Iz , Ii is the moment

of inertia in i-axis, i = x, y, z. In accordance with Laplace transform principle, the
movement of outer gimbal of DG-MEMS gyroscope subjected to MG is described
by the equation:
MG I1 Sθx
θy = =  2  ·

Iy S 2 + D y S + K y Iy S + 2ζy ωy S + ωy2
I1 ωx θx  90
=   ·
(4.6)
2 2
Iy ωy2 1 − (ωx /ωy) + (2ζy ωx /ωy)2

where θy is torsion amplitude in output axis in engineering unit (rad), Dy and ζy are
the damping coefficient and ratio, ζy = Dy /2 Ky · Iy , ωx and ωy are the resonant
radial frequency of drive and output axis, Ky is the rigidity of output axis, Ky = ωy2 Iy .
For DG-MEMS gyroscope, the electrostatic pickoff electrodes sense torsion am-
plitude (θy ) in output axis and output the capacitance difference ( C) between the
two electrostatic pickoff electrodes. The capacitance difference resulted from θy can
be given as:
⎛ ⎞
  ⎜ I1 ωx θx  90 ⎟
C = f θy = f ⎜
⎝ - ·
⎟ (4.7)
 ) 2 2   ) 2 ⎠
Iy ωy2 1 − ωx ωy + 2ζy ωx ωy

where f () is a function between C and θy . The readout of device is accomplished by

sensing C and output voltage processed by signal processing circuit can be written
as:
⎛ ⎛ ⎞⎞
⎜ ⎜ I1 ωx θx  90 ⎟⎟
uout = e ( C) = e ⎜ ⎜
⎝f ⎝ - ·
⎟⎟ (4.8)
 ) 2 2   ) 2 ⎠⎠
Iy ωy 1 − ωx ωy
2 + 2ζy ωx ωy

where e() is a function between output voltage and capacitance difference( C). The
output voltage in engineering unit (V ), uout is proportional to the input rate. The scale
factor of the sensor that relates the output voltage to the external input rotation rate
(
) is given by:
⎛ ⎛ ⎞⎞
  
e f θy ⎜ ⎜
I1 ωx θx  90 ⎟⎟ 1
S= = e⎜ ⎝ f⎜⎝ - ⎟⎟ . (4.9)

 2 2  2 ⎠⎠

Iy ωy2 1 − ωx /ωy + 2ζy ωx /ωy

4.2 Error Modeling and Compensation of Inertial Sensors 81

The manufacturing tolerance, material inhomogeneity will result in variation the res-
onant radial frequencies, ωx in drive axis and ωy in output axis, and mismatch
in two oscillating modes of DG-MEMS gyroscope. The air damping force on a mi-
crostructure reduces and the quality factor, Q, increases significantly in rare air with
the pressure drop. These inevitable mechanical characteristic variations of sensor
are primary factors of offset of scale factor. Moreover, heat results in a decrease in
Young’s modulus (E), and an increase in compressive thermal internal stress (σ ). The
two effects help to actively lower the resonant frequencies and result in the frequen-
cies mismatch of two oscillating modes too. It is unavoidable that the scale factor
thermal offset with heating.
In addition, the distortion and internal stressing effects of structure result in me-
chanical asymmetrical error of scale factor. The error of demodulation circuit will
result in electrical asymmetrical error of scale factor too. Compared with the con-
ventional rate gyroscope, the electromechanical asymmetrical error of scale factor of
DG-MEMS gyroscope is significant. The magnitude of asymmetrical error of scale
factor is about 0.1 % and cannot be neglected in compensation of scale factor errors.
The capacitance difference of the DG-MEMS gyroscope, as a function of torsion
displacement θy , is given as follows:

εhp z02 − (l + b) θy2

2
C = f (θy ) = − ln 2 (4.10)
θy z0 − (l · θy )2
where ε is the permittivity of free space, hp is the length of electrostatic pickoff
electrode, z0 is the original space between drive electrode and inner gimbal, l is the
space between inner edge of electrostatic pickoff electrode and y-axis, b is the width
of electrostatic pickoff electrode. Since C is nonlinear relative to θy , scale factor
includes nonlinear error. Equation (4.6) can be simplified:
εhp b(2s + b)
C ≈ P (θy ) = · θy (4.11)
z02
where P() is a simplified linear function between C and θy . According to the
Eq. (4.10) and (4.11), the nonlinear error of scale factor can be expressed as following
equation:


     1
Rc = e f θy − P θy = e f (ζ ) · θy
2
(4.12)
2
where Rc is nonlinear error in engineering unit (V ), ζ ∈ (0, θy ). Moreover, the
microstructure effect and other inevitable adverse sources contribute to the nonlinear
error too. An integrated expression of scale factor can be written as:

S(
) = St + SD + Sn + εS (4.13)

where St is the nominal scale factor, SD is offset of scale factor, Sn is nonlinear

and asymmetrical error of scale factor, εS is random error of scale factor. It is well-
known that nonlinear global model can be approximated by a set of piecewise linear
82 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

local models. The main idea of this fuzzy model is to derive several piecewise linear
models for some intervals of rotational rate by using a linear interpolation process
over the entire operating rotational rate. The fuzzy model is described by IF-THEN
rules, which represent local linear input–output relations of nonlinear scale factor,
and is saved into memory and used to compensate for the errors of scale factor. The
fuzzy model with n fuzzy rules is described as:
⎧
⎪ S(
1 ) − S(0)
⎪
⎪S(0) + ·
, 0 ≤
<
1
⎪
⎪ h
⎪
⎪
⎪
⎪ S(0) − S(
-1 )
⎪
⎪S(0) + ·
,
-1 <
< 0
⎪
⎪
⎪
⎪ h
⎪
⎪
⎪
⎪ S(
2 ) − S(
1 )
⎪
⎪S(
1 ) + (
−
1 ),
1 ≤
<
2
⎪
⎪ h
⎪
⎨
S(
-1 ) − S(
-2 )
S(
) = S(
-1 ) + (
−
-1 ),
-2 <
≤
-1
⎪
⎪ h
⎪
⎪
⎪
⎪
⎪
⎪
..
⎪
⎪ .
⎪
⎪
⎪
⎪ S(
n ) − S(
n-1 )
⎪
⎪
⎪S(
n-1 ) +
⎪ · (
−
n-1 ),
n-1 <
≤
n
⎪
⎪ h
⎪
⎪
⎪
⎪ S(
1-n ) − S(
-n )
⎩S(
1-n ) + · (
−
1-n ),
-n <
≤
1-n (4.14)
h
where n is the number of rules,
i and S(
i) are the ith externally input rotational
rate and scale factor of the ith rotational rate point, h is the density of intervals. The
final rotational rate compensated by fuzzy logic, can be written as:
uout

= . (4.15)
S(
)

3. Experimental Results and Discussions

The experiment equipment mainly includes a three-axis rate table, data collect,
and process system. To decrease temperature noise, the gyroscope is calibrated in
a stability temperature. Since the temperature variety is little (about 1 K), the scale
factor thermal offset can be neglected in the experiment. Moreover, a temperature
sensor is used to estimate and compensate bias thermal drift of gyroscope. Testing
has shown that the temperature sensor gives a temperature repetition of about ± 0.1 K.
The gyroscope is mounted in the fixture such that the nominal input axis is aligned
parallel with the spin axis of the rate table. The scale factor errors of gyroscope are
not observable unless the gyroscope is rotating. The calibration manoeuvre chosen
for the calibration run is a series of constant rotational rates. The rate ramped up from
zero to the positive constant rotational rate, then back to negative constant rotational
rate, followed by step-by-step ramp up to maximum rate with specifically rate step.
Concretely, the static outputs of gyroscope in two symmetrical orientations are saved
respectively. Then, the rate table rotated with ten rates, which include 20, − 20, 40,
− 40, 60, − 60, 80, − 80, 100, and − 100◦ /s along the spin axis of the rate table.
4.2 Error Modeling and Compensation of Inertial Sensors 83

0.02
actual
fuzzy
segmented
0.0199 1th order fitting
)

0.0198
−1
)S g / V⋅ (°) ⋅ s −1

0.0197
(
(

0.0196

0.0195

0.0194
-100 -80 -60 -40 -20 0 20 40 60 80 100
(
Ω / (°) ⋅ s −1 )
Fig. 4.3 Calibrated results of scale factor for different models

Every rate is kept for 2 min approximately. The power of gyroscope is on during the
entire calibration process. To every rotational rate, the data is collected respectively
after the rate up to stabilization. The rotational acceleration used in the ramp profile
is 5◦ /s2 in spin axis of the rate table. The rate is ramped up with a maximum value
of ± 100◦ /s, in order to utilize as much as possible of the dynamic range of the
gyroscope. The output of gyroscope in any rate is saved into memory as the data of
the ith rotational rate.
The rate are later read into a MATLAB program for preprocessing (rotational rate
and temperature converters, temperature drift compensated) and are then subjected to
calibration. Since the operating temperature range of the gyroscope is little, scale fac-
tor temperature-sensitivity compensation is not implemented. Based on these data in
memory, the scale factors of gyroscope in all rotational rate points are calibrated. The
fuzzy model of scale factor can be calibrated by employing the proposed piecewise
linear fuzzy rules and the results are shown as Fig. 4.3. In addition, to compare with
other conventional techniques, the 1th order curve fitting and segmented schemes
are calibrated and their results are shown in Fig. 4.3 too. From these results, it can
be seen that, the proposed fuzzy model is able to eliminate the offset, nonlinear
and asymmetry error of scale factor, and more approach to the practical scale factor
compared with other conventional methods.
The second experiment is to check for the performance compensated by proposed
fuzzy logic. The rate table rotated with another ten rates including 10, − 10, 30,
84 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

((°) ⋅h )
−1
15000
10000
5000
0
Ey

-5000
-100 -50 0 50 100
(
Ω / (°) ⋅ s−1 )
((°)⋅h )

2000
−1

0
En

-2000
-100 -50 0 50 100
(
Ω / (°) ⋅ s−1 )
((°)⋅h )

1000
−1

0
Ef

-1000
-100 -50 0 50 100
(
Ω / (°) ⋅ s−1 )
((°)⋅ h )
−1

100
0
-100
Ec

-100 -50 0 50 100

(
Ω / (°) ⋅ s−1 )
Fig. 4.4 Raw and compensated errors of scale factor

− 30, 50, − 50, 70, − 70, 90, and − 90◦ /s. The raw error of gyroscope, Ey , is shown
in Fig. 4.4. To compensate the dynamic error of scale factor, the proposed fuzzy
compensation method is implemented and the error, Ec , is shown in Fig. 4.4. In
addition, to compare with other compensation technique, the 1th order curve fitting
in least squares and segmented schemes are employed and the error, En , compensated
by 1th order curve fitting and the error, Ef , compensated by segmented scheme are
shown in Fig. 4.4 respectively.
From these results, it can be seen that, the raw error of gyroscope output, Ey , is
4053.2◦ /h (1σ). The error compensated by proposed fuzzy logic, Ec , is improved
to 79.0◦ /h (1σ). Compared with the conventional methods of 1th order curve fitting
and segmented methods, the precision of gyroscope compensated by fuzzy logic is
improved 15.4 and 7.5 times respectively.
In this section, we analyzed the physical origin of offset, nonlinear and asymme-
try error of scale factor for DG-MEMS gyroscope. Motivated by the capability of
fuzzy logic in managing nonlinear error, the fuzzy compensation was proposed to
derive several piecewise linear models for some intervals of rotational rate by using
a linear interpolation process. The experimental results showed that the scale factor
error is one of the main dynamic adverse resources, and the proposed fuzzy com-
pensation is able to compensate the offset, nonlinear and asymmetry errors of scale
4.2 Error Modeling and Compensation of Inertial Sensors 85

factor throughout the entire dynamic range. The results validated the veracity and
practicability of the proposed compensation method. The proposed fuzzy compen-
sation outperforms any of its constituent linear counterparts since nonlinear aspects
of the model have been taken into consideration. Moreover, this fuzzy compensation
outperforms segmented compensation method in same density of intervals. By doing
this, we can guarantee a robust performance of device in dynamic maneuvering.

4.2.3 Temperature Error Modeling of Gyroscope

1. Static Electromechanical-Thermal Error

The static thermal errors of MEMS gyroscopes are the electromechanical-thermal
errors in static condition [5]. Other than the electromechanical-thermal considered
in reference [6], the Seebeck effects of electronic circuit and distortion of structure
due to heating are significant for thermal errors of BG MEMS gyroscope [7]. Since
the DG-MEMS gyroscope is operated in a vacuum, the influence of heat flow is
very small. The Seebeck effect of material is prominent. The Seebeck coefficient
increases with temperature, which would cause the output voltage of the thermopiles
to increase with temperature. The output voltage of the thermopiles can be given by:
us = nβ T (4.16)
where n is the amplificatory coefficient of signal amplifying circuit, β is the Seebeck
coefficient, and T is the temperature difference. The Seebeck coefficient of ther-
mocouples is about 0.2 ∼ 0.4 mV/K depending on the doping factor. Thus, with a
several Kelvin ( T ), output voltages of 80 ∼ 100 mV can be easily obtained through
signal amplifying circuit.
For DG-MEMS gyroscope, the distortion of structure due to heating is one of the
factors resulting in mechanical-thermal errors. During the temperature changing, the
supporting beam of gyroscope inevitably presents torsion ( θy ) due to the manufac-
turing tolerances, material inhomogeneity, and thermal internal stress. The output
error resulted from torsion ( θy ) can be given as:
ud = Kv θy (4.17)
A possible way to reduce thermal error might be to model and compensate the
thermal error. Considering nonlinear characteristic of thermal error for silicon MEMS
gyroscope, the electromechanical-thermal error of gyroscope in static condition can
be given by the following equation:
r
Gb = u0 + kbi T ≈ u0 + kb1 T + kb2 T 2 (4.18)
i=1

where Gb is output of gyroscope over entire operating temperature, r is order of

equations, u0 is the bias of gyroscope in initial temperature, and kbi is coefficient of
the error model to best fit the experiment data.
86 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

2. The Resonant Frequency Depending on Temperature

The method presented in this work analyzes the temperature dependence of Young’s
modulus (E), the thermal stress, and thermal mismatch effects, which are exhibited
by silicon material. The structure of gyroscope is heated, which results in a decrease
of Young’s modulus. As the stiffness of structure is proportion to Young’s modulus,
the change of stiffness resulted from heat can be found using the following equation:

KE = −K0 λE T (4.19)

where K0 represents the stiffness of structure at initial temperature, λE is the temper-

ature coefficient of the Young’s modulus. λE is found to be 25 × 10−6 ∼ 75 × 10−6
by the external heating experiment, and the magnitude is close to previously reported
values. That is, as the temperature of sensor rises, its Young’s modulus drops and
this will cause a reduction in stiffness of the structure.
The thermal internal stress (σ ) of structure induced by mechanical boundary and
heat inhomogeneity is proportional to the temperature difference ( T ) and can be
expressed as:

σT = EεT = −αE T , (4.20)

where εT is the strain induced by heat, the coefficient of thermal expansion is denoted
by αT The intrinsic stiffness change ( Kσ ) of structure subjecting to σ can be given
by

Kσ = K0 λσ σT = K0 λσ αE T, (4.21)

where λσ is the coefficient of stiffness change induced by σT . Taking into account

the Young’s modulus and internal stress effects throughout the entire operating
temperature range, the general stiffness K of structure can be described by the
equation:

K = K0 + KE + Kσ = K0 [1 − (λE + λσ αE) T ]. (4.22)

During temperature change, the intrinsic resonant frequency of structure can be

expressed by the following equation:
- -
1 K 1 K0 [1 − (λE + λσ αE) T ]
f = = , (4.23)
2π I 2π I
where I represent the moment of inertia of structure. The heated sensor results in a
decrease in E and an increase in compressive σ . If a compressive σ is generated,
the structure becomes more compliant, which results in a lower resonant frequency.
The two effects help to actively lower the resonant frequencies and result in the
frequencies mismatch of two rocking modes. Moreover, the moment of inertia I
changing, as the dimensions of structure change due to temperature, results intrinsic
resonant frequency of structure to change too. It is unavoidable that the scale factor
thermal error due to frequencies mismatch.
4.2 Error Modeling and Compensation of Inertial Sensors 87

Fig. 4.5 Resonant frequency change by means of heating

The experiment is to check for resonant frequencies and the resonant frequency
split of two rocking modes over the entire operation temperature. In this test, the
gyroscope is heated from 300 to 325 K by placing the sensor in a temperature
chamber. The change of resonant frequencies of the two rocking modes versus the
applied heat is plotted as shown in Fig. 4.5. It is found that the resonant frequencies
of drive mode is detected to be 2921.2 Hz at 325 K, nearly 3.4 Hz lower than its
300 K counterpart measurement and response mode is detected to be 2925.9 Hz at
325 K, nearly 1.8 Hz lower than its 300 K counterpart measurement. The frequency
difference of the two rocking modes caused by heating is 4.7 Hz at 325 K, nearly
1.6 Hz higher than its 300 K counterpart measurement. Moreover, the heating makes
a phase shift in the response mode with θy .
3. Thermal Error Resulting from Accelerations
The thermal distortion of structure will result in inertial interferential moment (ME )
due to gravitational, linear, and angular accelerations. The ME results in thermal bias
drift of DG-MEMS gyroscope. The centroid offsets of the microstructure along x-,
y-, and z-axes can be respectively expressed as:

xc = Lx α T
yc = Ly α T (4.24)
zc = Lz α T ,
88 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

Fig. 4.6 The illustration of

ME induced by accelerations

where Li is the equivalent length in i-axis; i = x, y, z. As shown in Fig. 4.6, the ME

along y-axis can be expressed by the following equation:

ME = −mfz xc + mfx zc
= −m(az + gz − ω̇x yc + ω̇y xc )xc
+m(ax + gx − ω̇y zc + ω̇z yc )zc
= −(fz Lx + fx Lz )αΔT
   
+m Lx Ly ω̇x + Ly Lz ω̇z − L2x + L2z ω̇y α 2 ΔT 2 ,
(4.25)

where m is the effective mass, fi is the inertial acceleration applied in i-axis,

which includes gravitational acceleration (gi ), linear acceleration (ai ), and angular
acceleration (ω̇i ); i = x, y, z.
The output error of gyroscope induced by ME is described by the following
equation:
k V ME Q
uf =   = kV ME (4.26)
Iy S2 + 2ζy ωy S + ωy
2 K y

where kV is ratio coefficient of output relative to angular in output axis, Iy is the

moment of inertia, ζy are the damping ratio, ωy is the intrinsic resonant angular rate,
Ky is the stiffness, Ky = ωy2 Iy , Q is the quality factor of the gyroscope in output
axis; Q = 1/2ζy .
The quality factor Q and stiffness Ky will change with heating too. The change of
frequency difference f between the two rocking modes caused by heat will result
in error of gyroscope. Taking into account ME , the change of Q, Ky , and f induced
4.2 Error Modeling and Compensation of Inertial Sensors 89

Fig. 4.7 The operation principle and hardware components of RLG POS

by heating, the thermal error resulting from accelerations of DG-MEMS gyroscope

can be expressed by the following equation:

Gf = (kfx fx + kfz fz ) T + (kω̇x ω̇x + kω̇y ω̇y + kω̇z ω̇z ) T 2 , (4.27)

where kfi represents the temperature coefficient of the fi resulted in thermal error, kω̇i
represents the temperature coefficient of the ω̇i resulted in thermal error; i = x, y, z.
The integrated thermal error compensation method is to overcome the thermal
error of the DG-MEMS gyroscope. Based on the Seebeck theory of silicon material
and distortion of structure due to heating, electromechanical-thermal error was ana-
lyzed theoretically. The intrinsic resonant frequency change of micro structure was
analyzed by analyzing the temperature dependence of Young’s modulus as well as
the thermal stress effects exhibited by silicon materials. Motivated by the thermal
interferential moment induced by external accelerations, the bias thermal error re-
sulting from accelerations equation previously was first proposed. Taking advantage
of the proposed thermal model, the integrated compensation method was directly
used for compensation of the thermal error by measuring the temperature and sub-
tracting the estimated error from the raw data of MEMS gyroscope. The experimental
results indicate that the proposed integrated compensation method outperforms the
single electromechanical-thermal model, and is feasible and effective in temperature
drift modeling and compensation and verify that the bias thermal error induced by
external accelerations is the primary factor of thermal errors of DG-MEMS gyro-
scope. In the presence of varying external accelerations, the thermal error resulting
from accelerations must be considered. By doing this, we can guarantee a robust
(against temperature variation) sensor performance throughout the entire operating
temperature range.
90 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

4.3 Design, Error Calibration, and Compensation of Inertial

Measurement Units

4.3.1 Design of Inertial Measurement Units

Currently, only the performance of ring laser gyroscope (RLG) is able to meet the
requirement of more accurate IMU in some high-end applications [8,9]. The RLG
with low drift is an optimal angular rate sensor for a new generation high performance
IMU. The Applanix integrated RLG IMU and global positioning system (GPS), and
produced the best product (POS/AV610), it has been widely used as a component in
various airborne imaging payloads including multispectral scanners, scanning lasers,
cameras, and synthetic aperture radar (SAR) to successively provide an optimal posi-
tion, velocity, and attitude solution. The measurement method can be implemented in
real time or in postprocessing, and has become one of the key technologies to further
improve image quality and efficiency of airborne surveying and mapping systems
[10].The requirements of IMU used in position and orientation system (POS) are
fundamentally different from the traditional INS [11]. It is small, lightweight, and
can be mounted directly onto existing imaging payloads. It can accurately measure
the motion with high bandwidth and low position, velocity, as well as attitude noise,
and chiefly determine POS’s performance.
The RLGs are dithered to prevent phenomena known as lock-in [12]. By si-
nusoidally dithering the gyroscope, a continuous input rate is maintained on the
gyroscope to eliminate the lock-in effect. However, the mechanical dither will be-
come an adverse vibration coupling with external random vibration and result in
a measurement instability and error of POS [13]. For a POS used in the motion
compensation of imaging payloads, the RLG IMU requirements can close off the
RLG mechanical dithers transferred to imaging payloads, and avoid the secondary
disturbance [14]. In addition, the RLG IMU requirements can accurately measure
the external high-frequency vibration, and satisfy imaging payloads motion compen-
sation requirements. To solve the incompatible problem, a kinetics model and size
effect model of mechanically dithered RLG IMU must be studied.
As shown in Fig. 4.7, the RLG IMU mainly consists of Inertial Sensing Assembly
(ISA), secondary power module, data collecting module, I/F signal transform mod-
ule, and exterior supporting structure. The secondary power, I/F signal transform,
and data collecting modules are mounted inside of the exterior supporting structure.
The secondary power module provides various voltages to every sensor and mod-
ule. The output currents of three accelerometers can be converted to digital signals
by I/F signal transform module. The data of three gyroscopes and accelerometers
are collected by field-programmable gate array (FPGA) processor and calculated by
digital signal processor (DSP) of data collecting module. The ISA consists of three
gyroscopes and accelerometers assembled in an ISA structure in orthogonal triads.
The ISA structure with inertial sensors is installed on exterior supporting structure
by a vibration damping system (absorbers). The exterior supporting structure is fixed
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 91

Fig. 4.8 The operation sketch of ISA illustrating the mechanical dithers of three RLGs and external
vibration

to or close inside the imaging payloads or reference point, and the IMU can measure
the motion of sensing center for payloads.
The RLG IMU can be regarded as a second steps spring-damping-mass isolation
system. The design of IMU must take account of the coupling problems of three
RLGs’ mechanical dither and the external vibration. The RLG POS must be able to
work normally in various environments, where the adverse disturbance caused by
the mechanical dither of RLG should be eliminated and the external vibration must
be measured accurately, as shown in Fig. 4.8. The three gyroscopes are installed in
the ISA frame along X-, Y -, and Z-axes, respectively. Every gyroscope response to
dither torque can be modeled by a spring-damping-mass linear transfer function
   
MGi = IGi + mGi δ2Gi ω̇SIGi + ωSIGi × IGi + mGi δ2Gi ωSIGi + DGi ϕ̇i + TGi ϕi ,
(4.28)

where MGi is the external torque acting on dithered part of the ith gyroscope, IGi
represents the moment of inertia for dithered part along principal axes of inertia,
mGi is mass of the ith gyroscope, δGi is a distance between the inertia symmetry
axes and dithered axes of the ith gyroscope, ωSIGi is the dithered angular rate of
92 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

gyroscope with respect to the inertial frame, expressed in ISA structure frame, for
the ith gyroscope, DGi , TGi are the damping coefficient and rotational stiffness of
dithered part in dithered axis, and ϕi is the torsion angular of the
 dithered
 part in the
body frame along X-, Y -, and Z-axes, respectively, ϕi = ϕ̂i cos ωpi t with ωpj being
circular frequency of mechanical dither for RLG in j-axes. Neglecting the centroid
linear displacement of ISA frame with respect to the body frame, according to the
law of moment of momentum, and the inertial torque model of gyroscope assembly
can be written as
    
3
IGi + mGi δ2Gi ω̇BIGi + ωBIGi × IGi + mGi δ2Gi ωBIGi
MG = , (4.29)
i=1 + mGi RGi ω̇IS + ωIS × mGi RGi ωIS + mGi (g + a)RGi
2 B B 2 B

where ωBIGi is the dithered angular rate of gyroscope with respect to the inertial
frame, expressed in the body frame, for the ith gyroscope, a is the inputting linear
acceleration of the ISA frame with respect to the inertial frame, expressed in the
body frame, aIBB
= a = â cos (ωa t), with ωa being the circular frequency of the
acceleration, g is the local gravity, RGi is the distance vector of inertia symmetry
axes for the ith gyroscope in reference frame, and ωBIS is the angular rate of ISA
frame with respect to the inertial frame, expressed in the body frame. The inertial
torque models of accelerometers assembly and ISA structure can be written as
3
  B   B
MA = IAi + mAi RAi
2
ω̇IS + mAi (g + a)RAi + ωBIS × IAi + mAi RAi
2
ωIS
i=1
(4.30)
   
MS = IS + mS RS ω̇BIS + mS (g + a) RS + ωBIS × IS + mS RS ωBIS ,
2 2
(4.31)
where mAi , IAi are the mass and moment of inertia of the ith accelerometer, mS , IS are
the mass and moment of inertial of ISA structure, and RAi , RS are the distance vector
of inertia symmetry axes for the ith accelerometer and ISA structure in reference
frame. The angular rate and linear acceleration can be solved as
ωBIS = ωBIB + ωBBS =  + θ̇ (4.32)

ωBIGi = ωBIB + ωBBS + CBS ωSSGi =  + θ̇ + CBS ϕ̇i (4.33)

ωSIGi = CSB (ωBIB + ωBBS ) + ωSSGi = CSB ( + θ̇) + ϕi , (4.34)

where  is the external inputting angular rate of the body frame with respect to
the inertial frame, expressed in the body frame, ωBIB =  =  ˆ cos (ωb t), with ωb
being the circular frequency of the angular rate, and θ is the rotation angular of
ISA structure with respect to exterior supporting structure which must be less than
orientation error requirement of POS, θ = θ̂ cos (ωb t), θ̂ ≤ 10 . There exist the
functions, which are sin (θj ) ≈ 0, cos (θj ) ≈ 1, j = x, y, z and attitude matrix CBS
can be regard as an identity matrix. The Eqs. (4.33) and (4.34) can be rewritten as
ωBIGi = ωSIGi =  + θ̇ + ϕ̇i (4.35)
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 93

˙ + θ̈ + ϕ̈i + (θ̇ ×)−1 ϕ̇i

ω̇BIGi =  (4.36)

˙ + θ̈ + ϕ̈i + θ̇( + θ̇).

ω̇SIGi =  (4.37)

Generally, the external inputting angular rate  sensed by RLG is less than a magni-
tude which is 2πrad/s. The primary design parameters of typical RLG are required,
δGi ≈ 0, ϕ̂i ≤ 4 , 500H z ≤ ωpi ≤ 720H z, and the cross product section as well as
the first-order inertial torque can be neglected. Combining Eq. (4.28) to Eq. (4.36),
the rotational kinetics of mechanically dithered RLG IMU can be modeled by
 
  3
     
IS + mS RS +
2
IGi + mGi RGi + IAi + mAi RAi
2 2
˙ + θ̈
i=1

3 3
 
+ (mGi RGi + mAi RAi ) + mS RS (g + a) + IGi ϕ̈i + Dθ̇ + Tθ = 0,
i=1 i=1
(4.38)

where D, T are the torsion damping coefficient and rotational stiffness of absorbers
for IMU, respectively. According to Eq. (4.38), the integrated rotational kinetics
model can be rewritten as
3
Iθ̈ + ˙ + Dθ̇ + Tθ + M0 = 0
IGi ϕ̈i + I (4.39)
i=1

!3   !
3
where I = IS + mS RS2 + i=1 I Gi + IAi + m Gi R 2
Gi + m Ai R 2
Ai , M 0 = i=1
(mGi RGi + mAi RAi ) + mS RS ] (g + a). According to Eqs. (4.13), (4.19), and (4.37),
neglecting cross product section ωSIGi × (IGi + δ2Gi )ωSIGi and δ2Gi ω̇SIGi , the kinetics of
single RLG can be simply modeled by
˙ + IGi θ̈ + IGi ϕ̈i + DGi ϕ̇i + TGi ϕi = MGi .
IGi  (4.40)

On the other hand, the integrated linear kinetics of IMU with the vibration damping
system can be modeled as m̄aB + C
IS
˙ + K = 0, (4.41)

where C and K are linear damping coefficient and stiffness of the IMU, respectively,
 is the centroid linear displacement of ISA frame with respect to the body frame,
expressed in the body frame. m̄ is the integrated mass
! of ISA including threeB RLGs,
accelerometers, and ISA structure, m̄ = ms + 3i=1 (mGi + mAi ), and aIS is the
linear acceleration of ISA frame with respect to the inertial frame, expressed in the
body frame and can be expressed as
B
aIS
2
¨ + 2θ̇ × ,
= aBIB + g + aBBS = a + g + θ̈δS + θ̇ δS +  ˙ (4.42)
B
where aIB is the linear acceleration of the body frame with respect to the inertial
frame, expressed in the body frame, aIBB
= a, and aBS
B
is a linear acceleration of
94 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

ISA structure frame with respect to the body frame, expressed in the body frame,
2
¨ and
which including acceleration of following θ̈δS + θ̇ δS , relative acceleration 
˙
second derivative acceleration caused by Coriolis effect 2θ̇ × . δS is the position
vector of the centroid of ISA in the body frame and can be reduced to zero dur-
2
ing design, generally. The acceleration of following θ̈δS + θ̇ δS can be deleted in
˙ are enough less,
Eq. (4.42). Moreover, the rotate speed θ̇ and the relative velocity 
the Coriolis acceleration can be neglected, generally. Substituting Eq. (4.42) into
(4.41), the kinetics model of RLG IMU can be rewritten as
¨ + C
m̄(a + g + ) ˙ + K = 0. (4.43)

Compared with the typical strapdown inertial navigation system (INS), an excellent
design of the vibration damping system for mechanically dithered laser gyroscope
IMU used in POS is more vital. Considering the precise measurement of POS in
linear and angular vibration, the design of mechanically dithered RLG IMU must to
be optimized, including vibration uncoupling, resonance frequency and the moment
of inertia of ISA, and the installing position of absorbers, etc. Supposing the angular
displacement θj are enough little, there exist an equation, sin (θj ) ≈ θj . According to
Eq. (4.43), the spring-damping-mass linear kinetics model of IMU can be explicitly
modeled by
n n
¨x +
m̄(ax + gx ) + m̄ ˙x +
ckx ckx Zk θ̇y
k=1 k=1
n n n n
− ckx Yk θ̇z + kkx x + kkx Zk θy − kkx Yk θz = 0 (4.44)
k=1 k=1 k=1 k=1

n n
¨y +
m̄(ay + gy ) + m̄ ˙y −
cky cky Zk θ̇x
k=1 k=1
n n n n
+ cky Xk θ̇z + kky y − kky Zk θx + kky Xk θz = 0 (4.45)
k=1 k=1 k=1 k=1

n n
¨z+
m̄(az + gz ) + m̄ ˙z−
ckz ckz Xk θ̇y
k=1 k=1
n n n n
+ ckz Yk θ̇x + kkz z − kkz Xk θy + kkz Yk θx = 0 (4.46)
k=1 k=1 k=1 k=1

where ckj and kkj are the linear damping coefficient and stiffness of observers in j-axis,
j = X, Y , Z, k = 1, · · · , n, with n being the number of absorbers, and Xk , Yk , Zk are
the installing position of the kth absorber in X, Y -and Z-axes, respectively. Under the
assumption that the three RLGs are installed on ISA structure with the ith gyroscope
in j-axis, i = 1,2, 3, where IG1 = IGx , IG2 = IGy , IG3 = IGz , ϕ1 = ϕx , ϕ2 = ϕy ,
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 95

ϕ3 = ϕz . According to Eq. (4.46), the spring-damping-mass rotational kinetics model

of IMU can be rewritten as
n n
Ix θ̈x − Ixy θ̈y − Ixz θ̈z − ˙ y Zk +
cky ˙ z Yk
ckz
k=1 k=1
n
  n n
+ ckz Yk2 + cky Zk2 θ̇x − ckz Yk θ̇y Xk − cky Zk θ̇z Xk
k=1 k=1 k=1
n
  n n
+ kkz Yk2 + kky Zk2 θx − kkz Yk θy Xk − kky Zk θz Xk
k=1 k=1 k=1
n n
− kky y Zk + ˙ x + M0x = 0
kkz z Yk + IGx ϕ̈x + Ix
(4.47)
k=1 k=1

n n
Iy θ̈y − Ixy θ̈x − Iyz θ̈z + ˙ x Zk −
ckx ˙ z Xk
ckz
k=1 k=1
n
  n n
+ ckz Xk2 + cky Zk2 θ̇y − ckz Xk θ̇x Yk − ckx Zk θ̇z Yk
k=1 k=1 k=1
n
  n n
+ kkz Xk2 + kky Zk2 θy − kkz Xk θx Yk − kkx Zk θz Yk
k=1 k=1 k=1
n n
+ kkx x Zk − ˙ + M0y = 0
kkz z Xk + IGy ϕ̈y + Iy
(4.48)
k=1 k=1

n n
Iz θ̈z − Ixz θ̈x − Iyz θ̈y − ˙ x Yk +
ckx ˙ y Xk
cky
k=1 k=1
n
  n n
+ cky Xk2 + ckx Yk2 θ̇z − cky Xk θ̇x Zk − ckx Yk θ̇y Zk
k=1 k=1 k=1
n
  n n
+ kky Xk2 + kkx Yk2 θz − kky Xk θx Zk − kkx Yk θy Zk
k=1 k=1 k=1
n n
− kkx x Yk + ˙ z + M0z = 0
kky y Xk + IGz ϕ̈z + Iz
(4.49)
k=1 k=1

where Ix , Iy , Iz , Ixy , Iyz , Izx are the principal and cross-coupled moments of inertia of
ISA along X-, Y -, Z-axes. From Eq. (4.44) to (4.49), we can know that any free degree
of ISA is coupled with each other. Regarding centroid as origin point, the reference
frame axes are directed along the principal axes of inertia ellipsoid for RLG ISA. This
96 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

is an ellipsoid of revolution, as the ISA has a dynamic symmetry axis. The moment
of inertia of the ISA is uncoupled along any axes, Ixy = Iyz = Ixz = 0. Since these
same type absorbers are symmetrically installed on ISA structure along any axes in
couples, there exist those equations, kkj = kj , ckj = cj , Xk = X, Yk = Y , Zk = Z.
According
!n to symmetrical
!n integral!nprinciple, there !nexist two equations, !n which
are
!n k=1 k kx Y k = !n k=1 k kx Z k = !k=1 k ky X k = !n
k=1 k ky Z k = k=1 k kz Xk =
n
k=1 k kz Y k = 0, k=1 k kx Y k Z k = k=1 k ky Z k X k = k=1 k kz X k Y k = 0. Hence,
the spring force and torque of absorbers along every axis can be uncoupled. For the
same absorbers, the parameter kkj /c ! kj is constant. Therefore,
!n the integral
!n damping
n
force
!n and torque !n are zero, that!n is k=1 c kx Y k =
!n k=1 c kx Z i =!n k=1 cky Xk =
!k=1 c ky Z k = k=1 c k X k = k=1 c kz Y k = 0, k=1 c kx Y k Z k = k=1 cky Zk Xk =
n
k=1 c kz X k Y k = 0. As the centroid and the original point of the reference frame are
superposition, the centroid of ISA with inertial sensors can be expressed as
! 
3
i=1 (m Gi R Gi + m Ai R Ai ) + m S R I
Pc0 = !  = 0. (4.50)
3
i=1 (mGi + mAi ) + mS

Combining Eqs. (4.40) and (4.50), the disturbance torque caused by external linear
accelerations can be given by
1 3
2
M0 = m S RS + (mGi RGi + mAi RAi ) (g + a)
i=1
1 3
2
= Pco (mGi + mAi ) + mS (g + a) = 0. (4.51)
i=1

With mechanical dither of three RLGs and external inputting angular, linear vibration,
the integrated kinetics models of mechanically dithered RLG IMU in six free degrees
can be simply modeled by
¨ x + Cx
m̄ ˙ x + Kx x = −m̄(ax + gx )
¨ y + Cy
m̄ ˙ y + Ky y = −m̄(ay + gy ) (4.52)
¨ z + Cz
m̄ ˙ z + Kz z = −m̄(az + gz )

˙x
Ix θ̈x + Dx θ̇x + Tx θx = −IGx ϕ̈x − Ix

˙y
Iy θ̈y + Dy θ̇y + Ty θy = −IGy ϕ̈y − Iy
(4.53)
˙z
Iz θ̈z + Dz θ̇z + Tz θz = −IGz ϕ̈z − Iz

where Cj , Dj are the linear damping coefficient and stiffness of IMU in j-axis,
Cj = ncj , Kj = nkj , and Dj , T j are the rotational damping coefficient and rotational
stiffness of IMU, Dx = n(cz Y 2 + cy Z 2 ), Tx = n(kz Y 2 + ky Z 2 ), Dy = n(cz X 2 +
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 97

cx Z 2 ), Ty = n(kz X 2 + kx Z 2 ), Dz = n(cy X 2 + cx Y 2 ), Tz = n(ky X 2 + kx Y 2 ). The

solutions of the integrated kinetics models of RLG IMU can be defined as
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
x 0x ax
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ y ⎥ = ⎢ 0y ⎥ + ⎢ ay ⎥ (4.54)
⎣ ⎦ ⎣ ⎦ ⎣ ⎦
z 0z az
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
θx θ0x θbx
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢θy ⎥ = ⎢θ0y ⎥ + ⎢θby ⎥ (4.55)
⎣ ⎦ ⎣ ⎦ ⎣ ⎦
θz θ0z θbz
 T  T
where 0x 0y 0z , θ0x θ0y θ0z represent the linear and angular dis-
placement solutions of kinetics models for static environment where there exist only
 T  T
mechanical dither of RLG, and ax ay az , θbx θby θbz represent
the linear and angular displacement solutions of kinetics models for the case where
there exists only inputting external linear and angular vibration. In static environ-
˙ = 0,
ment, there is no external inputting linear and angular acceleration, that
=

a = 0. Since the six free degrees uncoupling of IMU, the mechanical dither of RLGs
cannot cause to linear vibration. The steady-state response solution of the linear
kinetics model Eq. (4.52) can be given by
⎡ ⎤ ⎡ ⎤
0x m̄gx /Kx
⎢ ⎥ ⎢ ⎥
⎢ 0y ⎥ = − ⎢m̄gy /Ky ⎥. (4.56)
⎣ ⎦ ⎣ ⎦
0z m̄gz /Kz

With the mechanical dither of three RLGs, the rotational kinetics model Eq (4.50)
can be solved as
⎡ ⎤ ⎡ θ̂ cos ω t + φ ⎤
θ0x x px px
⎢ ⎥ ⎢  ⎥
⎣ θ0y ⎦ = ⎢⎣ θ̂y cos ωpy t + φpy ⎦,
⎥ (4.57)
 
θ0z θ̂z cos ωpz t + φpz
 
2 2 2 2
where θ̂j = −IGjˆϕ j ωpj
2 2
/Ij (ω0j − ωpj ) + 4μ2j ωpj
2
= −IGjˆϕ j ωpj
2
/ (Tj − Ij ωpj ) + Dj2 ωpj
2
,
  
φpj = arctg 2μj ωpj / ω0j
2
− ωpj
2 2
, ω0j = Tj /Ij , 2μj = Dj /Ij . According to
the requirement of high-precision POS, the rotation angular with respect to exterior
supporting structure must be less than orientation error requirement δ. The moment
of inertial of ISA must be greater than that of single RLG. The gyroscope dithered
part can operate normally, and the measurement error can be reduced. Under the
assumption that the rotational damping coefficient and stiffness of IMU are zero, the
98 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

amplitude of the disturbance vibration of ISA caused by mechanical dither of RLG

can be expressed by
3 3 I ϕ̂
3 3 Gj j
3θ̂j 3 = ≤ δ. (4.58)
Ij
According to Eq. (4.58), the design requirement of moment of inertia for ISA is
given by
IGj ϕ̂j
Ij ≥ . (4.59)
δ
Generally, the rotational damping coefficient and stiffness of IMU is not zero. The
disturbance torque transmitted to imaging payloads caused by mechanical dither of
RLG can be given as
3 3 3 3 3 3
3MTj 3 = 3Tj θj + Dj θ̇j 3 = 3−IGi ϕ̈j − Ij θ̈j 3 = IGj ϕ̂j ω2
pj
⎛ ⎞
2
⎜ ωpj ⎟
⎝  − 1⎠. (4.60)
2 2
(ω0j − ωpj ) + 4μj ωpj
2 2 2

In order to further reduce disturbance to imaging payloads, the torque should be

reduced, furthest. The first-order differential equation of the disturbance torque
function with respect to inherent resonance frequency can be given by
 
3 3
3
d MTj 3 −2I Gj ϕ̂ j ω 4
pj ω 0j ω 2
0j − ω 2
pj
= 3 2 . (4.61)
dω0j  2
ω0j − ωpj + 4μj ωpj
2 2 2 2

3 3
Assuming that the first-order differential equation equal to zero, d 3MTj 3/dω0j = 0,
where the solutions of function existing minimum or maximum are that ω0j = 0,
ω0j = ωpj . When ωpj > ω0j > 0, then dMTj /dω0j ≥ 0, which represents the torque
monotonously increase with the inherent resonance frequency. Moreover, if ω0j >
ωpj , then dMTj /dω0j ≤ 0, which represents the torque monotonously decrease with
the inherent resonance frequency with zero in infinite frequency. Therefore, the less
the resonance frequency of IMU with absorbers is, the less the torsion disturbance
torque is, that ω0j = Kj /Ij → 0. Under the assumption that the rotational stiffness
of IMU is not zero, the moment of inertia for ISA required is enough. The rotational
angular with respect to exterior supporting structure is little, which minimize the
disturbance torque MKj . In another situation, if the stiffness and the moment of
inertia for ISA are constant, the disturbance caused by the mechanical dither of RLG
can be reduced by enhancing the rotational damping coefficient of the vibration
damping system.
On the other hand, the POS requires to measure external high-frequency vibration,
and the vibration must be transmitted directly to RLG ISA. The linear displacement
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 99

of ISA structure with respect to the body frame caused by external acceleration
should be less and can be given by
⎡ ⎤ ⎡ ⎤
ax ˆ x cos (ωax t + ψx )
⎢ ⎥ ⎢ ⎥
⎢ ay ⎥ = ⎢ ˆ ⎥
⎣ ⎦ ⎣ y cos (ωay t + ψy )⎦ (4.62)
az ˆ z cos (ωaz t + ψz )
   
ˆ j = −âj / (ω0aj
where 2 2 2
− ωaj ) + 4ξ 2 ωaj
2
, ψj = arctan 2ξj ωaj / ω0aj
2
− ωaj
2
,
with ω0aj being the linear inherent resonance circular frequency, ω0aj 2
= Kj /m̄,
2ξj = Cj /m̄. Since the linear displacement is small, j ≤ 2mm, and satisfies the
position precise requirement of POS. It is crucial to reduce the angular displacement
caused by the external angular vibration. With external angular vibration , the
steady-state response solution of rotational kinetics model for RLG IMU can be
solved as
⎡ ⎤ ⎡ θ̂ cos (ω t + φ )⎤
θbx bx bx bx
⎢ ⎥ ⎢  ⎥
⎣ θby ⎦ = ⎢
⎣ θ̂ by cos ωby t + φ by
⎥,
⎦ (4.63)
θbz θ̂bz cos (ωbz t + φbz )

where θ̂bj = −
ˆ j ωbj / (ω0j
2 2 2
− ωbj ) + 4μ2j ωbj2
, the phase φbj of angular response
  
with respect to , φbj = arctg 2μj ωbj / ω0j 2
− ωbj2
+π/2. The less the rotational
angular θj is, the more close to the actual external angular vibration the measurement
is. The first-order differential equation of the angular amplitude function with respect
to inherent resonance frequency can be given by
 
3 3 ˆ j ωbj
d 3θbj 3 −2
2 2
ω0j − ωbj
2

= 3 2 . (4.64)
dω0j  2
ω0j − ωbj + 4μj ωbj
2 2 2 2

3 3
Assuming that the first-order differential equation equal to zero, d 3θTj 3/dω0j = 0,
3 3 and maximum are that ω0j = 0,
where the solutions of function existing minimum
ω0j = ωbj . When ωbj > ω0j > 0, then d 3θj 3/dω0j ≥ 0, which represents the
amplitude of angular vibration monotonously 3 increases
3 with the inherent resonance
frequency. Moreover, if ω0j > ωbj , then d 3θj 3/dω0j ≤ 0, which represents the
amplitude of angular vibration monotonously decreases with the inherent resonance
frequency, and being close to zero in infinite frequency. The frequency of external
angular vibration should be less than the maximum frequency ωh of vibration mea-
sured by RLG POS. Therefore, the rotational inherent resonance frequency of RLG
IMU should be higher than the maximum frequency of vibration measured by RLG
POS. The larger the resonance frequency ω0j and damping ratio μj of IMU are, the
100 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

less the angular displacement θj of RLG ISA with respect to the body frame is. The
capability of measuring high-frequency vibration is better.
On the other hand, the first-order differential equation of the angular amplitude
function with circular frequency of measured vibration can be given by
3 3  
3 3 ˆ j ω0j
d 3θ̂bj 3
4
− ωbj
4

= 3 2 . (4.65)
dωbj  2
ω0j − ωbj + 4μj ωbj
2 2 2 2

From Eq. (4.65), we can know that the angular amplitude increases with the circular
frequency of external vibration and the minimum angular amplitude present in the
case where the circular frequency is zero, θbj = 0. When the circular frequency
of external vibration is equated to the inherent resonance frequency, there exist a
maximum value, θ̂j =
ˆ j /2μj . Under the assumption that the inherent resonance
frequency of IMU is higher than the frequency of external vibration, and the highest
frequency vibration measured by RLG POS and resonance frequency are defined as

hj =
ˆ hj cos (ωhj t), ω0j = qωhj . According to Eq. (4.63), the angular amplitude
can be expressed as
3 3 ˆ hj ωhj ˆ hj
3 3

3θ̂bj 3 =  = ≤ δ. (4.66)
2
2
(q 2 ωhj − ωhj
2
) + 4μ2j ωhj
2 (q 2 − 1)2 ωhj
2
+ 4μ2j

The inherent resonance frequency is designed to super to the maximal frequency of

external vibration. According to the parameters of the highest frequency vibration,
orientation error of RLG POS and damping ratio of system, the ratio coefficient of the
inherent resonance frequency for IMU to highest frequency for measured vibration,
can be given by
⎛  2 1/2 ⎞1/2
⎜ ˆ hj /δ

− 4μ2j ⎟
⎜ ⎟
qj ≥ ⎜ + 1⎟ . (4.67)
⎝ ωhj ⎠

Taking account of the mechanical dither of RLG and the external high-frequency
vibration, the optimal resonance frequency should be determined as
⎡ 1 21/2 ⎤1/2

2hj
ω0j = qωhj = ⎣ωhj − 4μ2j 2 ⎦
+ ωhj . (4.68)
δ2

With the external vibration frequency reducing, the maximal angular amplitude mea-
sured vibration is increased and can be calculated by Eq. (4.63). In addition, from
Eqs. (4.60) and (4.63), we can know that the greater the damping ratio μj is, the
less rotational angular error θ and the disturbance torque MT are, and contribute to
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 101

Z
Fig. 4.9 The size effect
mechanism of RLG IMU

θ& a f rzb
f ryb
&&
θa f rxb

O Y
X

accurately measuring the external vibration. However, the time-delay of RLG IMU
caused by vibration-damping system would become greater and should be considered
to compensate in algorithm.
As shown in Fig. 4.9, the accelerometers assembly is installed on ISA structure
along reference frame. The three accelerometers and the sensing centers are denoted
by cylinder and red marks, respectively. The position vector of sensing centers in
reference frame can be expressed as
⎡ ⎤ ⎡ ⎤
RA1 rxx rxy rxz
⎢ ⎥ ⎢ ⎥
⎢RA2 ⎥ = ⎢ryx ryy ryz ⎥. (4.69)
⎣ ⎦ ⎣ ⎦
RA3 rzx rzy rzz

Taking account of the external vibration and high-frequency mechanical dither of

RLG, the measurement error of three accelerometers sensing center in reference
frame can be modeled by
⎡ ⎤ ⎡ θ̇ 2 + θ̇ 2  r − θ̈ r + θ̈ r ⎤
arxx x y xx z xy y xz
⎢ ⎥ ⎢  2  ⎥
arx = ⎣arxy ⎦ = ⎣ θ̈z rxx − θ̇x + θ̇z rxy − θ̈x rxz ⎥
⎢ 2
⎦ (4.70)
 2 
arxz −θ̈y rxx + θ̈x rxy − θ̇x + θ̇y rxz
2

⎡ ⎤ ⎡ θ̇ 2 + θ̇ 2  r − θ̈ r + θ̈ r ⎤
aryx y z yx z yy y yz
⎢ ⎥ ⎢  2  ⎥
ary = ⎣aryy ⎦ = ⎢⎣ θ̈z ryx − θ̇x + θ̇z ryy − θ̈x ryz ⎦
2 ⎥ (4.71)
 2 
aryz −θ̈y ryx + θ̈x ryy − θ̇x + θ̇y ryz
2

⎡ ⎤ ⎡ θ̇ 2 + θ̇ 2  r − θ̈ r + θ̈ r ⎤
arzx y z zx z zy y zz
⎢ ⎥ ⎢  2  ⎥
arz = ⎣arzy ⎦ = ⎢⎣ θ̈z rzx − θ̇x + θ̇z rzy − θ̈x rzz ⎦.
2 ⎥ (4.72)
 2 
arzz −θ̈y rzx + θ̈x rzy − θ̇x + θ̇y rzz
2
102 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

Taking account of the installing misalignment αj , βj , γj between the jth accelerom-

eter sensing axes and x-, y-, z-axes of reference frame, the acceleration measurement
error caused by the size effect in three axes can be modeled by
⎡ b⎤ ⎡ ⎤
frx arxx cos (αx ) + arxy cos (βx ) + arxz cos (γx )
⎢ b⎥ ⎢      ⎥
frb = ⎢ ⎥ ⎢
⎣ fry ⎦ = ⎣ aryx cos αy + aryy cos βy + aryz cos γy ⎦.
⎥ (4.73)
frzb arzx cos (αz ) + arzy cos (βz ) + arzz cos (γz )

From above equation, we know that the size effect acceleration error frb is controlled
by installing position, orientation, and the motion state of body. The airborne imag-
ing payloads used in high-resolution earth observation system require high-precision
IMU. The installing position, orientation of three accelerometers, and the vibration
damping system must to be optimized and reduce the measurement error caused
by the mechanical dither of RLG and external high-frequency vibration. The main
methods can be implemented including reducing the body angular rate and accel-
eration, improving the orthogonally, eliminating the installing error, and reduce the
size effect acceleration error in hardware.
RLG is mechanically dithered by the method to overcome or reduce the lock-
in effects which occur at low rotational rates. The dither causes the signal noise,
nonlinear error, disturbance, and a coupling measurement error. The structure opti-
mization and the vibration damping system design are the key technologies for the
mechanically dithered RLG IMU which determines the performance of POS. The
design of vibration isolators for IMU in high-vibration environments can be critical
to block vibration frequencies above the sensor sampling frequency and to avoid
certain detrimental vibration modes including sculling and coning motion.
In order to reduce the coupling of three RLGs with each other and the mechanical
dither transmission, improve the performance and reliability, the inherent resonance
frequency of ISA structure is required to be higher twice more than the mechanical
dithered frequency of RLG. The design fundamentals of RLG ISA include small light
structure, six free-degree uncoupling, vibration isolators system, and size effect.
Based on the optimized design methods of RLG ISA, the installing positions,
orientations of three accelerometers have be optimized shown in Fig. 4.10. The
position vectors of sensing centers in reference frame are reduced to be 21, 37, and
38 mm, respectively, and the orthogonality of three accelerometers sensing axes is
improved to be less than 5". The size effect acceleration error decreases in hardware.
The gyroscope dithered part can be operated normally, which reduces the size effect
and conic errors. The ISA with inertial sensors is installed symmetrically on exterior
supporting structure by eight same absorbers along any axes in couples, and improves
the uncoupling effect. The inherent resonance frequencies of IMU with absorbers
are about 200 Hz with differences less 10 % in three rotational axes, and avoid to
resonance each other. The ISA not only eliminates the disturbance torque caused by
mechanical dither of RLG, but also accurately measures the external high-frequency
vibrations.
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 103

accelerometer sturcture

5/* RLG ISA absorbers

Fig. 4.10 The components of RLG ISA, which can not only eliminate mechanical dither effect, but
also accurately measure external high-frequency vibration

Fig. 4.11 The hardware

components of RLG POS

The airborne high-precision RLG POS applied in high-resolution earth obser-

vation system is developed and shown in Fig. 4.11. The POS uses an IMU that is
self-contained and separated from the PCS, connected to the PCS by a data interface
and power cable. The IMU is designed to be small (208 mm × 204 mm × 140 mm),
light (6.7 Kg) and less power consumption (25 W). The overall power consumption
of RLG POS including IMU and PCS is 30 W and commensurate in cost and per-
formance with the POS/AV610. The RLG IMU can measure the motion of sensing
centre of payloads by coordinate conversion and compensating the level errors. A
dual-frequency L1/L2 carrier phase differential GPS embedded receiver (Novotel
GPS) provides very good long-term accuracy but in the short-term suffers from out-
age, multipath, and noise. The inertial measurement system has excellent short-term
characteristics but suffers from long-term drift. By combining the two navigation
means, levels of performance can be achieved or exceed those of more expensive
inertial measurement system. The integrated system provides excellent short-term
dynamics greatly, and has none of the long-term drift problem associated with inertial
measurement system.
104 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

4.3.2 The Optimization Six-Position Hybrid Calibration for SINS

The core sensors of SINS are the gyroscopes and accelerometers, the gyroscopes are
sensitive to the rotation rate, and the accelerometers are sensitive to the acceleration.
Therefore, the precision of SINS depends on the precision of the gyroscopes and
accelerometers. The traditional calibration methods for SINS include static multi-
position [15], rotation rate [16], and dynamic-static hybrid calibration methods, and
so on. Among all the methods, the static multiposition calibration method can accu-
rately calibrate the error coefficients of acceleration channel, while the accuracy of
the error coefficients in angular rate channel is very low. On the other hand, the tradi-
tional rotation rate calibration can improve the precision of the angular rate channel
error coefficients, but reduces the precision of acceleration channel. Li et al. [17]
proposed a 24-point dynamic-static hybrid calibration method. The method improves
the precision of the calibrated coefficients in both of the two channels, but increases
the complexity of the calibration. So, it is another key technology in SINS to find a
better method to reduce the complexity and error of the calibration at the same time.
What is more, the change of the working environmental temperature also decreases
the precision of the SINS significantly [18]. The linear fitting method is often applied
to compensate for the temperature errors of inertial devices biases and scale factors.
The method is easy to be implemented, while the precision of compensation is low.
Other nonlinear methods, such as the high-order least-squares approximation method
and the RBF (radial basis function) neural network method, improve the precision of
the SINS, but they cannot meet the requirement of real-time processing because of the
large amount of calculation. Therefore, it is another key problem in SINS to resolve
the abovementioned conflicts. Based on the theoretical analysis and experimental
data, the calibration and compensation method of the temperature and dynamic error
for SINS are studied in this section, and an optimization calibration method with four-
direction rotations and nonlinear interpolation compensation method are proposed
for SINS in full temperature ranges [19].
1. Error Modeling of RLG SINS

1) Error modeling of the angular rate channel

Dithered RLG is not sensitive to the gravity acceleration. Considering the scale
factors, biases, and misalignment errors, the error model of angular rate channel is
written as:
⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎡ ⎤
Gx Gbx Sgx Exy Exz ωx
⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢Gy ⎥ = ⎢Gby ⎥ + ⎢Eyx Sgy Eyz ⎥ ⎢ωy ⎥, (4.74)
⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎣ ⎦
Gz Gbz Ezx Ezy Sgz ωz

where Gi and Gbi are gyroscope output and bias in i-axis, ωi is inputting rational rate,
Sgi is gyroscope scale factor, Eij is the misalignment error of angular rate channel.
i = x, y, z; j = x, y, z.
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 105

2) Error modeling of the acceleration channel

According to the characteristic of the quartz mechanical accelerometer (QMA), the
acceleration channel error model of the SINS can be given by:
⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎡ ⎤
Ax Abx Sax Mxy Mxz ax
⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢Ay ⎥ = ⎢Aby ⎥ + ⎢Myx Say Myz ⎥ ⎢ay ⎥, (4.75)
⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎣ ⎦
Az Abz Mzx Mzy Saz az

where Ai is the accelerometer output of RLG SINS in i-axis, ai is the accelerometer

input, Abi is the accelerometer bias, Sai is the acceleration scale factor, and Mij is the
misalignment error of the acceleration channel. i = x, y, z; j = x, y, z.
2. Improved Six-Position Hybrid Calibration for RLG SINS in Full Tempera-
ture
Due to the change of environment temperature and the thermo effect of system,
the self-generating temperature of inertial instrument of RLG SINS is changed, and
their scale and bias are altered in practical engineering applications. The precision
of initial alignment and navigation is affected ultimately. Therefore, we should set
the range of calibration temperature and the temperature points according to the
actual working environment and the accuracy required. Based on the error model,
an improved six-position hybrid calibration method is designed for RLG SINS.
1) Calibration of the coefficients in accelerometer channel
The IMU is fixed in a high accuracy hexahedral fixture, placed on the single axis
turntable with temperature control. The hexahedral fixture should be turned over six
times at each temperature point, making the X-, Y-, and Z-axes of IMU coincide with
ZT - and − ZT -axes of the turntable, respectively (ZT -axis points at the zenith in
geographic coordinate system). In each position, the turntable will be pivoted 180◦
to acquire two groups of static data (6-position 12-point). The scheme is shown in
Fig. 4.12.
Let F (i) = [āxi āyi āzi ] (i = 1, 2, 3. . . 12), F(i) is the ith group output of the X-,
Y - and Z-axes accelerometer.
⎡ ⎤T
1 1 1 1 1 1 1 1 1 1 1 1
⎢ ⎥
⎢g g −g −g 0 0 0 0 ⎥
⎢ 0 0 0 0 ⎥
A=⎢ ⎥ (4.76)
⎢0 0 0 0 g g −g −g 0 0 0 0 ⎥
⎣ ⎦
0 0 0 0 0 0 0 0 g g −g −g
⎡ ⎤
∇x ∇y ∇z
⎢ ⎥
⎢K Mzx ⎥
⎢ ax Myx ⎥
K=⎢ ⎥. (4.77)
⎢Mxy Kay Mzy ⎥
⎣ ⎦
Mxz Myz Kaz
106 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

; ;ಬ < <ಬ = =ಬ
=7 <ಬ =7 =ಬ =7 ;ಬ
=ಬ ;ಬ <ಬ
= ; <
< = ;
Position 1 Position 2 Position 3

=7 =ಬ =7 ;ಬ =7 <ಬ
<ಬ =ಬ ;ಬ
< = ;
= ; ;ಬ ; < <ಬ < = =ಬ

Position 4 Position 5 Position 6

Fig. 4.12 The scheme of static calibration

However, the state equation of the accelerometer channel can be modeled as follows:

F = AK (4.78)
 T
where F = F (1) F (2)    F (12) .
Accordingly, we can deduce the calibration coefficients as follows:
⎡ !12 !12 !12 ⎤
i=1 āxi i=1 āyi i=1 āzi
⎢ ⎥
⎢ā + ā − ā − ā āy1 + āy2 − āy3 − āy4 āz1 + āz2 − āz3 − āz4 ⎥
⎢ x1 x2 x3 x4 ⎥
K =⎢ ⎥.
⎢ā x5 + āx6 − āx7 − āx8 āy5 + āy6 − āy7 − āy8 āz5 + āz6 − āz7 − āz8 ⎥
⎣ ⎦
āx9 + āx10 − āx11 − āx12 āy9 + āy10 − āy11 − āy12 āz9 + āz10 − āz11 − āz12
(4.79)

2) Calibration of the coefficients in gyro channel

When X-axis, Y -axis, Z-axis is coincided with ZT -axis separately, make the rotation
rate of the turntable a constant, and rotate the table more than 720 ◦ in both clockwise
direction and counter-clockwise direction as shown in Fig. 4.13. The input angular
rate of the turntable is ω(i), local latitude is Φ, and the local acceleration of gravity
is g. So the input angular rate and acceleration in three sensitive axes of RLG POS
can be written as:
⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤
ωx (t) ωie cos (φ) sin ϕ(t) ax 0
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣ ωy (t)⎦ = ⎣ ωie cos (φ) cos ϕ(t)⎦, ⎣ ay ⎦ = ⎣ 0⎦, (4.80)
ωz (t) ω(i) + ωie sin (φ) az g
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 107

;
± ωi <
± ωi =
± ωi
=7 =7 =7

= ; <

< = ;
Position 1 Position 2 Position 3

Fig. 4.13 The scheme of dynamic calibration

where ωie is the Earth’s rotation rate, ϕ(t) is the angle between the initial orientation
of RLG POS and north direction.
When the turntable rotates two or more turns, the average input angular rate can
be calculated as follows:
⎡ ⎤ ⎡ ⎤
ωx 0
⎢ ⎥ ⎢ ⎥
⎢ ωy ⎥ = ⎢ 0 ⎥. (4.81)
⎣ ⎦ ⎣ ⎦
ωz ω(i) +
sin (φ)

Therefore, the state equation of the gyro channel can be modeled as:
⎡ ⎡⎤ ⎤
ω̄x1 ω̄y1 ω̄z1 1 ωi+ 0 0
⎢ ⎥ ⎢ ⎥⎡ ⎤
⎢ω̄x2 ω̄z2 ⎥ ⎢ 0 ⎥
⎢ ω̄y2 ⎥ ⎢1 ωi− 0 ⎥ ⎢ εx εy εz
⎥
⎢ ⎥ ⎢ ⎥
⎢ω̄x3 ω̄z3 ⎥ ⎢1 0 ⎥⎢ Ezx ⎥
⎥⎢ ⎥
ω̄y3 0 ωi+ Kx Eyx
⎢ ⎥=⎢
⎢ ⎥ ⎢ ⎥⎢⎢
⎥, (4.82)
⎢ω̄x4 ω̄y4 ω̄z4 ⎥ ⎢1 0 ωi− 0 ⎥ ⎣Exy Ky Ezy ⎥
⎦
⎢ ⎥ ⎢ ⎥
⎢ω̄ ⎥ ⎢ ωi+ ⎥
⎣ x5 ω̄y5 ω̄z5 ⎦ ⎣1 0 0 ⎦ Exz Eyz Kz
ω̄x6 ω̄y6 ω̄z6 1 0 0 ωi−

where ωi+ = ω(i) +

sin (φ), ωi− = −ω(i) +
sin (φ).
According to Eq. (4.82), we can solve the calibration coefficients of the gyro
channel as follows:
⎡ ⎤ ⎡! ⎤
6
εx i=1 ω̄xi
⎢ ⎥ 1 ⎢ !6 ⎥
⎢εy ⎥ = ⎢ ⎥ (4.83)
⎣ ⎦ 6 ⎣ i=1 ω̄yi ⎦
!6
εz i=1 ω̄yi
⎡ ⎤ ⎡ ⎤
Kx Exy Exz ω̄x1 − ω̄x2 ω̄x3 − ω̄x4 ω̄x5 − ω̄x6
⎢ ⎥ 1 ⎢ ⎥
⎢Eyx Eyz ⎥ ·⎢ ω̄y5 − ω̄y6 ⎥
⎣ Ky ⎦= ω̄ − ω̄y2
2 · ω(i) ⎣ y1
ω̄y3 − ω̄y4 ⎦. (4.84)
Ezx Ezy Kz ω̄z1 − ω̄z2 ω̄z3 − ω̄z4 ω̄z5 − ω̄z6
108 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

4.3.3 Integrated Calibration Method for RLG IMU Using a

Hybrid Analytic/Kalman Filter Approach

The motive of error calibration is to obtain the coefficients of mathematical error

model [20, 21]. Generally, the rotation rate of the Earth, inputting rotation rate of
turntable, and gravity acceleration are regarded as references, which are compared
with the output of gyroscopes and accelerometers [22]. The coefficients can be
resolved or estimated using analytic and Kalman filter methods. The typical analytic
calibration methods involve static multiposition calibration, rotation rate calibration,
and dynamic-static hybrid calibration methods. The static multiposition calibration
can calculate the error coefficients of IMU, but the precision is low [23]. The rotation
rate calibration can give the error coefficients of angular rate channel, but not give that
of acceleration channel. The dynamic-static hybrid calibration method inherits the
advantages of above two methods, but it is complicated. In addition, none of the above
analytic calibration methods can solve the rigorous random error problems caused
by anti-vibration system disturbance and gyroscope drift. On the other hand, the
Kalman filter method has been widely used in initial alignment and error calibration
for INS, Wang and Guo [24] presented an intelligentized and fast method based on
Kalman filter to improve the alignment precision of INS, Tadej et al. [25] proposed
an online automatic calibration method for a three-axial accelerometer, but the state
vector has only 13 dimensions.
To solve the above calibration problems, a novel integrated calibration method
using a hybrid analytic/Kalman filter approach was proposed in this section. A series
of experiments are implemented to validate the proposed calibration method. The
main contributions of this section are as follows:
• An optimization analytic calibration method with only four-direction rotations
is proposed to coarsely calculate all coefficients of RLG IMU error model. The
method improves the efficiency of the typical analytic calibration methods.
• An extended Kalman filter calibration method is proposed to finely estimate
and further amend the error coefficients calculated by analytic calibration. The
Kalman filter regards the velocity errors as observation using real-time lever arm
compensation techniques.
Using outputs of gyroscopes and accelerometers, the coefficients of mathematical
error model can be calculated analytically by comparing with the rotation rates of
the Earth, inputting rotation rate of turntable, and gravity acceleration. The precision
and efficiency are two key targets of RLG IMU calibration.
1. The Analytic Calibration of the RLG IMU
According to the above mathematical error model of RLG IMU, 12 or more error
equations are required to calculate all the 12 error coefficients of angular rate channel.
An optimization analytic calibration method is proposed to improve the calibration
precision and efficiency of RLG IMU using a three turntable without northward
reference as shown in Fig. 4.14. Op is the rotation center of the turntable coordinate
system (OpXpYpZp). During the calibration course, the RLG IMU will be overturn
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 109

Fig. 4.14 An optimum

analytic calibration method
with four-direction rotation
using a three turntable

three times, thus achieve x-, y- and z- axes of RLG IMU directing up in geographic
coordinate system, separately. The turntable is manipulated to rotate with a constant
rotation rate in clockwise and counter-clockwise directions. The rotation angular
should be 2πn where n is the cycle number in every rotation rate.
When x-axis directing up as the first direction, the inputting angular rates in three
sensing axis can be given by:
⎡ ⎤ ⎡ ⎤
ωx (t) ωie cos φ sin ϕ(t)
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ωy (t) ⎥ = ⎢ ωie cos φ cos ϕ(t)⎥ (4.85)
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
ωz (t)
+ ωie sin φ

where ωie is rotation rate of earth, φ is local latitude,

is inputting rotation rate
of turntable, and ϕ(t) is head of RLG IMU in time t. The inputting accelerations in
three sensing axis can be given by:
⎡ ⎤ ⎡ ⎤
ax (t) 0
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ay (t) ⎥ = ⎢ 0 ⎥, (4.86)
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
az (t) g

where g is value of the gravity acceleration. Taking into account time segment
0 ∼ 2π n/
, the mean value of inputting angular rates in three sensing axis can be
110 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

obtained as:
⎡ ⎤ ⎡4 ⎤ ⎡ ⎤ ⎡ ⎤
2π n/

ω̄x ω x (t) dt 0 0
⎢ ⎥ ⎢ 0 ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢

⎢ 4 2π n/
⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ω̄y ⎥ = ⎢ 0 ωy (t) dt ⎥ = ⎢ 0 ⎥ = ⎢ 0 ⎥. (4.87)
⎢ ⎥ 2π n ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣4 ⎦ ⎣ ⎦ ⎣ ⎦
2π n/

ω̄z 0 ω z (t) dt
+ ωie sin φ ω̄
The mean value of inputting accelerations in three sensing axes can be given by:
⎡ ⎤ ⎡4 ⎤ ⎡ ⎤
2π n/

āx ax (t) dt 0
⎢ ⎥ ⎢ 0 ⎥ ⎢ ⎥
⎢ ⎥ ⎢

⎢ 4 2π n/
⎥ ⎢ ⎥
⎢ ⎥ ⎥ ⎢ ⎥
⎢ āy ⎥ = ⎢ 0 ay (t) dt ⎥ = ⎢ 0 ⎥. (4.88)
⎢ ⎥ 2πn ⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣4 ⎦ ⎣ ⎦
2π n/

āz 0 az (t) dt g
The system status equation of angular rate channel of RLG IMU during optimization
analytic calibration can be given by:
⎡ ⎤ ⎡ ⎤⎡ ⎤
Gx1 Gy1 Gz1 1 ω̄ 0 0 Gbx Gby Gbz
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ Gx2 Gy2 Gz2 ⎥ ⎢ 1 −ω̄ 0 0 ⎥ ⎢ Sx Exy Exz ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥=⎢ ⎥⎢ ⎥, (4.89)
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ Gx3 Gy3 Gz3 ⎥ ⎢ 1 0 ω̄ 0 ⎥ ⎢ Eyx Sy Eyz ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎣ ⎦ ⎣ ⎦⎣ ⎦
Gx4 Gy4 Gz4 1 0 0 ω̄ Ezx Ezy Sz
where Gim is output of the i-axis gyroscope for the mth direction, i = x, y, z; m = 1,
2, 3, 4. The system status equation of acceleration channel of RLG IMU can be
expressed as:
⎡ ⎤ ⎡ ⎤⎡ ⎤
Ax1 Ay1 Az1 1 g 0 0 Abx Aby Abz
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ Ax2 Ay2 Az2 ⎥ ⎢ 1 −g 0 0 ⎥ ⎢ Kx Myx Mzx ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥=⎢ ⎥⎢ ⎥, (4.90)
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ Ax3 Ay3 Az3 ⎥ ⎢ 1 0 g 0 ⎥ ⎢ Mxy Ky Mzy ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎣ ⎦ ⎣ ⎦⎣ ⎦
Ax4 Ay4 Az4 1 0 0 g Mxz Myz Kz
where Aim is the output of the i-axis accelerometer for the mth direction. According
to Eq. (4.89), the error coefficients of angular rate channel of RLG IMU can be
calculated by:
⎡ ⎤ ⎡ ) ⎤
Gbx Sx Gx2 + Gx1 (Gx1 − Gx2 ) ω̄
⎢ ⎥ ⎢ ⎥
⎢ ⎥ 1⎢  ) ⎥
⎢ ⎥ ⎢ ⎥
⎢ Gby Eyx ⎥ = ⎢ Gy2 + Gy1 Gy1 − Gy2 ω̄ ⎥ (4.91)
⎢ ⎥ 2⎢ ⎥
⎣ ⎦ ⎣ ) ⎦
Gbz Ezx Gz2 + Gz1 (Gz1 − Gz2 ) ω̄
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 111
⎡ ⎤ ⎡ ⎤
Exy Exz Gx3 − Gbx Gx4 − Gbx
⎢ ⎥ 1 ⎢ ⎥
⎢Sy Eyz⎥ ⎢ Gy4 − Gby⎥
⎣ ⎦ = ω̄ ⎣Gy3 − Gby ⎦. (4.92)
Ezy Sz Gz3 − Gbz Gz4 − Gbz

According to Eq. (4.90), the error coefficients of acceleration channel of RLG IMU
can be calculated as:
⎡ ⎤ ⎡   ⎤
Abx Aby Abz g (Ax1 + Ax2 ) g Ay1 + Ay2 g (Az1 + Az2 )
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ Kx Myx Mzx ⎥ ⎢ A − A A − A A − A ⎥
⎢ ⎥ 1 ⎢ x1 x2 y1 y2 z1 z2
⎥
⎢ ⎥= ⎢   ⎥.
⎢ ⎥ 2g ⎢ ⎥
⎢ Mxy Ky Mzy ⎥ ⎢ 2 (Ax3 − Abx ) 2 Ay3 − Aby 2 (Az3 − Abz ) ⎥
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
 
Mxz Myz Kz 2 (Ax4 − Abx ) 2 Ay4 − Aby 2 (Az4 − Abz )
(4.93)

2. Extended Kalman Filter Calibration with Lever Arm Compensation

To make sure that the dithered RLG works normally, eight antivibrators are assembled
between the ISA and supporting structure which can prevent the RLG mechanical
dithers is transferred to payloads. Due to the impact of the antivibrators, the instal-
lation errors of RLG IMU will change when IMU moves or rotates. In addition, the
gyroscope bias will drift during calibration course [26]. However, the above analytic
method cannot eliminate the installation errors disturbance and gyroscope drifts. It
is necessary to find a Kalman filter calibration method to finely estimate and amend
the error coefficients calibrated by analytic calibration.
1) The random error modeling of RLG POS
The integrated random error of angular rate channel for RLG IMU can be expressed
as:

δGx = δSx ωx + δExy ωy + δExz ωz + εgx + wgx

δGy = δEyx ωx + δSy ωy + δEyz ωz + εgy + wgy
δGz = δEzx ωx + δEzy ωy + δSz ωz + εgz + wgz , (4.94)

where δGi is the angular rate random error of RLG IMU in i-axis, i = x, y, z. δEij
is the installation error disturbance of angular rate channel caused by distortion of
anti-vibration system. δSi is the scale factor disturbance of angular rate channel. εgi
is the random walk drift of gyroscope. wgi denotes the driving white noise of the
random walk in i-axis. The integrated random error of acceleration channel for RLG
IMU can be given by:

δAx = δKx ax + δMxy ay + δMxz az + εax + wax

δAy = δMyx ax + δKy ay + δMyz az + εay + way
112 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

δAz = δMzx ax + δMzy ay + δKz az + εaz + waz , (4.95)

where δAi is the acceleration random error of RLG IMU. δMij is the installation error
disturbance of acceleration channel caused by distortion of anti-vibration system. δKi
is the scale factor disturbance of acceleration channel. εai is the random walk bias
of accelerometer. wai denotes the driving white noise of the random walk in i-axis.
2) The state equation modeling
According to the random error models of angular rate and acceleration channels,
an extended Kalman filter calibration method is designed for RLG IMU. Compared
with the conventional movement alignment as in [27], [28], the Kalman filter used
in the proposed method can not only estimate the navigation information but also
calibrate the error coefficients of the RLG IMU error model, including the scale
factors, installation errors disturbances, and random walk errors. The selected state
vector X has total 30 dimensions, and can be given by:

X = [δVE δVN δVU φE φN φU εax εay εaz δKx δKy δKz

δMxy δMxz δMyx δMyz δMzx δMzy εgx εgy εgz (4.96)

δSx δSy δSz δExy δExz δEyx δEyz δEzx δEzy ]T ,

where δVE , δVN , δVU are east, north, and up velocity errors. φE , φN , φU are pitch,
roll, and head angle errors, respectively. The random error of angular rate channel
expressed in navigation frame can be given by:
⎡cos γ cos ψ − sin γ sin θ sin ψ − cos γ sin ψ − sin γ sin θ cos ψ − sin γ cos θ
⎤

δGnib = Cbn δGi =⎣ cos θ sin ψ cos θ cos ψ sin θ ⎦

sin γ cos ψ − cos γ sin θ sin ψ − sin γ sin ψ − cos γ sin θ cos ψ cos γ cos θ
⎡ ⎤
⎡ ⎤ 1 ⎡ ⎤⎡ ⎤
εgx δSx δExy δExz ⎢ ⎥ T11 T12 T13 δGx
⎢ω ⎥
⎢ ε δE δS δE ⎥ ⎢ x ⎥ ⎢ ⎥⎢ ⎥
⎣ gy yx y yz ⎦ ⎢ ⎥ = ⎣ T21 T22 T23 ⎦ ⎣ δGy⎦,
⎢ωy ⎥
εgz δEzx δEzy δSz ⎣ ⎦ T31 T32 T33 δGz
ωz
(4.97)

where Cbn denotes the transformation matrix between the IMU frame and navigation
frame. θ , γ , ψ are the pitch, roll, and head angles. Tij is a element of the transforma-
tion matrix Cbn . i, j = 1, 2, 3. The random error of acceleration channel expressed in
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 113

navigation frame can be expressed as:

⎡ ⎤
⎡ ⎤⎡ ⎤ 1
⎢ ⎥
T T12 T13 ε δKx δMxy δMxz ⎢ ⎥
⎢ 11 ⎥ ⎢ ax ⎥⎢ ⎥
⎢ ⎥⎢ ⎥ ⎢ ax ⎥
⎢ ⎥⎢ ⎥⎢ ⎥
δAnib = Cbn δAbib = ⎢ T21 T22 T23 ⎥ ⎢ εay δMyx δKy δMyz ⎥ ⎢ ⎥
⎢ ⎥⎢ ⎥⎢ ⎥
⎣ ⎦⎣ ⎦ ⎢ ay ⎥
⎢ ⎥
T31 T32 T33 εaz δMzx δMzy δKz ⎣ ⎦
az
⎡ ⎤⎡ ⎤
T11 T12 T13 δAx
⎢ ⎥⎢ ⎥
⎢ ⎥⎢ ⎥
⎢ ⎥⎢ ⎥
= ⎢ T21 T22 T23 ⎥ ⎢ δAy ⎥. (4.98)
⎢ ⎥⎢ ⎥
⎣ ⎦⎣ ⎦
T31 T32 T33 δAz
In the calibration course, the velocity error equation of RLG strapdown INS can be
expressed as:
⎧
⎪ VN VE tan L
⎪
⎪δ V̇E = tan L · δVE + (2ωie sin L + )δVN
⎪
⎪ R Re
⎪
⎪
e
⎪
⎪
⎪
⎪ − (2ωie cos L +
VE
)δVU + gφN + T11 δAx + T12 δAy + T13 δAz
⎪
⎪
⎪
⎨ Re
2VE tan L VU VN ,
⎪
⎪ δ V̇N = (−2ωie sin L + )δVE − δVN − δVU − gφE
⎪
⎪ R R R
⎪
⎪
e e e
⎪
⎪ + + +
⎪
⎪
T 21 δA x T 22 δA y T 23 δA z
⎪
⎪
⎪
⎪ 2V 2V
⎩δ V̇U = (2ωie cos L + E )δVE + N δVN + T31 δAx + T32 δAy + T33 δAz
Re Re
(4.99)
where Re is the radius of the Earth. L is local latitude. VE , VN , VU are velocity results
of RLG SINS in east, north, and up axis, respectively. The attitude error equation of
RLG SINS can be expressed as:
⎧ δVN VE tan L VE
⎪
⎪φ̇ E = − Re + φN (ωie sin L +
⎪
⎪
) − φU (ωie cos L + ) + T11 δGx
⎪
⎪
Re Re
⎪
⎪
⎨ + T12 δGy + T13 δGz
⎪
δVE VE tan L VN .
⎪
⎪φ̇N = − φE (ωie sin L + ) − φU + T21 δGx + T22 δGy + T23 δGz
⎪
⎪ R R R
⎪
⎪
e e e
⎪
⎪
⎪
⎩φ̇U = δV E V E V N
tan L + φE (ωie cos L + ) + φN + T31 δGx + T32 δGy + T33 δGz
Re Re Re
(4.100)
Accordingly, the state equation of RLG POS is expressed as follows:
Ẋ = F · X + G · w, (4.101)
114 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

where X is the system state vector, a column vector with 30 rows. F is the dynamic
coefficient matrix, a 30 × 30 matrix. G is a dynamic noise distribution matrix, which
can be an identity matrix; and w is a zero-mean white-noise vector representing
dynamic disturbance noise, also called process noise. The dynamic disturbance noise
w is a six-dimension vector and can be given by:
 T
w = wax way waz wgx wgy wgz . (4.102)

The dynamic noise distribution matrix G can be given by:

⎡ ⎤
Cbn 03×3
⎢ ⎥
⎢ ⎥
⎢ ⎥
G = ⎢ 03×3 Cb ⎥. n (4.103)
⎢ ⎥
⎣ ⎦
024×3 024×3

The dynamic coefficient matrix F denotes a 30 × 30 dimension state transition

matrix, which is expressed as follows:
⎡ ⎤
A C3×12 03×12
⎢ 3×6 ⎥
⎢ ⎥
⎢ ⎥
F = ⎢ B3×6 03×12 D3×12 ⎥ (4.104)
⎢ ⎥
⎣ ⎦
024×6 024×12 024×12

⎡ ⎤
VN
Re
tan L 2ωie sin L + VE tan L
Re
−2ωie cos L − VE
Re
0 g 0
A3×6 = ⎣−2ωie sin L + 2VE tan L
Re
− VRUe − VRNe −g 0 0⎦
2VN
2ωie cos L + 2VE
Re Re
0 0 0 0
(4.105)

⎡ 1 VE tan L VE
⎤
0 − 0 0 ωie sin L + −ωie cos L −
⎢ Re Re Re ⎥
⎢ 1 VE tan L VN ⎥
B3×6 =⎢
⎢ Re
0 0 −ωie sin L −
Re
0 −
Re
⎥
⎥
⎣1 VE VN
⎦
tan L 0 0 ωie cos L + 0
Re Re Re
(4.106)

⎡ ⎤
T11 T12 T13 T11 ax T12 ay T13 az T11 ay T11 az T12 ax T12 az T13 ax T13 ay
⎢ ⎥
C3×12 = ⎣ T21 T22 T23 T21 ax T22 ay T23 az T21 ay T21 az T22 ax T22 az T23 ax T23 ay ⎦
T31 T32 T33 T31 ax T32 ay T33 az T31 ay T31 az T32 ax T32 az T33 ax T33 ay
(4.107)
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 115

⎡ ⎤
T11 T12 T13 T11 ωx T12 ωy T13 ωz T11 ωy T11 ωz T12 ωx T12 ωz T13 ωx T13 ωy
⎢ ⎥
D3×12 = ⎣ T21 T22 T23 T21 ωx T22 ωy T23 ωz T21 ωy T21 ωz T22 ωx T22 ωz T23 ωx T23 ωy ⎦.
T31 T32 T33 T31 ωx T32 ωy T33 ωz T31 ωy T31 ωz T32 ωx T32 ωz T33 ωx T33 ωy
(4.108)

3) Measurement equation modeling with lever arm compensation

In a real calibration course, the sensing center of IMU is not superposed with the
rotation center of the turntable. There is a distance vector that is arm lever. The IMU
will get an additional velocity when the turntable is rotating. Therefore, the lever
arm compensation must be calculated in the course of calibration [29]. The theories
of lever arm effect are constructed by introducing the geographic coordinate system
t b
(otxtytzt). ωtb and ωtb are the rotation rate of the turntable in the geographic and
body coordinate system, respectively, rb is the position vector of the accelerometers
sensing centre of RLG IMU in body coordinate system, and they can be expressed
as:
T
rb = [rx ry rz ] (4.109)

T
t
ωtb = Cbn ωtb
b
= Cbn [ωx ωy ωz ] . (4.110)

The lever arm effect can be written as:




drb  drb  drb 
= ωtb × rb +
t
= Cb ωtb × rb +
n b
, (4.111)
dt t dt b dt b

where (drb /dt)|t and (drb /dt)|b are the body velocity in the geographic and body
coordinate system separately, ωtb t
× rb denotes the derivative velocity caused by
the rotation rate of the turntable. Considering that the RLG POS is a rigid body,
the position vector of IMU in body coordinate system is constant in the calibration
course, so that (drb /dt)|b = 0. The function can be simplified as:


drb 
= Cbn ωtb
b
× rb . (4.112)
dt t

The velocity error caused by the lever arm effect is expressed in the geographic
coordinate system as follows:


drb 
dVln = = Cbn (ωtb
b
× rb ). (4.113)
dt t
116 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

The velocity error in the navigation frame is given by:

⎡ ⎤ ⎡ ⎤⎡ ⎤
dVlEn
T11 T12 T13 ωy rz − ωz ry
⎢ ⎥ ⎢ ⎥⎢ ⎥
dVln = ⎢ n ⎥ ⎢ ⎥⎢
⎣dVlN ⎦ = ⎣T21 T22 T23 ⎦ ⎣ ωz rx − ωx rz ⎦
⎥

dVlUn
T31 T32 T33 ω x ry − ω y r x
⎡ ⎤
T11 (ωy rz − ωz ry) + T12 (ωz rx − ωx rz ) + T13 (ωx ry − ωy rx)
⎢ ⎥
=⎢ ⎣T21 (ωy rz − ωz ry) + T22 (ωz rx − ωx rz ) + T23 (ωx ry − ωy rx)⎦.
⎥ (4.114)
T31 (ωy rz − ωz ry) + T32 (ωz rx − ωx rz ) + T33 (ωx ry − ωy rx)

The velocity errors are chosen as the measurement, and the measurement equation
is derived as follows:

Z = H X + η, (4.115)

where Z is measurement vector, H is the measurement matrix, η is the measurement

T
noise, η = [ηE ηN ηU ] . According to Eq. (4.113), the measurement vector can be
given as:
⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤
δVE VE n
dVlE VE − dVlEn
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
Z=⎢ ⎥ ⎢ ⎥ ⎢ n⎥ ⎢
⎣δVN ⎦ = ⎣VN ⎦ − ⎣dVlN ⎦ = ⎣VN − dVlN ⎦.
n ⎥ (4.116)
δVU VU n
dVlU VU − dVlUn

The measurement matrix H can be expressed as:

 
H = I3×3 03×27 . (4.117)

4) The final estimated coefficients

For white Gaussian noise systems, the Kalman filter is the optimal minimum mean
square error (MSE) estimator [30]. Compared to the simple Kalman filter, the
performance of EKF is superior when the state-space model is nonlinear. The
30-dimensions state vector X, which includes coefficients of the error model for
RLG IMU, can be estimated using the extended Kalman filter. The final estimated
coefficients of the acceleration channel for RLG IMU can be written as:
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
Ābx Āby Ābz Abx Aby Abz εax εay εaz
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ K̄ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ x M̄yx M̄zx ⎥ ⎢ Kx Myx Mzx ⎥ ⎢ δKx δMyx δMzx ⎥
⎢ ⎥=⎢ ⎥+⎢ ⎥.
⎢M̄xy K̄y M̄zy ⎥ ⎢Mxy Ky Mzy ⎥ ⎢δMxy δKy δMzy ⎥
⎣ ⎦ ⎣ ⎦ ⎣ ⎦
M̄xz M̄yz K̄z Mxz Myz Kz δMxz δMyz δKz
(4.118)
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 117

The final estimated coefficients of angular rate channel are expressed as:
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
Ḡbx Ḡby Ḡbz Gbx Gby Gbz εgx εgy εgz
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ S̄ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ x Ēyx Ēzx ⎥ ⎢ Sx Eyx Ezx ⎥ ⎢ δSx δEyx δEzx ⎥
⎢ ⎥=⎢ ⎥+⎢ ⎥. (4.119)
⎢ Ēxy S̄y Ēzy ⎥ ⎢ Exy Sy Ezy ⎥ ⎢δExy δSy δEzy ⎥
⎣ ⎦ ⎣ ⎦ ⎣ ⎦
Ēxz Ēyz S̄z Exz Eyz Sz δExz δEyz δSz

4.3.4 Temperature Error Modeling of IMU Based on Neural

Network

The output errors of gyroscope assembly in angular rate channel for IMU mainly
include bias and scale factor errors. Scale factor error varies little with the change
of temperature, while bias presents the opposite property. Hence, the temperature
error of gyroscope assembly mainly refers to the temperature error of gyroscope bias
influenced by the temperature, rate of temperature change, and temperature gradient.
The output errors of accelerometer assembly in linear acceleration channel for IMU
include bias and scale factor error. Temperature changes have little influence on the
scale factor error, which make it available to ignore scale factor error [31]. All these
properties mentioned above make it necessary to compensate bias temperature errors
of gyroscope and accelerometer assemblies to improve the efficiency and precision
of IMU.
1. Temperature Error Analysis of RLG Bias
The influence of temperature on RLG bias is a synthetic process [32]. The major influ-
encing factors include temperature T, rate of temperature change Ṫ and temperature
gradient ∇T . They impact on the physical parameters and geometric deformation of
RLG.
The temperature often causes the thermal deformation of different materials in
RLG, which leads to the change of the light path of RLG [33]; The temperature
gradient will cause the extrusion and deflection of components in RLG, this also
affects the output of RLG [34]; The rapid external temperature change will accentuate
the effects of temperature gradient. In addition, RLG itself is a heat source when it
is working. It takes time for RLG to attain thermodynamic equilibrium. The inner
structure of RLG is shown in Fig. 4.15.
In the model of the temperature error of the ith RLG bias, the three embedded
thermometers in RLG measure the temperature of anode (TaGi), cathode (TcGi), and
shell (TsGi), respectively. Temperature TGi is the average of the three temperature
values for gyroscope in the ith axes, which has been smoothed to estimate the mea-
surement error. The rate of temperature change ṪGi is the derivative of TGi. Limited
by the measurement condition, temperature gradients ∇T1Gi and ∇T2Gi are replaced
by the temperature differences of TaGi, and TcGi from TsGi, respectively, because
TsGi is the closest to the external environment temperature. The temperature input
118 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

Fig. 4.15 The inner structure Spherical Light Path

of RLG mirror Shell T3

Anode
Anode
T1

Cathode
T2

XGi of gyroscope in the ith axes is given as:

⎡ ⎤ ⎡ ⎤
TGi (TaGi + TcGi + TsGi )/3
⎢ ⎥ ⎢ ⎥
⎢ Ṫ ⎥ ⎢(Ṫ + Ṫ + Ṫ )/3⎥
⎢ Gi ⎥ ⎢ aGi cGi sGi ⎥
X Gi = ⎢ ⎥=⎢ ⎥. (4.120)
⎢∇T1Gi ⎥ ⎢ T − T ⎥
⎣ ⎦ ⎣ aGi sGi ⎦
∇T2Gi TcGi − TsGi

Therefore, the nonlinear multiparameters temperature error of gyroscope in the ith

axes can be expressed qualitatively as the function of the temperature input XGi:
T
δ ḠT i = f (XGi ) = f ([TGi , ṪGi , ∇T1Gi , ∇T2Gi ] ). (4.121)

2. The Preprocessing of Temperature Error

The original output of RLG bias under variable temperature condition includes the
quantization noise and temperature error.

Goi (n) = qi (n) + δ ḠT i (n) (n = 1,2, 3 . . .), (4.122)

where Goi (n) is the original output of the ith RLG bias; qi (n) is the quantization noise
of the ith RLG; δ ḠT i (n) is the temperature error of the ith RLG bias. The temperature
error should be extracted from the original output of RLG bias first. As the type of
temperature error is unclear beforehand, traditional trend extraction methods such
as low pass filter and polynomial curve fitting are not suitable. Therefore, empirical
mode decomposition (EMD) method is chosen to extract the temperature error of
RLG bias [35]. This method can decompose any complicated signal with unknown
model under different temporal scales, retaining the original characteristics of the
signal [36].
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 119

Fig. 4.16 Temperature error

modeling for RLG bias based
on RBF neural network

EMD method decomposes the original output of RLG bias Goi (n)into the tem-
perature error δ ḠT i (n) and a series of noise components with different frequencies.
The frequency of each noise component is higher than the temperature error signal.
N
Goi (n) = qij (n) + δ ḠT i (n), (4.123)
j =1

where N is the number of the noise components; qij (n) is the jth noise component
with high frequency, qij should satisfy two conditions: (i) The number of zero-
crossings and the number of extrema must be equal or differ at most by one. (ii) At
any time point, envelopes defined by the local maxima and the local minima have a
mean value of zero.
The noise components are obtained using a sifting process: First, identify all the
local extrema (maxima and minima) of Goi(n), and estimate upper and lower en-
velope with a cubic spline interpolation. The mean envelope m1(n) is the average
of the two envelopes. Second, obtain a new signal by taking the difference between
Goi(n) and m i1(n), si1(n) = Goi(n)—mi1(n). If si1(n) satisfies the above two con-
ditions of noise component, si1(n) is the first noise component qi1(n). Otherwise
si1(n) should be sifted again until the noise component is obtained. Then, sift the
signal Goi(n) − qi1(n) and repeat above steps until the residue cannot be sifted or
the frequency is lower than the cut-off frequency.
The temperature error δ ḠT i (n) is used as the training targets of RBF neural net-
work. On the other hand, the temperature values measured by the three embedded
thermometers should also be preprocessed. As the rate of temperature change in
inner RLG is not rapid, a simple smoothing process is sufficient.
3. Nonlinear Multiparameters Temperature Error Modeling of Gyroscope
Assembly
RBF neural network is a special feedforward network with only three layers (input,
hidden, and output layer). The hidden layer transfers the linear model into a non-
linear one and maps the input space to a new solution space [37]. According to the
characteristic of temperature errors of RLG bias, the RBF neural network is as given
in Fig. 4.16.
120 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

Fig. 4.17 Temperature error

modeling for accelerometer
assembly

Radial basis function is usually defined as a function of the Euclidean distance

from arbitrary input XGi to the center CjGi, it can be written as hjGi. The most
common radial basis function is Gauss kernel function:
 3 3 
3XGi − Cj Gi 32
hj Gi = exp − , (4.124)
2σj2Gi

where CjGi is the basis function center of the jth node in the hidden layer for
gyroscope in the ith axes, σ jGi represents the spread of the basis function.
The neural network theory holds that any function can be expressed as a weighted
sum of a set of basis functions in solving function approximation problem. Therefore,
the temperature error model of gyroscope assembly can be given as:
 3 3 
MG MG 3XGi − Cj Gi 32
δ ḠT i = f (XGi ) = wj Gi hj Gi = wj Gi exp − , (4.125)
j =1 j =1
2σj2Gi

where wjGi is the jth weight value from the hidden layer to the output layer of
gyroscope in the ith axes; MG represents the node number of the hidden layer. CjGi,
σ jGi, and wjGi can be obtained by training process of RBF neural network during
the temperature compensation.
4. Nonlinear Multiparameters Temperature Error Modeling of Accelerometer
Assembly
Unlike RLG, QMA has only one embedded thermometer. The accelerometer assem-
bly temperature error model can only obtain temperature and rate of temperature
change without temperature gradient. According to the characteristic of temperature
error for accelerometer assembly, the nonlinear multiparameters temperature error
model of accelerometer in the ith axes can be expressed qualitatively as the function
of the temperature input XAi:
T
δAT i = f (XAi ) = f ([TAi , ṪAi ] ). (4.126)

The RBF neural network is given in Fig. 4.17.

4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units 121

T
The temperature input of accelerometer in the ith axes is XAi = [TAi ṪAi ] with
ṪAi is the output of thermometer embedded in accelerometer in the ith axes; ṪAi is
the derivative of TAi . The radial basis function is also Gauss kernel function:
 3 3 
3XAi − Cj Ai 32
hj Ai = exp − , (4.127)
2σj2Ai

where CjAi is the basis function center of the jth node in the hidden layer for ac-
celerometer in the ith axes, σ jAi represents the spread of the basis function. The
temperature error model of accelerometer assembly can be given as:
 3 3 
MA MA 3XAi − Cj Ai 32
δ ĀT i = f (XAi ) = wj Ai hj Ai = wj Ai exp − , (4.128)
j =1 j =1
2σj2Ai

where wjAi is the jth weight value from the hidden layer to the output layer of
accelerometer in the ith axes; MA represents the node number of the hidden layer.
CjAi, σ jAi, and wjAi can be obtained by training process of RBF neural network
during the temperature compensation.
5. Temperature Error Compensation Method of RBF Neural Network Based
on Bayesian Regularization
The poor generalization ability is the most troubling problem affecting the application
of RBF neural network. In the traditional algorithm of RBF neural network, MSE is
usually used as the evaluation criterion. The MSE of temperature error in gyroscope
and accelerometer assemblies can be given as:
⎧ N
⎪
⎪ 1
⎪
⎪ dGi
E = (f(XGi (n)) − δ ḠT i (n))
2
⎪
⎨ N n=1
, (4.129)
⎪
⎪ N
⎪
⎪ 1 2
⎪
⎩ EdAi = N (f(XAi (n)) − δ ḠT i (n))
n=1

where δ ḠT i (n) and δAT i (n) are the nth training target of the sample for gyroscope and
accelerometer in the ith axes, respectively; XGi(n) and XAi(n) are the corresponding
temperature input for gyroscope and accelerometer in the ith axes, respectively;
f(XGi (n)) and f(XAi (n)) are the output of the training sample for gyroscope and
accelerometer in the ith axes, respectively; N is the total number of the training
sample.
The MSE of the training sample can only reflect the ability to approximate the
sample of RBF neural network, not representing the generalization ability. A major
issue for traditional RBF neural network methods is the potential for overfitting which
leads to a fitting of the noise, and loses generalization of the network [38]. To reduce
the potential for overfitting, an improved RBF neural network based on Bayesian
regularization is proposed to prevent the weight values from growing too large by
appending weight values in the evaluation criterion. The smaller the weight values,
122 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

better is the generalization capability of the network. The weight decay regularizer
can be given as:
⎧ MG
⎪
⎪ 1
⎪
⎪ wGi
E = w2
⎪
⎨ MG k=1 j Gi
. (4.130)
⎪
⎪ MA
⎪
⎪ 1
⎪
⎩ EwAi = M wj2Ai
A k=1

Therefore, the evaluation criterion of Bayesian regularization can be given as:


FGi (wGi ) = αGi EwGi + βGi EdGi
, (4.131)
FAi (wAi ) = αAi EwAi + βAi EdAi
where αGi, βGi, αAi, and βAi are the evaluation criterion function parameters. Under
the situation of αGi  βGi or αAi  βAi, overfitting may occur due to the overem-
phasis on reducing training error. While on the other hand, too much concentration
on limiting the network weight values, when αGi  βGi or αAi  βAi, may prob-
ably lead to large training error. Therefore, how to find optimal values of αGi, βGi,
αAi, and βAi is a key point of Bayesian regularization. In the Bayesian framework,
the weight values of the network are assumed to be random variables. According
to Bayesian’s rule [39], the optimization of the regularization
 MP parameters αGi,
 MP  βGi,
αAi, and βAi requires solving the Hessian matrix of FGi wGi and FAi wAi at the
MP MP MP MP
minimum points wGi and wAi . The optimal weight values wGi and wAi should
maximize the posterior probability, this is equivalent to minimizing the regularized
evaluation criterion functions FGi (wGi ) and FAi (wAi ).
Initializing the value of αGi(0), βGi(0), αAi(0), and βAi(0) by prior distribution,
the updating formula of the evaluation criterion function parameters in gyroscope
and accelerometer assemblies can be expressed as:
⎧  MP   MP 
⎪
⎪ HGi (k) = βGi (k)∇ 2 Ed Gi wGi (k) + αGi (k)∇ 2 EwGi wGi (k)
⎪
⎪
⎪
⎪ γGi (k) = MG − 2αGi (k)tr(HGi (k))−1
⎪
⎪
⎪
⎨
γGi (k)
αGi (k + 1) =  MP  (4.132)
⎪
⎪ 2E wGi wGi (k)
⎪
⎪
⎪
⎪
⎪
⎪ N − γGi (k)
⎪
⎩ βGi (k + 1) =  MP 
2EdGi wGi (k)
⎧  MP   MP 
⎪HAi (k) = βAi (k)∇ ED Ai wAi (k) + αAi (k)∇ EwAi wAi (k)
2 2
⎪
⎪
⎪
⎪
⎪ γAi (k) = MA − 2αAi (k)tr(HAi (k))−1
⎪
⎪
⎪
⎨
γAi (k)
αAi (k + 1) =  MP  (4.133)
⎪
⎪ 2E w (k)
⎪
⎪ wAi Ai
⎪
⎪
⎪
⎪ N − γ (k)
⎪
⎩βAi (k + 1) =
Ai
 MP 
2EdAi wAi (k)
4.4 High Dynamic Strapdown Inertial Algorithm 123

where γGi (k) and γAi (k) are called the effective number of parameters. HGi (k) and
HAi (k) are the Hessian matrix of the evaluation criterion function.
The training process of Bayesian regularization neural network is iterative
algorithm for compensating temperature error of POS. The limited function is:
⎧
⎪ |αGi (k + 1) − αGi (k)| ≤ eαGi
⎪
⎪
⎪
⎨ |βGi (k + 1) − βGi (k)| ≤ eβGi
, (4.134)
⎪
⎪ |αAi (k + 1) − αAi (k)| ≤ eαAi
⎪
⎪
⎩
|βAi (k + 1) − βAi (k)| ≤ eβAi

where eαGi, eβGi, eαAi, eβAi are infinitesimals, this is equivalent to the convergence
of the evaluation criterion function. When the training process of Bayesian regular-
ization neural network is finished, the optimization of the neural network parameters
wj∗Gi , C∗j Gi , σj∗Gi and wj∗Ai , C∗j Ai , σj∗Ai can be obtained. The final result of temperature
∗
error δ Ḡ∗T i and δAT i can be updated by wj∗Gi , C∗j Gi , σj∗Gi and wj∗Ai , C∗j Ai , σj∗Ai . The
temperature error δ Ḡ∗T i is used to compensate the original output of RLG bias Goi
∗
in the ith axes. The temperature error δAT i is used to compensate the original output
of QMA bias Aoi in the ith axes. The final output of gyroscope and accelerometer in
the ith axes can be given by:
⎛ ⎧ 3 32 ⎫⎞
MG ⎨ 3
⎪ ∗ 3 ⎪
3XGi − Cj Gi 3 ⎬⎟
⎜
Gbi = Goi − δ ḠT i = Goi − ⎝ wj∗Gi exp − ⎠ (4.135)
j =1
⎪
⎩ 2(σj∗Gi )2 ⎪
⎭

⎛ ⎧ 3 32 ⎫⎞
MA ⎨ 3
⎪ ∗ 3 ⎪
3XAi − Cj Ai 3 ⎬⎟
⎜
Abi = Aoi − δAT i = Aoi − ⎝ wj∗Ai exp − ⎠. (4.136)
j =1
⎪
⎩ 2(σj∗Ai )2 ⎪
⎭

Bayesian regularization method reduces the network effective weight values un-
der the condition of minimizing network training error by new evaluation criterion.
Therefore, the opportunity of overfitting decreases rapidly, which eventually im-
proves the generalization ability of neural network and consequently extends the
application of RBF neural network. Bayesian regularization method is used as the
evaluation criterion to optimize the configuration of temperature error for IMU, and
extends the application of RBF neural network.

4.4 High Dynamic Strapdown Inertial Algorithm

Inertial guidance technology centering on SINS is one of the key technologies

to realize precise strike by weapon systems such as short-range ballistic missile
and various tactical missiles, so its maneuvering characteristics are increasingly
124 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

demanding. Attitude algorithm among SINS algorithm is the main factor affect-
ing precision of SINS under high-dynamic environmental conditions. An improved
single-subsample rotating vector attitude algorithm will be primarily introduced in
the following sections.

4.4.1 Error Analysis and Gyro Biases Calibration of Analytic

Coarse Alignment for Airborne POS

The POS is a dedicated SINS/GPS-integrated measurement system for airborne re-

mote sensing. It can successively provide position, velocity, and attitude of the
airborne sensor and has become one of the key technologies for airborne direct geo-
referencing. The performance of the airborne POS chiefly depends on high precision
SINS. The SINS solves Newton’s equations of motion by integrating accelerations
and angular rates sensed by inertial sensors. The navigation errors of SINS diverge
slowly with time due to the existence of the initial misalignment and inertial sensor
biases. It is an effective way to improve the measurement accuracy of SINS that
optimizes initial alignment and gyros biases calibration algorithm.
Normally, initial alignment process is divided into two phases—coarse and fine
alignment. The purpose of coarse alignment is to provide a fairly good initial condi-
tion for the fine alignment processing. Since the conventional error model of SINS
is not completely observable in stationary base, and the degree of observability of
some states is weak, it is ineffective to estimate the states by a Kalman filter at this
time, and it even greatly influences the speed and accuracy of initial alignment and
calibration. In this case, can the gyros biases be calibrated accurately, and can initial
misalignment be reduced in analytic coarse alignment phases? The above interesting
idea stimulated me to study the error properties of analytic coarse alignment and
gyros biases calibration for SINS.
The gyro biases can be calibrated through mutiposition alignment by analytic
method. Priel presents a 90◦ double position alignment method through which the
gyro biases can be calibrated and the accuracy of initial alignment can be improved
[40]. Cheng et al. proposed a method of accurate gyro biases measurement for SINS
[41]. By using gyrocompass loop, two horizontal axes gyro biases can be measured
and compensated [42]. Chung et al. proposed a 180◦ double position alignment,
which rotate SINS with respect to heading axis by a rotatable device [43]. Those
methods can achieve a high accuracy in short time. But, a special rotatable device
is prerequisite to rotate SINS 90 or 180◦ along heading axis for all the double-
position alignment method introduced above, which is very inconvenient for many
applications [44].
To solve those problems, according to the analytic coarse alignment principle, the
sensitivity of Euler angles with respect to inertial sensor biases are analyzed uniquely,
and the errors of Earth rotation rate in body frame caused by initial misalignment
are obtained. Based on the transformation model of gyro biases, a novel analytic
4.4 High Dynamic Strapdown Inertial Algorithm 125

algorithm of gyro biases calibration based on arbitrary double-position is proposed.

The simulation and experiment results show that the method can accurately calibrate
the gyro biases and achieve accurate initial attitude of SINS in short time without
other accessory instrument. Thus, the measurement precision of SINS is improved,
and verifies the rationality and efficiency of the proposed method.
1. The Principle of Analytic Coarse Alignment
It is well known that the self-alignment requirement is to be the measurement of two
noncollinear vectors, the local gravity g, and the Earth rate ωie , in both body and
navigation frames. The local gravity vector and Earth rate can be sensed in the body
frame by the accelerometers and gyros, respectively. Without the loss of generality,
the local-level frame n is selected as the navigation frame, with x directing east, y
directing north and z upward vertical. Denote by b the SINS body frame, by e the
Earth frame and by i inertial frame. Then, g and ωie can be expressed in the navigation
frame as:
 T  T
gn = 0 0 −g , ωnie = 0
(L)
sin(L) (4.137)
where g and
are the magnitude of gravity and Earth rate, respectively, L is local
latitude, and the superscript T denotes the transpose of matrix. The purpose of initial
alignment is to obtain the initial body attitude matrix with respect to the navigation
frame Cnb . The time and accuracy are two crucial destinations. The accuracy of align-
ment affects the measurement precision, and time denotes the response capability
of SINS, respectively. It is common to use the analytic coarse alignment based on
stationary base.
⎡ ⎤ ⎡ ⎤⎡ ⎤
gxb c11 c12 c13 0
⎢ ⎥ ⎢ ⎥⎢ ⎥
g = Cn g , ⎣gy ⎦ = ⎣c21 c22 c23 ⎦ ⎣ 0 ⎥
b b n ⎢ b ⎥ ⎢ ⎥ ⎢
⎦ (4.138)
gzb
c31 c32 c33 −g
⎡ ⎤ ⎡ ⎤⎡ ⎤
ωxb c11 c12 c13 0
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ b⎥ ⎢ ⎥⎢ ⎥
ωbie = Cnb ωnie , ⎢ ωy ⎥ = ⎢ c21 c22 c23 ⎥ ⎢
cos (L) ⎥. (4.139)
⎢ ⎥ ⎢ ⎥⎢ ⎥
⎣ ⎦ ⎣ ⎦⎣ ⎦
ωzb c31 c32 c33
sin (L)
In practical implementation, ωbie and gb are replaced by the gyros or accelerometers
T
outputs ωbib = [ωibx
b b
ωiby b ] , f = [ b
ωibz b fx fyb fzb ] of SINS at some fixed location,
respectively. The body attitude matrix with respect to the navigation frame can be
expressed explicitly as:
⎡ ⎤ ⎡ ⎤
c11 c12 c13 c22 c33 − c32 c23 c12 c13
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
Cnb = ⎢ c21 c22 c23 ⎥ = ⎢ c13 c32 − c12 c33 c22 c23 ⎥
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
c31 c32 c33 c12 c23 − c13 c22 c32 c33
126 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)
⎡ ⎤
b
fyb ωibz − fzb ωiby
b b
ωibx fb fb
⎢ + x tan (L) − x ⎥
⎢ g
cos (L)
cos (L) g g ⎥
⎢ b b ⎥
⎢ f ω − f b ωb b
ωiby fyb fyb⎥
=⎢
⎢
z ibx x ibz
+ tan (L) − ⎥.
⎥ (4.140)
⎢ g
cos (L)
cos (L) g g ⎥
⎢ b b ⎥
⎣ fx ωiby − fyb ωibx
b b
ωibz fb f b ⎦
+ z tan (L) − z
g
cos (L)
cos (L) g g
On the other hand, the body attitude matrix with respect to the navigation frame can
be defined considering three successive rotation of Euler angles, that are azimuth φ,
pitch θ , and roll γ , around the z-, x-, y-axes, respectively and in that order. The final
elements of attitude matrix as a function of Euler angles can be expressed as:
⎡ ⎤
c11 c12 c13
⎢ ⎥
⎢ ⎥
⎢ ⎥
Cnb = ⎢ c21 c22 c23 ⎥
⎢ ⎥
⎣ ⎦
c31 c32 c33
⎡
cos (γ ) cos (φ) − sin (γ ) sin (θ ) sin (φ)
⎢
⎢
⎢
=⎢ − cos (θ ) sin (φ)
⎢
⎣
sin (γ ) cos (φ) + cos (γ ) sin (θ ) sin (φ)
⎤
cos (γ ) sin (φ) + sin (γ ) sin (θ ) cos (φ) − sin (γ ) cos (θ )
⎥
⎥
⎥
cos (θ ) cos (φ) sin (θ) ⎥. (4.141)
⎥
⎦
sin (γ ) sin (φ) − cos (γ ) sin (θ ) cos (φ) cos (γ ) cos (θ )
According to Eqs. (4.140) and (4.141), the initial attitude angles of body can be
computed as follows:
1 2
−1 −1
fyb
θ = sin (c23 ) = sin − (4.142)
g


b
c13 fx
γ = tan−1 − = tan−1 (4.143)
c33 fzb

 1 2
−1 c21 −1
b
fxb ωibz − fzb ωibx
b
ϕ = tan − = tan . (4.144)
c22 b
gωiby + fyb
sin (L)
Through distinguishing the element signs of attitude matrix, the true calculation
attitude angles of body can be determined as some functions of θ , γ , and ϕ, and can
be elided, since they do not influence the following analysis.
4.4 High Dynamic Strapdown Inertial Algorithm 127

2. Euler Angles Error Model

When sensor errors exist, the Euler angles for the body initial attitude matrix will
deviate from the true value. It is illuminating to investigate the change or sensitivity
of Euler angles as a function of gyros/accelerometers biases. Consider a stationary
alignment for a navigation-grade SINS, the error of Euler angles for initial attitude
matrix are given up to one order of time by:
∂θ −1
dθ = b
· ∇y =   2 ∇y (4.145)
∂fy
g 2 − fyb
 b   b 
∂γ ∂γ fz ∇x − fxb ∇z fz ∇x − fxb ∇z
dγ = · ∇x + b · ∇z =  2  2  ≈   2  (4.146)
∂fxb ∂fz fzb + fxb g 2 − fyb

∂ϕ ∂ϕ ∂ϕ ∂ϕ ∂ϕ ∂ϕ
dϕ = b
· εx + b
· εy + b
· ε z + b · ∇x + b · ∇y + b · ∇z
∂ωibx ∂ωiby ∂ωibz ∂fx ∂fy ∂fz
 b b 
fz ωibx − fx ωibz (gεy +
sin (L)∇y )
b b
= 2  2
b
gωiby + fyb
sin (L) + fxb ωibzb
− fzb ωibx
b

  
b
gωiby + fyb
sin (L) fxb εz + ωibzb
∇x − fzb εx − ωibx
b
∇z
+  2  2 ,
b
gωiby + fyb
sin (L) + fxb ωibz b
− fzb ωibx
b

(4.147)

where εj , ∇j are biases of gyros and accelerometers in three axes, j = x, y, z,

respectively. For a navigation-grade SINS, the gyros biases εj are less than
0.1◦ /h, and accelerometer biases ∇i are less than 10 μg. When SINS starts sta-
tionary alignment, the pitch angle of the body is generally small. Supposing
|θ | ≤ 45◦ , then the measured absolute
√ acceleration in y-axis by accelerometers
in the body frame fyb is less then 2g/2 . The Euler angles errors caused by
3   2 3 √
accelerometers biases dθ  = 3∇y / g 2 − fyb 3 < 2∇y /g ≈ 14μrad,
 ) 2  b 2 
dγ  =  gzb ∇x − gxb ∇z g − gy  < 2g(∇z + ∇x )/g 2 ≈ 40μrad are less,
respectively, and can be ignored in some application.
128 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

3. Error Model of Earth Rotation Rate in Body Frame

b
According to Eqs. (4.139) and (4.141), the Earth rotation rate in body frame ωie can
be rewritten as:
⎡ ⎤ ⎡ ⎤
b
ωiex 0
⎢ ⎥ ⎢ ⎥
⎢ωb ⎥ = C b ⎢
cos (L)⎥
⎣ iey⎦ n⎣ ⎦
b
ωiez
sin (L)
⎡  ⎤
cos (γ ) sin (ϕ) + sin (γ ) sin (θ ) cos (ϕ)
cos (L) − sin (γ ) cos (θ )
sin (L)
=⎣ cos (θ ) cos (ϕ)
cos (L) + sin (θ )
sin (L) ⎦.
 
sin (γ ) sin (ϕ) − cos (γ ) sin (θ ) cos (ϕ)
cos (L) + cos (γ ) cos (θ)
sin (L)
(4.148)

The errors of Earth rotation rate in the body frame caused by initial misalignment in
three axes are given by:
b b b
∂ωiex ∂ωiex ∂ωiex
b
dωiex = dϕ + dθ + dγ
∂ϕ ∂θ ∂γ
 
= cos (γ ) cos (ϕ) − sin (γ ) sin (θ) sin (ϕ)
cos (L)dϕ
 
+ cos (θ ) cos (ϕ) cos (L) + sin (θ ) sin (L)
sin (γ )∂θ
 
+ ( cos (γ ) sin (θ) cos (ϕ) − sin (γ ) sin (ϕ) cos (L)

− cos (γ ) cos (θ) sin (L)
∂γ
(4.149)
b b b
∂ωiey ∂ωiey ∂ωiey
b
dωiey = dϕ + dθ + dγ
∂ϕ ∂θ ∂γ

= − cos (θ ) sin (ϕ)
cos (L)dϕ + cos (θ ) sin (L)

− sin (θ ) cos (ϕ) cos (L)
dθ
(4.150)
b b b
∂ωiez ∂ωiez ∂ωiez
b
dωiez = dϕ + dθ + dγ
∂ϕ ∂θ ∂γ
 
= sin (γ) cos (ϕ) + cos (γ ) sin (θ) sin (ϕ)
cos (L)dϕ
 
− cos (γ ) cos (θ) cos (ϕ) cos (L) + cos (γ ) sin (θ) sin (L)
dθ

+ cos (γ ) sin (ϕ) + sin (γ ) sin (θ) cos (ϕ)
cos (L)

− sin (γ ) cos (θ)
sin (L) dγ .
(4.151)

Thereinto, the errors of Earth rotation rate in body frame caused by Euler angles error
dθ, dγ can be calculated 3using the
3 Eqs.
3 (4.149) to (4.151). Since dθ
) ) <3 14μrad,
dγ  < 40μrad, so that 3dωiex b 3
= 3dθ · ∂ωiexb
∂θ + dγ · ∂ωiex
b
∂γ 3 < 1.98×
4.4 High Dynamic Strapdown Inertial Algorithm 129
3 3 3 ) ) 3
3 b 3 3 3
10−3◦ / h, 3dωiey 3 = 3dθ · ∂ωiey b
∂θ + dγ · ∂ωiey
b
∂γ 3 < 3.63 × 10−4◦ / h and
3 b 3 3 ) ) 3
3dω 3 = 3dθ · ∂ωb ∂θ + dγ · ∂ωb ∂γ 3 < 1.98×10−3◦ / h. The results show
iez iez iez
that the errors of Earth rotation rate in body frame caused by dθ , dγ are evidently less
than the gyros biases εi = 0.1◦ / h, therefore can be ignored in following analysis.
4. Transformation Model of Gyro Biases
One of the purposes for SINS is the determination of the transformation matrix from
body frame to navigation frame. The attitude navigation equation is used to make the
time update. For attitude navigation equations, the rotation rate of the body frame
with respect to the navigation frame, expressed in the body frame, can be written as:

ωbnb = ωbib − Cnb (ωnie + ωnen ) (4.152)

where ωnen is rotation rate of the navigation frame with re-

spect to the Earth frame, expressed in navigation frame, ωnen =
 ) ) ) T
−VN (RN + h)VE (RE + h)VE tan L (RE + h) ,

RE and RN are respectively the transverse and meridian radii of curvature, h is the
altitude. The SINS alignment problem is equivalent to the unique determination of
current attitude using gyros and accelerometers outputs. Suppose that the latitude
and the height are known by GPS. When the SINS is at rest, the ground velocity
is zero, and ωnen = 0. Substituting Eqs. (4.148–4.151) into (4.152), it can be found
that the change or sensitivity of rotation rate of the body frame with respect to the
navigation frame, expressed in the body frame in three axes, is given by:
b
ωnbx = ωibx
b
− ωieb
n
= (ω̄ibx
b
+ εx ) − (ω̄iex
b
+ dωiex
b
) = εx − dωiex
b

= εx − ( cos (γ ) cos (ϕ) − sin (γ ) sin (θ) sin (ϕ))

cos (L)dϕ (4.153)
b
ωnby = ωiby
b
− ωiey
b
= (ω̄iby
b
+ εyb ) − (ω̄iey
b
+ dωiey
b
) = εyb − dωiey
b

= εyb + cos (θ ) sin (ϕ)

cos (L)dϕ (4.154)
b
ωnbz = ωibz
b
− ωiez
b
= (ω̄ibz
b
+ εz ) − (ω̄iey
b
+ dωiez
b
) = εz − dωiez
b

= εz − ( sin (γ ) cos (ϕ) + cos (γ ) sin (θ) sin (ϕ))

cos (L)dϕ, (4.155)
b b
where ω̄ibj , ω̄iej are theoretic values of gyros outputs and the Earth rotation rate of
three axes j = x, y, z in the body frame, noting the fact ω̄ibj b
= ω̄iej
b
. According to
Eqs. (4.153) and (4.154), the gyro biases equation in X- andY -axes can be expressed:


cos (γ )  
εx + cot (ϕ) − sin (γ ) tan (θ) εy − ωnby
b
= ωnbx
b
. (4.156)
cos (θ )
According to Eqs. (4.154) and (4.155), the gyro biases equation in Y - and Z-axes can
be expressed:


cos (γ )  
εz + cot (ϕ) − sin (γ ) tan (θ) εy − ωnby
b
= ωnbz
b
. (4.157)
cos (θ )
130 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

5. Gyro Biases Calibration Algorithm

According to Eqs. (4.156) and (4.157), the gyro biases in three axes of SINS can be
calibrated by arbitrary double position. Assuming azimuth, pitch and roll of SINS
are φ1 , θ1 and γ1 , respectively, in the first position. The rotation rate of the body
frame with respect to the navigation frame, expressed in the body frame, in three
b1 b1 b1
axes of SINS are ωnbx , ωnby and ωnbz . The Euler angles of SINS are assumed as ϕ2 ,
θ2 , γ2 in the second position. The rotation rate of the body frame with respect to the
b2 b2 b2
navigation frame, expressed in the body frame, are ωnbx , ωnby and ωnbz , respectively.
According to Eq. (4.156), the gyros biases in X- and Y -axes can be expressed:


cos (γ1 )  
ωnbx = εx +
b1
cot (ϕ1 ) − sin (γ1 ) tan (θ1 ) εy − ωnbyb1
(4.158)
cos (θ1 )


cos (γ2 )  
b2
ωnbx = εx + cot (ϕ2 ) − sin (γ2 ) tan (θ2 ) εy − ωnbyb2
. (4.159)
cos (θ2 )

Finally, by subtracting Eq. (4.158) from (4.159), then formula transforming, the
gyros biases in X- and Y -axes can be calibrated as:
1  b1 
εy = ωnbx − ωnbx
b2
+ Aωnby
b1
− Bωnby
b2
(4.160)
(A − B)


AB 1 b1 1 b2
εx = b1
ωnby − ωnby
b2
+ ωnbx − ωnbx (4.161)
(B − A) A B

where A = cot (ϕ1 )cos (γ1 )/cos (θ1 )−sin (γ1 ) tan (θ1 ), B = cot (ϕ2 )cos (γ2 )/cos (θ2 )
− sin (γ2 ) tan (θ2 ), According to Eq. (4.157), the gyros biases in Y -and Z-axes can
be expressed as follows:


sin (γ1 )  
ωnbz = εz +
b1
cot (φ1 ) + cos (γ1 ) tan (θ1 ) εy − ωnby
b1
(4.162)
cos (θ1 )


sin (γ2 )  
b2
ωnbz = εz + cot (φ2 ) + cos (γ2 ) tan (θ2 ) εy − ωnby
b2
. (4.163)
cos (θ2 )

By subtracting Eq. (4.162) from (4.163), then formula transforming, the gyro bias
along Z-axis is calibrated as:


CD 1 b1 1 b2
εz = ωnby − ωnby + ωnbz − ωnbz ,
b1 b2
(4.164)
(D − C) C D

where C = cot (φ1 )sin (γ1 )/cos (θ1 )+cos (γ1 ) tan (θ1 ), D = cot (ϕ2 )sin (γ2 )/cos (θ2 )
+ cos (γ2 ) tan (θ2 ). According to Eqs. (4.160), (4.161) and (4.164), the gyros biases
in three axes can be calibrated. However, the coarsely aligned Euler angles diverge
theoretic true value, the calibrated results of the gyros biases by the proposed method
exist a small quantity of systemic error. In order to improve the calibration precision,
an iterative compensation algorithm of gyro biases and realignment (generally, twice
4.4 High Dynamic Strapdown Inertial Algorithm 131

iteration step) should be adopt in practical application which the initial azimuth errors
are bigger. Besides, the azimuth increment between the arbitrary double positions
is better close to 90◦ . Thus, the initial alignment and navigation precision of SINS
is improved evidently. The proposed method just needs SINS to be rotated from
first position to another arbitrary position with any axis by anyway without rotatable
device.

4.4.2 Conical Motion Analysis and Evaluation Criteria for

Conical Error Compensation Algorithm

Traditional quaternion algorithm inevitably introduces conical error and becomes

a primary error source of strapdown attitude calculation under dynamic environ-
ment due to noncommutativity of rotation under dynamic environmental conditions.
Equivalent rotating vector concept and rotating vector differential equation proposed
in bibliography [35] effectively solves non-commutativity error problem. This sec-
tion first conducts an analysis of conical motion and gives evaluation criteria for
conical error compensation algorithm to serve as a basis for subsequent algorithm
design.
When described attitude motion of the carrier through rotating vector, the
following rotating vector differential equation can be used as simplified expression:
1
Φ̇ = ω + Φ × ω (4.165)
2
wherein, Φ is the rotating vector, and ω is angular velocity vector of the carrier.
Typically, conical motion is used to research non-commutativity error of algo-
rithm. Assume conical motion rotating around axis x of the carrier coordinate system
( α is the amplitude and
the angular frequency) is expressed with rotating vector
as
 T
Φ = 0 α cos (
t) α sin (
t) . (4.166)

In accordance with the relationship between quaternion and rotating vector, it is

known that rotating quaternion of the system is
 α  α  α  T
Q(t) = cos 0 sin cos (
t) sin sin (
t) . (4.167)
2 2 2
If the attitude updating cycle is h, actual motion of the carrier within h can be
expressed with rotating vector as
⎡ α  ⎤
−2sin2 sin (
h)
⎢ ⎥
⎢
2 
⎥
⎢
h h ⎥
Φ = ⎢ ⎢−2 sin (α) sin 2 sin
t + 2 ⎥ .
⎥ (4.168)
⎢
 
⎥
⎣
h h ⎦
2 sin (α) sin cos
t +
2 2
132 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

Based on equation Eq. (4.167) and in accordance with quaternion differential equa-
tion, it is known that corresponding angular velocity output of conical motion
is
⎡ ⎤
−2
sin2 (α/2)
⎢ ⎥
⎢ ⎥
⎢ ⎥
ω(t) = ⎢ −
sin (α) sin (
t) ⎥ (4.169)
⎢ ⎥
⎣ ⎦

sin (α) cos (
t)

and angular increment of gyro output within current attitude updating cycle h is

5 t+h
θ = ω(τ )dτ
t
⎡ α  ⎤
−2 (
h) sin2
⎢ ⎥
⎢
 2
⎥
⎢
h h ⎥
=⎢
⎢−2 sin (α) sin 2 sin
t + 2 ⎥
⎥ (4.170)
⎢
 
 ⎥
⎣
h h ⎦
2 sin (α) sin cos
t +
2 2

and estimated value of rotating vector within the attitude updating cycle h can be
expressed as:

Φ̂ = θ + Cp (4.171)

wherein, Cp is conical error compensation term. Attitude algorithm of rotating vector

is to construct conical error compensation term Cp by using angular increment of
the gyro to make difference value between Φ and Φ̂ minimum. Other two terms
present periodicity since only the first term among their components is non-zero
constant value. Therefore, algorithm error is defined as follows:
   
φε =  Φ x − Φ̂ x  = ( Φ − θ)x − Cpx . (4.172)

Design criteria for attitude algorithm of rotating vector is to make the low power
term of φε zero to achieve minimum algorithm error.

4.4.3 An Improved Single-Subsample Rotating Vector Attitude

Algorithm

In [45], a research on several multisubsample rotating vector strapdown attitude

algorithms based on the analysis of conical motion is conducted. In [46], an improved
4.4 High Dynamic Strapdown Inertial Algorithm 133

multisubsample algorithm by using output angular increment of the gyro within the
previous attitude updating cycle is proposed to further improve the precision. In
[47], general expression for multisubsample rotating vector attitude algorithm based
on previous achievements is provided. It is observed that multisubsample rotating
vector attitude algorithm is pretty mature at present subject to its development over
the previous years.
However, direct application of multisubsample rotating vector attitude algorithm
under existing sampling frequency conditions will reduce the system attitude updat-
ing frequency; and improvement of sampling frequency is required to remain attitude
updating frequency, which will increase burden on navigational computer hardware,
and quantization error caused by high-frequency sampling becomes the main error
source affecting system precision [48], so its popularization and application under
high-dynamic environmental conditions are limited.
This section first popularizes it based on the idea of improving multisubsample
rotating vector attitude algorithm to introduce a single-subsample rotating vector
attitude algorithm using current and N angular increments within previous attitude
updating cycle; it also deduces correcting algorithm of single-subsample rotating vec-
tor attitude algorithm by combining features of digital filter used in practical system.
Under identical gyro sampling frequency condition, compared with multisubsample
rotating vector attitude algorithm, the algorithm will not reduce the system attitude
updating frequency and will obtain high attitude calculation precision.
1. Design of Improved Single-Subsample Rotating Vector Attitude Algorithm
Multisubsample rotating vector attitude algorithm conducts multiple sampling for
gyro output within one attitude updating cycle h, and uses multiple subsamples θ 1 ,
θ 2 . . . θ N to construct conical error compensation term C p ; single-subsample ro-
tating vector attitude algorithm researched in this section only conducts one sampling
within each attitude updating cycle h, and uses angular increment θ within cur-
rent attitude updating cycle and N angular increments θ (1) , θ (2) . . . θ (N ) within
previous attitude updating cycles to construct conical error compensation term C p
to make single-subsample rotating vector attitude algorithm update the attitude for
once after each sampling; it can obtain high attitude updating frequency compared
with multisubsample rotating vector attitude algorithm with identical sampling fre-
quency. The relationship between attitude updating cycle h and sampling cycle T
is shown in Fig. 4.18.
It can be obtained by summing up the above that single-subsample rotating vector
attitude algorithm using current and N angular increments within previous attitude
updating cycles is defined as following form:
N
Φ̂ = θ + Gi ( θ (i) × θ) (4.173)
i=1

!
wherein, N i=1 Gi ( θ
(i)
× θ) is conical error compensation term C p , θ (i) is an-
gular increment with previous i attitude updating cycles, Gi is algorithm coefficient,
and i = 1Ñ .N is defined as order of single-subsample algorithm here.
134 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

Δ θ1 Δθ2… ΔθN
…

Multi-subsample rotating vector algorithm h=

Δθ … Δ θ ( 2 ) Δ θ (1 )
(N ) Δθ
…
Simple-subsample rotating vector algorithm

h=
Fig. 4.18 Relationship between attitude updating cycle and angular increment sampling cycle

The following is the derivation of new algorithm coefficient according to design

criteria for rotating vector attitude algorithm based on analysis of conical motion in
Sect. 4.4.2.
In equation Eq. (4.173), angular increment of gyro output within previous i attitude
updating cycles can be expressed as:
5 t−(i−1)h
θ =(i)
ω (τ ) dτ
t−ih
⎡ α  ⎤
−2 (
h) sin2
⎢ 2 ⎥
⎢
 
⎥
⎢
h (2i − 1) h ⎥
=⎢
⎢ −2 sin (α) sin 2 sin
t −
⎥.
⎥
⎢ 2
⎣
 
 ⎥

h (2i − 1) h ⎦
2 sin (α) sin cos
t −
2 2
(4.174)

After substituting Eqs. (4.170) and (4.173) into (4.172), acyclic term of conical error
compensation term Cp in the new algorithm can be expressed as:
N N
Cpx = Gi ( θ (i) × θ) |x = 4 Gi sin2 (α)sin2 (
h/2) sin (i
h). (4.175)
i=1 i=1
4.4 High Dynamic Strapdown Inertial Algorithm 135

It can be obtained from Eqs. (4.168) and (4.170) that

α 
( Φ − θ)x = sin2 (− sin (
h) + (
h)). (4.176)
2
After substituting above two equations into Eq. (4.171) and considering α as small
quantity, it can be obtained that algorithm error of the new algorithm is:
N
 
α2 2
h
φε = − sin (
h) + (
h) − 8 Gi sin sin (i
h) . (4.177)
2 i=1
2

Design criterion for rotating vector algorithm is to make low power term of φε relative
to (
h) zero. Make λ =
h, conduct Taylor expansion for the first two terms within
the bracket of Eq. (4.177), and write it as the form of multiplication of vectors to get
⎡ ⎤
C
⎢ 1⎥
⎢C ⎥
⎢ 2⎥
  ⎢ . ⎥
⎢ ⎥
(
h) − sin (
h) = λ λ · · · λ
3 5 2N +1
· · · · ⎢ .. ⎥ (4.178)
⎢ ⎥
⎢ ⎥
⎢CN ⎥
⎣ . ⎦
..

m+1
wherein, coefficient Cm = (−1)
(2m+1)!
, m = 1,2, 3 . . . , constituting column vector C.
Similarly, conduct Taylor expansion for the last term within the bracket of Eq.
(4.177) and write it as the form of matrix multiplication to get
N
 N

h
8 Gi sin 2
sin (i
h) = Gi (4 sin (i
h) − 2 sin ((i + 1)
h)
i=1
2 i=1

−2 sin ((i − 1)
h))
⎡ ⎤
A11 A12 · · · A1N ⎡ ⎤
⎢ ⎥
⎢A ⎥ G1
⎢ 21 A22 · · · A2N ⎥ ⎢ ⎥
  ⎢ . .. ⎥ ⎢ G2 ⎥
⎥ ⎢
⎢ .. ⎥
= λ3 λ5 · · · λ2N +1 · · · · ⎢ .. . ··· . ⎥·⎢ . ⎥
⎢ ⎥ ⎢ .. ⎥
⎢ ⎥ ⎣ ⎦
⎢AN 1 AN 2 · · · AN N ⎥
⎣ . .. .. ⎦ GN
.. . ··· .
(4.179)
m
wherein, coefficient Amn = (2m+1)!
(−1)
[4 · n2m+1 − 2 · (n + 1)2m+1 − 2 · (n − 1)2m+1 ],
m = 1,2, 3 . . . , n = 1 ∼ N , constituting matrix A; algorithm coefficient to be solved
is Gi , i = 1 ∼ N , constituting column vector G. If corresponding terms of Eqs.
(4.178) and (4.140) relative to various powers of λ are equal, i.e. coefficient equation
136 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

C = A · G is true, the algorithm error is minimum. Algorithm coefficient Gi can be

worked out by using the first N rows of A and C. In such case, low power of φε is 0,
and its higher order term is the algorithm error of single-subsample rotating vector
algorithm:
1 N
2
1 2
φε = α CN +1 − AN +1,i Gi λ2N +3 . (4.180)
2 i=1

2. Algorithm Correction Based on Filter Characteristics

In addition, gyro output is often subjected to digital filtering processing in practical
SINS. However, the filter will change amplitude–frequency characteristic of gyro
output, which will further cause inaccurate conical error compensation. In [49], a
method to correct the rotating vector attitude algorithm by combining frequency
characteristics of filter is introduced, and the system therein adopts digital filter able
to improve the gyro resolution. It is pointed out that when amplitude–frequency
characteristic F (
) of digital filter satisfies

F (0) = 1, lim F (
) = 1, |F (
)| ≤ Fmax (4.181)

→0

and when it is true for any

, the thought may be used to correct the conical
compensation algorithm.
This section takes first-order digital lowpass filter commonly used in SINS for
instance and corrects the single-subsample rotating vector attitude algorithm by
combining characteristics of filter. Assume single-subsample rotating vector attitude
algorithm form subject to correction as follows:
N

Φ̂ = θ + Gi ( θ (i) × θ) (4.182)
i=1

wherein, Gi is algorithm coefficient subject to correction according to characteristics
of the filter.
The following is derivation of algorithm coefficient for the correcting algo-
rithm taking characteristics of the filter into account. Assume first-order lowpass
filter W (s) = 1+RCs
1
, and T = RC is filter time constant. Its amplitude-frequency
characteristic is:
1
F (
) = √ . (4.183)
1 + T 2
2
It obviously satisfies with Eq. (4.181).
4.4 High Dynamic Strapdown Inertial Algorithm 137

Since lowpass filter has primary influence on alternating component, equivalent

rotating vector within attitude updating cycle h subject to filtering is:
⎡ α  ⎤
−2F 2 (
) sin2 sin (
h)
⎢ 2 ⎥
⎢
 
⎥
⎢
h h ⎥
Φ = ⎢ ⎢−2F (
) sin (α) sin 2 sin
t + 2 ⎥
⎥
⎢
 
 ⎥
⎣
h h ⎦
2F (
) sin (α) sin cos
t +
2 2

and angular increment of gyro output subject to filtering:

5 t+h
θ = ω (τ ) dτ
t
⎡ α  ⎤
−2F (0) (
h) sin2
⎢ 2 ⎥
⎢
 
⎥
⎢
h h ⎥
=⎢
⎢−2F (
) sin (α) sin 2 sin
t + 2 ⎥.
⎥
⎢
 
 ⎥
⎣
h h ⎦
2F (
) sin (α) sin cos
t +
2 2

Similarly, angular increment of gyro output within previous I attitude updating cycles
subject to filtering is

5 t−(i−1)h
θ (i)
= ω(τ )dτ
t−ih

⎡ α  ⎤
−2F (0)(
h)sin2
⎢
 
2 ⎥
⎢ ⎥
⎢−2F (
) sin (α) sin
h sin
t − (2i − 1)h ⎥
=⎢ 2 ⎥ .
⎢
⎣
2 
⎥
⎦

h (2i − 1)h
2F (
) sin (α) sin cos
t −
2 2
 
In such case, ( Φ − θ)x = sin2 α2 (−F 2 (
) sin (
h) + (
h)).
and acyclic term of conical error compensation term C p can be expressed as:
N N


h
Cpx = Gi ( θ (i)
× θ) |x = 4F (
) 2
Gi sin2 (α)sin2 sin (i
h).
i=1 i=1
2

Therefore, algorithm error expression is:

 
α2 
N

h
φε = −F 2 (
) sin (
h) + (
h) −8F 2 (
) Gi sin2 sin (i
h) .
2 i=1
2
138 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

Fig. 4.19 Swing experiment

conducted on three-axle table

Substitute Eq. (4.179) to get

2 N
 
α2 2 T 2
h
φε = F (
) − sin (
h) + (
h) + (
h) − 8
3
Gi sin sin (i
h) .
2 h i=1
2
(4.184)

Similarly, after making λ =

h, conducting Taylor expansion for first three terms
and last term within the bracket of Eq. (4.184), and making corresponding terms
of the two equations relative to various powers of λ equal, the algorithm error is
minimum, and the coefficient equation C = A · G is derived.
It can be obtained by comparing Eq. (4.178) with (4.184) that matrix A = A

2 T
T
in the coefficient equation; column vector C = C1 + C2 · · · CN · · · , and
h
C1 , C2 , . . . CN , . . . are corresponding elements in vector C; G is column vector
consisting of correcting algorithm coefficient Gi to be solved. Algorithm coefficient

Gi subject to correction can be worked out by using first N rows of A and C .
In addition, derivation thought in this section may also be popularized and applied
to SINS adopting other forms of digital filter.
3. Experimental Verification
Small flexible POS model machine developed by this laboratory is used to verify
effect of the algorithm, where zero-bias stability of the gyro is about 0.1◦ /h, bias
stability of accelerometer is about 20 μg, data updating frequency set as 50 Hz,
and experimental verification conducted on three-axle table with swing function, as
shown in Fig. 4.19.
4.4 High Dynamic Strapdown Inertial Algorithm 139

1) Algorithm design
First, a single-subsample rotating vector attitude algorithm adopting current and
angular increments within previous three attitude updating cycles and called as three-
order algorithm is designed as per Eq. (4.176).

Φ̂ = θ + 0.1345( θ (1) × θ) − 0.031( θ (2) × θ) + 0.0036( θ (3) × θ).

(4.185)

Then, gyro output within the system is subject to digital lowpass filtering processing
to reduce random gyro drift, where the cut-off frequency of the filter is 20 Hz,
corresponding time constant T = 0.008s, and it can be obtained subject to correction
of above three-order algorithm according to Eq. (4.182) that:

Φ̂ = θ + 0.2779( θ (1) × θ) − 0.0696( θ (2) × θ) + 0.0082( θ (3) × θ)

(4.186)

2. Experimental scheme design

Install IMU of SINS on the surface of three-axle table. Conduct initial alignment
first, setting the alignment time at 5 min; and then make the three-axle table operate
under two-axle swing state, swinging, respectively, on the heading axis and pitch
axis, where the swing amplitude is 6◦ , swing frequency is 0.5 Hz, and time is 2
min. It can be known from theoretical analysis that under above operating condition,
conical motion generated on horizontal roller of the system will stimulate conical
error. Restricted by experimental condition, though it is not real high-dynamic envi-
ronment, it is also possible for assessment of strapdown attitude algorithm designed
in this chapter. Therefore, roll angle drift before and after swing is deemed to be the
precision index.
3. Experimental result
First, conduct attitude calculation for swing experimental data not subject to digital
filtering, respectively, by adopting quaternion algorithm, three-subsample rotat-
ing vector algorithm and three-order single-subsample rotating vector algorithm
designed in this chapter, and the experimental result is as shown in Fig. 4.20.
It is observed from Fig. 4.20 that precision of traditional quaternion algorithm is
the worst due to absence of conical error compensation when the gyro output has
not been subject to filtering, and the roll angle drift is 0.0859◦ ; but three-subsample
rotating vector algorithm and three-order single-subsample rotating vector algorithm
both conduct compensation for conical error, and the roll angle drifts are, respectively,
0.025 and 0.0244◦ , as shown in Fig. 4.20a,b. Roll angle drift declines obviously
subject to conical error compensation, and precision of three-order single-subsample
algorithm is the same as that of three-sample algorithm. It is also observed from
Fig. 4.10 that under the same sampling frequency condition, three-order single-
subsample algorithm updates the attitude for once after each sampling, but three-
sample algorithm updates the attitude for once after three samplings. Therefore,
140 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

Roll angle drift curve Roll angle drift curve

Quaternion algorithm Quaternion algorithm
Roll angle

Roll angle
Three-subsample algorithm
Three-order single-subsample algorithm

a Time t (s) b Time t (s)

Fig. 4.20 Roll angle drift curve

Roll angle drift curve

Quaternion algorithm
Roll angle

Three-order single-subsample algorithm

Corrected single-subsample algorithm

Time t (s)

Fig. 4.21 Roll angle drift curve (subject to filtering)

attitude updating frequency of the former is three times of the latter. Superiority of
algorithm in this chapter is thereby verified.
In addition, in accordance with Eq. (4.100) and under experimental condition
of this chapter, theoretical calculation value of conical error generated from system
calculation approximates to 0.0778◦ , which indicates that the experimental result is
basically consistent with theoretical calculation.
Conduct attitude calculation for experimental data subject to filtering respectively
by adopting quaternion algorithm, three-order single-subsample rotating vector al-
gorithm designed in this chapter and corrected algorithm, and the experimental result
is shown in Fig. 4.21.
It is observed from Fig. 4.21 that precision of quaternion algorithm is the worst
due to absence of conical error compensation filter influence consideration subject
to digital lowpass filtering to reduce random drift of the gyro, and the roll angle drift
changes to 0.144◦ ; three-order single-subsample rotating vector algorithm takes no
account of filter characteristics though it has conducted conical error compensation,
4.5 Chapter Conclusion 141

so the roll angle drift is increased compared with that not subject to filtering and
changes to 0.0833◦ . The corrected three-order single-subsample rotating vector al-
gorithm takes filter characteristics into account based on conical error compensation,
so the roll angle drift is only 0.0238◦ , equal to that not subject to filtering, which has
inhibited influences of digital filtering on effect of conical error compensation.
4. Conclusion
Multisubsample rotating vector attitude algorithm will reduce attitude updating
frequency of the system when directly applied to SINS, and change of amplitude-
frequency characteristic subject to filtering processing of gyro output will have
influence on compensation effect of conical error, so multisubsample rotating vector
attitude algorithm is hard to satisfy the requirements of high-dynamic environment
application for precision and data updating rate. Specific to above shortcomings, this
section introduces a single-subsample rotating vector attitude algorithm using cur-
rent and angular increments within previous N attitude updating cycles and deduces
correcting algorithm of single-subsample rotating vector attitude algorithm by com-
bining characteristics of gyro filter. The experimental result indicates that (1) when
the gyro sampling frequency is the same, single-subsample rotating vector attitude
algorithm introduced in this section may obtained attitude precision equivalent to
that of multisubsample rotating vector attitude algorithm without decline of attitude
updating frequency; (2) correcting algorithm of single-subsample rotating vector at-
titude algorithm derived in this section may also effectively inhibit the influence of
filter on the effect of conical error compensation subject to digital filtering processing
of gyro output.

4.5 Chapter Conclusion

As the main development direction of INS, SINS has become the most important au-
tonomous navigation equipment for various carriers in motion due to its advantages
such as complete autonomy, comprehensive motion information, high short-time
precision and low cost, etc. This chapter mainly introduces primary research achieve-
ments of the author in terms of strapdown inertial navigation technology in recent
years.
First, error modeling, measurement and compensation methods for inertial com-
ponents are introduced. With traditional rotor gyro as an example, gyro error model
and accelerometer error model under normal temperature condition are introduced.
Deep analysis is conducted specific to primary error parameters of low-precision
MEMS gyro—scale factor, and a test method for decoupling of misalignment angle
between the scale factor and input axis and a piecewise interpolation compensation
method of scale factor are introduced, which can effectively improve application
precision of MEMS gyro. However, influences of vibration, temperature, magnetic
field, pressure and other external environment on error of inertial components remain
142 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

to be further researched, especially for various new inertial components under rapid
development at present, such as optical fiber gyro, MEMS gyro, MOEMS gyro, etc.
After that, calibration method for inertial measurement unit error is introduced.
Traditional static calibration may calibrate all error coefficients of IMU, but precision
of calibration is low due to weak input excitation. Also called as rate calibration, dy-
namic calibration has strong input excitation, so it has high calibration, but it can only
calibrate error coefficients of gyro scale factor and installation error. Specific to this
problem, this chapter introduces an IMU calibration method with dynamic and static
feature, which integrates advantages of dynamic calibration and static calibration to
effectively improve calibration precision of IMU. Then, specific to calibration prob-
lem of low-precision MIMU, a six-position positive and negative calibration method
of MIMU is introduced to provide effective solution for calibration of low-precision
MIMU. However, contents introduced in this chapter are mainly calibration done
through conventional precise test equipment, and specific to certain SINS with high
overload application, test equipment such as centrifuge and rocket sled may also be
used for more precise calibration, which will not be introduced in detail here.
Strapdown algorithm, especially attitude algorithm of SINS is the main factor af-
fecting the precision of SINS under high-dynamic environmental condition. Limited
rotating non-commutativity of rigid body is a primary error source of SINS attitude
calculation. Therefore, new single-subsample rotating vector attitude algorithm and
conical compensation algorithm subject to angular rate input are introduced finally
in this chapter.
In conclusion, this chapter introduces modeling and calibration methods as well
as high-dynamic strapdown algorithm of SINS from three levels of component, part
and system to lay a theoretical foundation for research of subsequent chapters.

References

1. Stan SD, Balan R, Maties V (2008) Modeling, design and control of 3DOF medical parallel
robot. Mechanika 6(1):68–71
2. Li JL, Fang JC (2011) Fuzzy modeling and compensation of scale factor for MEMS gyroscope.
Mechanika 17(4):408–412
3. Liu Q (2002) Error analysis and compensation of strapdown inertial navigation system. J
Beijing Inst Technol 11(2):117–120
4. Davis WOD (2001) Mechanical analysis and design of vibratory micromachined gyroscopes.
Berkeley, University of California, pp 156–160
5. Fang JC, Li JL (2009) Integrated model and compensation of thermal errors of silicon
microelectromechanical gyroscope. IEEE Transact Instrum Meas 58(9):2923–2930
6. Mankame ND, Ananthasuresh GK (2001) Comprehensive thermal modeling and characteriza-
tion of an electro-thermal compliant microactuator. Micromech Microeng 11(2):452–462
7. Michael I, Ferguson A (2005) Effect of temperature on MEMS vibratory rate gyroscope.
Aerospace, 2005 IEEE Conference, 5–12 March 2005, pp 431–435
8. Kim K, Park CG (2007) Drift error analysis caused by RLG dither axis bending. Sens Actuators
A 133:425–430
9. Grandin J (2007)Aided inertial navigation field tests using an RLG IMU. School ofArchitecture
and the Built Environment Royal, Institute of Technology, Stockholm, pp 17–27
References 143

10. Fang JC, Yang S (2011) Study on innovation adaptive EKF for in-flight alignment of airborne
POS. IEEE Transact Instrum Meas 60(4):1378–1388
11. Mohamed MR, Mostafa JH (2001) Direct positioning and orientation systems, How do they
work? What is the attainable accuracy? American Society of photogrammetry and remote
sensing annual meeting. St. Louis, MO, USA, Apr. 2001, pp 1–11
12. Rowe CH, Schreiber UK, Cooper SJ (1999) Design and operation of a very large ring laser
gyroscope. Appl Opt 38(12):2516–2523
13. Stancic R, Graovac S (2010) The integration of strap-down INS and GPS based on adaptive
error damping. Robot Auton Syst 58(6):1117–1129
14. Zhang LD, Lian JX, Wu MP (2009) Research on auto compensation technique of strap-down in-
ertial navigation systems. Information in control, automation and robotics, Bangkok, Thailand,
Feb 2009, pp 350–353
15. Hall JJ, Williams RL (2000) Case study: inertial measurement unit calibration platform. J
Robot Syst 17(11):623–632
16. Lee JG, Prak CG (1993) Multiposition alignment of strapdown inertial navigation system.
IEEE Transactions Aerosp Electron Syst 29(4):1323–1328
17. Li JL, Fang JC, Ge SS (2013) Kinetics and design of a mechanically dithered ring laser
gyroscope position and orientation system. IEEE IEEE Transact Instrum Meas 62(1):210–220
18. Kim A, Golnaraghi MF (2004) Initial calibration of an inertial measurement unit using an
optical position tracking system, Position Location and Navigation Symposium, pp 96–101,
26–29 April 2004
19. Jiao F, Li JL (2012) An improved six-position hybrid calibration for RLG POS in full tempera-
ture. 8th international symposium on instrumentation and control technology, London, United
Kingdom, July 2012, pp 246–250
20. Chen XY, Shen C (2012) Study on error calibration of fiber optic gyroscope under intense
ambient temperature variation. Appl Opt 51(17):3755–3762
21. Rogers RM (2003) Applied mathematics in integrated navigation systems. American Institute
of Aeronautics and Astronautics, New York, pp 101–116
22. Joseph S, Joseph H (2004) Automated attitude sensor calibration: progress and plans.
AIAA/AAS Astrodynamics Specialist Conference and Exhibit, Providence, RI, United States,
Aug 2004, pp 268–281
23. Li JL, Jiao F, Fang JC, MaYH (2013) Integrated calibration method for dithered RLG POS using
a hybrid analytic/Kalman filter approach, IEEE Transact Instrum Meas 62(12):3333–3342
24. Wang XL, Guo LH (2009) An intelligentized and fast calibration method of SINS on moving
base for planed missiles. Aerosp Sci Technol 13:216–223
25. Tadej B, Janez P, Marko M (2012) Three-axial accelerometer calibration using Kalman filter
covariance matrix for online estimation of optimal sensor orientation. IEEE Transact Instrum
Meas 61(9):2501–2511
26. Naser ES, Hou HY, Niu XJ (2008) Analysis and modeling of inertial sensors using Allan
variance. IEEE Transact Instrum Meas 57(3):140–149
27. Pittelkau ME (2001) Kalman filtering for spacecraft system alignment calibration. J Guid
Control Dynamic 24(6):1105–1113
28. Wang XL (2009) Fast alignment and calibration algorithms for inertial navigation system.
Aerosp Sci Technol 13(4):204–209
29. Seo J, Lee JG, Park CG (2005) Lever arm compensation for integrated navigation system of
land vehicles. IEEE International Conference Control Applications, Toronto, ON, Canada Aug.
2005, pp 523–528
30. Grewal MS, Weill LR (2007) Global positioning system, inertial navigation, and integration.
Wiley, Hoboken, pp 255–278
31. Xu D, Chen YX, Kang R (2011) Study of accelerated stability test method for quartz flexible
accelerometer. IEEE Transact Instrum Meas 11(1):148–157
32. Li JL, Jiao F, Fang JC, Cheng JC (2014) Temperature error modeling of RLG based on neural
network optimized by PSO and regularization. IEEE Sens J 14(3):912–919
144 4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)

33. Hong WS, Lee KS, Paik BS (2008) The compensation method for thermal bias of ring laser
gyro. Proceedings of the 21st Annual Meeting of the IEEE Lasers and Electro-Optics Society.
2008, pp 723–724
34. Yang PX, QinYY,You JC (2010) Temperature compensation for RLG based on neural network.
Proceedings of the Sixth International Symposium on Precision Engineering Measurements
and Instrumentation, vol 7544, March 2010, pp 1–6
35. Moghtaderi A, Flandrin P, Borgnat P (2013) Trend filtering via empirical mode decompositions.
Comput Stat Data Anal 58(1):114–126
36. Liu HT, Ni ZW, Li JY (2007) Extracting trend of time series based on improved empirical mode
decomposition method. Proceedings of the 9th Asia-Pacific Web Conference on advances in
data and web management, June 2007, pp 341–349
37. Fan CL, Jin ZH, Tian WF, Qian F (2004) Temperature drift modeling of fiber optic gyroscopes
based on a grey radial basis neural network. Meas Sci Technol 15(1):119–126
38. Ticknor JL (2013) A Bayesian regularized artificial neural network for stock market forecasting.
Expert Syst Appl 15(4):5501–5506
39. Kumar P, Merchant SN, Desai UB (2004) Improving performance in pulse radar detection
using Bayesian regularization for neural network training. Digit Signal Process 14(7):438–448
40. Li JL, Fang JC, Du M (2012) Error analysis and Gyro biases calibration of analytic coarse
alignment for airborne POS. IEEE Transact Instrum Meas 61(11):3058–3064
41. Cheng JH, Wang XZ, Wu L (2008) Method of accurate gyro drift measurement for inertial
navigation system. IEEE international conference on mechatronics and automation, Takamatsu,
Japan, Aug.2008, pp 1–5
42. Arunasish A, Smita S, Ghoshal TK (2009) Improving self-alignment of strapdown INS using
measurement augmentation. 12th international conference on information fusion, Seattle, USA,
July 2009, pp 1783–1789
43. Chung D, Lee JG, Park CG (1996) Strapdown INS error model for multiposition alignment.
IEEE Transact Aerosp Electron Syst 32(4):1362–1366
44. Yu F, Ben YY, Li Q (2008) Optimal two-position alignment for strapdown inertial naviga-
tion system. International conference on intelligent computation technology and automation,
Hunan, China, Oct 2008, pp 158–164
45. Miller RB (1983) A new strapdown attitude algorithm. J Guid Control Dynamic 6(4):287–291
46. Jiang YF, Lin YP (1992) Improve strapdown coning algorithm. IEEE Transact Aerosp Electron
Syst 28(2):484–490
47. Park CG (1999) Formalized approach to obtaining optimal coefficients for coning algorithms.
J Guid Control Dynamic 22(1):165–168
48. Savage PG (2002) Analytical modeling of sensor quantization in strapdown inertial navigation
error equations. J Guid Control Dynamic 25(5):833–842
49. Mark JG, Tazartes DA (2001) Tuning of coning algorithms to gyro data frequency response
characteristics. J Guid Control Dynamic 24(4):641–647
Chapter 5
Star Map Processing Algorithm of Star Sensor
and Autonomous Celestial Navigation

5.1 Introduction

Celestial navigation is a kind of fully autonomous navigation method. It has the

following characteristics: (1) no additional equipment, except the standard attitude
sensors such as star sensor and earth sensor, is needed; (2) both orbit information and
attitude data can be provided; and (3) self-contained, nonradiating. So, it has broad
application in satellites, airships, deep space explorers, etc. The method is based on
information about a celestial body (Sun, Earth, Moon, and stars) in an inertial frame
at a certain time. With the increasing development of autonomous celestial navigation
technology, the method of using celestial sensors to determinate spacecraft attitude
has been widely studied. Currently, star sensors are widely used to realize high-
precision spacecraft navigation. In this Chapter, star map preprocessing method of
star sensor is studied first, which includes processing method for fuzzy star maps and
distortion correction method for star maps, providing high-quality star maps for star
map matching; secondly, an effective star map identification methods are introduced
to acquire effective starlight vector direction; at last, a start sensor-based celestial
navigation method is proposed.

5.2 Star Map Preprocessing Method for Star Sensors

During spacecraft autonomous celestial navigation based on star sensors, star map
preprocessing is the premise of high-efficiency identification of star maps. Due to
system noise, stochastic noise, vibration, and lens distortion of a star sensor, the
image from the sensing head of a sensor is a two-dimensional gray blurred image
polluted and distorted by various noises. Images of a star map must be subjected
to preprocessing to implement high-efficiency star map identification, i.e., a fuzzy
image must be restored and a distorted image must be corrected. This section mainly
focuses on a method of blurred star image processing for star sensors under dynamic
conditions.

© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 145
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_5
146 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

The precision of star point location is significant in identifying the star map and
acquiring the aircraft attitude for star sensors. Under dynamic conditions, star images
are not only corrupted by various noises but also blurred due to the angular rate of the
star sensor. According to different angular rates under dynamic conditions, a novel
method is proposed in this article, which includes a de-noising method based on
adaptive wavelet threshold and a restoration method based on a large angular rate.
The adaptive threshold is adopted for de-noising the star image when the angular rate
is in the dynamic range. Then, the mathematical model of motion blur is deduced
so as to restore the blurred star map due to a large angular rate. Simulation results
validate the effectiveness of the proposed method, which is suitable for blurred star
image processing and practical for attitude determination of satellites under dynamic
conditions.

5.2.1 Problem Statements

1. Coordinate Frames
In developing a set of formulas to be mechanized by a celestial navigation system
and star sensors or in studying the behavior of a given system, it is necessary to
introduce several sets of orthogonal coordinates:
1. Inertial coordinate system (XL -YL -ZL ) has its origin at the center of the Earth
and is nonrotating with respect to the fixed stars. Its x-axis is in the equatorial
plane and the z-axis is normal to that plane; and the y-axis complements the
right-handed system.
2. Star sensor coordinate system (Xs -Ys -Zs ) has its origin at the center of mass of a
star sensor. Its x-axis points along the longitudinal axis of a star sensor; the z-axis
is perpendicular to the longitudinal plane of symmetry and is along the bore sight
of the star sensor; and the y-axis completes a right-handed system.
3. Focal plane coordinate system (Xp -Yp ) has its origin at the center of the focal
plane. Its x-axis points along the longitudinal axis of the focal plane; the y-axis
is perpendicular to the longitudinal plane.

2. Imaging Theory of Star Sensors

Figure 5.1 illustrates the general large field of view (FOV) star sensor for attitude
determination. After star images are captured in the sky by a star sensor, the attitude
information is completed by an autonomous procedure (including image pretreat-
ment, star centroiding, star map matching, attitude determination, etc.). A schematic
of imaging is also shown in Fig. 5.1, where f is the focal length of lens and α is the
angle of FOV.
According to the coordinates of the star points in the focal plane coordinate system
Xp -Yp , it is easy to obtain the coordinate matrix S of star points in the star sensor
coordinate system Xs -Ys -Zs . Combined with star coordinates G in the inertial coor-
dinate system XL -YL -ZL , attitude A can be determined as the form of 3 × 3 direct
5.2 Star Map Preprocessing Method for Star Sensors 147

Zp

boresight
Lens
hood

Real sky

lens
α
f Yp

Autonomous Xp
Procedure Image plane

Fig. 5.1 Large FOV star sensor for attitude determination (left) and imaging schematic (right)

cosine matrix by:

A = G−1 S (5.1)

3. Characteristics of a Star Image Under Dynamic Conditions

Under static conditions, the distribution of star points is generally represented as a
two-dimensional Gaussian with a 3 × 3 or 5 × 5 dispersion circle by defocusing
technology [1], so that the accuracy of star centroid can be kept within a subpixel
level. However, under dynamic conditions, the original star image is perturbed and
blurred by various additive noises, which mainly include photon response uniform
noise, photon shot noise, dark current noise, readout noise, etc. [2].
At present, the dynamic range of a large FOV star sensor is about 3–5◦ /s [3].
Suppose the angular rate is w. If w ranges by 1–1.5◦ /s under dynamic condition, the
rotation of the star sensor has little effect on the star images. However, a star sensor
may lose tracking using the technique under static conditions due to the various
noises caused by the dynamic conditions. On the other hand, if w is larger than the
dynamic range, the star point constantly shifts in the focal plane and appears to trail
badly during exposure time, which may affect the star centroid accuracy and even
result in the failure of attitude determination.
Based on the foregoing discussions, de-noising and de-blurring are two crucial
parts for the pretreatment of blurred star images. Performance parameters of the star
sensor used in this chapter are shown in Table 5.1. The CMOS image sensor chip is
the STAR 1000 from the Cypress Semiconductor Co. [4].
148 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

Table 5.1 Performance Performance parameters Value Unit

parameters of CMOS star
sensor [4] Sensitive area format 1024 × 1024 Pixels
◦
FOV 20 × 20
Pixel size 15 × 15 mm
Focal length 54.646 mm
Exposure time 200 ms
Radius of Gaussian PSF 1 σPSF
◦
Dynamic range 2 /s
Aperture 40 mm
SNR after defocus 5.5 –

5.2.2 Blurred Star Image De-noising

1. De-noising Modeling Based on Wavelet Transform

Supposing the size of the clear image f(i, j) is N × N, a common model of the
corrupted image g(i, j) is mathematically defined as:

g(i, j ) = f (i, j ) + n(i, j ), (5.2)

where 0 ≤ i, j ≤ N−1, and n(i, j) is additive random noise and independent of f (i, j).
The goal is to remove n(i, j) and estimate f (i, j), which minimizes the mean squared
error (MSE) [5].
In general, the important information of f (i, j) is mostly distributed as a smooth
signal at low frequency, while n(i, j) is distributed at high frequency. Based on this,
a two-dimensional (2-D) discrete wavelet transform (DWT) can be implemented
to transform g(i, j) into the wavelet domain. Then, wavelet coefficients denoting
different scales and orientations can be obtained by the Mallet algorithm [6].
Figure 5.2 shows the subbands of the orthogonal DWT of three levels. LL 3 is an ap-
proximation subband (or the resolution residual), which contains the low-frequency
portion of g(i, j). The subbands HL k , LH k , HH k (k = 1, 2, 3), respectively, denote
the details of vertical, horizontal, and diagonal orientations, where the scale k and
size of a subband at scale k is N/2i × N/2i . There is no space here to go into detail
on this method, and for this level of detail the reader can refer to [5, 7] for more
information. It is important, however, to point out the small coefficients mostly due
to noise and large coefficients due to important signals. Accordingly, de-noising can
be accomplished by thresholding these coefficients.
2. Threshold Selection
Thresholding is simple because it operates on one wavelet coefficient at a time.
The method of using an adaptive threshold to implement de-noising described by
5.2 Star Map Preprocessing Method for Star Sensors 149

Fig. 5.2 Subbands of the 2-D

orthogonal wavelet transform LL3 HL3
HL2
LH3 HH3
HL1

LH2 HH2

LH1 HH1

Lakhwinder Kaur et al. [8] appears more suitable, in which threshold choice is:

βσ 2
TN = , (5.3)
σy
where σy is the standard deviation of each subband, and β is the scale parameter for
each scale computed by:
-
Lk
β = log , (5.4)
J
where J is the largest scale and Lk is the length of subband at the scale of k (k = 1,
2. . . J). The noise variance σ 2 is obtained by:

σ 2 = Median(|H H1 |)/0.6745. (5.5)

Studies in [9] indicate that the square error relating to HH 1 of g(i, j) almost equals the
noise variance σ 2 . On the other hand, the larger the decomposition level is, the smaller
the weight of noise in the coefficient variance will be. For this reason, it is more
convenient to complete de-noising for the star images than general images because
the contrast between star signal on a black background and noise is more distinct,
even when the star point is blurred under dynamic condition. We pay attention to
TN in scale J, where has the approximation of star points. This proposed method
processes each coefficient in scale J using a different threshold. It can be executed
mainly by the following steps:
1. Apply an M × M local window to compute σ 2 lJ , which denotes the coefficients
variance of window l in scale J. M is determined by the square root of the number
of pixels occupied by the star point, and generally is not more than seven.
2. Compute noise variance σ 2 according to formula (5.5).
150 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

3. Obtain the threshold by:

σ2
Thl = β , (5.6)
σlJ2
where Thl is the threshold in window l of scale J.
3. Star Image De-noising
Based on the foregoing analysis, the proposed method for star image de-noising is
summarized as follows:
1. Execute decomposition of the initial blurred star image using a wavelet transform
at level K.
2. Compute the noise variance σ 2 according to formula (5.5).
3. Compute the scale parameter β of level K using formula (5.4).
4. Use a 4 × 4 square window in LLK to obtain Thl by formula (5.6).
5. Process coefficients in scale K using the following threshold function:

sgn (x) (|x| − Thl ), |x| ≥ Thl
x̂ = , (5.7)
0 , |x| ≤ Thl
which keeps the coefficient information if it is larger than threshold; otherwise, it is
set to zero.
6. Invert the multiscale decomposition to reconstruct the de-noised star image.

5.2.3 Blurred Star Image Restoration

The current angular rate w of satellite can be obtained by the attitude update. As
mentioned in Sect. 2, if w is larger than the dynamic range, the attitude cannot be
correctly computed because of the “trailed” image. This section mainly focuses on
the restoration of motion-blurred star image as a result of large w.
1. Mathematical Model of Blurred Star Image
Due to the motion of the star sensor during exposure time, what a star sensor captures
is a motion-blurred image g(x, y). Suppose, a clear image f (x, y) moves on the focal
plane, its displacement components of direction x and y can be, respectively, termed
as x(t) and y(t), where t is the movement time during exposure time T. Then, the
expression of g(x, y) can be obtained from formula (5.8), and the expression of point
spread function (PSF) in frequency domain can be obtained from formula (5.9),
which is similar to a previous reference [10]:
5 T
g(x, y) = f (x − x(t), y − y(t))dt, (5.8)
0
5 T
e−
j 2π[ux(t)+vy(t)]
H (u, v) = dt. (5.9)
0
5.2 Star Map Preprocessing Method for Star Sensors 151

Fig. 5.3 Procedure of star

image motion-blurred (Where
Xp -Y p is the coordinate
system of focal plane,
Xp -Y p is the corresponding
coordinate of Xp -Y p after
rotation, and θ = wz t, where t
is the rotation time in T. The
star point P shifts to P along
the circular arc s with radius r
in the focal plane. It is
reasonable to assume that the
rotation axis and the motion
parameters are constant, and l
is approximate to s.)

If the satellite rotates clockwise about the boresight Zp with an angular rate wz during
exposure time T, it seems that the star point displaces anticlockwise on focal plane,
which results in the trail effect. The motion-blurred procedure can be illustrated in
Fig. 5.3.
As a result l can be expressed as:

l = rwz t, (5.10)

where r = xp2 + yp2 and (xp , yp ) are the coordinates of P in the coordinate system
Xp -Yp .
Suppose P moves along track l with angle γ to the horizontal axis Xp by velocity
v, where v = wz r and γ can be obtained by θ , xp , and yp . Combined with formula
(5.8), the PSF of motion-blurred star image can be obtained by:
T sin (πul cos γ ) −j π lu cos γ
H (u, v) = e
πul cos γ
T sin (πvl sin γ ) −j π lv sin γ
+ e . (5.11)
πvl sin γ
Then, the expression of H(u, v) in the time domain is:

1/ l 0 ≤ x ≤ l cos γ , y = x tan γ
h(x, y) = . (5.12)
0 else

2. Blurred Star Image Restoration

To accomplish restoration of the original image, the traditional method is to employ
Wiener filtering in the frequency domain [7]. The Wiener filter is intended to be an
optimal filter in the sense that it delivers the best estimate of the object in a least-
squares sense for additive Gaussian noise. However, the noise n(x, y) is typically
152 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

unknown in practice and the classical Wiener filter is problematic [11]. Therefore,
we use the modified Wiener filter which is given by:

H ∗ (u, v)
F̂ (u, v) = G(u, v), (5.13)
H (u, v)H ∗ (u, v) + a
where H ∗ (u, v) denotes the complex conjugate of H(u, v) and a can be considered
as an adjustable empirical parameter chosen to balance sharpness against noise.
In order to overcome the edge error, a major factor affects the quality in Wiener
filter restoration and the optimal window method is used for star image [12]. Then
the steps of restoration based on Wiener filtering are detailed as follows:
Introduce h(x, y) according to analysis in Sect. 4.1.
Apply the optimal window w(x, y) as a weight factor to g(x, y), then execute the
discrete Fourier transform (DFT) of g(x, y) and h(x, y).
Use the Wiener filter for deconvolution filtering in the frequency domain, and
obtain the estimate of F(u, v) by formula (5.13).
Compute the Inverse DFT (IDFT) of f(x, y) to generate f (x, y) by:
5 +∞ 5 +∞
f (x, y) = F (u, v)ej 2π(ux+vy) du dv . (5.14)
−∞ −∞

5.2.4 Results and Analysis

In order to verify the proposed method when a star sensor works under dynamic
conditions, simulations and experiments are implemented to accomplish de-noising
and restoration according to blurred star images caused by different w. Comparison
of power signal-to-noise ratio (PSNR) and the star centroid are also analyzed to
estimate the effect of algorithm in this section.
1. De-noising of Blurred Star Image
Based on the performance of the star sensor shown in Table 5.1, the SkyMap star
map simulation software is used to generate the original star image, as shown in
Fig. 5.4. The boresight direction is set as (150◦ , 15◦ ) and the 14,581 stars brighter
than 6.95 m are selected in Tycho2n star catalog.
The experiments are conducted on several blurred star images at different noise
levels σ = 70, 80, 90 and different angular rates w under the dynamic range. For the
wavelet transform, four levels of decomposition are used, and the wavelet employed
is sym8 (from the MATLAB wavelet toolbox).
To assess the performance of the de-noising method proposed in this chapter,
it is compared with several common de-noising techniques such as BayesShrink
[5], SureShrink, and Lowpass filter. The fixed threshold Th is used first to segment
the background and the star object. Based on Th, different de-noising methods are
employed to estimate the original clear star image. Figure 5.5 shows the noisy image
and resulting images at σ = 90 and w = 0.6◦ /s. We can see that the image processed
5.2 Star Map Preprocessing Method for Star Sensors 153

Fig. 5.4 The original star

image by simulation

by the proposed method outperforms the others in terms of visual quality. Then, the
PSNRs from various methods are compared in Table 5.2, and the data are collected
from an average of four runs. The AdaptThr method, namely, is the proposed adaptive
thresholding method.
Results in Table 5.2 show that the lower w is, the better AdaptThr performs than
other methods, especially when σ is large. AdaptThr approximately has the same
poor effect of de-noising along with the increase of w. Actually, in dynamic condition
with high w, star image is not only perturbed by various noises, but is also blurred
by the motion of star sensor.
This also means that by only using the proposed de-noising method under dynamic
conditions with high w, one cannot obtain the star centroid accurately. One also
needs to restore the motion-blurred image. To further verify the proposed de-noising
algorithm, a real star image is adopted in this section. Figure 5.6 shows the original
star image obtained by a star sensor and its gray distribution, from which we can
see that the background value in the star image is large. What’s more, there is a big“
“singularity spot,” which is larger and lighter than other star points. After discarding
the singularity spot, a clear star image can be obtained as shown in Fig. 5.7. It can be
seen that the dim star object is extracted perfectly from the background noise. This
confirms the notable effect of the proposed method, which can adapt to the complex
dynamic conditions.
2. Restoration of Blurred Star Image
Table 5.2 shows that if w is larger than the dynamic range, star image needs to be
restored by deblurring rather than by de-noising directly. Figure 5.8a is a real star
image selected from the original images obtained by the CMOS star sensor in this
chapter. Based on the supposition w = 10◦ /s, the blurred star image can be generated
according to the degradation model, as shown in Fig. 5.8b.
154 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

Fig. 5.5 Comparison of noisy image and resulting images. a Noisy image at σ = 90, w = 0.6◦ /s.
b Resulting image with fixed threshold. c Resulting image with BayesShrink. d Resulting image
with SureShrink. e Resulting image with Lowpass filter. f Resulting image with AdaptThr

Table 5.2 Comparison of PSNR for each method with various σ and w
Method Noise Fixed BayesShrink SureShrink Lowpass AdaptThr
image threshold filter
(Th = 128)
w = 0.1 σ = 70 22.15 25.27 28.56 28.89 30.15 35.20
σ = 80 17.57 24.78 27.78 27.15 28.53 34.03
σ = 90 15.20 24.42 27.43 26.45 27.85 32.56
w=1 σ = 70 17.52 24.52 26.15 26.53 26.34 32.79
σ = 80 16.13 23.68 25.42 24.78 25.89 32.21
σ = 90 14.00 23.06 24.77 24.02 25.09 30.04
w=5 σ = 70 15.73 20.79 20.46 20.71 21.16 22.23
σ = 80 14.08 20.23 18.58 19.33 19.54 20.56
σ = 90 12.42 17.01 17.17 17.07 18.12 18.16

Gray distributions of the same star point in two different images of Fig. 5.8 are
shown in Fig. 5.9, which show that due to the motion blur, the star point smears out
intensely and the gray value decreases.
We implement star centroiding to assess the performance of the proposed deblur-
ring method. The comparison results are shown in Table 5.3, where the angular rate
is 10◦ /s.
5.2 Star Map Preprocessing Method for Star Sensors 155

Fig. 5.6 The original real star image and its gray distribution
156 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

Fig. 5.7 Star image and its gray distribution after discarding singularity spot
5.2 Star Map Preprocessing Method for Star Sensors 157

Fig. 5.8 Star images. a Real star image. b Blurred star image

100

100 80

60

40
50
20

0
0 0
0
5 10

10 15 20
15 10
30
20 5 15
10
25 40 5
a b
Fig. 5.9 Gray distribution of the same star point. a Gray distribution of original star point. b Gray
distribution of blurred star point

In Table 5.3 it is shown that the extraction errors of δx and δy are mostly larger
than a pixel for each star centroid without deblurring. This is because the SNR of star
image decreases as a result of the star points smearing significantly. Moreover, six
star points fail to be extracted due to the low gray value of dim blurred star points (as
shown in Fig. 5.9b), which may affect the star recognition and attitude determination.
However, after restoration in advance, the star centroid can be obtained accurately
in that the extraction errors of δx and δy are within subpixel range and the dim star
points with low gray value are extracted. As can be seen in Fig. 5.10, the extraction
error of δx + δy error is larger than three pixels. With the restoration method, all lost
star points can be extracted and the extraction error of δx + δy is restricted within
one pixel. This is because the proposed deblurring method can keep the accuracy of
δx and δy within subpixel levels, even when w is larger than the dynamic range, and
the variation of angular rate w has little effect on the star centroid.
158 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

Table 5.3 Comparison of star centroiding against blurred star image

Index of star point Ideal coordinates Coordinates of blurred Coordinates of
(pixel) star image (pixel) restored star image
(pixel)
1 (1.4757, 17.4887) (2.5754, 15.0621) (1.6321, 17.7413)
2 (61.1800, 247.9956) (60.3000, 247.2000) (61.0166, 247.6088)
3 (66.3157, 72.2644) (65.5620, 71.5855) (66.5137, 72.4756)
4 (67.0611, 199.5018) (66.2736, 198.5205) (67.1176, 199.8830)
5 (106.2782, 235.2798) (105.0847, 234.3384) (106.5062, 235.5103)
6 (134.6371, 68.7471) (133.4000, 68.0637) (134.9628, 69.0000)
7 (150.0000, 218.0000) Fail (150.4107, 218.5203)
8 (200.5139, 20.3319) Fail (200.8509, 20.9140)
9 (203.0953, 56.8732) (201.6690, 56.0245) (203.2726, 56.9546)
10 (201.2458, 251.7475) Fail (200.8412, 251.3327)
11 (223.0692, 113.0331) (222.3383, 112.0692) (223.1885, 113.1873)
12 (231.4061, 10.9092) Fail (231.8121, 11.4121)
13 (236.5062, 5.5021) (235.1504, 7.4118) (236.2813, 5.7401)
14 (244.0000, 58.0000) Fail (244.5132, 58.6318)
15 (269.2561, 139.0000) (268.2537, 137.5502) (269.3864, 139.1944)
16 (271.5072, 114.7826) (270.5577, 113.3748) (271.6662, 114.8652)
17 (299.0000, 145.0000) Fail (299.5125, 145.4218)

5.2.5 Conclusions

In this section research about how to process blurred star images according to dif-
ferent angular rates of star sensors under dynamic conditions is elaborated. A new
de-noising method based on adaptive wavelet threshold is proposed along with a
restoration method according to large angular rate out of the dynamic range. Be-
sides, experiments on different types of star images have been conducted with the
proposed algorithm.
The PSNRs of images with different types of angular velocity show the proposed
de-noising method; in comparison with the normal de-noising methods, they have
good performance, namely, better PSNRs than other methods under the same condi-
tions when the angular velocity is in the dynamic range and in terms of visual quality.
Additionally, star centroiding against blurred star images have been analyzed to as-
sess the effectiveness of restoration. It is confirmed that the restoration maintains
the extraction error within subpixel levels, and the variation of angular velocity has
little effect on the accuracy of star centroid, which shows that the proposed method
is both effective and feasible. Experimental results show that the processing method
5.3 Star Map Identification Method of Star Sensor 159

4
before(w=5)
before(w=10)
3.5
before(w=20)
after(w=5)
3 after(w=10)
Extract error of x+ y (pixel)

after(w=20)
2.5

1.5

0.5

0
0 2 4 6 8 10 12 14 16 18
Star point

Fig. 5.10 Extraction error of star centroid with different w

according to angular velocity in before/after using the restoration method with dif-
ferent angular velocity are analyzed, and star points which can be extracted in each
method are also shown. Without restoration, the larger the angular velocity is, the
more star points cannot be extracted while the extraction under dynamic conditions
reported in this chapter could keep star sensors stable within a certain range and meet
the requirements of attitude determination, which needs uninterrupted output data
and attitude accuracy of arcsecond level.

5.3 Star Map Identification Method of Star Sensor

Star map identification is the core algorithm of star sensor and is the key to accomplish
starlight vector acquisition and attitude measurement. Presently, certain deficiencies
exist in the sky coverage, database size, noise stability, matching speed, feature di-
mension, and other aspects of sensitive star map matching and identification. What’s
more, redundant matching and mismatching will lead to significant reduction of iden-
tification success rate in case of great measurement error. A star recognition method
based on the adaptive ant colony (AAC) algorithm for star sensors is presented.
A new star recognition method based on the AAC algorithm has been developed
to increase the star recognition speed and success rate for star sensors. This method
draws circles, with the center of each one being a bright star point and the radius
160 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

being a special angular distance, and uses the parallel processing ability of the AAC
algorithm to calculate the angular distance between any pair of star points in the
circle. The angular distance of two star points in the circle is solved as the path of the
AAC algorithm, and the path optimization feature of the AAC is employed to search
for the optimal (shortest) path in the circle. This optimal path is used to recognize the
stellar map and enhance the recognition success rate and speed. The experimental
results show that when the position error is about 50", the identification success rate
of this method is 98 %, while with the Delaunay identification method it is only 94 %.
The identification time of this method is up to 50 ms.

5.3.1 Introduction

In order to supply precise attitude for control systems, almost all spacecraft need
to obtain the attitudes. There are several sensors to determine the attitude relative
to reference objects. Star sensors are the most effective among them, acquiring
the attitude information by star map-processing methods and attitude-determining
algorithms.
An autonomous star recognition method is one of the core technologies of space-
craft attitude measurements with a star sensor. According to the original star map
data obtained by the star sensor, the identification method transforms, transfers, or
combines the star points, which are included in a star map, and comes up with charac-
teristic information that reflects this star map as far as possible. Then, the information
is compared with the Guidance-star database to complete the identification of the
star map. The identification method must be able to achieve the rapid acquisition
of spacecraft attitude as well as rapid attitude reconstruction. Therefore, the speed
and success rate of identification are the key factors for judging the performance of
identification algorithms.
There are many popular star map recognition methods that can be divided into
three groups: the graph theory-based, the primary star-based and the intelligence-
based. For example, the triangular [13–14], the quadrilateral [15] and the Delaunay
triangulation [16] fall into the first group. Planar triangles [17], grids [18], iden-
tification method based on constellations [19], statistical characteristics [20], the
Hausdorff distance [21], and the pyramid algorithm [22] fall into the second group.
Finally, the star pattern recognition method based on neural network [23] and ge-
netic algorithm [24] falls into the third group. These algorithms have their respective
merits in speed recognition, recognition of success rate, sky coverage rate, database
size, anti-noise performance, and stability. But in the event of a large FOV, which is
no less than 20◦ × 20◦ , and high sensitivity, recognition speed and success rate are
two outstanding problems for star recognition methods.
The ant algorithm (AA) was first proposed by Italian scholar M. Dorigo in 1991.
It is an optimization algorithm for simulating the foraging behavior of ant groups
[25]. The AA uses parallel self-catalytic mechanism of positive feedback and has
strong robustness, good distributed computing capacity, and quick optimal (shortest)
5.3 Star Map Identification Method of Star Sensor 161

path searching ability. In recent years, in order to improve the performance of the
ant colony algorithm, some improved ant colony algorithms have been presented by
other scholars [26–32]. Among them, the AAC algorithm not only has the ability of
a global search but can also effectively restrain local convergence and prematurity
[28, 31]. It has been widely used in solving the vehicle-routing problem, cluster
analysis, image processing, data mining, track layout optimization, and planning.
The AAC algorithm has been first introduced into star identification in this chapter
and successful results have been achieved.

5.3.2 Star Recognition Method Based on AAC Algorithm

To address the problem of many star points caused by a large FOV and the high
sensitivity of the star sensor, an AAC algorithm is introduced into star recognition,
which has the parallel processing capabilities and features of fast path optimization.
First, the algorithm calculates the average gray value of the set of star points and
chooses the star points that meet the average gray value to form a new set of star
points. Many circles are drawn that are centered on each star point in this set and the
radius is set to a special angular distance, which is less than half of FOV. Then a set
including all star points in every circle is composed. The angular distance between
two star points in each set is calculated and marked as the path of ants. There is a
unique and shortest path that passes through all star points in every set. If the AAC
parameters can be properly selected and the size of star points is appropriate, using
the fast path optimization capability of the AAC algorithm, the shortest (optimal)
path can be found rapidly. In the same way, the Guidance-star database can be
constructed by AAC algorithm. Finally, a star pattern recognition can be quickly
done by matching the shortest path between a star point set and the Guidance-star
database, which stores many of the shortest paths optimized by the AAC algorithm
previously and has small, non-redundant capacity. The experimental results show
that successful results can be achieved. So, this method is fast and robust, has a high
recognition success rate, and only needs a small database, which is better than the
star recognition based on the Delaunay cutting algorithm and the improved triangle
identification.
1. The Principle of the AAC Algorithm
Scientists have found that although there is no visual sense in an ant, an optimal
path is found by the pheromone, which is released by an ant during movement [32].
As social insects, the ants in a colony transfer information to each other through
pheromones and cooperate with each other to complete complex tasks. An ant colony
has good learning and has the capability to adapt to environmental changes [33, 34].
The theory of a specific ant colony algorithm can be described as follows:
Suppose G is the set of food sources found by the entire ant colony, G = {g|1,
2,. . ., M}. S is the ant set, N is the number of ants, S = {s|1, 2,. . ., N}. gij denotes
the distance from food source i to food source j, namely the distance of the path
162 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

(i, j), where i, j ∈ G. Ci (t) denotes the number of ants at time t at the food source i,
M
N= Ci (t). (5.15)
i=1

τij (t) denotes the amount of pheromone on the path (i, j) at time t. The amount of
pheromone at every path is equal at initial time, set τij (0) is a constant. The table
uk (k = 1, 2,. . ., N) is used to store the visited food source. During the movement
of the ant k, the ant chooses a path according to the transition probability, which is
calculated by using the amount of pheromone and heuristic information of the ant
path; pijk (t) denotes the transition probability of the ant from food source i to food
source j at time t.
⎧
⎪
⎪ [τij (t)]α [ηij (t)]β
⎪
⎨ ! , j ∈ dk
[τis (t)]α [ηis (t)]β
pij (t) = s ∈ dk
k
,
⎪
⎪
⎪
⎩
0, j∈/ dk (5.16)

where dk = {G−uk } indicates the food sources for the ant k to choose from. α and
β are two constants that, respectively, define the influence of heuristic component
and the trail information on the decision of ants [15]. ηij (t) is the heuristic function;
ηij (t) = 1/gij indicates the expected time that the ant takes to go from food source
i to food source j. With the movement of the ant colony, the pheromone released
previously will gradually desalt. After time T, all ants will complete one circulation,
and the amount of pheromone will be updated by the following formula on the path
(i, j) at time t + T :

τij (t + T ) = (1 − ρ)τij (t) + τij (t), (5.17)

N
τij (t) = τijk (t), (5.18)
k=1

where ρ is the information volatile factor. τij (t) is the increment of pheromone on
the path (i, j) during this circulation, τij (0) = 0 at initial time. τijk (t) is the number
of pheromones released by the ant k on the path (i, j) in this circulation [29].
⎧
⎪
⎨ λk Q , T
τij (t) =
k Lk
⎪
⎩ 0, not T
(5.19)

(Lavg − Lk )/(Lavg − Lopt ), Lk < Lavg
λk = ,
0, Lk ≥ Lavg
N
1
Lavg = Lk , Lopt = min (Lk ), (5.20)
N k=1
k=1,2,...,N
5.3 Star Map Identification Method of Star Sensor 163

where Q is the extent of pheromone, the default is a constant, which affects the
rapidity of convergence of algorithm at the extent. Lk is the total length that the ant k
travels in this circulation. T denotes the ant k passes the path (i, j) in this circulation.
2. The Construction of the Guidance-Star Database Based on the AAC
Algorithm
The Guidance-star database is a unique basis for star map identification of a star
sensor. The speed of identification depends to some extent on the capability and
searching strategy of the Guidance-star database in addition to the processor. The
Guidance-star database stores the data of star point recognition and star declination,
right ascension, star magnitude, etc. It is commonly loaded in the memory of a
star sensor. The star declination, right ascension, and star magnitude are generally
obtained from the basic star catalog, which is downloadable. The data of star point
recognition is closely related to the star recognition algorithm.
Different star recognition methods can construct variant Guidance-star databases,
especially the part that is used for star point recognition, determining the storage
capacity of the Guidance-star database. Aiming at the star recognition method based
on the AAC algorithm, the construction of the Guidance-star database is as follows:
1. Select guidance stars. In order to guarantee existing guidance stars in random
FOV, the screening of all stars is carried out. According to the spectral response
curve of sensitive chip, optical lens and sensitivity of the star sensor, some stars
will be selected from the basic star catalog to cover the overall celestial sphere,
as far as possible [35].
2. For any guidance star i, a circle is drawn that is centered on the guidance star i,
with radius r, which is used to guarantee at least three stars in the circle for the
Guidance-star database construction based on AAC algorithm. If the guidance
star j meets the formula (5.20), it will be put into the star point set.


ī · j¯
r > θ = arccos , (5.21)
|ī||j¯|

where θ is the angular distance between stars ī and j¯ in the celestial coordinate
system, arccos means anti-cosine function, ī · j¯ means the dot product between ī and
j¯.
3. For the stars in each star point set, the angular distance between any pair of star
points in the star sets is calculated and the star point closest to the center of the
star point set as the starting point is taken. Then the optimal (shortest) path of this
star point set is retrieved with the AAC. Suppose the star point P is the starting
point, then the path in Fig. 5.11 shows the optimal path.
4. At last, the angular distance values of the path optimization for each star point
set are stored in an ascending-order database as a record. At the same time, the
coordinate information of the stars is stored as well. Finally, the Guidance-star
database is constructed. Under the condition of guaranteeing a successful rate
of identification, and referring to the triangle identification algorithm (which
164 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

A D
Guidance
stars

Other
stars
r
P

B C
Fig. 5.11 Schematic plan of the optimal path from path optimization using the AAC

only uses three angular distance paths), and also considering the memory load
of the Guidance-star database, only two angular distance paths and three star
points’ sequence are involved. The Guidance-star database will be constructed
by this method, without redundant data and with a smaller capacity than the
triangle identification method, which needs some redundant triangles. Because
this database stores angular distances in an ascending order, the quick look-up
algorithm and the binary search algorithm can be used to further enhance the
identification speed.
The part of information stored in the Guidance-star database constructed by this
method is shown in Table 5.4.
3. Star Recognition Based on the AAC Algorithm
The AAC algorithm has the ability to search globally and effectively restrains the
local convergence and parallel processing ability, etc. If the star recognition method
based on the AAC algorithm meets the following precondition, it can produce good
results.
1) The precondition of recognition algorithm
This recognition algorithm is suitable for the star recognition for a star sensor with
a large FOV, which is no less than 20 × 20◦ and is highly sensitive, with no less
than 6.90. The precondition of recognition algorithm is that the highest magnitude
of star detected by the star sensor should be no more than the highest instrumental
magnitude of the star in the Guidance-star database.
5.3 Star Map Identification Method of Star Sensor 165

Table 5.4 Part of data stored in the Guidance-star database

Serial Angular Angular Star point 1 Star point 2 Star point 3
number distance 1 distance 2
Degree/◦ (right ascension/◦ , declination/◦ )
. . . . . .
. . . . . .
. . . . . .
2558 1.9001 1.9385 (185.55, 24.774) (187.23, 25.913) (185.07, 26.002)
2559 1.9009 2.0574 (185.08, 26.620) (187.16, 26.227) (185.55, 24.774)
2560 1.9013 2.1628 (187.19, 25.899) (185.07, 26.002) (184.87, 28.157)
. . . . . .
. . . . . .
. . . . . .

2) Using the adaptive ant colony algorithm to identify a star map quickly
The most important reason for applying this recognition algorithm based on the
AAC algorithm to recognize a star map is that each star map has a feature—except
for uncertainty factors such as background flashes from satellites and space junks,
cosmic ray hits, etc. Every star map should be a subset of the all-sky star map.
There is a star map IMG, which includes Q light points. The point set S is composed
of the Q light points, S = {s1 , s2 , . . ., sQ}. Using the AAC algorithm to identify the
star map IMG, the steps are as follows:
1. The average gray value Davg of point set S is calculated. Suppose the centroid
position of element si is (xi, yi ), whose pixel value is Vx i yi , the intensity Dsi of
element si is calculated by the following formula. Here, a circle average is taken
for the star intensity of which the radius is three pixels.
xi +2 yi +2
Dsi = Vpq +Vxi−3,yi + Vxi,yi−3 + Vxi+3,yi + Vxi,yi+3 .
p=xi −2 q=yi −2

So, the Davg is

Q
1
Davg = · Dsj . (5.22)
Q j =1

Selecting the light points whose value D and angular distance φ between itself and
the center point of the star map meet the following formula (5.20) to form the light
point set T, T = {t1 , t2 , . . ., tR }, R is the size of set T.


 6 F ov
D > Davg φ+r ≤ . (5.23)
2
166 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

Fig. 5.12 The results after steps (1) and (2) of the adaptive ant colony algorithm used to identify a
star map

Here, Fov is the FOV of the star sensor.

2. Centering on every light point of the point set T, a circle is drawn with a radius
equal to angular distance r. Figure 5.12 shows the results of steps (1) and (2) for a
star map. There are four light points (P0 , P1 , P2 , and P3 ), which meet the formula
(5.20).
3. Select a light point (P0 ), which is nearest to the center of the star map from P0 ,
P1 , P2 , and P3 . There is a circle drawn with the center as light point P0 and the
radius equal to special angular distance r. The light points that belong to the circle
form the light point set M, and the angular distance between all light point pairs
is found as M. Then the path optimization of M is accomplished by the AAC.
Figure 5.13 is the result of optimization.
4. The angular distance values of P0 P01 and P01 P02 are stored and compared, re-
spectively, with angular distance d1 and angular distance d2 of the Guidance-star
database. If the formula (5.24) is true, the result of identification is successful.

(P0 P01 − d1 )2 + (P01 P02 − d2 )2 < θth , (5.24)
where θth is the minimal threshold of identification, the default value is the mean
square deviation of system error.
Otherwise, the light point P1 is selected and a circle is drawn with the center as
the light point P1 and the radius being the angular distance r. Then steps (3) and (4)
5.3 Star Map Identification Method of Star Sensor 167

Fig. 5.13 The Optimal route map of light points in the circle by the AAC

are repeated. If the light points P2 and P3 do not meet the Formula (5.24) no other
light points are selected and the result of identification is unsuccessful.

5.3.3 Hybrid Simulation Result and Analysis

In this chapter, the FOV is set to 20 × 20◦ and the resolution is set to 1024 × 1024
pixels. For the purpose of validation of the presented method, the fundamental star
catalog is composed of 14,581 stars, which are selected from the Tycho2 star catalog.
The highest magnitude detected by the star sensor is 6.95. The range of star position
error is set to less than 120 according to the precision of star centroid extraction.
In regard to the problem of how to choose the ACC parameters, according to
reference [34], the maximal cycle index is commonly set to 10,000/N (the number of
ants), α and β belong to [0, 5] (default α = β = 1), the Q can be set in the range from
0 to 7000, and the volatile coefficient of pheromone ρ is always in the range of [0.1,
0.99] (which relates to the ability of global optimality and the convergence speed;
the bigger ρ is, the faster the speed, the easier the local optimality, and the more
difficult the global optimality). According to the FOV and the highest magnitude,
we know that there are about 50 stars in every star map, the number of ants N is set
to 50 and the initial maximal cycle index is set to 200. Combining the experimented
data, α and β are set to 1; the intensity of pheromone Q in every path is set to 2000;
168 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

0.95 (A)
The Successful Rate of Identification

0.9

0.85 (C)

0.8

0.75
(B)
(A)ant colony identification
0.7 (B)Delaunay cutting recognition
(C)improved triangle identification

0.65
0 20 40 60 80 100 120
The Error of Star Position(")

Fig. 5.14 Relationship between star position error and success rate of identification

the ρ is set to 0.2; and the angular distance r is set to 1/12* FOV to guarantee the
global optimality and uniqueness of results.
Hybrid simulation is carried out for this identification method. Considering the
hybrid simulation time, ten stochastic optic axes are produced by the stochastic toss
of the Monte Carlo method. For the every optic axis, there is a standard star map.
And for every standard star map (total of ten star maps), the star position error, which
is from 0 to 120 , is added to the star map. There are 120 error points of star position
for hybrid simulation. For every error point, 100 trials of the identification method
are carried out and the mean value of the successful rate of identification is calculated
as this error point’s successful rate of identification. Finally, the hybrid simulation
results of ten star maps are averaged, according to every error point. The average
value of the successful rate is shown in Fig. 5.14. In Fig. 5.14, curve (A) shows the
relationship between position error and success rate of the proposed identification.
During the course of hybrid simulation, compared with the star recognition based
on the Delaunay cutting algorithm [16] curve (B) and the improved triangle identifi-
cation [13] curve (C), the results of hybrid simulation are as follows (the Delaunay
identification data and improved triangle data originate from reference [16]).
As seen in Fig. 5.14, when the position error is small (less than 12 ), the identi-
fication success rate of this method is about 99.5 % and equivalent to the Delaunay
identification method. Because the Delaunay identification adopts the angular dis-
tance information of three vertices for cutting the triangle to identify a star map, when
the position error gradually becomes larger, the accuracy of the Delaunay identifi-
cation quickly falls. However, the identification success rate of the star recognition
based on the AAC algorithm is not sensitive to the position error and still keeps a
high success rate of recognition. When the position error is about 50 , the identifica-
tion success rate of this method is about 98 %, while for the other two identification
5.3 Star Map Identification Method of Star Sensor 169

methods it is only about 94 %. As a whole, it can be seen from Fig. 5.14 that the
recognition success rate of this method is higher than the Delaunay identification
method and the improved triangle identification method.
With regard to the aspect of recognition speed, the identification time is mainly
relative to the performance of the AAC algorithm, the capacity of the Guidance-
star database, the star position error, and the number of stars per image. A main
factor is that the AAC algorithm has a good parallel processing ability to improve the
recognition speed. The Guidance-star database adopts the storage mode of database
in this method, and the angular distance optimization results of the basic star table
by using the AAC algorithm are stored in the ascending-order database, which has
small data redundancy. Here, the binary search algorithm is used. Thus, the star
position error is one of the important factors affecting the identification time of the
AAC identification method.
Because the star position error is less than 50 , the time of identification changes
very slightly for the three identification methods. For every standard star map (total
ten star maps), the star position error, which is from 50 to 120 , is added to the
star map. There are 70 error points in star positions for hybrid simulation using
two AT91RM9200 180 MHz digital processors, which are linked to each other with
Linear Array Structure. Similarly, for every error point of star position, 100 trials
are still carried out and the mean value of the execution time of identification is
calculated as this error point’s execution time. Finally, we still average the hybrid
simulation results of ten star maps according to every error point. The average value
of the execution time is shown in Fig. 5.15, which shows the relationship of position
error and the time of identification. The results of the star recognition based on
the Delaunay cutting algorithm and the improved triangle identification are shown,
respectively, in Fig. 5.15 Curve (B) and Fig. 5.15 Curve (C).
As seen in Fig. 5.15, when the error of star position is 50 and under the same
condition for the three-star identification methods, the identification time of (A) is
less than 50 ms, because of the parallel processing ability The identification time of
(B) is more than 50 ms while (C) is more than 100 ms because of the processing
ability and the large capacity of the Guidance-star database. With the augmentation
of the error of star position, the identification time gradually becomes longer. The
growth rate of the identification time of (A) is the slowest among the three methods.
When the error of star position is 120 , (A) is about 100 ms because it is not sensitive
to the position error, (B) and (C) are, respectively, 200 and 280 ms.

5.3.4 Conclusions

A new star recognition method has been presented in this chapter. This method
employs an AAC algorithm to search optimal angular distance of star points set,
completes the angular distance optimization of star points set, and achieves quick
identification of a star map. This identification method calculates the angular distance
of any pair of star points by the parallel processing ability of the AAC algorithm to
170 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

300

(A)ant colony identification

250 (B)Delaunay cutting recognition (C)
(C)improved triangle identification
The time of Identification(ms)

200

(B)
150

100 (A)

50

0
50 60 70 80 90 100 110 120
The Error of Star Position(")

Fig. 5.15 Relationship graph of position error and the time of identification

improve the speed of identification. The angular distance of two star points is solved
as the path of the AAC algorithm. The optimal path is found by the AAC path
optimization characteristics and is used to recognize the stellar map and enhance
the recognition success rate. The presented method can solve the problems of star
recognition speed and reduced success rate due to a large FOV and the high sensitivity
of a star sensor, and overcomes the drawback of redundant matches because of small
feature dimensionality of the triangle identification and deficiency caused by the
Delaunay identification sensitive to the position error. Hybrid simulation results
show that when the measured position error is about 50 , the identification success
rate of this method still keeps 98 %, while the Delaunay identification method is
about 94 %, and the identification time of this method is merely 50 ms. As a result,
the method has a high recognition success rate, and is fast and robust. It only needs
a small database and has fast search speed.

5.4 Celestial Navigation Method Based on Star Sensor and

Semi-physical Simulation Verification

Celestial navigation realizes positioning and navigation by measuring the vector

direction of a natural celestial body relative to the spacecraft. The inertial reference
frame consisting of the Sun, Moon and the stars has incomparable accuracy and
reliability, and the development of navigation method on the basis of a reference
frame of stars and planets has direct, natural, reliable, and accurate advantages. In
5.4 Celestial Navigation Method Based on Star Sensor and Semi-physical . . . 171

this chapter, autonomous celestial navigation for satellite using unscented Kalman
filter-based information fusion has been presented.
A reliable and secure navigation system and the assured autonomous capability of
a satellite are in high demand in case of emergencies in space. Celestial navigation is a
fully autonomous navigation method for satellites. For near Earth satellites, the mea-
surement of the Earth’s direction is the most important measurement and the horizon
sensing accuracy is the most important factor which effects celestial navigation ac-
curacy. According to the mode of acquiring horizon measurement, satellite celestial
navigation methods can be broadly separated into two approaches: directly sensing
horizon using an earth sensor and indirectly sensing horizon by observing starlight
atmospheric refraction. These two methods are complementary to each other, a new
unscented Kalman filter (UKF)-based information fusion method has been proposed
here for hybriding them. Compared to the traditional celestial navigation method,
this method can provide better navigation performance and higher reliability. The
hardware-in-loop test results demonstrate the feasibility and effectiveness of this
method; in most cases the accuracy is sufficient for near Earth civilian satellites;
moreover, it can be used as a backup system to provide redundancy.

5.4.1 Introduction

The orbit determination of a satellite is performed primarily by ground tracking,

but this earthbound tracking suffers from many limitations. Rapid increase in space
missions results in rapid increase in satellite numbers. Considering the expected
growth of space “traffic” in the future, the autonomy property of a satellite is an
important factor in improving navigation performance and reducing total mission
operation costs. In recent years, a variety of autonomous navigation methods have
been proposed and explored, including the method using magnetometer [36], Global
Positioning System [37, 38], inter-satellite link [4], and celestial navigation [39–43].
The first method is better suited for the low Earth orbit (LEO) satellites. The GPS
and inter-satellite link can provide real-time highly accurate navigation information,
but in some sense they are only semiautonomous methods.
Celestial navigation is a kind of fully autonomous navigation method. It needs no
additional equipment, except standard attitude sensors such as star sensor and earth
sensor. Besides, both orbit information and attitude data can be provided. Moreover,
celestial navigation is a self-contained, nonradiating navigation method. So, it has
broad application in LEO satellites, high Earth orbit (HEO) satellites, and deep space
explorers. The celestial navigation method is based on the fact that the position of
a celestial body (Sun, Earth, Moon, and stars) in an inertial frame at a certain time
is known and its position observed in the satellite is a function of the spacecraft’s
position. The position and velocity of satellites can be estimated by applying an
optimal estimation method to the difference between the measured and estimated
celestial body data. For attitude determination, measuring the stars is enough, but for
position determination measurements of nearby bodies are needed. For near Earth
172 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

satellites, the measurement of the Earth is the most important measurement and the
horizon-sensing accuracy is the most important factor, effecting celestial navigation
accuracy. According to the mode of horizon-sensing, satellite celestial navigation
methods can be broadly divided into two major approaches: directly sensing horizon
method [39–40] and indirectly sensing horizon method [41–42]. In the first method,
an earth sensor (horizon sensor) is usually used to get the measurement of the Earth’s
direction. This technique is mature, stable, and easy to realize. But compared to
the accuracy of the horizon sensor, the position determination performance of this
method is not satisfactory. In the second method, a high-accuracy star sensor is used to
observe refracted stars to get horizon information. This method has better navigation
performance but the number of refracted stars is limited and their appearance is
arbitrary. As these two approaches are complementary to each other, a combination
of them can provide an accurate and reliable navigation system.
An effective autonomous celestial navigation method for satellite is presented
in this section, one which combines the directly sensing horizon method with the
indirectly sensing horizon method. Because the orbit dynamic model and celes-
tial measurement formulas of celestial navigation system are nonlinear, an advanced
UKF instead of traditional extended Kalman filter (EKF) and the corresponding infor-
mation fusion method are used to deal with the data association problem. Compared
to the original celestial method, this new method can efficiently improve the accuracy
and reliability of entire navigation system. The feasibility of this new method is val-
idated using hardware-in-loop test. The position and velocity accuracy are estimated
within 70 m and 0.1 m/s for a low Earth orbit satellite and this accuracy is acceptable
for a usual civilian satellite. These results verify that this method is promising and at-
tractive for satellite orbit determination. This chapter is systematized in five sections.
The principles of two celestial navigation methods are outlined in the first section,
followed by measurement formulas and orbit dynamic model in Sect. 2. The UKF
information fusion method is described in detail in Sect. 3. Simulation results and the
demonstration of performance improvement are presented in Sect. 4. Conclusions
are drawn in Sect. 5.

5.4.2 Celestial Navigation Measurements and Orbit Dynamic

Model

The difference between directly sensing horizon method and indirectly sensing hori-
zon method mainly lies in celestial measurement and the corresponding measurement
formula.
1. Directly Sensing Horizon Method
In this method, an earth sensor (horizon sensor) is used to directly sense horizon or
the Earth’s direction. The sun, moon, and star directions are usually used as assisting
measurements [39, 43]. Because the accuracy of a star sensor is far better than a
planet sensor such as the sun sensor, the star–earth angle is used as a measurement
5.4 Celestial Navigation Method Based on Star Sensor and Semi-physical . . . 173

Fig. 5.16 Diagram of directly

sensing horizon celestial
measure-ment

to improve the navigation precision, which is subtended at the spacecraft between

the line of sight of the star and the center of the Earth (shown as α in Fig. 5.16). The
measurement model using the star–earth angle is given by:
α = arccos (−s · r/r). (5.25)
Assume measurement Z1 = [α], measurement noise V1 = [vα ], the measurement
formula of this method can be presented as:
Z1 (t) = h1 [X(t), t] + V1 (t) = arccos (−s · r/r) + vα , (5.26)
where s is the position vector of a navigation star in earth-centered inertial coordinate
obtained by star identification. r is the position vector of a satellite. vα mainly comes
from the installation error and measurement error of the horizon sensor.
2. Indirectly Sensing Horizon Method
The indirectly sensing horizon method by stellar horizon atmospheric refraction
(SHAR) is a kind of low-cost autonomous navigation plan for a satellite that was
developed at the beginning of the 1980s. This plan makes use of the high-accuracy
star sensor and the mathematics model of starlight refraction in the atmosphere to
sense the horizon and achieve high-accuracy satellite autonomous navigation. The
USA carried out the space experiment in the 1990s and the multitask autonomous
navigation system (MADAN)uses only the starlight refraction principle [40–42].
The principle of indirect sensing horizon method by SHAR is shown in Fig. 5.17.
First, observe two stars at the same time using two star sensors. The navigation star
is in near vertical direction and its light is not refracted. The refracted star is near
the Earth’s surface that its light is refracted as it grazes the Earth’s atmosphere. So,
the angle between the navigation starlight and refracted starlight will be different
from the standard value, this difference is called the starlight refraction angle (R in
Fig. 5.17). The starlight refraction angle is mainly determined by starlight frequency
174 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

Navigation star
Apparent star
direction

us a

R ha hg ha Refracted star

g
Satellite

Re
r

u
O
earth center

Fig. 5.17 Diagram of indirectly sensing horizon celestial measurement

and atmospheric density, while the atmospheric density can be described by an ex-
ponential function of altitude. This information provides an indication of the Earth’s
horizon and the satellite position in an earth-centered inertial coordinate system [40].
However, R has no direct relation with the position of a satellite. The position is
represented as a function of the apparent ray height (ha ). The mathematical relation
of the ha and R is as follows [40]:

1 
1
2π Re 2 H Re 2
ha (R, ρ) = h0 − H ln (R) + H ln k(λ)ρ0 +R , (5.27)
H 2π
where ρ is the density at altitude h, ρ0 is the density at altitude h0 , and H is the
density scale height. k(λ) is denoted as the dispersion parameter and a function of
the wavelength of the light, Re is the Earth’s radius. From these known parameters,
we can get ha from measurement R.
Also note that in Fig. 5.17

ha = r 2 − u2 + u tan (R) − Re . (5.28)
Take measurement Z2 = [ha ], measurement noise V2 = [vha ], the measurement
formula of indirectly sensing horizon celestial navigation is equal to

Z2 (t) = h2 [X(t), t] + V2 (t) = r 2 − u2 + u tan (R) − Re + vha , (5.29)
5.4 Celestial Navigation Method Based on Star Sensor and Semi-physical . . . 175

where u = |r · us |, us is unit vector to refracted star direction got by star identification,

r is the satellite position vector. Formula (5.28) shows the relationship between
measurements and satellite position. vha is mainly coming from the atmosphere’s
model error and measurement error of star sensor.
3. Orbit Dynamic Model
The orbit dynamic model used in this work is an improved one of the simple two-
body orbital model, which uses a Singer model to describe a part of perturbing
accelerations. The formulas of this model in the earth-centered inertial Cartesian
coordinate (J2000.0) are given as:
⎫
dx/dt = vx + wx ⎪
⎪
⎪
⎪
⎪
⎪
dy/dt = vy + wy ⎪
⎪
⎪
⎪
⎪
⎪
dz/dt = vz + wz ⎪
⎪
⎪
⎪
dvx /dt = −μx/r · [1 − J2 (Re /r)(7.5z /r − 1.5)] + ax + wvx ⎪
3 2 2 ⎪
⎪
⎬
dvy /dt = −μy/r 3 · [1 − J2 (Re /r)(7.5z2 /r 2 − 1.5)] + ay + wvy
⎪
⎪
⎪
dvz /dt = −μz/r 3 · [1 − J2 (Re /r)(7.5z2 /r 2 − 1.5)] + az + wv z ⎪
⎪
⎪
⎪
⎪
dax /dt = −αx ax + wax ⎪
⎪
⎪
⎪
⎪
⎪
day /dt = −αy ay + way ⎪
⎪
⎪
⎪
⎪
⎭
daz /dt = −αz az + waz
(5.30)


r= x 2 + y 2 + z2

Formula (5.6) can be written in the general state formula as

Ẋ(t) = f (X, t) + w(t), (5.31)

 T
where state vector X = x y z vx vy vz ax ay az · x, y, z, vx , vy , vz
are satellite positions and velocities of the three axes, respectively. μ is the gravita-
tional constant of the Earth, r is the position vector of satellite. J2 is the second
zonal coefficient and has the value 0.00108263. ax , ay , az are parts of perturb-
ing accelerations of the three axes, which can be approximated by the Singer
model. These perturbations include high-grade nonspherical earth perturbations,
third-body perturbations, atmospheric drag perturbations, solar radiation pertur-
bations, and other perturbations except the nonspherical earth J2 perturbations.
α1 , α2 , α3 are reciprocals of correlation time constants of the three axes, system
noise: w = [wx , wy , wz , wvx , wvy , wvz , wax , way , waz ]T .
This orbit dynamic model is more precise compared with the simple two-body
orbital model and has less computational cost compared with the complex models
on which most of the perturbations are modeled [37].
176 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

5.4.3 UKF Information Fusion Algorithm

From measurement formulas (5.2 and 5.5) and state formula (5.7), we can now
formulate the optimal filter based on a directly sensing horizon and an indirectly
sensing horizon celestial navigation and perfect data association. In previous works
[39], EKF has been used in spacecraft celestial navigation. The major drawback of
EKF is that it only uses the first-order terms in the Taylor series expansion. Sometimes
it may introduce a large estimation error in a nonlinear system. The UKF [44, 45]
has been proved to have a better performance than EKF in nonlinear system state
estimation [46, 47], which uses the true nonlinear model and a set of sigma sample
points produced by the unscented transformation to capture the mean and covariance
of state. Because the state formula and measurement formulas of celestial navigation
described above are nonlinear, UKF is used here in each local filter. Using formulas
(5.31) and (5.26) form one local filter of directly sensing horizon celestial navigation,
formulas (5.31) and (5.29) form another local filter of indirectly sensing horizon
celestial navigation. When no refracted star appears, only the first local filter works
and outputs the navigation information. Once a refracted star is observed, both the
local filters work and get two local optimal state estimations Xi (k)(i = 1,2), then
the whole optimal state estimation can be obtained by a master filter. A pseudocode
description of this method is as follows.
1. Local Filter Design Based on UKF
The UKF uses unscented transformation to capture the mean and covariance estimates
with a minimal set of sample points. If the dimension of state is n × 1, the 2n + 1
sigma point and their weight are computed by [14]
χ0,k = x̂k ; W0 = τ/(n + τ ),

√    
χi,k = x̂k + n+τ P (k|k) ; Wi = 1/ 2(n + τ ) ,
i

√    
χi+n,k = x̂k − n + τ P (k|k) ; Wi+n = 1/ 2(n + τ ) ,
i
√
where τ ∈ R, ( P (k|k))i is the ith column of the matrix square roots. The UKF
formulas used in each local filter are given as (8) to (19).

Initialization
  T 
x̂0 = E[x0 ], P0 = E x0 − x̂0 x0 − x̂0 . (5.32)

Calculate Sigma Points

 √   √ √ 
χk−1 = x̂ k−1 x̂ k−1 + n + τ √Pk−1 x̂ k−1 − n + τ Pk−1 i ,
i

i = 1,2, . . ., n. (5.33)
5.4 Celestial Navigation Method Based on Star Sensor and Semi-physical . . . 177

Time Update
χk|k−1 = f (χk−1 ,k−1 ), (5.34)
2n
x̂k− = Wi χi,k|k−1 , (5.35)
i=0

2n
  T
Pk− = Wi χi,k|k−1 − x̂k− χi,k|k−1 − x̂k− + Qk , (5.36)
i=0

Zk|k−1 = h(χk|k−1 ,k ), (5.37)

2n
ẑk− = Wi Zi,k|k−1 . (5.38)
i=0

Measurement Update
2n
  T
Pẑk ẑk = Wi Zi,k|k−1 − ẑk− Zi,k|k−1 − ẑk− + Rk , (5.39)
i=0

2n
  T
Px̂k ẑk = Wi χi,k|k−1 − x̂k− Zi,k|k−1 − ẑk− , (5.40)
i=0

Kk = Px̂k ẑk Pẑ−1

k ẑk
, (5.41)

x̂k = x̂k− + Kk (Zk − ẑk− ), (5.42)

Pk = Pk− − Kk Pẑk ẑk KkT , (5.43)

where Qk and Rk are system and measurement noise covariance matrices. When
x(k) is Gauss distributed, usually n + τ = 3 is selected.
2. Information Fusion in Master Filter
X1 (k)and P1 (k) are state estimation values and X2 (k) and P2 (k) are the error covari-
ance matrices of directly and indirectly sensing horizon celestial navigation local
filters. The global optimal state estimation in master filter is obtained by the following
information fusion formulas [48]:
 −1  −1 
X̂g (k) = P1−1 (k) + P2−1 (k) · P1 (k)X1 (k) + P2−1 (k)X2 (k) ,
 −1
Pg (k) = P1−1 (k) + P2−1 (k) ,

X̂i (k) = X̂g (k).

As the stability of UKF is not as good as EKF, the feedback is only performed on state
variable not to the covariance matrix. The block diagram of this UKF information
fusion algorithm is shown in Fig. 5.18.
178 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

Star-earth angle Z1 X 1 , P1
measurement Directly sensing horizon
local UKF filter1

Information fusion
X 1 , P1

Master filter
X g , Pg
Orbital dynamics X g , Pg

Indirectly sensing horizon

If there is refracted Z2 local UKF filter 2
star measurement? X 2 , P2
Yes

No

Fig. 5.18 Information fusion algorithm

Horizon sensor measurement

Directly sensing horizon

Orbit and measurement
Attitude

Star Chart Simulator Star Sensor Simulator D

A B

Position,Velocity

Trajectory
Generator Navigation
Computer

Orbit and Indirectly sensing horizon

Attitude measurement
Star Chart Simulator Star Sensor Simulator C

Atmosphere refraction model

Fig. 5.19 Block diagram of hardware-in-loop simulator

5.4.4 Simulation Results

This section presents simulation results to demonstrate the performance improvement

of this new method compared to the original method. All simulation data come from
the celestial navigation hardware-in-loop simulation platform showed in Fig. 5.19.
This platform is composed of satellite orbit generator (A), a real-world model char-
acterizing both navigation environment and on-board sensors (B, C) and a navigation
computer (D).
In Fig. 5.19, part A is trajectory generator, on which software Satellite Tool Kit
(STK) is installed, and it provides a typical satellite flight path to evaluate navigation
performance. Parts B and C are used to produce simulate measurements of directly
and indirectly sensing horizon celestial subsystems. They both consist of a star chart
simulator and a star sensor simulator. The core part of the star chart simulator is a
 5.4 Celestial Navigation Method Based on Star Sensor and Semi-physical . . . 179

Position Estimation Error Velocity Estimation Error

2000 4.5
directly sensing horizon directly sensing horizon
indirectly sensing horizon indirectly sensing horizon
1800 new method 4 new method

1600
3.5

1400
position estimation error/m

velocity error/(m/s)
1200
2.5

1000
2
800

1.5
600

1
400

0.5
200

0 0
0 50 100 150 200 250 300 350 400 0 50 100 150 200 250 300 350 400
time/min time/min

a b
Fig. 5.20 Position and velocity estimation errors of directly sensing horizon method, indirectly
sensing horizon method, and information fusion method

liquid crystal light valve (produced by Seiko Instruments Inc.), whose resolution is
up to 1024 × 768 pixels. The star sensor simulator consists of CCD catcher, OK
series of graphic card, and star pattern identification program. The horizon sensor
measurement is input in part B to produce the star-earth angle measurement and the
atmosphere refraction model (CIRA1986) is input in part C to produce the apparent
ray height measurement. Part D is a navigation computer, whose function is running
the navigation software, including filtering algorithm, information fusion algorithm
as well as the software of dynamic demonstration and verification. The trajectory used
in the following simulation is an LEO satellite of which orbital parameters are: semi-
major axis a = 7136.635444 km, eccentricity e = 1.809 × 10−3 , inclination i = 65◦ ,
right ascension of the ascending node Ω = 30◦ , the argument of perigee ω = 30◦ .
And the accuracy of star sensor and horizon sensor is selected 3 and 0.02◦ based on
the characteristics of the real star sensor and horizon sensor used in a satellite. The
stellar database used in simulation is the Tycho stellar catalog.
Figure 5.20 shows the performance comparison among the directly sensing hori-
zon method, indirectly sensing horizon method, and information fusion method,
which is obtained with 3 s sampling interval during the 400-min period (four orbits).
In the simulation, there are 269 apparent ray height measurements of refracted star
observed in one orbit. After the filter convergence period, the position estimation er-
rors of the directly sensing horizon method and indirectly sensing horizon method are
about 180–220 m and 70–110 m RMS, respectively. The velocity estimation errors of
these two methods are around 0.19–0.23 m/s and 0.14–0.19 m/s RMS, respectively.
The position and velocity estimation errors of the UKF-based information fusion
method are 40–70 m and 0.07–0.10 m/s RMS, respectively. With the combination
of the star–earth angle and the apparent ray height measurement, the position and
velocity estimation accuracy is significantly improved. The navigation performance
of this new method mainly depends on the number of observed refracted stars and
the accuracy of measurement devices as we can legitimately expect.
 180 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

Position Estimation Error Velocity Estimation Error

400 1.8
UKF UKF
EKF EKF
350 1.6

1.4
300

1.2

velocity error/(m/s)
250
position error/m

200
0.8

150
0.6

100
0.4

50 0.2

0 0
0 50 100 150 200 250 300 350 400 0 50 100 150 200 250 300 350 400
time/min time/min
a b
Fig. 5.21 Position and velocity estimation errors of EKF- and UKF-based information fusion
methods

Table 5.5 Simulation results of EKF- and UKF-based methods

Navigation EKF-based method UKF-based method
method
Position Velocity Position Velocity
estimation estimation estimation estimation
error/m error/(m/s) error/m error/(m/s)
Directly sensing 220–270 0.23–0.28 180–220 0.19–0.23
horizon
method
Indirectly 180–300 0.21–0.33 70–110 0.14–0.19
sensing
horizon
method
Information 110–150 0.10–0.15 40–70 0.07–0.10
fusion method

The performance improvement from the original EKF method to UKF method is
also investigated. Figure 5.21 shows the simulation results of EKF-based information
fusion method and the UKF-based one obtained under the same simulation condition
described above. The position and velocity estimation errors of the EKF-based in-
formation fusion method are 110–150 m and 0.10–0.15 m/s RMS, respectively; the
method performs poorly compared with UKF-based information fusion method.
The results of EKF-based directly sensing horizon method, indirectly sensing
horizon method, and information fusion method and the UKF-based ones are shown
in Table 5.1. Similar results in both cases are obtained, which means that the in-
formation fusion method can improve the navigation accuracy no matter what filter
method is used. However, the UKF-based methods perform much better than the
EKF-based ones (Table 5.5).
References 181

5.4.5 Conclusions

In this chapter, a new autonomous celestial navigation method for satellites, which
effectively integrates directly sensing horizon method and indirectly sensing horizon
method by UKF-based information fusion is studied. The navigation algorithm is
tested using hardware-in-loop simulator and a position estimation error within 70 m
is obtained. Compared to the original celestial method such as the directly sensing
horizon method using EKF, this method can provide better performance because of
the full use of both kinds of celestial measurements and the use of the UKF instead
of EKF to solve the problem of nonlinear state estimation. The main advantage
of this algorithm lies in its full autonomy, its relative simplicity for operational
implementation, and its potential ability to be included in most existing spacecraft
navigation system. These results demonstrate that this method can be a good choice of
satellite autonomous navigation method; moreover, it can also be used as a backup
system to provide redundancy. The performance evaluation via real-orbit data is
under investigation.

5.5 Chapter Conclusion

The celestial navigation method based on semi-physical simulation has been elabo-
rated in this chapter. Star map preprocessing is the premise of star map identification,
which means that for the purpose of realizing efficient star map identification star
maps must be preprocessed first. Initially, a method of blurred star image processing
for star sensors under dynamic conditions has been elaborated in this chapter. This
new method can guarantee the effect star map identification considerably. Then a star
recognition method based on the AAC algorithm for star sensors has been presented,
which is quick and takes small memory storage. This method can greatly improve
the successful rate of star map identification and is practical from the engineering
perspective. Additionally, in order to fulfill the goal of autonomous celestial navi-
gation based on star sensors, a celestial navigation method based on a star sensor
and semi-physical simulation verification is proposed, which has obtained perfect
results. The research presented in this chapter is the basis for further study of celestial
navigation.

References
1. Hancock BR, Stirbl RC, Cunningham TJ, Pain B, Wrigley CJ, Ringold PG (2001) CMOS active
pixel sensor specific performance effects on star tracker/image position accuracy. Proc SPIE
4284:43–53
2. Pasetti A, Habine S, Creasey R (1999) Dynamical binning for high angular rate star tracking.
Proceedings of the 4th international conference on spacecraft guidance, navigation and control
systems, Noordwijk, 18–21 Oct, pp 255–266
182 5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .

3. Li X, Zhao H (2009) Analysis of star image centroid accuracy of an APS star sensor in rotation.
Aerosp Control Appl 35:11–16
4. Star1000 1M Pixel Radiation Hard CMOS Image Sensor (2007) Cypress Semiconductor
Corporation, San Jose
5. Chang SG, Yu B, Vetterli M (2000) Adaptive wavelet thresholding for image de-noising and
compression. IEEE Trans Image Process 9:1532–1546
6. Mallat S (1989) A theory for multiresolution signal decomposition: the wavelet representation.
IEEE Trans Patt Anal Mach Intell 11:674–693
7. Gonzalez RC, Woods RE (2002) Digital image processing, 2nd ed. Prentice Hall, Englewood
Cliffs
8. Kaur L, Gupta LS, Chauhan RC, (2002) Image de-noising using wavelet thresholding. Pro-
ceeding of the third Indian conference on computer vision, graphics & image processing,
Ahmadabad, 16–18 Dec, pp 1522–1531
9. Mihcak KM, Kozintsev L, Ramchandran K (1999) Low-complexity image de-noising based
on statistical modeling of wavelet coefficients. IEEE Sign Process Lett 6:300–303
10. Quan W, Zhang W (2011) Restoration of motion-blurred star image based on Wiener Filter.
Proceeding of the fourth IEEE international conference on intelligent computation technology
and automation, Shenzhen, 28–29 March, pp 691–694
11. Costello TP, Mikhael WB (2003) Efficient restoration of space-variant blurs from physical
optics by sectioning with modified Wiener filtering. Digital Signal Process 13:1–22
12. Tan KC, Lim H, Tan BTG (1991) Windowing techniques for image restoration. Graph Mod
Image Process 53:491–500
13. Zhang GJ, Wei XG, Jiang J (2006) Star map identification based on a modified triangle
algorithm. ACTA Aeronaut ET Astronaut SINICA 27:1150–1154
14. Rousseau GLA, Bostel J, Mazari B (2005) Star recognition algorithm for APS star tracker:
oriented triangles. IEEE Aero El Sys Mag 20:27–31
15. Lin T, Zhou JL (2000) All-sky automated quaternary star pattern recognition. J Astronaut
21:82–85
16. Wang ZL, Quan W (2004) An all-sky autonomous star map identification algorithm. IEEE Aero
El Sys Mag 19:10–14
17. Cole CL, Crassidis JL (2006) Fast star-pattern recognition using planar triangles. J Guid Control
Dynam 29:64–71
18. Lee H, Bang H (2007) Star pattern identification technique by modified grid algorithm. IEEE
Trans Aero Elec Sys 43:202–213
19. Liebe CC (1992) Pattern recognition of star constellations for spacecraft applications. IEEE
Aero El Sys Mag 7:10–16
20. Udomkesmalee S, Alexander JW, Tolivar AF (1994) Stochastic star identification. J Guid
Control Dynam 17:1283–1286
21. Wang GJ, Fang JC (2005) New star pattern recognition approach based on Hausdorff distance.
J Beijing Univ Aeronaut Astronaut 31:508–511
22. Mortari D, Junkins J, Samaan M (2001) Lost-in-space pyramid algorithm for robust star pattern
recognition. Proceedings of 24th annual AAS guidance and control conference, Breckenridge
23. Jian H, Dickerson JA (2000) Neural network based autonomous star identification algorithm.
J Guid Control Dynam 23:728–735
24. Paladugu L, Williams BG, Schoen MP (2003) Star pattern recognition for attitude determination
using genetic algorithms. Proceedings of the 17th AIAA/USU conference on small satellites,
Logan
25. ColorniA, Dorigo M, Maniezzo V (1991) Distributed optimization by ant colonies. Proceedings
of the 1st European conference on artificial lift, Paris
26. Li Y, Hilton ABC (2007) Optimal groundwater monitoring design using an ant colony
optimization paradigm. Environ Modell Softw 22:110–116
27. Bautista J, Pereira J (2007) Ant algorithms for a time and space constrained assembly line
balancing problem. Eur J Opter Res 177:2016–2032
References 183

28. Maniezzo V, Carbonaro A (2000) An ants heuristic for the frequency assignment program.
Future Gener Comput Syst 16:927–935
29. Gao SW, Guo L (2007) Adaptive ant colony algorithm based on dynamic weighted rule. Comput
Appl 27:1741–1743
30. Watanabe I, Matsui S (2003) Improving the performance of ACO algorithms by adaptive control
of candidate set. Proceedings of the 2003 congress on evolutionary computation, Canberra
31. Dorigo M, Luca M (2002) The Ant-Q: algorithm applied to the nuclear reload problem. Ann
Nucl Energ 29:1455–1470
32. Jackson DE, Holcombe M, Ratnieks FLW (2004) Trail geometry gives polarity to ant foraging
networks. Nature 432:907–909
33. Mekle D, Middendorf M (2002) Modeling the dynamics of ant colony optimization. Evolut
Comput 10:235–262
34. Duan HB (2005) Ant colony algorithms: theory and applications. Science, Beijing
35. Hye YK, John LJ (2002) Self-organizing guide star selection algorithm for star trackers: thin-
ning method. Proceedings of the 2002 IEEE Aerospace Conference Proceedings, Big Sky
Montana, USA
36. Psiaki ML, Huang LJ, Fox SM (1993) Ground tests of magnetometer-based autonomous
navigation (MAGNAV) for low-earth-orbiting spacecraft. J Guidance Control Dyn 16:206–214
37. Yoon JC, Lee BS (2000) Spacecraft orbit determination using GPS navigation solutions
aerospace science and technology, pp 215–221
38. Truong SH, Hart RC, Shoan WC, Wood T (1997) High accuracy autonomous navigation using
the Global Positioning System (GPS). Proceeding of the 12th international symposium on
‘Space Flight Dynamics’, ESOC, Darmstadt, 2–6 June, pp 73–78
39. Long A, Leung D, Folta D, Gramling C (2000) Autonomous navigation of high-earth satel-
lites using celestial objects and Doppler measurements. AIAA/AAS Astrodynamis Specialist
Conference, pp 1–9
40. White RL, Gounley RB (1987) Satellite autonomous navigation with SHAD. The Charles Stark
Draper Laboratory, April
41. Riant P (1986) Analysis of a satellite navigation system by stellar refraction. 36th congress of
the international astronautical federation, Stockholm, Sweden, 7–12 Oct 1986, pp 1–8
42. Gounley R, White R, Gait E (1984) Autonomous satellite navigation by stellar refraction.
Guidance and control conference, vol 7, Mar–Apr 1984, pp 129–134
43. Hosken RW, Wertz JR (1995) Microcosm autonomous navigation system on-orbit operation.
Adv Astronaut Sci 1–15
44. J. Simon J, Uhlmann JK (1997) A new extension of the Kalman Filter to nonlinear systems.
In: The 11th international symposium on aerospace/defense sensing, simulation and controls,
multi sensor fusion, tracking and resource management, SPIE
45. Julier S, Uhlmann J, Durrant-Whyte HF (2000) A new method for the non linear transformation
of means and covariances in filters and estimators. IEEE Trans Autom Control AC–45 3:477–
482
46. Wan EA, Van Der Merwe R (2000) The unscented Kalman filter for nonlinear estimation.
Adaptive systems for signal processing, communications, and control symposium, pp 153–158
47. Li W, Leung H, Zhou YF (2004) Space-time registration of radar and ESM using unscented
Kalman Filter. IEEE Trans Aerosp Electron Sys 40:824–836
48. Vershinin YA, West MJ (2001) A new data fusion algorithm based on the continuous-time
decentralized Kalman filter, target tracking: algorithms and applications (Ref. No. 2001/174).
IEE 1:16/1–16/6
Chapter 6
INS/GNSS Integrated Navigation Method

6.1 Introduction

Ship intertial navigation system (SINS) and global navigation satellite system
(GNSS) are two kinds of common navigation equipments with respect to the ad-
vantages and disadvantages during application. The former has strong autonomy,
high short-term precision, and continuous output, but errors accumulates with time;
the latter has high positioning and velocity measurement precision and errors do
not accumulate with time, but has incontinuous output information and suscepti-
bility to interference. Integration of the two to realize complementary advantages
will significantly improve overall performance of the navigation system. At present,
the SINS/GNSS integrated navigation system has been widely used in fields such
as aviation, aerospace, and sailing, and it is a relatively ideal integrated navigation
system.
How to give full play to overall performance of the SINS/GNSS integrated nav-
igation to truly achieve complementary advantages of the two is a problem during
practical application of the navigation system. Position and velocity or pseudor-
ange and pseudorange rate are generally used as the observation quantities of the
SINS/GNSS integrated navigation, free of attitude measurement information, but
for certain application, such as the onboard high-resolution earth observation, the
integrated navigation system is required to maintain high attitude precision besides
possessing high-precision position information. In this way high requirements are
made for filtering model precision, filtering algorithm precision, and air alignment
precision of the SINS/GNSS integrated navigation system, i.e., attitude and inertial
component errors of the SINS are required to be estimated while estimating the posi-
tion and velocity errors of the SINS. In addition, the navigation error of a pure SINS
diverges in case of GNSS signal lock losing caused by electromagnetic interference
or block suffered by the GNSS signal. It is thereby required to study error inhibition
and compensation techniques of the SINS during the GNSS lock losing period to
maintain continuous and high-precision navigation.
This chapter systematically discusses the principle, method, and engineering ap-
plication problem of the SINS/GNSS integrated navigation system. First, it discusses

© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 185
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_6
186 6 INS/GNSS Integrated Navigation Method

briefly the basic principle and operating mode of the SINS/GNSS integrated nav-
igation system, based on which modeling method of the SINS/GNSS integrated
navigation system will be introduced specifically; then, the SINS/GNSS integrated
navigation method will be introduced from two perspectives of high precision, mi-
crominiature, and low cost. Finally, it introduces briefly the SINS/global positioning
system (GPS) integrated navigation system used for the onboard SAR imaging
motion compensation and its test.

6.2 Principle of Inertial/Satellite Integrated Navigation

Application of the SINS/GNSS integrated navigation system aims at correcting SINS

position, velocity, and attitude errors caused by gyro drift, accelerometer offset,
and initial misaligned angle by using the information of the GNSS to obtain high-
precision navigation information.

6.2.1 Combination Mode of Inertial/Satellite Integrated

Navigation

The SINS and GNSS may have different levels of combination according to different
application requirements, i.e., different combination depth. In accordance with the
combination depth, the SINS/GNSS integrated navigation system may be classified
as shallow combination, deep combination, and ultra-deep combination.
1. Shallow Combination
Shallow combination is a combination mode using only the GNSS to assist in correct-
ing errors of the SINS, where the SINS and GNSS both can operate independently. Its
main features include simple operation and convenience for engineering realization,
which makes navigation system have certain redundancy.
At present, shallow combination generally adopts position and velocity infor-
mation combination to use error equation of the SINS as the state equation of the
integrated navigation system, and the respective output position and velocity differ-
ence of the GNSS and SINS as measurement quantity; it adopts the filter to conduct
an optimal estimation for the position, velocity, attitude, and inertial component
errors of the SINS, and then outputs or makes feedback corrections for the SINS.
Advantages of such a combination mode are that it can estimate velocity and
position errors of the integrated navigation system and properly inhibit attitude di-
vergence; it can also improve positioning precision of the system greatly through the
compensation to make the SINS have moving base and air alignment capacity [1, 2],
but it can not assist the GNSS in enhancing the tracking of the satellite signal, which
is its disadvantage.
6.2 Principle of Inertial/Satellite Integrated Navigation 187

2. Deep Combination
Deep combination is a combination mode in which the GNSS and SINS assist each
other. Under such a mode, the GNSS can assist in correcting errors of the SINS, and
SINS, in turn, can also assist in correcting errors of the GNSS to improve the precision
of the GNSS. For engineering realization, the combination mode requires the GNSS
receiver to have the capacity for real-time adjustment of an intrinsic parameter.
The basic mode of deep combination is the combination of pseudorange and
pseudorange rate; it uses pseudorange and pseudorange rate given by the GNSS and
the difference of pseudorange and pseudorange rate obtained from the conversion of
position and velocity given by the SINS as measurement quantities and then conducts
the correction for system errors.
The advantages of such a combination mode include high integrated navigation
precision, dynamic performance, and robustness, and its overall performance is
superior to shallow combination, but the structure of the GNSS receiver is complex,
which is its disadvantage.
3. Ultra-deep Combination
Ultra-deep combination is a combination mode connecting the GNSS tracking signal
with the SINS/GNSS integrated navigation system into an optimal filter to improve
the capacity of GNSS to track the satellite signal. Its advantages include that it
can improve the signal-to-noise ratio (SNR) of the GNSS tracking signal, reduce
influence of multipath effect, and realize reacquisition rapidly when the signal is
blocked out or interrupted, but it has also disadvantages such as complex structure,
heavy computation, and strict time synchronization, etc.
The SINS/GNSS integrated navigation system based on such combination mode
is under development abroad and not researched yet at home.

6.2.2 Basic Principle for Inertial/Satellite Integrated Navigation

The basic principle for the SINS/GNSS integrated navigation is to use error equations
of the SINS and GNSS as system state equations and respective output information
difference of the SINS and GNSS as measurement quantity; it adopts an optimal
filter to realize high-precision integrated navigation. The system state and measure-
ment equations must be established first before designing the Kalman filter of the
SINS/GNSS integrated navigation system. If it takes the navigation parameter of
each navigation system directly as the state, i.e., taking the navigation parameter of
each navigation system directly as the object of estimation, then the method is called
the direct filter method. If it takes the navigation error of each navigation system as
the state, i.e., taking the navigation error as the object of estimation, then the method
is called indirect filter method [3, 4]. Sketches of the two methods are shown in
Figs. 6.1 and 6.2, respectively.
The magnitude of state variables (such as velocity, position, attitude angle, etc.) in
the direct method is large, and it changes quickly; its state equation is often nonlinear
188 6 INS/GNSS Integrated Navigation Method

Navigation parameter
Motion of the carrier calculated by SINS Optimal
SINS estimation of
navigation
parameter
Carrier Kalman Filter

GNSS
Navigation parameter
obtained from GNSS

Fig. 6.1 Sketch map of the direct filter method. SINS ship inertial navigation system, GNSS global
navigation satellite system

Optimal
estimation of
Navigation parameter calculated by SINS navigation
Motion of the carrier + parameter
SINS

+ - Output
Carrier Kalman correction
Filter

GNSS
-
Feedback
Navigation parameter obtained from GNSS
correction

Fig. 6.2 Sketch map of the indirect filter method. SINS ship inertial navigation system, GNSS
global navigation satellite system

in practice, which will have influence on the estimation precision of each state. The
state in the indirect method is the navigation error, which is much smaller than the
magnitude of the navigation parameter and changes slowly. For the indirect method,
the transfer rule of the navigation error can be described accurately by using the linear
state equation in practice, so that state estimation precision is easy to be corrected,
and it becomes a widely used method in an engineering application.
There are generally two methods to correct a navigation parameter when the filter
receives the estimation value of the state, such as the navigation error—one is to
correct the output of the SINS directly by using the state estimation value, which is
called the output correction method; the other is to feed the state estimation value
back to the calculation flow of the SINS and other navigation subsystems, which is
called the feedback correction method. Schematic diagrams of the two correction
methods are shown in Figs. 6.3 and 6.4 [4]. The state estimation obtained from the
direct filter method and the indirect filter method can adopt both the output correction
method and feedback correction method for correction.
If the system state equation and measurement equation are accurate, the estimation
effects of the output correction method and the feedback correction method will be
the same; otherwise, filtering is easy to be diverged during long working hours since
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System 189

X1 +
SINS
+
×
× Kalman Filter ΔX̂ 1
_
X2 _
GNSS ΔX 1 − ΔX 2

Fig. 6.3 Schematic diagram of the output correction. SINS ship inertial navigation system, GNSS
global navigation satellite system

X1 X1
ΔX̂ 1
SINS
+
× Kalman Filter
X2 _
GNSS ΔX 1 − ΔX 2

ΔX̂ 2

Fig. 6.4 Schematic diagram of the feedback correction. SINS ship inertial navigation system, GNSS
global navigation satellite system

the output correction can not correct internal state errors of the system; but longtime
filtering stability can be guaranteed since the feedback correction can correct internal
state errors of the system.

6.3 Modeling Method of Inertial/Satellite Integrated Navigation

System

For the SINS/GNSS integrated navigation system, the establishment of the accurate
filtering mathematical model [5, 6] is the basis for integrated navigation by adopting
filter techniques. The filtering mathematical model of the SINS/GNSS integrated
navigation system can be classified into the linear model and nonlinear model ac-
cording to different estimation values of the attitude error (misalignment angle). If
three misalignment angles of the mathematics platform are all of small quantities,
the linear model can be established; otherwise, the nonlinear model is required to
be established [7]. Derivation methods of the SINS error equation, such as  angle
method and  angle method, generally used both, assuming the misalignment angle
to be of small quantity. When the attitude error is a large misalignment angle, the
filtering precision will be reduced greatly if the traditional linear error equation is
still used, and even filtering divergence will be caused. Therefore, it is required to
establish a mathematical model suitable for it according to the practical situation of
the system.
This section introduces typical linear and nonlinear modeling methods of the
SINS/GNSS integrated navigation system separately. The linear model adopts the
190 6 INS/GNSS Integrated Navigation Method

error angle ( angle) between the mathematics platform coordinate system and the
real geographic coordinate system to represent the attitude error of the SINS and
establishes the linear mathematical model of the SINS/GNSS integrated navigation
system based on it. The nonlinear model adopts the calculation error of quaternion to
describe the misalignment angle of the SINS, where the relationship among additive
quaternion error, multiplicative quaternion error, and rotating vector error is given
first, and then the SINS error equation suitable for the circumstance of three large
misalignment angles is introduced, based on which the nonlinear mathematical model
of the SINS/GNSS integrated navigation system is established.

6.3.1 Linear Modeling Method of Inertial/Satellite Integrated

Navigation System Based on the  Angle

1. Error Equation of Inertial Navigation System

With a north, east, and vertical direction navigation coordinate system as basic co-
ordinate system for navigation calculation, think of flight height h and suppose the
earth to be a rotational ellipsoid. Refer to Sect. 2.3 of this book for the specific
attitude error equation, velocity error equation and position error equation.
2. System Model
1) State equation

Ẋ(t) = F (t)X(t) + G(t)W (t), (6.1)

 
wherein X = φE φN φU δvE δvN δvU δL δλ δh εx εy εz ∇x ∇y ∇z T,
wherein φE , φN , and φU are misalignment angles of mathematics platform; δvE ,
δvN , and δvU are east, north and vertical direction velocity errors of the carrier,
respectively; δL, δλ, and δh are latitude error, longitude error, and height error,
respectively; εx , εy , εz , ∇x , ∇y , and ∇z are random constant drifts of the gyro and
random constant null bias of the accelerometer.
Noise transfer matrix G(t) of the system is:
⎡ ⎤
Cbn 03×3
⎢ ⎥
G(t) = ⎢ ⎣ 03×3 Cb ⎦
n ⎥ .
09×3 09×3
15×6

System noise vector consists of the random error of the gyro and accelerometer with
expression as follows:
 T
w = wεx wε y w εz w∇x w∇ y w∇z .
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System 191

State transfer matrix F (t) of the system is:

⎡ ⎤
FN FS
F (t) = ⎣ ⎦,
06×9 FM
wherein FN is corresponding to a 9-dimensional basic navigation parameter system
matrix [8, 9], and its nonzero element is:


vE vE
F (1,2) = ωie sin L + tgL; F (1,3) = − ωie cos L + ;
RN + h RN + h
1 vE
F (1,5) = − ; F (2,1) = −ωie sin L − tgL;
RM + h RN + h
vN 1
F (2,3) = − ; F (2,4) = ;
RM + h RN + h
vE
F (2,7) = −ωie sin L; F (3,1) = ωie cos L + ;
RN + h
vN 1
F (3,2) = ; F (3,4) = tgL;
RM + h RN + h
vE
F (3,7) = ωie cos L + sec2 L; F (4,2) = −fU ;
RN + h
vN vU
F (4,3) = fN ; F (4,4) = tgL − ;
RM + h RM + h


vE vE
F (4,5) = 2ωie sin L + tgL; F (4,6) = − 2ωie cos L + ;
RN + h RN + h
vE vN
F (4,7) = 2ωie cos L · vN + sec2 L + 2ωie sin L · vU ;
RN + h
F (5,1) = fU ; F (5,3) = −fE ;


vE vU
F (5,4) = − 2ωie sin L + tgL ; F (5,5) = − ;
RN + h RM + h


vN vE
F (5,6) = − ; F (5,7) = − 2ωie cos L + sec2 L vE ;
RM + h RN + h
F (6,1) = −fN ; F (6,2) = fE ;


vE 2vN
F (6,4) = 2 ωie cos L + ; F (6,5) = ;
RN + h RM + h
1
F (6,7) = −2vE ωie sin L; F (7,5) = ;
RM + h
sec L vE
F (8,4) = ; F (8,7) = sec LtgL;
RN + h RN + h
F (9,6) = 1.
192 6 INS/GNSS Integrated Navigation Method

FS and FM are:
⎡ ⎤
Cbn 03×3
⎢ ⎥
FS = ⎢
⎣ 03×3 Cbn ⎥
⎦ and FM = [06×6 ],
03×3 03×3

respectively.
2) Measurement equation
Take the difference of the output position and velocity of the SINS and GNSS as
measurement quantity, wherein the velocity measurement vector Zv is:
⎡ ⎤
vIE − vGE
⎢ ⎥
Zv (t) = ⎢ ⎥
⎣ vIN − vGN ⎦ = Hv X(t) + Vv (t), (6.2)
vIU − vGU

wherein
   
Hv = 03×3 diag 1 1 1 03×9 , (6.3)
 T
Vv = vGE vGN vGU . (6.4)

Position measurement vector Zp is:

⎡ ⎤
LIE − LGE
⎢ ⎥
Zp (t) = ⎢ ⎥
⎣ λIN − λGN ⎦ = Hp X(t) + Vp (t), (6.5)
hIU − hGU

wherein
   
Hp = 03×6 diag RM RN cos L 1 03×6 , (6.6)
 
Vp = NGE NGN NGU . (6.7)

In the above equations, the subscript I represents the SINS, the subscript G represents
the GNSS; vGE , vGN , and vGU are velocity errors of the GNSS along east, north, and
vertical directions; NGE , NGN , and NGU are position errors of the GNSS along east,
north, and vertical directions.
Combine velocity and position measurement equations, and the measurement
equation of the INS/GNSS integrated navigation shall be:
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
Zv (t) Hv Vv (t)
Z(t) = ⎣ ⎦ = ⎣ ⎦ X(t) + ⎣ ⎦ = H (t)X(t) + V (t). (6.8)
Zp (t) Hp Vp (t)
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System 193

6.3.2 Nonlinear Modeling Method of the Inertial/Satellite

Integrated Navigation System Based on Quaternion Error

The attitude error angle of the SINS generally refers to the misalignment angle be-
tween the mathematics platform coordinate system and the navigation coordinate
system. The misalignment angle is often described by adopting the additive quater-
nion error, multiplicative quaternion error, and rotating vector error. This subsection
mainly takes the additive quaternion for instance to give a brief introduction to
nonlinear modeling method of the SINS/GNSS integrated navigation system [10].
The additive quaternion error is defined as the difference between calculated
quaternion and real quaternion:

δQ = Q̂nb − Qnb = [δq0 δq1 δq2 δq3 ]T , (6.9)

wherein Qnb is the real quaternion from the carrier system to the navigation system,
p
and Q̂nb = Qb is the calculated quaternion.
1. Error Equation of the Strapdown Inertial Navigation System Based on
Quaternion Error
In the SINS, the inertial component is directly installed on the carrier, and it substi-
tutes the mathematics platform [11] (determined by the direction of cosine matrix Cbn
from the carrier coordinate system to the navigation coordinate system) for physics
platform. During the attitude calculation of the SINS, the quaternion algorithm is
widely used due to its simple calculation and high precision. The description of the
attitude error angle of the SINS by using the calculation error of quaternion can
obtain nonlinear quaternion error equation suitable for the large attitude error angle
[12].
1) Attitude error equation
When the SINS carrier system (system b) rotates around the navigation system
(system n), differential equation of quaternion Qnb can be written as:

1  n b 1  n b 1  n b 1  n b
Q̇nb = Qb ωnb = Qb (ωib − ωin b
)= Qb ωib − Qb ωin
2 2 2 2
1 7 b 8 n 1  n   n −1 n  n 
= ωib Qb − Qb Qb ωin Qb (6.10)
2 2
17 b8 n 1 n n
= ωib Qb − [ωin ]Qb ,
2 2
⎡ ⎤
q0 −q1 −q2 −q3
⎢ ⎥
  ⎢ q1 q0 −q3 q2 ⎥
wherein Qnb = ⎢ ⎢
⎥. ωb represents the projection of the
⎥ ib
⎣ q2 q3 q0 −q1 ⎦
q3 −q2 q1 q0
angular velocity of system b relative to the geocentric inertial system (system i) on
194 6 INS/GNSS Integrated Navigation Method

n
the system b. ωin represents the projection of the angular velocity of system n relative
to the system i on the system n.
⎡ ⎤
0 −ωx −ωy −ωz
⎢ ⎥
7 b8 ⎢ ⎢ ωx 0 ωz −ωy ⎥ ⎥
ωib = ⎢ ⎥, (6.11)
⎢ ωy −ω 0 ω ⎥
⎣ z x ⎦
ωz ωy −ωx 0
⎡ ⎤
0 −ωE −ωN −ωU
⎢ ⎥
  ⎢ ω −ωU ωN ⎥
⎢ E 0 ⎥
ωinn
=⎢ ⎥. (6.12)
⎢ ωN ωU 0 −ωE ⎥
⎣ ⎦
ωU −ωN ωE 0

The differential equation of the SINS used for quaternion updating is:
17 b8 n 1 n n
Q̂˙ nb = ω̂ib Q̂b − [ω̂in ]Q̂b , (6.13)
2 2
b
wherein ω̂ib = ωib
b
+ δωibb b
is the measurement value of the gyro, δωib is the measure-
ment error of the gyro; ω̂in = ωin + δωin is the angular velocity of system n relative
n n n

to system i obtained by using the position and velocity calculation values.

Subtract Eq. (6.10) from Eq. (6.13) to obtain the attitude error equation of the
SINS represented by the additive quaternion [13, 14]:
17 b8 n 1 n n 17 b8 n 1 n n
δ Q̇ = ω̂ Q̂ − [ω̂ ]Q̂ − ω Q + [ω ]Q
2 ib b 2 in b 2 ib b 2 in b
17 b8 n 17 b8 n 17 b8 n
= ωib Qb + ωib δQb + δωib Q̂b
2 2 2
1 n n 1 n 1 17 b8 n 1 n n
− [ωin ]Qb − [ωin ] δQb − [δωin
n n
]Q̂nb − ωib Qb + [ωin ]Qb
2 2 2 2 2
17 b8 n 1 n 1 7 8 1
= ωib δQb − [ωin ] δQnb + δωib b
Q̂nb − [δωin
n
]Q̂nb .
2 2 2 2
(6.14)

It is known that
7 b8 n    
δωib Q̂b = U Q̂nb δωib
b
, n
[δωin ]Q̂nb = Y Q̂nb δωin
n
, (6.15)
⎡ ⎤
−q̂1 −q̂2 −q̂3
⎢ ⎥
  ⎢ q̂ −q̂3 q̂2 ⎥
⎢ 0 ⎥
wherein U Q̂nb =U=⎢ ⎥.
⎢ q̂3 q̂0 −q̂1 ⎥
⎣ ⎦
−q̂2 q̂1 q̂0
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System 195

From

UT U = I3×3 , YT U = Ĉnb (6.16)

it is obtained that
1
δ Q̇ = MδQ + (Uδωib
b
− Yδωin
n
), (6.17)
2
wherein
17 b8 1 n
M≡ ω − [ωin ]. (6.18)
2 ib 2
Equation (6.17) is just a linear differential equation of δQ, i.e., the attitude error
equation of the SINS under large misalignment angle circumstance.
2) Velocity error equation
Velocity differential equation of the SINS on the matrix of system n is expressed as
[4, 9]:
 n 
V̇tn = Cnb f b − 2ωie + ωen
n
× Vtn + gn , (6.19)

wherein subscript t represents the value under the real geographic coordinate system.
In practice, the velocity equation of the SINS used for navigation calculation is:
 n 
V̇cc = Ĉnb f̂ b − 2ω̂ie + ω̂en
n
× Vcc + gc , (6.20)

wherein Vcc is the calculation value of the velocity, and g c is the calculation value of
the local gravity vector.
Subtract Eq. (6.19) from Eq. (6.20) to obtain the velocity error equation and then
use:

f̂ b = f b + ∇ b , Vcc = Vtn + δV, ω̂ie

n
= ωie
n
+ δωie
n n
, ω̂en = ωen
n
+ δωen
n

to obtain
 n     n 
δ V̇ = Cnb f̂ b + Cnb ∇ b − ωie + ωin
n
× δV − 2δωie
n
+ δωen
n
× Vt + δV + δgn ,
(6.21)

wherein Cnb = Ĉnb − Cnb is the calculation error of the attitude matrix, and Cnb f̂ b
is the nonlinear function of δQ.    
And from Eq. (6.16), i.e., Ĉnb = YT Q̂nb U Q̂nb , it is obtained that
       
Cnb = YT Q̂nb U Q̂nb − YT Qnb U Qnb
       
= YT Q̂nb U Q̂nb − YT Q̂nb − δQ U Q̂nb − δQ
   
= YT (δQ)U Q̂nb + YT Q̂nb U(δQ) − YT (δQ)U(δQ).
196 6 INS/GNSS Integrated Navigation Method

From the expressions of Y and U, it is verified that

   
YT (δQ)U Q̂nb = YT Q̂nb U(δQ).

That is, the above equation can be rewritten as:

 
Cnb = 2YT (δQ)U Q̂nb − YT (δQ)U(δQ). (6.22)

Or, make Q̂nb = Qnb + δQ, and Cnb can be written as the function of the real
quaternion Qnb and δQ:
       
Cnb = YT Qnb + δQ U Qnb + δQ − YT Qnb U Qnb
   
= YT (δQ)U Qnb + YT Qnb U(δQ) + YT (δQ)U(δQ)
 
= 2YT (δQ)U Qnb + YT (δQ)U(δQ). (6.23)

Think about

δgn = 0.
n
Ignore small quantity of second order (2δωie + δωen
n
) × δV, and from the equation
     
2YT (δQ) U Q̂ f̂ b = −2 Ĉnb f̂ b × YT Q̂ δQ + 2Ĉnb f̂ b Q̂T δQ (6.24)

the nonlinear velocity error equation of the SINS under large misalignment angle
circumstance can be obtained:
   
δ V̇ = −2 Ĉnb f̂ b × YT Q̂ δQ + 2Ĉnb f̂ b Q̂T δQ − YT (δQ) U (δQ) f̂ b
 n   
+ Cnb ∇ b − 2ωie + ωen
n
× δV − 2δωie
n
+ δωen
n
× Vn . (6.25)

3) Position error equation

The position error equation is the same as the linear modeling method. Refer to
Sect. 2.3 of this book for details.
4) Inertial component error equation
Refer to Sect. 2.3 of this book for details.
2. Nonlinear Model of the SINS/GNSS Integrated Navigation System Based on
Quaternion Error
With the error equation of the SINS as the state equation, the SINS/GNSS integrated
navigation system takes the difference of respective output position and the velocity
of the SINS and GNSS as measurement quantity [15].
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System 197

1) State equation
It is the error equation of the SINS represented by the additive quaternion, rewritten
as follows:
Attitude error equation:
17 b8 1 n 1
δ Q̇ = ω δQ − [ωin ]δQ + (Uδωib
b
− Yδωin
n
).
2 ib 2 2
Velocity error equation:
   
δ V̇n = −2 Ĉnb f̂ b × YT Q̂ δQ + 2Ĉnb f̂ b Q̂T δQ − YT (δQ) U (δQ) f̂ b
 n   
+ Cnb ∇ b − 2ωie + ωen
n
× δV − 2δωie
n
+ δωen
n
× Vn .

Position error equation:

L̇ 1
δ L̇ = − δh + δvN .
RM + h RM + h
λ̇ sec L
δ λ̇ = λ̇ tan L · δL − δh + δvE .
RN + h RN + h
δ ḣ = δvU .

Inertial component error equation:

∇˙ x = 0, ∇˙ y = 0, ∇˙ z = 0, ε̇x = 0, ε̇y = 0, ε̇z = 0.

Suppose the state variable of the system to be x = [xTa , xTe ]T .

 T
xa = δL δλ δh δvE δvN δvU δq0 δq1 δq2 δq3 .

 T
xe = ∇x ∇y ∇z εx εy εz .

So the state equation of the system can be written as:

ẋ(t) = f (x, t) + w (t), (6.26)

wherein f is the nonlinear function of the system, w is the white noise of the system,
and E[w(t)wT (t)] = Q(t).
Express it in the matrix form as follows:
⎡ ⎤ ⎡ ⎤
 
ẋa FN FS
⎣ ⎦=⎣ ⎦ xa + q (x, t) + GW, (6.27)
ẋe 06×10 06×6 xe
198 6 INS/GNSS Integrated Navigation Method
⎡ ⎤
03×3 03×3
⎢ ⎥
⎢ n ⎥
wherein FS = ⎢ Cb 03×3 ⎥;
⎣   ⎦
1
04×3 U Q̂nb
2
⎡ ⎤
F11 F12 03×4
⎢ ⎥
coefficient matrix FN = ⎢
⎣ F21 F22 F23 ⎥
⎦; q(x, t) is the nonlinear part,
F31 F32 F33
⎡ ⎤ ⎡ ⎤
03×1 03×1
⎢ ⎥ ⎢ ⎥
q (x, t) = ⎢ ⎥ ⎢ b ⎥
⎣ f1 (x, t) ⎦ = ⎣ −Y (δQ) U (δQ) f̂ ⎦;
T

010×1 010×1
⎡ ⎤
03×3 03×3
⎢ ⎥
⎢ Cn ⎥
⎢ 03×3 ⎥
⎢ b
⎥
the system noise transfer matrix G = ⎢
⎢ 04×3 1  n ⎥;
⎥
⎢ U Q̂b ⎥
⎣ 2 ⎦
06×3 06×3
 T
the system noise W = w∇x w∇y w∇z wεx wεy wεz , and the elements are
the random errors of the accelerometer and gyro,
⎡ ⎤
0 0 0
⎢ ⎥
⎢ vE
sec L tan L 0 0 ⎥
wherein F11 = ⎢ ⎥,
⎣ RN + h ⎦
0 0 0
⎡ ) ⎤
0 1 (RM + h) 0
⎢ ) ⎥
F12 = ⎢⎣ sec L (RN + h) 0 0 ⎥⎦,
0 0 1
⎡
v  ⎤
E
sec2 L + 2
cos L vN + 2
sin L · vU 0 0
⎢ RN + h
 ⎥
⎢ ⎥
F21 = ⎢⎢ −
vE
sec L + 2
cos L vE
2 ⎥
0 0 ⎥,
⎣ RN + h ⎦
−2
sin L · vE 0 0
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 199
⎡ ) )     ⎤
vN tan L (RN + h) − vU (RN + h) 2
+ λ̇ sin L − 2
+ λ̇ cos L
⎢   ) ⎥
⎢ ⎥
F22 =⎢ −2
+ λ̇ sin L −vU (RM + h) −L̇ ⎥,
⎣ ⎦
 
2
+ λ̇ cos L 2L̇ 0
   
F23 = −2 Ĉnb f̂ b × YT Q̂ + 2Ĉnb f̂ b Q̂T ,
⎡ ⎤
−q̂1 −q̂2 −q̂3 ⎡ ⎤
⎢ ⎥ 0 0 0
⎢ ⎥
⎢ q̂0 q̂3 −q̂2 ⎥ ⎢ ⎥
1⎢ ⎥⎢ ⎥
=− ⎢ ⎥⎢ −
sin L ⎥
F31 ⎢ ⎥⎢ 0 0 ⎥,
2 ⎢ −q̂3 q̂0 q̂1 ⎥ ⎢ ⎥
⎢ ⎥⎣ vE ⎦
⎣ ⎦
cos L + sec2 L 0 0
q̂2 −q̂1 q̂0 R N + h
⎡ ⎤
−q̂1 −q̂2 −q̂3 ⎡ ⎤
⎢ ⎥ 0 −1/(RM + h) 0
⎢ ⎥
⎢ q̂0 q̂3 −q̂2 ⎥ ⎢
⎢
⎥
⎥
1⎢ ⎥⎢ ⎥
F32 =− ⎢ ⎢
⎥ ⎢ 1/(RN + h)
⎥ 0 0 ⎥,
2 ⎢ −q̂3 q̂0 q̂1 ⎥ ⎢
⎣
⎥
⎦
⎢ ⎥
⎣ ⎦ tan L/(R + h) 0 0
N
q̂2 −q̂1 q̂0
17 b8 1 n
F33 = ω − [ωin ].
2 ib 2

2) Measurement equation
Take the difference of the output position and the velocity of the SINS and GNSS as
measurement value, and the measurement equation can be written as:

z = Hx + v, (6.28)
 T
wherein the measurement quantity z = δL δλ δh δvE δvN δvU , the mea-
  
surement matrix H = I6×6 | 06×10 , the measurement noise v = vδL vδλ vδh vvE
T
vvN vvU , and the elements are position and velocity measurement noises.

6.4 High-Precision Inertial/Satellite Integrated Navigation

Method

As dual-use high technology, the SINS/GNSS integrated navigation technology has

been widely used in such fields as aviation and sailing [16] and is developing to-
ward high precision and microminiature and low cost at present. Specific to the
200 6 INS/GNSS Integrated Navigation Method

high-precision SINS/GNSS integrated navigation system, this section mainly in-

troduces the SINS/GNSS integrated navigation method based on mixed correction,
self-adaptive feedback correction filter method based on the observability normal-
ization processing method, the SINS/GNSS outlier-resistant integrated navigation
method based on Kalman filtering innovation orthogonality, air maneuvering align-
ment method based on observability analysis and lever arm error compensation, and
SINS/GPS integrated estimation method based on unscented Rauch–Tung–Striebel
(R–T–S) smoothing.

6.4.1 Inertial/Satellite Integrated Navigation Method Based on

Mixed Correction

It is known from Sect. 6.2 that the SINS/GNSS integrated navigation system has two
correction ways—output correction and feedback correction. In practice, the Kalman
filter needs a period of time from the beginning of filtering to the stability. At the
beginning of filtering, the estimation error is large, which is not suitable for feedback.
If the filtering is fully synchronized with the feedback during the operation of the
whole system, i.e., the feeding estimation value obtained during each step back to
the SINS before setting the predictive value of the next step as zero, precision of the
whole system will be reduced. To solve this problem, a mixed correction program
combining the output correction with the feedback correction will be introduced
below to improve the estimation precision of the Kalman filter [17].
1. Mixed Correction Program of the SINS/GNSS Integrated Navigation System
The SINS/GNSS integrated navigation system adopts the Kalman filter for parameter
estimation, and the functional block diagram of its mixed correction program is as
shown in Fig. 6.5.
In Fig. 6.5, the dotted line represents the feedback correction; the rate of output
correction is the same as that of the Kalman filter, and the rate of feedback correction
is adjustable. Suppose the rate of feedback correction is slower than the rate of output
correction and the output correction is still conducted during the period of feedback
correction, so the Kalman filter of mixed correction is formed.
During the output correction, the filtering equation used is as follows:
 
9k = P
K 9k k−1 H T + Rk −1 .
9k/k−1 H T Hk P
k / k
X 9k/k−1 + K
9k = X 9k/k−1 ).
9k (Zk − Hk X
9k/k−1 = φk,k−1 X
X 9k−1 . (6.29)
P 9k Hk )P
9k = (I − K 9k/k−1 (I − K
9k Hk )T + K
9k Rk (K
9k )T .
9k−1 φ T
9k/k−1 = φk,k−1 P
k,k−1 + k−1 Qk−1 k−1 .
T
P
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 201

Navigation error correction

(Feedback
(Misalignment angle, position,
correction)
velocity)
Navigation data
SINS
IMU mechanical layout

(Output
correction)
+
Kalman
GNSS receiver filter
 State estimation value

Fig. 6.5 Functional block diagram for mixed correction program of the ship inertial navigation
system (SINS)/global navigation satellite system (GNSS) integrated navigation system. IMU inertial
measurement unit

During the feedback correction, the filtering equation used is as follows:

9k/k−1 = 0.
X
9k = K
X 9k Zk .
9k = P
K 9k/k−1 H T + Rk )−1 .
9k/k−1 H T (Hk P (6.30)
k k
P 9k−1 φ T
9k/k−1 = φk,k−1 P + T
k−1 Qk−1 k−1 .
k,k−1
9k = (I − K
P 9k Hk )P
9k/k−1 (I − K 9k Hk )T + K
9k Rk (K
9k )T .

2. SINS/GNSS Integrated Navigation System Model

Adopt the linear model of the SINS/GNSS integrated navigation system based on 
angle in Sect. 6.3 to conduct the integrated navigation filtering simulation.
3. Comparison of Kalman Filtering Simulation Results Under Different Cor-
rection Modes
In the SINS, the constant drift of the gyro is 0.02◦ /h, the random drift is 0.01◦ /h,
the constant bias of the accelerometer is 100 μg, the random bias is 50 μg, the
misalignment angle of the initial level is 20 , and the misalignment angle of initial
heading is 6 . The GNSS adopts the GPS, its measurement data position error is 0.3 m,
and the velocity error is 0.02 m/s. The initial geographic position is 40◦ north latitude
and 116.3◦ east longitude. The simulated flight path adopted is flying straightly
toward north for 3000 s, where the velocity is 150 m/s, the flight height is 8000 m,
and the cycles of output correction and feedback correction are both 1 s.
202 6 INS/GNSS Integrated Navigation Method

Fig. 6.6 East position and

velocity error subject to
output correction

Fig. 6.7 East position and

velocity error subject to
feedback correction

1) Filtering simulation results of output correction and feedback correction

Under the above simulation conditions, the filtering simulation results of the east
position error and the east velocity error subject to output correction and feedback
correction are as shown in Figs. 6.6 and 6.7.
It is observed from the figures of simulation results that the output navigation
parameters of output correction and feedback correction programs are both subject
to deviation with the increase of time.
2) Filtering simulation results of mixed correction
East position error and east velocity error simulation results subject to adjustment of
feedback correction cycle to 20 s by using above mixed correction program are as
shown in Fig. 6.8.
It is observed from the above figures that the navigation precision of the Kalman
filter program adopting mixed correction is higher than that of adopting output cor-
rection and feedback correction, and the navigation parameters are not subject to the
drift with time, so that it is suitable for longtime autonomous operation [17].
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 203

Fig. 6.8 East position and

velocity errors subject to
mixed correction

4. Conclusion
1. Output correction only improves the accuracy of the output but does not correct
the internal error state of the inertial navigation and has errors accumulating with
time, which will finally lead to the misfit of the mathematical model for the actual
system and reduction of the integrated navigation precision.
2. Feedback correction can correct the internal error state of the inertial naviga-
tion, but the initial filtering state is not estimated accurately since the Kalman
filter requires certain time for convergence, which will lead to a deviation of the
navigation error of the integrated navigation system after being fed back.
3. Mixed correction synthesizes the advantages of output correction and feedback
correction and improves the SINS/GNSS integrated navigation precision without
addition of any calculation quantity, so its overall performance is superior to
output correction and feedback correction.

6.4.2 Self-Adaptive Feedback Correction Filter Method Based on

Observability Normalization Processing Method

Mixed correction method combining output correction and feedback correction in-
troduced previously improves the SINS/GNSS integrated navigation precision, but
it can only improve the precision of the observable state within the system. Gener-
ally, the SINS/GNSS integrated navigation system is not a completely observable
system, and the traditional Kalman filter method decides the feedback qualitatively
only according to the estimation precision of the filter, which is determined by the
observability of the system state variable in essence [18, 19]. Therefore, determining
the feedback quantity through the establishment of quantitative relation between the
feedback quantity of the system state variable and the observability can improve the
precision of the SINS/GNSS integrated navigation system fundamentally.
204 6 INS/GNSS Integrated Navigation Method

Generally, observability is defined as the observability of a certain state quantity

being equivalent to the ratio between the corresponding singular value of the state
quantity and the corresponding singular value of the external observation quantity,
as shown in Eq. (6.31) [18]:
σk
ηk = , (6.31)
σo
wherein ηk is the observability of the k system state, σk is the corresponding singular
value of the k system state, and σo is the corresponding maximum singular value of
the system state with external observation quantity.
Observability of the attitude angle will be greater than that of velocity with the
external observation quantity when applying Eq. (6.31) to the SINS/GNSS inte-
grated navigation system for information fusion [19], but the attitude angle can not
be observed directly in practice, and it is known from qualitative analysis that its ob-
servability is smaller than that of velocity. Thus, it can be seen that Eq. (6.31) is not
suitable for the judgment of observability of the SINS/GNSS integrated navigation
system.
1. An Improved Method for Normalization Processing of System State Observ-
ability
Some researchers have researched the influences of various maneuvering modes on
the observability of the SINS/GNSS integrated navigation system according to anal-
ysis methods of observability [20], and analysis results indicated that maneuvering
of the carrier may improve the observability of the system state. When the carrier is
subject to S maneuvering, the observability of the SINS/GNSS integrated navigation
system is the highest, so S maneuvering is also called optimal maneuvering. This
subsection defines the ratio between the corresponding singular value of each state
and the corresponding singular value of the state during S maneuvering as observ-
ability of the state, as shown in Eq. (6.32). In such a case, the state observability of
the system is limited within the scope of [0,1] under all maneuvering modes of the
carrier.
σk
ηk = , (6.32)
σSk
wherein σSk is the corresponding singular value of the k system state subject to S
maneuvering of the carrier.
2. A New Self-adaptive Feedback Correction Filter Method of State Observ-
ability Based on the Improved Normalization Processing Method
In the SINS/GNSS integrated navigation system, the feedback quantity of each sys-
tem state variable is closely related to the estimation precision of the state, so that
the reduction or even divergence of precision of the Kalman filter will be caused
if the feedback of the state with low estimation precision is given directly. Since
the precision and velocity of the system state estimation is determined by the ob-
servability of the system state, it is required to consider the establishing quantitative
relation between the observability of the system state and the feedback quantity
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 205

Output position, velocity, attitude and other

SINS navigation information of the carrier

+
Kalman filter X̂
-
GNSS

η k Xˆ η
Weighted feedback factor k
(Observability subject to normalization processing

Normalization processing of observability of system state variable

Analysis of system state observability based on singular value decomposition

Fig. 6.9 Ship inertial navigation system (SINS)/global navigation satellite system (GNSS) self-
adaptive feedback filtering based on observability analysis

to decide the feedback quantity according to the observability of the system state.
During the observability analysis based on the singular value decomposition, the
observability of each system state is generated subject to normalization processing,
and the observability of the system state will be limited within the scope of [0,1].
Therefore, it is considered to take the observability of the system state directly as the
feedback factor of the filter state estimation, so when the observability of a certain
system state is 1, it represents that the system state is completely observable, and
the estimation of the system state estimated by the filter can be fed back completely.
When the observability of a certain system state is 0, it represents the system state
is completely unobservable, and the state will not be fed back. The functional block
diagram of the SINS/GNSS self-adaptive feedback correction filtering based on the
system observability analysis is as shown in Fig. 6.9.
Implement integrated navigation filtering for real flight data by adopting the linear
model of the SINS/GNSS integrated navigation system based on  angle in Sect. 6.3
and combining the above self-adaptive feedback correction thought [3]. The GNSS
adopts single-point GPS during the test and combines with the flexible gyro SINS.
The three constant drifts of gyro in the SINS are 0.103◦ /h, 0.112◦ /h, and 0.137◦ /h,
respectively; the three random drifts of gyro are 0.053◦ /h, 0.051◦ /h, and 0.048◦ /h,
respectively; the three constant biases of accelerometer are 115 μg, 89 μg, and
106 μg, respectively, and the three random biases are 61 μg, 44 μg, and 50 μg.
The velocity error of GPS is 0.1 m/s, the single-point positioning error is 5 m. The
position error is influenced by the distance between GPS base station and the moving
206 6 INS/GNSS Integrated Navigation Method

Fig. 6.10 Flight path of the SINS/GPS integrated navigation system during a flight test

station during differentiation, and the differential position error is 0.05 m when the
distance between the two is less than 20 km.
A test was conducted at an airport located in the suburban area of Beijing. One
flight path is as shown in Fig. 6.10.
Since actual position, velocity, and attitude of the aircraft can not be obtained
during flight test [21, 22], it is hard to calculate the position error, velocity error,
and attitude error of the SINS/GPS integrated navigation system. The position and
velocity provided by the carrier phase double-frequency differential GPS may be
adopted as evaluation benchmark to evaluate the precision of self-adaptive feedback
correction SINS/GPS Kalman filter; since the distance between the GPS base station
and the moving station is less than 20 km, the positioning precision of the carrier-
phase double-frequency differential GPS can reach centimeter level and the velocity
error can be less than 0.05 m/s. Its precision is much higher than that of the SINS
and the single-point GPS combination [23]. For the attitude error of self-adaptive
feedback correction SINS/GPS Kalman filter, evaluation is impossible since there is
no more accurate attitude reference.
The traditional feedback correction Kalman filtering and self-adaptive feedback
correction Kalman filtering are adopted during the test for comparison, and the test
results are as shown in Fig. 6.11.
It is observed from Fig. 6.11 that traditional feedback correction Kalman filtering
directly gives feedback on an unobservable state or state with low observability since
the SINS/GPS integrated navigation system is not completely observable, which
leads to instability of the filter and decline of precision. However, the new self-
adaptive feedback correction filter method based on state observability conducts
self-adaptive feedback correction according to the observability of the system state,
and its system error does not diverge with time, which improves the precision of the
SINS/GPS integrated navigation system [3].
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 207

East velocity error

Traditional feedback correction filtering

Self-adaptive feedback correction filtering

North velocity error

Traditional feedback correction filtering

Self-adaptive feedback correction filtering

Vertical direction velocity error

Traditional feedback correction filtering

Self-adaptive feedback correction filtering

Fig. 6.11 SINS/GPS error comparison based on self-adaptive feedback correction filtering and
traditional feedback correction Kalman filtering
208 6 INS/GNSS Integrated Navigation Method

Height error

Traditional feedback correction filtering

Self-adaptive feedback correction filtering

Latitude error

Traditional feedback correction filtering

Self-adaptive feedback correction filtering

Longitude error

Traditional feedback correction filtering

Self-adaptive feedback correction filtering

Fig. 6.11 (Fortsetzung)

6.4 High-Precision Inertial/Satellite Integrated Navigation Method 209

6.4.3 Inertial/Satellite Outlier-Resistant Integrated Navigation

Method Based on Kalman Filtering Innovation
Orthogonality

What was discussed in Sects. 6.4.1 and 6.4.2 are the SINS/GNSS integrated nav-
igation methods during normal operation of the GNSS. However, during GNSS
measurement, abnormalities including electromagnetic interference, shelter (such as
buildings and boulevard in the city), and carrier maneuvering will make the GNSS
signal quality decline, which will result in the occurrence of “outliers” in result
of GNSS measurement. During SINS/GNSS integrated navigation data processing,
the GNSS provides filtering with measurement information and outliers which will
directly influence the precision and stability of filtering[24–28]. This subsection in-
troduces a SINS/GNSS outlier-resistant integrated navigation method based on the
Kalman filtering innovation orthogonality. The method judges whether there is an
outlier appearing in the measurement by judging whether the orthogonality of the
Kalman innovation process is lost, and then weighting an activation function to the
measurement quantity to inhibit the adverse influence of the GNSS outlier on filtering
and realize high-precision SINS/GNSS integrated navigation [29].
1. Outlier Identification Principle Based on Kalman Filtering Innovation Or-
thogonality
Think about the multiple-input-multiple-output linear discrete system given in Eqs.
(3.1) and (3.2) in Sect. 3.2 of this book. Zk − H k X̂ k/k−1 in the basic Eq. (3.5) of
standard discrete Kalman filtering is called innovation process and denoted by ek ,
which reflects new information brought by current measurement.
It is observed from Eq. (3.5) that since Kalman filtering is a linear optimal fil-
tering [30], when the measurement quantity contains an outlier, in accordance with
the principle of linear superposition, the outlier will influence the state estimation of
Kalman filtering directly in the form of linear superposition, which will result in the
decline of reliability and the convergence rate of the Kalman filter, or even filtering
divergence and stability loss.
Since the innovation process ek has orthogonality property [30, 32] and contains
all information during the measurement process, the original property of ek is bound
to be changed when an outlier appears during the measurement process. Therefore,
it is possible to judge whether there is an outlier in the measurement quantity by
judging whether orthogonality of the innovation process is lost.
In accordance with the orthogonality of the innovation process, we have [31]
     T

E Zk ZTk = E ek eTk + E Ẑk Ẑk , (6.33)

wherein Ẑk = H k X̂ k/k−1 is the optimal estimation of Zk .

Expand the right side of Eq. (6.33), and we can obtain
  T
E Zk ZTk = H k P k/k−1 H Tk + Rk + H k X̂ k/k−1 X̂ k/k−1 H Tk . (6.34)
210 6 INS/GNSS Integrated Navigation Method

Make
T
Dk = H k P k/k−1 H Tk + Rk + H k X̂ k/k−1 X̂ k/k−1 H Tk . (6.35)

Assume and judge whether each component of the measurement quantity Zk is the
outlier according to the diagonal element of the matrix at both sides of Eq. (6.34),
where the judgment is as follows:

Mi,i (k) ∈ [Di,i (k) − εi , Di,i (k) + εi ] i = 1, 2, · · · , m, (6.36)

wherein Mi,i (k) and Di,i (k) represent the i element on diagonal of E[Zk ZTk ] and Dk ,
respectively. Refer to the References [4] for the computing method of E[Zk ZTk ]. If
Eq. (6.36) is true, then Zi (k) is deemed to be the normal measurement quantity;
otherwise, it is deemed to be an outlier. In view of the calculation error, a small
quantity disturbance εi is added to the above judgment.
2. Measurement Correcting Algorithm of Kalman Filtering [29]
After identifying the outliers in the measurement quantity Zk , adopt the method of
weighting and limiting Zk by using an activation function to eliminate the adverse
influence of outliers on Kalman filtering and maintain the orthogonality of the Kalman
filtering innovation process.
In view of measurement correction, correction forecast equation as shown in Eq.
(6.37) is obtained from Eq. (3.5) of Chap. 3 of this book. Other equations in basic
Kalman filtering equation remain unchanged.
 
X̂ k = X̂ k/k−1 + K k Bk − H k X̂ k/k−1 , (6.37)
 T
wherein Bk = Z1 (k) f1 (r1 ) Z2 (k) f2 (r2 ) · · · Zm (k) fm (rm ) , and
fi (ri )(i = 1,2 · · ·, m) is the activation function with the following properties:
1. When ri → ∞, fi (ri ) → 0.
2. Cutoff property, when ri < C or ri ≥ C, fi (ri ) is a constant, where C is called
threshold value.
3. fi (ri ) is subject to monotonic decrease and continuous differentiability.
To make orthogonality of the Kalman filtering innovation process subject to correc-
tion, it should remain unchanged. The activation function taken shall be relevant to the
current measurement quantity Zk [31]. The following sectionally smooth activation
function is adopted:
⎧
⎨ 1 ri < di
fi (ri ) = i = 1,2 · · ·, m,
⎩ di /ri r ≥d i i

 
wherein ri = Mi,i (k); di = Di,i (k) + εi .
The thought of the above Kalman filtering measurement correcting algorithm is:
Within each filtering cycle, conduct outlier identification and correction for each
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 211

Plane trajectory

Latitude (deg)

Longitude (deg)

Fig. 6.12 Plane trajectory curve of the DGPS

component of the measurement quantity Zk . When Zi (k) is an outlier, there is ri >

di (k), so di (k)/ri less than 1 is used for weighting limitation of Zi (k) to inhibit
the increase of the innovation process variance and to make it retain the original
orthogonality. When Zi (k) is not an outlier, there is ri ≤ di (k), so that the weighted
value of Zi (k) is a unit value, which will not change the innovation series.
3. Field Test
Refer to Sect. 6.3 for system state equation and measurement equation.
A field test was conducted along a highway inYunnan. During the test, the follow-
ing precision of SINS inertial components was observed: Constant drift and random
drift of the gyro were both 0.1◦ /h, constant bias and random bias of the accelerometer
were both 0.0001 g, and data output frequency of inertial component was 100 Hz.
The GNSS adopts high-precision double-frequency carrier phase differential GPS
(hereinafter referred to as DGPS, including moving station and base station) of No-
vatel Company, and its differential positioning precision is of centimeter level with
a data output frequency of 20 Hz.
The area along the test line is not open, outliers appear in position and velocity
data of DGPS for many times, and there is a short-term locking losing phenomenon.
Among them, the altitude curve reflects outliers of DGPS obviously. Figures 6.12 and
6.13 are test plane trajectory and altitude drawn according to DGPS data, respectively.
It is observed from Fig. 6.13 that there are outliers in several locations of the altitude
curve.
Conduct integrated filtering for IMU and DGPS data by adopting the Kalman fil-
ter (KF) outlier-resistant method based on the innovation orthogonality given in this
section and standard KF method. Figures 6.14–6.17 provide longitude, latitude, alti-
tude, and course angle filtering results of the correction KF method and comparison
with corresponding DGPS data and standard KF result.
212 6 INS/GNSS Integrated Navigation Method

altitude

Altitude (m)
Outliers

Magnification

Time (ms)

Fig. 6.13 Altitude curve of the DGPS

DGPS
Correction KF
Standard KF
Correction KF

Latitude (deg) Standard KF

DGPS outlier

Time (s)

Fig. 6.14 Latitude comparison among correction Kalman filtering (KF), differential global
positioning system (DGPS), and standard KF

It is observed from Figs. 6.14–6.17 that when an outlier appears in the DGPS,
standard KF is unstable and the precision declines, but the correction KF method
introduced in this section can effectively identify and inhibit the adverse influence of
DGPS outlier on filtering. For normal DGPS data, filtering precision with the method
of this subsection is the same as that of standard KF. When an outlier appears in the
DGPS, filtering precision and stability with the method of this section are obviously
superior to those of standard KF.
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 213

DGPS
Standard KF
DGPS outlier Correction KF

Longitude (deg)

Standard KF

Correction KF

Time (s)

Fig. 6.15 Longitude comparison among correction Kalman filter (KF), differential global position-
ing system (DGPS), and standard KF

DGPS
Standard KF
Correction KF
Correction KF Standard KF

Altitude (m)

DGPS isolate outlier

Time (s)

Fig. 6.16 Altitude comparison one among correction Kalman filter (KF), differential global
positioning system (DGPS), and standard KF
214 6 INS/GNSS Integrated Navigation Method

DGPS
Standard KF
Correction KF

Correction KF

Altitude (m)

Standard KF

DGPS discrete outlier

Time (s)

Fig. 6.17 Altitude comparison two among correction Kalman filter (KF), differential global
positioning system (DGPS), and standard KF

4. Conclusion
This subsection introduced a SINS/GNSS outlier-resistant integrated navigation
method based on KV innovation orthogonality. Judge whether there is an outlier
appearing in the measurement by judging whether orthogonality of the Kalman in-
novation process is lost, and then weight an activation function to the measurement
quantity to make the innovation process subject to correction to maintain its orthog-
onality. This method may effectively inhibit adverse influence of GNSS outliers on
integrated filtering and improve navigation precision when outliers appear in the
GNSS.

6.4.4 An Air Maneuvering Alignment Method Based on

Observability Analysis and Lever Arm Error Compensation

The previous three subsections improve the SINS/GNSS integrated navigation pre-
cision from the perspective of the filter method, but during practical application,
besides the filter method, the initial alignment precision of the SINS and the lever
arm error between the SINS and GNSS may also influence the integrated navigation
precision. Air maneuvering alignment through improvement of observability for the
SINS/GNSS integrated navigation system during aircraft maneuvering is often an
effective means to improve the integrated navigation precision during flight [33–36].
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 215

Position, velocity, attitude and other navigation

information of the output carrier
SINS
+
Self-adaptive X̂
feedback filter
Lever arm -
GNSS error
compensation

ηk X̂ Weighted feedback factor ηk

(State observability subject to
normalization processing)

Analysis method of SINS/GNSS system state observability

Turning maneuvering of aerial carrier

Fig. 6.18 Air maneuvering alignment method based on observability analysis and lever arm error
compensation. SINS ship inertial navigation system, GNSS global navigation satellite system

Specific to this problem, this subsection introduces a new SINS/GNSS air maneu-
vering alignment method based on system state variable observability analysis and
lever arm effect error compensation [37] to compensate the lever arm effect error be-
tween the GNSS antenna and the SINS and determine the feedback factor according
to the observability of each state variable. Finally, conduct a self-adaptive feedback
correction for the SINS system for the purpose of further improvement of integrated
navigation precision.
1. Air Maneuvering Alignment Program Based on Observability Analysis and
Lever Arm Effect Error Compensation
The basic principle for SINS/GNSS air maneuvering alignment program based on ob-
servability analysis and lever arm effect error compensation is as shown in Fig. 6.18.
Observability of the SINS/GNSS integrated navigation system is improved during
turning maneuvering of the aerial carrier; calculate the observability of each system
state with the observability analysis method and then estimate and correct the head-
ing misalignment angle, two horizontal alignment angles, three gyro drifts, three
accelerometer biases, and other errors by adopting the self-adaptive feedback cor-
rection filter introduced in Sect. 6.4.2. Meanwhile, it can improve the precision of
the GNSS observation quantity and then compensate the lever arm effect error of the
GNSS observation quantity.
216 6 INS/GNSS Integrated Navigation Method

Fig. 6.19 Lever arm effect

error between GNSS antenna
and ship inertial navigation
z
system (SINS) A

SINS measurement

O y
center

x C B

2. Mathematical Model of Air Maneuvering Alignment

Refer to Sect. 6.3 for the system state equation and measurement equation.
3. Method for Lever Arm Effect Error Compensation of GNSS Observation
Quantity
During the practical application of the aerial carrier SINS/GNSS integrated naviga-
tion system, the SINS is often installed on the belly of the aircraft, and the GNSS
antenna on top of the aircraft, which results in misalignment between GNSS antenna
phase center A and SINS measurement center O, and the distance between the two
is r, as shown in Fig. 6.19. The relative velocity error, i.e., lever arm effect error, is
caused between point A and point O during air maneuvering alignment due to the
angular motion of the aircraft, which reduces the precision of the GNSS velocity
observation quantity severely.
Generally, the lever arm effect error maximizes when the aircraft makes a turn,
which severely influences the precision of the air maneuvering alignment, so that the
compensation for it is required.
The angular velocity of the body movement during the turning maneuvering of
b
the aircraft is ωnb , and V represents lever arm effect error caused by misalignment
between the GNSS antenna phase center and the SINS measurement center. The
projection of V on the proprio-coordinate system b of the SINS is Vb , so we
have:
⎡ ⎤ ⎡ ⎤
vbx b
ωnby rz − ωnbz
b
ry
⎢ ⎥ ⎢ ⎥
Vb = ⎢ ⎥ ⎢ b
⎣ vby ⎦ = ⎣ ωnbz rx − ωnbx rz ⎦.
b ⎥ (6.38)
vbz ωnbx ry − ωnby rx
b b

The projection of Vb on the navigation coordinate system n is:

Vn = Cbn Vb . (6.39)
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 217

The velocity observation quantity of the GNSS subject to lever arm effect error
compensation is:

Vn = VGN SS − Vn , (6.40)

wherein VGN SS is the velocity observation quantity of the GNSS prior to compen-
sation, including east velocity, north velocity, and vertical direction velocity; Vn
is the lever arm effect error of GNSS observation quantity, and Vn is the velocity
observation quantity of the GNSS subject to compensation.
4. Simulation Test of Air Maneuvering Alignment
Suppose a constant drift and a random drift of the gyro in the SINS/GNSS integrated
navigation system are 0.1◦ /h and 0.05◦ /h, respectively, the constant bias and random
bias of the accelerometer are 100 μg and 50 μg, respectively; the velocity error of
GNSS is 0.1 m/s, the position error is 5 m, the initial point is 39◦ north latitude and
116◦ east longitude, the initial leading is 45◦ (clockwise is positive), and the aircraft
flies at constant speed of 180 m/s straightly for 900 s and then makes a turn of 180◦
at constant speed, finally flies at constant speed of 180 m/s straightly for 900 s.
Implement the simulation test by adopting the new air maneuvering alignment
method based on observability analysis and lever arm error compensation proposed
in this subsection. The simulation results are as shown in Fig. 6.20.
It is observed from Fig. 6.20 that the observability of the SINS/GNSS integrated
navigation system is poor since the aircraft is under straight flight at constant speed
prior to the air maneuvering alignment, so that the error of SINS/GNSS integrated
navigation system is large. Among them, the gyro constant drift of axis z, ac-
celerometer constant bias of axis x, and the accelerometer constant bias of axis y
are unobservable. The corresponding error cannot be estimated by the filter when the
leading misalignment angle is 22 ; two horizontal misalignment angles are both 10 ;
and for the gyro of axis x and axis y, only about 50 % of the gyro drift is estimated
by the filter. The estimation effect of the filter for the accelerometer constant bias of
axis z is good.
The system observability is improved after the turning maneuvering of the aircraft,
and the precision of the SINS/GNSS integrated navigation system can be greatly
improved by adopting the air maneuvering alignment. Among them, the course error
decreases from 22 to about 2 , two horizontal misalignment angles decrease from
10 to 3 , and more than 95 % of error of the three gyro drifts and three accelerometer
biases can be effectively estimated by adopting the air maneuvering alignment.

6.4.5 SINS/GPS Integrated Estimation Method Based on

Unscented R–T–S Smoothing

KF is the most commonly used algorithm for fusing the outputs of the SINS and GPS
to provide more accurate information than each single sensor [37]. The previous four
subsections introduce the integrated estimate method of the SINS/GNSS based on
218 6 INS/GNSS Integrated Navigation Method

30 10
25 5
20
0
15

Δθ(″)
-5
10
ΔΦ(′) -10
5
0 -15
-5 -20
-10 -25
0 400 800 1200 1600 2000 0 400 800 1200 1600 2000
a Time (t) b Time (t)
Heading misalignment angle Pitching misalignment angle
20 0.04
15 0.02

10 0

εx(°/h)
-0.02
5
Δγ('')

-0.04
0
-0.06
-5
-0.08
-10 -0.1
-15 0 400 800 1200 1600 2000 -0.12 0 400 800 1200 1600 2000
c Time (t) d Time (t)

Roll misalignment angle Gyro drift of axis x

0.02 0.01
0
0
-0.01
-0.02 -0.02
-0.04 -0.03
εy(°/h)

-0.04
εz(°/h)

-0.06 -0.05
-0.08 -0.06
-0.1 -0.07
-0.08
-0.12 -0.09
0 400 800 1200 1600 2000 0 400 800 1200 1600 2000
e Time (t) f Time (t)
Gyro drift of axis y Gyro drift of axis z
140 120
120 100
100
80
80
60
∇y(ug)
∇x(ug)

60
40 40
20 20
0 0
-20 -20
0 400 800 1200 1600 2000
0 400 800 1200 1600 2000 Time (t)
g Time (t) h
Accelerometer bias of axis x Accelerometer bias of axis y
140
120
100
80
∇z(ug)

60
40
20
0
-20
0 400 800 1200 1600 2000
Time (t)

i Accelerometer bias of axis z

Fig. 6.20 Semi-physical simulation results of air maneuvering alignment based on observability
analysis and lever arm error compensation
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 219

the filter. For some applications, such as the airborne earth observation, the images
are processed mostly after the flight. In such off-line cases, post processing methods
such as smoother can be employed to provide a better solution. That is, the optimal
smoothing algorithm’s precision is theoretically higher than that of the KF, since
the smoother uses more observations than the filter [38]. Three classes of smoothers
can be found in the literature [38, 39]: The fixed-interval smoother, the fixed-point
smoother, and the fixed-lag smoother. And a R–T–S fixed-interval smoother [40],
which is based on the KF, has been used in the SINS/GPS problems due to its
robustness and effectiveness.
Generally the post processing of SINS/GPS integration system’s data requires a
few minutes of static data to finish the coarse alignment [41]. However, in some
emergency situations, the SINS/GPS integration is needed and expected to be started
after the carrier moves and before the carrier reaches the area where the mission
takes place, which brings the uncertain errors of initial attitude. In this case, the
misalignment of initial attitude is not small, and the linear SINS error model be-
comes inapplicable. Moreover, the performance of R–T–S smoother based on the
KF degrades due to the linearization of the model. To solve this problem, a noliniear
estimate method named unscented R–T–S smoother (URTSS) will be introduced
below to improve the estimation precision of SINS/GPS integration.
1. Nonlinear Kalman Smoothing Methods
In this subsection, we will give a brief introduction to the extended R–T–S smoother
(ERTSS) and URTSS. Both the ERTSS and URTSS algorithms are based on the
R–T–S smoother which can be classified into two parts: the forward filtering process
and the backward recursion process. The measurements are first processed by the
forward filter, and then, a separate backward smoothing pass is used for obtaining
the smoothing solution [39]. The extended and unscented R–T–S smoother can be
derived as follows.
Consider an n-state discrete-time system:

xk = f (xk−1 , k − 1) + Γ k−1 wk−1 , (6.41)

zk = h(xk , k) + vk , (6.42)

where xk is the state vector,

zk is the measurement vector at time tk , wk−1 ∼ N(0, Qk−1 ) is the Gaussian process
noise, Γk−1 is the process noise distribution matrix, vk ∈ N (0, Rk ) is the Gaussian
measurement noise, f (·) is the dynamic model function, and h(·) is the measurement
function.
1) Extended R–T–S smoother
The forward filtering process of ERTSS is implemented by the extended Kalman
filter (EKF), which is based upon the principle of linearizing the state transition
matrix and the measurement matrix with Taylor series expansions. The EKF time
update and measurement update equations are recursively processed to achieve the
220 6 INS/GNSS Integrated Navigation Method

optimal estimates as follows [42]:

x̂−
k = F k−1 x̂k−1 ,

P−
k = F k−1 P k−1 F k−1 + Γk−1 Qk−1 Γk−1 ,
T T

 −1
K k = P− T − T
k H k H k P k H k + Rk , (6.43)
 −
x̂k = x̂−
k + K k zk − H k x̂k ,

Pk = (I − K k H k ) P k/k−1 (I − K k H k )+ K k Rk K kT ,
where x̂k and x̂−
k are the posterior and prior estimates of state xk , respectively, with
associated estimation error covariance matrix Pk and Pk− . K k is the EKF gain matrix.
System transition matrix Fk−1 and the linearized measurement matrix H k are given
by:

∂f (xk−1 , k − 1) 
Fk−1 =  ,
∂x
 x=x̂k−1
(6.44)
∂h (xk , k) 
Hk =  .
∂x x=x̂k

The backward recursion process of ERTSS is based on the R–T–S formulation, which
propagates the filtering outputs and achieves the smoothing results. The smoothed
state estimate and its covariance can be computed with the following equations for
k = N − 1, N − 2, . . ., 0 (N is the last time step) [43]:
 − −1
KkS = Pk FTk−1 Pk+1 ,
 
x̂Sk = x̂k + KkS x̂Sk+1 − x̂−
k+1 ,
(6.45)
 S −
  S T
PkS = Pk + KkS Pk+1 − Pk+1 Kk ,
where the superscript S denotes the smoothing, and the filter information are stored
in the forward process. The recursion is started from the filter results at the last time
step: x̂SN = x̂N , PNS = PN .
2) Unscented R–T–S smoother
The unscented Kalman filter (UKF) approximates the probability density resulting
from the nonlinear transformation of a random variable instead of approximating the
nonlinear functions with a Taylor series expansion [44]. In order to approximate the
mean and covariance of xk, a set of deterministic vectors named sigma-points are
defined as follows [45]:
     
√ √
χ̂k−1 = x̂k−1 x̂k−1 + n + λ Pk−1 x̂k−1 − n + λ Pk−1 j = 1,2, ..., n
j j
(6.46)
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 221

where the parameter λ is given by λ = a 2 (n + k) − n, n is the dimension of state

vector, the constant α determines the spread of the sigma points and is usually set
to a small positive value(e.g., 10−4 ≤ α ≤ 1), the constant k is usually set to 0 or
3 − n. And the corresponding weights are given by:

W0(m) = λ/(n + λ)W0(c) = λ/(n + λ) + (1 − α 2 + β)

(6.47)
W1(m) = W1(c) = 0.5/(n + λ) i = 1,2, ...2n

where the superscripts m and c denote the weights of mean and covariance, respec-
tively; the parameter β is used to incorporate prior knowledge of the distribution (for
Guassian distribution, β = 2 is optimal).
Just like the ERTSS, the nonlinear R–T–S smoother based on the UKF also
contains two parts. Equations of URTSS algorithm are given as follows [46]:
Part I: Forward filtering process (for k = 1,2, . . ., N ).
• Propagate the sigma points through the dynamic model.
 
χ̂k = f χ̂k−1 , k − 1 (6.48)

• Compute the prior state estimate x̂k− , the prior error covariance Pk− , and the
cross-covariance Ck between x̂k−1 and x̂k− .

!
2n
x̂−
k = Wi(m) χ̂ −
k,i ,
i=0
!
2n
P−
k = Wi(c) (χ̂ − − − − T
k,i − x̂k )(χ̂ k,i − x̂k ) + Γ k−1 Qk Γk−1 ,
T
(6.49)
i=0
!
2n
Ck = Wi(c) (χ̂ k−1,i − x̂− − − T
k−1 )(χ̂ k,i − x̂k ) .
i=0

• Propagate the sigma points through the measurement model and compute the
predicted measurement ŷk .

γ̂k = h(χ̂ −
k , k),

!
2n (6.50)
ŷk = Wi(m) γ̂k,i .
i=0

• Compute the covariance of the predicted measurement P yy , and the cross-

covariance P xy between x̂−
k and ŷk .

!
2l
P yy = Wi(c) (γ̂ k,i − ŷk )(γ̂k,i − ŷk )T ,
i=0
(6.51)
!2l
P xy = Wi(c) (χ̂ −
k,i − x̂−
k )(γ̂ k,i − ŷk ) ,
T
i=0
222 6 INS/GNSS Integrated Navigation Method

where l is the dimension of measurement vector.

• Compute the filter gain K k , the posterior state estimate x̂k , and the posterior error
covariance P k .

K k = P xy P yy −1 ,

x̂k = x̂−
k + K k (zk − ŷk ),
(6.52)

Pk = P−
k − K k P yy K k .
T

Part II: Backward recursion process (for k = N − 1, N − 2, . . ., 0)

• Compute the smoother gain K Sk .

K Sk = C k+1 (P − −1
k+1 ) . (6.53)

• Compute the smoothed state estimate x̂Sk and the smoothed error covariance P Sk .

 
x̂Sk = x̂k + K Sk x̂Sk+1 − x̂−
k+1 ,
(6.54)
   S T
P Sk = P k + K Sk P Sk+1 − P −
k+1 K k .

The forward filtering process is computed by the UKF. The prior estimates of mean
x̂− −
k , covariance P k , and crosscovariance C k should be stored in the forward filter
stage. Since the backward recursion distribution and the forward filtering distribution
of the last time step N are the same, we have x̂SN = x̂N ; P SN = P N .
2. Smoothing Methods for the SINS/GPS Integration System
1) SINS error model
The SINS error equations are the basis of the SINS/GPS integrated estimation, which
include the attitude, velocity, and position errors. The coordinate frames used to
derive the SINS/GPS equations can be found in Sect. 2.2 and the reference [28].
The differential equation of the attitude error based on the angle error model is
given as follows [47]:
  n
ˆ = I − Cnp ωin + δωinn
− Ĉbn ε b , (6.55)

where  = [ E N U ]T is the attitude error vector, ωin n

is the rotation velocity
n
of the n-frame relative to the i-frame expressed in the n-frame, and δωin is the error
of ωin .ε = [ εx εy εz ] is the gyroscope’s random constant drifts in the b-frame
n b T

with models ε̇ b = 0, Ĉbn is the estimation of direction cosine matrix Cbn , which is the
p
transformation matrix between the b-frame and the n-frame. Cn is the attitude error
matrix.
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 223

And the corresponding differential equation of the velocity error is given as

follows:
     n 
δ V̇n = I − Cpn Ĉbn f̂ b − 2δωie
n
+ δωen
n
× Vn − 2ωie + ωen
n
× δVn + Ĉbn ∇ b ,
(6.56)

where Vn = [VE VN VU]T is the velocity of the b-frame relative to the e-frame ex-
pressed in the n-frame, and δVn is the velocity error. fb is the specific force measured
n n
by the accelerometers expressed in the b-frame, δωie and δωen are the rotation ve-
locity errors of the e-frame relative to the i-frame and n-frame relative to the e-frame
expressed in the n-frame, respectively. ∇ b = [∇x ∇y ∇z ]T is the accelerometer’s
random constant biases in the b-frame with models ∇˙ b = 0.   n
The nonlinear terms of attitude and velocity error equations are I − Cpn ωin in
  n b p  n T
Eq. (6.55) and I − Cp Ĉb f̂ in Eq. (6–56), and the nonlinear matrix Cn = Cp
n

is expressed as follows:
⎡
cos N cos U − sin E sin N sin U
⎢
⎢
Cn = ⎣ − cos E sin U
p

sin N cos U + sin E cos N sin U

⎤
cos N sin U + sin E sin N cos U − cos E sin N
⎥
cos E cos U sin E ⎥. (6.57)
⎦
sin N sin U − sin E cos N cos U cos E cos N
The differential equation of position error is given as follows [48]:

VN 1
δ L̇ = − δh + δVN ,
(RM + h) 2 RM + h
VE sin L VE 1
δ λ̇ = − δL − δh + δVE , (6.58)
(RN + h)cos L2
(RN + h) cos L
2 (RN + h) cos L
δ ḣ = δVU ,

where δL, δλ, and δh represent the latitude, longitude, and altitude errors, respec-
tively. RM and RN are the main curvature radii along the meridian and the vertical
plane of meridian, respectively.
2) Design of the smoother
In order to achieve better estimation accuracy of the SINS error, the gyroscope drift
and accelerometer bias are considered [49]. The state vector is defined by:
 
x = φE φN φU δVE δVN δVU δL δλ δH εx εy εz ∇x ∇y ∇z T .
224 6 INS/GNSS Integrated Navigation Method

Since the system dynamics model of ERTSS and URTSS is a discrete-time nonlinear
equation, we need to discretize the state model based on the SINS error equations.
A four-step Runge–Kutta numerical solution [50] is employed in this chapter. Thus,
the discrete-time model Eq. (6.41) can be formed.
 T
The process noise w = wεx wεy wεz w∇x w∇y w∇z , and E[wwT ] =
Q denotes the intensity of gyroscope random white Gaussian noise. The model error
distribution matrix G is given as follows:
⎡ ⎤
Ĉnb 03×3
⎢ ⎥
G=⎢ n
⎣ 03×3 Ĉb ⎦.
⎥ (6.59)
09×3 09×3

The measurement vector is defined by:

z = [ δVE δVN δVU δL δλ δH ]T . δVE , δVN , and δVU represent
the differences between the east velocity, north velocity, and up velocity from the
SINS and GPS, respectively. δL , δλ , and δH represent the differences between the
latitude, longitude, and altitude from the SINS and GPS, respectively.
Since the measurement model based on the velocity and position provided by the
GPS is a discrete-time linear equation, there is no need to transform the measurement
model into the discretized form. And the measurement model given in Eq. (6.42) can
be rewritten as:

zk = H k xk + vk . (6.60)

The measurement noise v = [vδVE vδVN vδVU vδL vδλ vδH ]T , and E[vvT ] = R is the
intensity of GPS velocity and position measurement noises. The measurement matrix
H is given as follows:
⎡ ⎤
03×3 diag{1,1, 1} 03×3 03×6
H=⎣ ⎦. (6.61)
03×3 03×3 diag{RM + h, (RN + h) cos L, 1} 03×6

Also, for the linear measurement model, the Eqs. (6.50) and (6.51) in URTSS
algorithm can be rewritten as:

ŷk = H k x̂−
k,

P yy = P − T
k Hk ,

P xy = H k P −
k H k + Rk .
T
(6.62)

In summary, the ERTSS and URTSS of SINS/GPS integration post-processing can

be summarized into two parts: the filter solution and the smoother solution.
Figure 6.21 shows the relation between the error-state vectors of forward filtering
δ x̂F and backward recursion δ x̂S at time k and k + 1. In filter solution, the filter
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 225

Filter Solution

δ Xˆ Fk +1 δ Xˆ Sk + 1 Smoother Solution
δ Xˆ Fk δ Xˆ S
k Ture Trajectory

Strapdown Navigator Strapdown Navigator Strapdown Navigator

Time Epoch k
k k+1
Fig. 6.21 Relation between filter solution and smoother solution

Fig. 6.22 Simulation

trajectory

results are applied to the nominal trajectory during each filtering step and will be
reset to zero after the adjustment of velocity, position, and attitude. The function of
the smoother solution is to compensate for errors of the forward filter stored during
the first solution. The filter and smoother are computed and the output at the GPS
rate. And after each estimation points, a strapdown inertial navigation algorithm is
programmed to provide navigation information.
3. Simulation and Analysis
In order to analyze the performance of the URTSS, the numerical simulation has
been done using the simulated data of a SINS/GPS integration system based on one
flight trajectory.
1) Designs of simulation
In this subsection, a typical sequential U-form imaging flight trajectory based on the
practical situation of airborne earth observation flight tests is designed. Figure 6.22
shows the simulation flight trajectory whose three long sides (AB, CD, and EF seg-
ments) can be seemed as the imaging segments. The parameters of the flight trajectory
226 6 INS/GNSS Integrated Navigation Method

Table 6.1 Characteristics of sensors

Characteristics Magnitude (1 σ )
Gyroscope Drift 0.2◦ /h
White noise 0.2◦ /h
Accelerometer Bias 100 μg
White noise 50 μg
GPS(velocity) Horizon 0.03 m/s
Height 0.05 m/s
GPS(position) Horizon 0.1 m
Height 0.15 m

are listed as follows: the initial longitude is 116◦ , latitude is 40◦ , and height is 500 m,
Initial velocity is 100 m/s, and heading is 40◦ (pitch and roll are zero). Firstly, the
aircraft flies 400 s at even velocity, then turns 180◦ clockwise (100 s) and flies 400 s
at even velocity, finally turns 180◦ anti-clockwise (100 s) and continually flies 400 s
at even velocity.
The errors of sensors in SINS/GPS integration are listed in Table 6.1.
Based on the estimate model of the SINS/GPS integration system introduced in
Sect. 6.2, different cases of initial attitude errors (heading, pitch, and roll error) are
designed. And the simulated data are processed by ERTSS and URTSS methods.
Three cases are listed as follows: Case A is designed in the way thatall the initial
attitude errors are10◦ ; Case B is designed thatall the initial attitude errors are 20◦ ;
and Case C is designed that all the initial attitude errors are 30◦ .
2) Simulation results and analyses
Figure 6.23a–6.23c are the attitude estimate errors of three cases using both methods.
It can be seen from Fig. 6.23, the URTSS method has higher estimation accuracy
than the ERTSS method with the increase of the initial attitude error, especially the
heading error.
Besides, in order to make a general comparison between the ERTSS and URTSS,
100 Monte Carlo simulations have been done. The parameters are selected to be the
same as used in case C, and the results of this two smoothing methods are analyzed.
The root mean square (RMS) and standard deviation (STD) values of attitude errors
are used to evaluate the performances of different smoothers.
Results of 100 Monto Carlo simulations are given in Table 6.2.
From Table 6.2 we can find the URTSS method gives results that are superior to
the ERTSS method in terms of both RMS and STD values of attitude errors. Although
it seems that the URTSS equations are more complicated than the ERTSS equations,
the actual computational complexity is almost equal.
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 227

c
Fig. 6.23 Attitude estimate errors of different cases. a Case A: Initial attitude error is 10◦ . b Case
B: Initial attitude error is 20◦ . c Case C: Initial attitude error is 30◦ . ERTSS extended Rauch–Tung–
Striebel smoother, URTSS unscented Rauch–Tung–Striebel smoother

4. Flight test and Discussion

In Sect. 6.3, the validity of URTSS has been proved by the simulation test. And in
this subsection, a practical flight test is utilized to evaluate the performances of the
proposed smoother.
228 6 INS/GNSS Integrated Navigation Method

Table 6.2 Means of RMS and STD values of attitude errors for 100 Monto Carlo simulations (deg)
Heading error Pitch error Roll error
ERTSS URTSS ERTSS URTSS ERTSS URTSS
E(RMS) 0.0990 0.0045 0.0051 0.0026 0.0187 0.0018
E(STD) 0.1141 0.0015 0.0045 0.0015 0.0073 0.0016
ERTSS extended Rauch–Tung–Striebel smoother, URTSS unscented Rauch–Tung–Striebel
smoother, RMS root mean square, STD standard deviation

1) Experiment arrangement
A 2-h flight test was carried out in the calibration field of Pingding Mountain, Henan
Province, China. And the calibration field has a large number of ground control
points.
The GPS antenna is fixed on the top of an ultralight A2C aircraft, the SINS/GPS in-
tegration system and the airborne photography camera were mounted on the backseat
of the aircraft.
The SINS/GPS integration system (TXD10-A2) is developed by the Beihang Uni-
versity of China, which consists of the dynamically tuned gyroscope (DTG)-based
IMU and the trimble 5700 dual-frequency GPS receiver. The gyroscope constant drift
is 0.1◦ /h, and the accelerometer constant bias is 100 ug. Besides, the post-processed
position and velocity precision of GPS is 10 mm + 1.5 ppm and 0.01 m/s, respec-
tively [51]. During this flight, the original output data of the SINS/GPS integration
system are recorded, and the update rates of IMU and GPS are 100 Hz and 10 Hz,
respectively.
Figure 6.24 shows the dynamic trajectory of the aircraft obtained from the GPS.
The trajectory marked by the broken line is the imaging area, and the IMU is started
up before entering this imaging area (see the A point in Fig. 6.24). There are four
imaging segments in this flight test which includes about 2000 s of data.
The velocity and position precision are mainly decided by the GPS accuracy,
due to their good observability in the SINS/GPS integration system. Therefore, the
initial attitude error has little effect on the smoothing results of velocity and position.
However, the heading error, whose accuracy is a significant performance index of
orientation, is difficult to estimate. Currently, the aero-triangulation method has been
recognized as a better way to calibrate the navigation data processed by SINS/GPS
integration [52]. In this flight test, the attitude information of higher precision is
provided by the Chinese Academy of Surveying and Mapping, which is computed
by the aero-triangulation method with the ground control points and the imaging
data.
Figure 6.25 shows the four imaging segments of this test, and each of them has a
group of corresponding discrete imaging points with precise attitude values computed
by the aero-triangulation method.
6.4 High-Precision Inertial/Satellite Integrated Navigation Method 229

Height(m) 600

400

$

200

100
33.79
Imaging Area

33.78 Ground
Latitude(deg) 113.24
113.2
33.772 113.16
113.12
Longitude(deg)

Fig. 6.24 Flight trajectory

Flight Trajectory
Imaging
33.78 Imaging Points
Area

Imaging Segment 1
33.778
Latitude(deg)

33.776 Imaging Segment 2

Imaging Segment 3
33.774

Imaging Segment 4
33.772

113.14 113.15 113.16 113.17 113.18 113.19

Longitude(deg)

Fig. 6.25 Four imaging segments of the flight test

2) Flight test results and analysis

In the in-flight startup case, the initial position and velocity of the aircraft can be
obtained from the GPS outputs directly, but the initial attitude is still unknown. In
general, the approximate value of initial heading can be computed by the east and
north velocities measured by the GPS. However, the misalignment of the initial
heading is uncertain due to the existence of drift angle, which depends on the rela-
tionship between aircraft velocity and wind speed. According to the other flight tests
of higher flying velocity than this, the drift angles are around 10◦ in fine weather,
230 6 INS/GNSS Integrated Navigation Method

Fig. 6.26 The standard deviations of pitch and roll error in different cases. EKF extended Kalman
filter, ERTSS extended Rauch–Tung–Striebel smoother, UKF unscented Kalman filter, URTSS
unscented Rauch–Tung–Striebel smoother

and the values increase in adverse conditions. So the misalignment of initial heading
in this flight test is assumed as 30◦ in general. Moreover, based on the many times of
tests we have taken, there is no exquisite change of the pitch and roll angles during
the preparation period, and the maximal value is about 5◦ for this kind of aircraft.
Hence, before entering the imaging area, the pitch and roll can be initialized to
zero, and the absolute value of misalignment in initial pitch and roll is assumed as 5◦ .
To test and compare the performance of the estimation methods, different cases
of the heading misalignment (5◦ , 10◦ , 15◦ , 20◦ , 25◦ , 30◦ ) are designed, and the pitch
and roll misalignment is set to 5◦ for these cases. Then the aforementioned flight
data are processed by the EKF, UKF, ERTSS, and URTSS.
Figure 6.26 gives the STDs of pitch and roll error in different cases. As we can
see, there are no major differences between the estimates of ERTSS and URTSS.
And both smoothers give better estimation than the related filter, and the UKF is
quite better than the EKF with the increase of errors.
For heading estimation, the differences of these four methods are shown in
Fig. 6.27. The graphs of remnant error (the absolute error subtract the mean of
all errors) indicate that, for the whole imaging segments, the heading estimate error
received from URTSS and UKF is smaller than that from ERTSS and EKF.
Table 6.3 gives the corresponding comparisons of heading error STD values in
different cases of initial misalignment angle. And a further comparison between the
ERTSS and URTSS is illustrated in Fig. 6.28.
From Table 6.3 and Fig. 6.28 it is obvious that the smoothing methods based on
EKF and UKF reduce the heading error, and the efficiency of URTSS is upgraded
by the increase in heading misalignment. The conclusion above conforms to the
subsection named analysis results discussed. That is, the URTSS can produce a
better quality of heading estimation.
6.5 Chapter Conclusion 231

Fig. 6.27 Remnant heading error using different estimation methods. EKF extended Kalman filter,
UKF unscented Kalman filter, GPS global positioning system, ERTSS extended Rauch–Tung–
Striebel smoother, URTSS unscented Rauch–Tung–Striebel smoother

Table 6.3 Standard deviations of heading error (deg)

Initial heading error EKF ERTSS UKF URTSS
5 0.2674 0.0461 0.0880 0.0415
10 0.3936 0.0492 0.0987 0.0416
15 0.4902 0.0521 0.1112 0.0418
20 0.5469 0.0540 0.1235 0.0419
25 0.5905 0.0555 0.1339 0.0421
30 0.6141 0.0565 0.1431 0.0422
EKF extended Kalman filter, ERTSS extended Rauch–Tung–Striebel smoother, UKF unscented
Kalman filter, URTSS unscented Rauch–Tung–Striebel smoother

6.5 Chapter Conclusion

There is a strong complementarity between the SINS and GNSS, and the integrated
navigation system combining the SINS and GNSS makes the most of respective
advantages and enables obtaining of precision and reliability superior to a single
navigation system. This chapter systematically introduced the principle and meth-
ods of the SINS/GNSS integrated navigation system. To be specific, it discussed
several SINS/GNSS integrated navigation methods specific to the features of the
high-precision SINS/GNSS integrated navigation system based on the introduction
of the linear model and nonlinear model of the SINS/GNSS integrated navigation
system.
232 6 INS/GNSS Integrated Navigation Method

Fig. 6.28 Standard deviations of heading error using extended Rauch–Tung–Striebel smoother
(ERTSS) and unscented Rauch–Tung–Striebel smoother (URTSS)

First, a mixed correction program of the SINS/GNSS integrated filtering was in-
troduced; the program effectively combines the advantages of output correction and
feedback correction and improves estimation precision of observable state in inte-
grated filtering. However, for a partial state with low observability or unobservable
state, a complete feedback adopted after the filter is stabilized may result in decline
or even divergence of filtering precision. Therefore, a new self-adaptive feedback
correction filter method was introduced; the method conducts a self-adaptive ad-
justment for the system state feedback quantity according to the observability of
the system state, which solves the problem of the state with low observability or
unobservable state likely to result in decline or even divergence of filtering precision
subject to a complete feedback. Then, specific to the outlier problem of the GNSS
signal, a SINS/GNSS outlier-resistant integrated navigation method based on KV
innovation orthogonality was introduced; it realizes high-precision navigation when
outliers appear in the GNSS signal. Besides, a new SINS/GNSS air maneuvering
alignment method based on system state variable observability analysis and lever
arm effect error compensation was introduced by using the feature of improvement
of observability for the SINS/GNSS integrated navigation system state during carrier
maneuvering. It compensates the lever arm effect error between the GNSS antenna
and SINS and effectively estimates and corrects the azimuth misalignment angle and
inertial component error by combing a self-adaptive feedback correction method.
Finally, aiming at the nonlinear model problem, the URTSS smoothing algorithm
References 233

was applied to the off-line estimation of SINS/GPS integration. The smoother esti-
mate method based upon the unscented transformation had better performance than
that based upon the linearization by Taylor series expansion. The smoother had an
obvious accuracy advantage over the filter.
The effect of modeling, filtering, and other key technologies of the SINS/GNSS
integrated navigation system on improvement of attitude precision and correction of
inertial component error is poor since the SINS/GNSS integrated navigation was gen-
erally the lack of attitude measurement information. With the increase of navigation
time, attitude precision of the SINS/GNSS integrated navigation system will decline
slowly. The next chapter will introduce the inertial/celestial integrated navigation
method to obtain a reliable and high-precision attitude information and effectively
eliminate the inertial component error.

References

1. Baziw J, Leondes CT (1972) In-flight alignment and calibration of inertial measurement units,
Part I: General formulation. IEEE Trans Aerosp Electron Syst 8(4):439–449
2. Baziw J, Leondes CT (1972) In-flight alignment and calibration of inertial measurement units,
Part II: Experimental results. IEEE Trans Aerosp Electron Syst 8(4):450–465
3. Liu B, Fang Ji (2008) A new adaptive feedback Kalman filter based on observability analysis
for SINS/GPS. AcAAS 29(2):430–436
4. Qin Y, Zhang H, Wang S (1998) Theory of Kalman filter and integrated navigation. Northwest
industrial university press, Xi’an
5. Geng Y, Cui Z, Zhang H, Fang J (2004) Adaptive fading Kaman filter with applications in
integrated navigation system. BUAAJ 30(5):434–437
6. Chen B, Zhang Y (2001) Error models of inertial navigation system in initial alignment and
calibration. Aerosp Shanghai (6):4–8
7. Dmitriyev SP (1997) Nonlinear filtering methods application in INS alignment. IEEE Trans
Aerosp Electron Syst 33(1):260–271
8. Yuan X, Yu J, Chen Z (1993) Navigation system. Aviation Industry Press, Beijing
9. Yi G et al (1987) Theory of inertial navigation. Aviation Industry Press, Beijing
10. Shen Z, Yu W, Fang J (2007) Nonlinear algorithm based on UKF for low-cost SINS/GPS
integrated navigation system. Syst Eng Electron 29(3):408–411
11. Yu MJ, Lee JG, Park HW (2004) Nonlinear robust observer design for strapdown INS in-flight
alignment. IEEE Trans Aerosp Electron Syst 40(3):797–807
12. Yu MJ, Lee JG, Park HW(1999) Comparison of SDINS in-flight alignment using equivalent
error models. IEEE Trans Aerosp Electron Syst 35(3):1046–1053
13. Geng Y, Guo W, Cui Z, Fang J (2004) Observability on attitude errors in GPS/SINS in-flight
alignment. J Chin Inert Technol 12(1):37–42
14. Lee JG,YoonYJ, Mark JG (1994) Extension of strapdown attitude algorithm for high-frequency
base motion. IEEE Trans Aerosp Electron Syst 30(4):306–310
15. Wan D, Fang J (1998) Initial alignment of inertial navigation. Southeast university press,
Nanjing
16. Dong X, Zhang S, Hua Z (1998) GPS/INS integrated navigation and its application. National
University of Defence Technology Press, Changsha
17. Lin M, Fang J, Gao G (2003)A new composed kalman filtering method for GPS/SINS integrated
navigation system. J Chin Inert Technol 11(3):29–33
234 6 INS/GNSS Integrated Navigation Method

18. Cheng X, Wan D, Zhong X (1997) Study on observability and its degree of strapdown inertial
navigation system. J Southeast Univ 27(6):6–11
19. Shuai P, Cheng D, Jiang Y (2004) Observable degree analysis method of integrated GPS/SINS
navigation system. J Astronaut 25(2):219–225
20. Meskin G, Itzhack B (1992) Observability analysis of piece-wise constant systems, Part I:
theory. IEEE Trans Aerosp Electron Syst 28(4):1056–1067
21. Zhang J, Tan M, Ren S (2003) A cascade SINS/GNSS integrated navigation system with closed
loop feedback. J Chin Inert Technol 11(5):12–15
22. Simon J, Julier JK, Uhlmann H et al (1995) A new approach for filtering nonlinear systems.
In: Proceedings of the American Control Conference. American Control Conference, vol 1.,
IEEE Service Center, Virginia, pp 1628–1632, Seattle, June 1995
23. Rui Z, Qitai G (2000) Nonlinear filtering algorithm with its application in INS alignment.
Proceedings of the tenth IEEE workshop on statistical signal and array processing, 2000, pp
510–513
24. Liu X, Fang J (2003) An abnormal data eliminating method used in low-dynamic carrier phase
DGPS position system. J Chin Inert Technol 11(6):64–68
25. Mohamed AH, Schwarz KP (1999) Adaptive Kalman filtering for INS/GPS. J Geod 73(4):
193–203
26. Gul F, Fang J, Gaho AA (2006) GPS/SINS navigation data fusion using quaternion model and
unscented Kalman filter. 2006 IEEE international conference on mechatronics and automation,
2006, pp 1854–1859
27. Gaho AA, Fang J, Gul F (2006) GPS/GLONASS/SINS synergistic integration into flight vehicle
for optimal navigation solution. 2006 IEEE international conference on mechatronics and
automation, 2006, pp 1848–1853
28. Farrell J, Barth M (1998) The global positioning system and inertial navigation. McGraw-Hill,
NewYork
29. Gong X, Fang J (2009) Application of modified Kalman Filtering restraining outliers based on
orthogonality of innovation to POS. AcAAS 30(12):2349–2354
30. Kalman RE (1960) A new approach to linear filtering and prediction problems. Trans ASME
J Basic Eng (82):35–45
31. Liu H, Yao Y, Lu D et al (2003) Study for outliers based on Kalman filtering. Study Electr
Mach Control 7(1):40–42
32. Haykin S, Liang L (1994) Modified Kalman filtering. IEEE Trans Signal Process 27(1):1239–
1242
33. Liu B, Gong X, Fang J (2007) In-flight self-alignment for airborne SINS/GPS based on GPS
observation and model predict filter. J Chin Inert Technol 15(5):568–572
34. Sheng J, Fu M, Liu Y (2003) Rapid initial alignment of SINS for stationary base with wavelet
network. Acta Sci Nat Univ NeiMongol 34(4):468–471
35. Bevly DM, Parkinson B (2007) Cascaded Kalman filters for accurate estimation of multiple
biases, dead-reckoning navigation, and full state feedback control of ground vehicles. IEEE
Trans Control Syst Technol 15(2):199–208
36. Liu Z, Chen Z (2004) Application of condition number in analysis observability of system. J
Syst Simul 16(7):1552–1555
37. Sohne W, Heinze O et al (1994) Integrated INS/GPS system for high precision navigation
applications. IEEE Position Location Navigation, 310–313
38. Meditch J (1973) A survey of data smoothing for linear and nonlinear dynamic systems.
Automatica 9:151–162
39. Simon D (2006) Optimal state estimation: Kalman, H1 and nonlinear approaches. Wiley,
NewJersey
40. Rauch H, Tung F, Striebel C (1965) Maximum likelihood estimates of linear dynamic systems.
AIAA Journal 3(8):1445–1450
41. Post-processing, software version 8.30 manual. http://www.novatel.com
42. Gelb A (1974) Applied optimal estimation. MIT Press, Cambridge
References 235

43. Särkkä S, Viikari V et al (2011) Phase-based UHF RFID tracking with nonlinear Kalman
filtering and smoothing. Sensors J IEEE 12(5): 904–910
44. Wan EA, Merwe R Van Der (2000) The unscented Kalman filter for nonlinear estimation. In:
Proceedings of the IEEEAS-SPCC, Alta, Oct 2000, pp 153–158
45. Julier S, Uhlmann J et al (2000) A new method for nonlinear transformation of means and
covariances in filters and estimators. IEEE Trans Automat Control 45:477–482
46. Särkkä S (2008) Unscented Rauch-Tung-Striebel smoother. IEEE Trans Automat Control
53(3):845–849
47. Goshen-Meskin D, Bar-itzhack IY (1992) Unified approach to inertial navigation system error
deling. J GCD 15(3):648–653
48. Fang J-c, Gong X-l (2010) Predictive iterated Kalman filter for INS/GPS integration and its
application to SAR motion compensation. IEEE Trans Instrum Meas 59(4):909–915
49. David T, John W (2004) Strapdown inertial navigation technology. The institution of electrical
engineers, pp 344–345
50. Wang Y-f, Sun F-C et al (2012) Central difference particle filter applied to transfer alignment
for SINS on missiles. IEEE Trans Aerosp Electron Syst 48(1):375–387
51. Trimble R7 GNSS and 5700 GPS receivers user guide version 3.64. http://www.trimble.com
52. Toth CK (2002) Sensor integration in airborn emapping. IEEE Trans Instrum Meas 1(6):1367–
1373
Chapter 7
INS/CNS Integrated Navigation Method

7.1 Introduction

The previous chapter has introduced the principle, methods, and engineering appli-
cation technology of the SINS/GNSS integrated navigation system, precision and
reliability of which is difficult to be guaranteed during military application due to
GNSS’s susceptibility to external disturbance though navigation position and velocity
precision of such integrated navigation system is high; and azimuthal measurement
precision of such integrated system is low. However, the inertial/celestial integrated
navigation system (SINS/CNS) is an autonomous navigation system obtaining high-
precision navigation parameters through celestial measurement information and
inertial measurement information; it has features such as strong autonomy and high-
attitude precision, and can effectively correct gyro drift error, so it has become an im-
portant development direction for high-performance navigation of the new generation
of flying machines including ballistic missile, satellite, and deep space probe [1].
Requirement of ballistic missile for navigation precision is high, so pure depen-
dence on the inertial navigation system is bound to bring great technical difficulty
and sharp increase of cost. SINS/CNS integrated application to realize complemen-
tary advantages and collaborative transcendence is an effective way to solve this
problem. The SINS/CNS integrated navigation system has following advantages:
(1) addition of CNS for integrated navigation will improve precision of navigation
system when performance of SINS is given; (2) requirement for inertial compo-
nent may be reduced; (3) correction of position error during initial period by using
SNS information may enable relaxation of requirement for initial alignment, which
is particularly important for mobile launching or underwater launching of ballistic
missile.
At present, lunar vehicle has become the main exploration device for lunar ex-
ploration. Lunar vehicle generally conducts navigation and control with help of the
ground station through wireless measurement and control, but radio communication
may be interrupted on one hand, and the existence of long-time delay is adversely
affected by real-time control of lunar vehicle on the other hand; however, SINS/CNS
integrated navigation is one of the best solutions for long distance and autonomous
positioning and navigation of lunar vehicle.
© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 237
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_7
238 7 INS/CNS Integrated Navigation Method

In addition, in terms of high-precision attitude of satellite, though CNS may

provide high-precision attitude information, the output information is discontinuous,
which fails to satisfy high-stability control requirement of satellite. SINS can measure
angular rate information of satellite precisely in an autonomous, continuous, and
real-time manner, but the existence of gyro drift error makes attitude error of SINS
accumulate with time, which fails to satisfy long-time in-orbit operation requirement
of satellite. The SINS/CNS integrated navigation system can not only correct attitude
error of SINS but also estimate gyro drift on line and output high-precision attitude
information for a long time continuously, so that it becomes the main development
direction for high-precision attitude determination of satellite.
For this purpose, this chapter first introduces basic principle of the SINS/CNS
integrated navigation system, based on which the SINS/CNS integrated navigation
method of ballistic missile is focused on; then, the INS/CNS integrated naviga-
tion method of lunar vehicle and the SINS/CNS high-precision integrated attitude
determination method of satellite are introduced separately.

7.2 Basic Principle of Inertial/Celestial Integrated Navigation

The inertial/celestial integrated navigation system corrects attitude error of the inertial
navigation system through celestial measurement information for further correc-
tion of gyro drift, initial misalignment angle and initial position error to obtain
high-precision attitude, position, and velocity of the carrier. The following is an in-
troduction from three aspects including operating mode, combination mode of the
inertial/celestial integrated navigation system, and principle of inertial component
error correction.

7.2.1 Operating Mode of the Inertial/Celestial Integrated

Navigation System

At present, the inertial/celestial integrated navigation system includes several operat-

ing modes, which are classified into full platform mode, platform inertial navigation
system and strapdown star sensor mode, strapdown inertial navigation system (SINS)
and platform star sensor mode, and full strapdown model according to different
installation ways of star sensor and inertial component.
1) Full platform mode
This mode adopts platform-based inertial navigation and star sensor tracking specific
star (also called star tracker), where MK5 inertial/starlight integrated system used
by American IC4 Trident missile is relative typical. It is characterized in that the star
sensor tracks specific star to obtain the carrier attitude free of influence of carrier vi-
bration and other factors, and its field of view can be made small (about 3◦ generally)
7.2 Basic Principle of Inertial/Celestial Integrated Navigation 239

Lens hood of star sensor

Inertial
component

Navigation base
Star sensor

Fig. 7.1 Installation arrangement for inertial component, star sensor, and navigation base

and measurement precision is high. However, since the star sensor is installed on
the tracking platform, the system structure, information input/output mode and drive
circuit are all complex.
2) Platform inertial navigation system and strapdown star sensor mode
This operating mode adopts platform-based inertial navigation and directly connects
star sensor fixedly to the carrier dispense with the tracking platform. Typical appli-
cation is inertial/starlight guidance system of space shuttle Columbia (as shown in
Fig. 7.1). Optical axis direction of the star sensor in this solution varies with variation
of the carrier attitude, so the star sensor is required for star map identification, and
the field of view is generally made large (more than 10◦ generally). Carrier attitude
obtained from measurement of star sensor is required to be converted to the platform
coordinate system to further bring in error of platform angle sensor based on measure-
ment error; and since the star sensor is directly connected to the carrier fixedly, vibra-
tion caused by internal maneuvering of the carrier and various disturbances makes the
star sensor operate under dynamic environment, which influences its measurement
precision and has high requirement for dynamic performance of star sensor.
3) SINS and platform star sensor mode
This operating mode adopts strapdown inertial navigation and star sensor tracking
specific star. The SINS makes computer simulation platform system to complete
real-time correction of strapdown matrix; compared with platform system, it has
240 7 INS/CNS Integrated Navigation Method

Position, velocity
and attitude
+ Position, velocity
and attitude
Inertial navigation

-
Star sensor Attitude

Fig. 7.2 Simple combination mode

advantages such as low cost and high reliability, etc., but its requirement for perfor-
mance of gyro and accelerometer is high. With appearance of various new gyros and
accelerometers, SINS is more competitive. However, the system still has shortcom-
ings such as complex structure and difficult drive circuit design since it adopts star
sensor tracking specific star.
4) Full strapdown mode
Full strapdown mode refers to both the inertial navigation system and star sensor
adopt strapdown mode for installation, so it is the most flexible operating mode.
Requirement of this mode for measurement precision, integration level, and dy-
namic characteristics of gyro, accelerometer, and star sensor is high, but solving this
problem becomes possible with development of electronic, optical, material, and
processing technology and appearance of CMOS APS technology. From the per-
spective of future development trend, inertial/celestial integrated navigation system
of full strapdown mode has better development prospect and is one of the impor-
tant development directions for inertial/celestial integrated navigation system, so the
following contents of this chapter will discuss full strapdown mode, for instance.

7.2.2 Combination Mode of Inertial/Celestial Integrated

Navigation System

Inertial/celestial combination mode generally includes simple combination mode

and combination mode based on the optimal estimation [2].
1. Simple Combination Mode
Under such mode, the inertial navigation system operates independently to provide
various navigation data including attitude, velocity and position; the star sensor
measures azimuth information of celestial body, works out carrier attitude by deter-
mining algorithm through attitude, and corrects attitude of inertial navigation system,
as shown in Fig. 7.2. Such combination mode has been widely used at home and
abroad, for example, it is adopted by NAS-26 series of inertial/celestial integrated
navigation system on American B2 bomber. The mode is simple and reliable, but has
low precision.
7.2 Basic Principle of Inertial/Celestial Integrated Navigation 241

Inertial navigation Position, velocity and attitude

system
Attitude +

Filter
Attitude -
Star sensor
Gyro drift

Fig. 7.3 Shallow combination mode based on the optimal estimation

2. Shallow Combination Mode Based on the Optimal Estimation

With the development of optimal estimation theory, SINS/CNS shallow combination
mode based on optimal estimation appears. Basic principle of the mode is to take
error equation of SINS as the system state equation, use star sensor to obtain high-
precision attitude information, take attitude difference between SINS and star sensor
as measurement, and estimate and compensation SINS attitude error and gyro drift
through the filter, as shown in Fig. 7.3.
3. Deep Combination Mode Based on the Optimal Estimation
Deep combination mode develops based on shallow combination mode. The mode
not only uses CNS to assist in correcting error of SINS, but also uses SINS to
assist CNS in enhancing identification success rate of star to improve CNS attitude
determination velocity and precision, so it is one of the important development
directions for inertial/celestial combination mode. During engineering realization,
the combination mode requires CNS to have on-line parameter adjustment capacity.

7.2.3 Principle of Inertial Component Error Correction Based on

Celestial Measurement Information

This section will give a brief introduction to the principle of inertial component error
correction based on celestial measurement information since attitude determination
principle of CNS has been discussed in detail in Sect. 2.5 of this book.
Under full strapdown mode of SINS/CNS integrated navigation system, three-axis
attitude information of the carrier obtained by SINS through strapdown calculation
is the pitching angle θ0 , course angle ϕ0 , and roll angle γ0 , and three-axis attitude
information of the carrier, i.e., pitching angle θ , course angle φ, and roll angle γ can
also be obtained through attitude information acquired by the star sensor. Subtract
the two to obtain three-axis attitude error angle a of the carrier as follows:
⎡ ⎤ ⎡ ⎤
aθ θ − θ0
⎢ ⎥ ⎢ ⎥
a = ⎢ ⎥ ⎢
⎣ aφ ⎦ = ⎣ φ − φ 0 ⎦
⎥ (7.1)
aγ γ − γ0
242 7 INS/CNS Integrated Navigation Method

εx, εy , εz x, y, z
3 gyros
SINS
algorithm
3 accelerometers + Attitude ∆a Mathematics ∆ a' Optimal
error platform estimation
CNS
Star sensor attitude
calculation

Fig. 7.4 Block diagram for calculation of SINS/CNS integrated navigation system

Since mathematics platform misalignment angle is adopted in attitude error equation

of SINS, attitude error angle of Eq. (7.1) is required to be converted into mathe-
matics platform misalignment angle to serve as measurement quantity of the filter.
Conversion relationship is as follows:

a = M · a (7.2)

Wherein, M is the error conversion matrix of attitude angle.

In SINS/CNS integrated navigation system, with error equation of SINS as the
integrated system state equation and a as system measurement value, estimate
and correct mathematics platform misalignment angle of SINS in a real-time manner
through the filter, and also estimate drift error of the three gyros in real time, finally
correct gyro measurement information directly, where its flow block diagram is as
shown in Fig. 7.4.
Attitude updating frequency of general star sensor data is lower than frequency of
output angular rate, specific force and other original data of SINS, so filtering cycle
is generally much greater than calculation cycle of SINS [2].

7.3 Modeling Method of Inertial/Celestial Integrated Navigation

System

It is first required to establish mathematical model of SINS/CNS integrated navi-

gation system to realize high-performance navigation of flying machine by using
inertial/celestial integrated navigation system. Since ballistic missile and aircraft
adopt different navigation coordinate systems, it is required to establish system state
equation and measurement equation under different navigation coordinate systems
[3–5]. This section will introduce their modeling methods, respectively, specific
to ballistic missile adopting inertial coordinate system of the launching point and
aircraft adopting local geographic coordinate system.
7.3 Modeling Method of Inertial/Celestial Integrated Navigation System 243

7.3.1 State Equation of Inertial/Celestial Integrated Navigation

System

1. State Equation of Inertial/Celestial Integrated Navigation System for Ballistic

Missile
State equation of inertial/celestial integrated navigation system for ballistic missile
mainly consists of velocity error equation, position error equation, and attitude error
equation. The following system state equation can be obtained by combining error
equations of inertial system in Sect. 2.3 of this book:

Ẋ(t) = F(t)X(t) + G(t)W(t) (7.3)

The state variable is:

 T
X(t) = φx φy φz δvx δvy δvz δx δy δz εx εy εz ∇x ∇y ∇z
(7.4)

Wherein, variable includes three mathematics platform misalignment angles under

inertial coordinate system of the launching point, velocity error and position error
on three axes, three random constant drifts of the gyro and three random constant
biases of the accelerometer, and F(t) is state transition matrix.
⎡ ⎤
03×3 03×3 03×3 Clib 03×3
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ Fb 03×3 Fa 03×3 Clib ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
F(t) = ⎢ 03×3 I3×3 03×3 03×3 03×3 ⎥ (7.5)
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ 03×3 03×3 03×3 03×3 03×3 ⎥
⎢ ⎥
⎣ ⎦
03×3 03×3 03×3 03×3 03×3
15×15
⎡ ⎤
Cli 03×3
⎢ b ⎥
⎢ ⎥
⎢ ⎥
⎢ 03×3 Clib ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
G(t) = ⎢ 03×3 03×3 ⎥ (7.6)
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ 03×3 03×3 ⎥
⎢ ⎥
⎣ ⎦
03×3 03×3
15×6
244 7 INS/CNS Integrated Navigation Method
⎡ ⎤ ⎡ ⎤
f14 f15 f16 0 ȧz −ȧy
⎢ ⎥ ⎢ ⎥
Wherein, Fa = ⎢ ⎥ ⎢
⎣f24 f25 f26⎦ , Fb = ⎣−ȧz 0 ȧx ⎥⎦, referring to Sect. 2.3
f34 f35 f36 ȧy −ȧx 0
of this book for special component of Fa .
System noise is
 T
W(t) = wεx wεy wεz w∇x w∇y w∇z (7.7)

Wherein, wεx , wεy , and wεz are random noises of the gyro; w∇x , w∇y , and w∇z are
random noises of the accelerometer.
Noise variance matrix of the system is:
 
Q(t) = diag σεx 2 2
σεy 2
σεz 2
σ∇x 2
σ∇y 2
σ∇z (7.8)

2. State Equation of Inertial/Celestial Integrated Navigation System for Aircraft

State equation of inertial/celestial integrated navigation system for aircraft consists
of attitude error equation, velocity error equation, and position error equation. The
following system state equation is obtained by solving attitude error, velocity er-
ror, and position error equations described under geographic coordinate system in
Sect. 2.3 of this book simultaneously:
Ẋ = FX + GW (7.9)
The state variable is:
 T
X = ϕE ϕN ϕU δvE δvN δvU δL δλ δh εx εy εz ∇x ∇y ∇z
(7.10)

In above equation, variables [ φE φN φU ] are three mathematics platform mis-

alignment angles, [ δvE δvN δvU ] are three velocity errors, [ δL δλ δh] are
three position errors, [ εx εy εz ] are three random constant drifts of the gyro, and
[∇x ∇y ∇z ] are three random constant biases of the accelerometer; F is system
transition matrix.
⎡ ⎤
FN FS
F =⎣ ⎦ (7.11)
06×6 FM

Wherein, elements of matrix F are described in Sect. 5.3 of this book.

Matrix G and W are:
⎡ ⎤
Cbn 03×3 03×9
⎢ ⎥
⎢ ⎥
⎢ ⎥
G = ⎢ 03×3 Cbn 03×9 ⎥ (7.12)
⎢ ⎥
⎣ ⎦
09×3 09×3 I9×9
7.4 New Inertial/Celestial Integrated Navigation Method of Ballistic Missile 245
 T
W = wεx wε y w εz w∇x w∇ y w∇z 0 0 0 0 0 0 0 0 0
(7.13)
Matrix G and W can be abbreviated to:
⎡ ⎤
Cbn 03×3
⎢ ⎥
⎢ ⎥
⎢ ⎥
G = ⎢03×3 Cbn ⎥ (7.14)
⎢ ⎥
⎣ ⎦
09×3 09×3
 T
W = wεx w εy wε z w∇ x w∇y w ∇z (7.15)

In above equation, variables wεx , wεy , wεZ and w∇x , w∇y , w∇z are respectively random
noises of the gyro and accelerometer.

7.3.2 Measurement Equation of Inertial/Celestial Integrated

Navigation System

For ballistic missile and aircraft, measurement equations of SINS/CNS integrated

navigation system are the same [6–8]. Output quantity of star sensor is attitude of
the carrier; ballistic missile is taken for instance hereby, and mathematics platform
misalignment angles φx , φy , and φz subject to conversion are taken as observation
quantities of measurement equation. Thus, the measurement equation is as follows:
⎡ ⎤
φx
⎢ ⎥
Z(t) = ⎢ ⎥
⎣ φy ⎦ = H X(t) + V (t) (7.16)
φz
⎡ ⎤
XS
⎢ ⎥
wherein, H = [I3×3 03×3 03×9 ]; V = ⎢ ⎥
⎣ YS ⎦; XS , YS , and ZS are measure-
ZS
ment noises of star sensor [9].

7.4 New Inertial/Celestial Integrated Navigation Method of

Ballistic Missile

Specific to high-precision navigation of ballistic missile, this section first introduces

principle for initial position error correction of missile launching point based on
246 7 INS/CNS Integrated Navigation Method

celestial measurement information; second, on the basis of previously established

SINS/CNS integrated navigation system model, method for estimation of navigation
error of missiles during boost phase by using state transition matrix and two new
SINS/CNS integrated navigation methods of ballistic missile are introduced; third,
simulation performance analysis of SINS/CNS integrated navigation is reported.

7.4.1 Principle for Initial Position Error Correction of Missile

Launching Point Based on Celestial Measurement
Information

For ballistic missile, due to limitation of underwater launching or ground mobile

launching conditions, it is often hard to determine position of the launching point
precisely, which results in initial position error of missile launching point, as shown
in Fig. 7.5. Prior to missile launching, its actual position is in point A, but the
launching point is deemed to be point C due to position error of the launching point,
and elevation angle θ of certain star can be worked out according to level of point
C and prior knowledge. However, it is actually the level of point A that is measured
by inertial navigation system of missile; after the missile flying off the atmospheric
layer, actual elevation angle θ of the star is observed through star sensor, θ of
difference with elevation angle θ , so θ is just the included angle between the two
Earth radii from point A and point C to the Earth’s core O, based on which position
error of the lunching point along the star direction can be obtained thereby [1]

AC = R · θ (7.17)

wherein, R is earth radius. Observe two stars with azimuth angle approximately
perpendicular with each other based on the principle to correct two-dimensional
position error of the ground launching point. Obviously, level alignment of missile-
borne inertial navigation system prior to launching must have precision high enough
when correcting position error of the launching point through the principle to ignore
level alignment error during subsequent calculations [1, 2].

7.4.2 Inertial/Celestial Integrated Navigation Method of Ballistic

Missile Based on UKF

The previous subsection gives a brief introduction to principle for initial position
error correction of missile launching point based on celestial measurement informa-
tion, and this subsection will briefly introduce a nonlinear filtering method [10, 11] of
SINS/CNS integrated navigation of ballistic missile based on UKF specific to reduc-
tion of INS/CNS integrated navigation precision caused by gravitational acceleration
deviation due to inaccurate Earth ellipsoid model establishment [12] resulting from
7.4 New Inertial/Celestial Integrated Navigation Method of Ballistic Missile 247

z x
θ′
Δθ
θ S
x′

z′
z

x θ
x′
C
A

Δθ

Fig. 7.5 Principle for initial position error correction of the carrier through SINS/CNS integrated
navigation system

factors including uneven Earth quality distribution, irregular surface shape, lunisolar
gravitational perturbation and tide, etc.
1. Basic Principle for Inertial/Celestial Integrated Navigation of Ballistic Missile
Based on UKF
Structure of SINS/CNS integrated navigation system is as shown in Fig. 7.6. 3-axis
angular rate and 3-axes acceleration information of the carrier are obtained through
measurement of IMU in SINS, and position, velocity, and attitude information of the
carrier are determined through error compensation and navigation computer calcu-
lation. Star sensor is CNS generated two-dimensional star map image and realizes
high-precision attitude calculation of CNS by combining ephemeris data through
star map preprocessing, star map matching identification, attitude determination,
and other data processing. Optimal filter fuses data of SINS and CNS to finally
obtain high-precision navigation of ballistic missile.
248 7 INS/CNS Integrated Navigation Method

Fig. 7.6 Block diagram of

SINS/CNS integrated 3-axis Error Strapdown
navigation system IMU compensate calculation
State
estimation

data processor
Star Ephemeris
Observation Optimal
sens
Processin quantity filter
or
g circuit

For optimal filter, one of the important problems affecting its filtering perfor-
mance is nonlinear problem of system model. For nonlinear SINS/CNS integrated
navigation system, nonlinear UKF filtering method with good filtering performance
is adopted here, and perturbation-based state equation and attitude measurement
equation is adopted for integrated navigation system model. The biggest pertur-
bation influence factor affecting ballistic missile orbit is gravitational acceleration
deviation caused by Earth ellipsoid model, and this problem can be solved by adopt-
ing the familiar multiple harmonic expansion method. In this system state equation,
relevant term of gravitational acceleration is a nonlinear term.
Perturbation-based state equation adopted here is shown as follows:

φ̇ = Cbli (ε b + ωb )
δ v̇li = f li × φ li + Cbli (∇ b + ν b ) + δg li (7.18)
δ ṙ = δv
li li

wherein, φ is mathematics platform misalignment angle, Cbli is relevant coordinate-

transformation matrix, ε and ω are respectively constant drift and random drift of the
gyro; δv is velocity error, f is apparent acceleration sensed by the accelerometer,
∇ and ν are respectively constant bias and random bias of the accelerometer; δr is
position error; δg is acceleration error caused by gravity, which can be worked out
through following equations:
⎡
  ⎤
3  i 2  i
⎢ 1 + 2 J2 (re /r) 1 − 5 rz /r
2
rx ⎥
⎢
 ⎥
⎢     ⎥
μ ⎢ 3 2 ⎥
g li = −Cili 3 ⎢ 1 + J2 (re /r)2 1 − 5 rzi /r ryi ⎥ (7.19)
r ⎢ 2 ⎥
⎢
  ⎥
⎣ 3  2  i ⎦
1 + J2 (re /r)2 3 − 5 rzi /r rz
2
⎡ ⎤⎡ ⎤
∂gxi /∂rxi ∂gxi /∂ryi ∂gxi /∂rzi ∂rxi
⎢ ⎥⎢ ⎥
δg li = Cili ⎢ i i i i i i ⎥⎢
⎣ ∂gy /∂rx ∂gy /∂ry ∂gy /∂rz ⎦ ⎣ ∂ry ⎦
i ⎥ (7.20)
∂gzi /∂rxi ∂gzi /∂ryi ∂gzi /∂rzi ∂rzi
7.4 New Inertial/Celestial Integrated Navigation Method of Ballistic Missile 249

Initial
variance Samplin State,
and g point measure
state creation Updatin
Samplin ment
variable g state
g point value K+1
and
spread and
variance
variance
forecast
Weighting calculation

Fig. 7.7 UKF algorithm flow chart

wherein, r is the distance between the carrier and earth’s core, rx , ry , and rz are
three components of r, and re is the earth radius. μ is gravitational constant, and J2
is gravitational potential spherical harmonic of the earth. Cili is relevant coordinate-
transformation matrix.
Mathematics platform misalignment angle φ observed by using star sensor is
adopted as the measurement quantity, and equation described in Eq. (7.16) is adopted
as attitude measurement equation.
Simulation of SINS/CNS integrated navigation system may be conducted based on
perturbation-based state equation and measurement equation introduced previously
and through UKF algorithm introduced in Sect. 3.4 of this book and flow chart shown
in above Fig. 7.7, and misalignment angle and gyro drift can be estimated through
integrated filtering [13, 14]. However, the star sensor operates after the missile takes
off the atmospheric layer, and the position error and velocity error accumulated
during independent operation of SINS previously cannot be corrected. Hence, a
special method must be used to correct velocity and position error of the missile
during boost phase caused by three error sources including initial misalignment
angle, gyro drift, and null bias of accelerometer (see [1, 15] for specific method).
2. Computer Simulation of Inertial/Celestial Integrated Navigation of Ballistic
Missile Based on UKF
Suppose the scope of ballistic missiles to be 4300 km, and the flight time to be
1458 s. The missile was flying within the atmospheric layer 40 s ago, and SINS/CNS
integrated navigation system only conducts pure strapdown calculation, which is pure
SINS navigation phase; the missile flies off the atmospheric layer at 40 s and the star
sensor begins operating; 41–157 s is SINS/CNS integrated navigation phase, during
which gyro drift is estimated through integrated filtering, attitude error of three axes
is corrected, and velocity and position error caused by initial misalignment angle
and gyro drift is calculated by using state transition matrix before compensation; the
engine is shut down at 158 s, after which the accelerometer output is deemed to be
null bias of accelerometer, velocity and position error caused by which is calculated
250 7 INS/CNS Integrated Navigation Method

Fig. 7.8 Simulation results during accelerated error correction

by using state transition matrix at 180 s before compensation, and simulation lasts
for 200 s.
Simulation conditions: (1) position of missile launching point is 116◦ E, 39.9◦ N;
(2) the missile is launched eastward, and pitching angle of launching is 90◦ (vertical
launching); (3) azimuth misalignment angle aligned initially is 6 , and horizontal
misalignment angle is 10"; (4) constant drift of the gyro is 0.1◦ / h(1σ ), and random
drift is 0.05◦ / h(1σ ); constant null bias of the accelerometer is 100 μg (1σ), and
random null bias is 50 μg(1σ); precision of star sensor is 10"(1σ).
Computer simulation results are as shown in Fig. 7.8. It is observed from simula-
tion results that SINS/CNS integrated navigation based on UKF has high gyro drift
and attitude estimation precision, so position error and velocity error of SINS accu-
mulated within the atmospheric layer is corrected by using state transition matrix,
which effectively improves precision of SINS/CNS integrated navigation system.

7.5 Inertial/Celestial Integrated Navigation Method of Lunar

Vehicle

With the development of lunar exploration technology, lunar vehicle has become main
exploration device for lunar exploration. Realization of lunar vehicle navigation and
control is an important guarantee for successful completion of lunar exploration task.
7.5 Inertial/Celestial Integrated Navigation Method of Lunar Vehicle 251

information output

During motion
Navigation
Platform error angle , position error , velocity error and
inertial component error

Navigation
IMU SINS Position, velocity and attitude Position, velocity and attitude
(during rest )
SINS/CNS
integrated
navigation system
Celestial sensor CNS
Position and
attitude

Fig. 7.9 Structure diagram of SINS/CNS integrated navigation

At present, lunar vehicle generally conducts navigation and control with the help of
ground station through wireless measurement and control, but radio communication
may be interrupted on one hand, and existence of long time delay is adverse to real-
time control of lunar vehicle on the other hand. Therefore, autonomous navigation
and control of lunar vehicle by using its built-in measurement equipment has become
an important development direction [16, 17].
At present, there are two methods for autonomous navigation of lunar vehicle:
absolute navigation and relative navigation. Among them, relative navigation method
primarily includes visual navigation, that is, using mark points for navigation and
obstacle detection, route planning, etc., which is suitable for short-distance naviga-
tion. Absolute navigation method primarily includes inertial navigation and celestial
navigation, etc., which is suitable for long-distance autonomous navigation. Specific
to respective characteristics of inertial and celestial navigation, integrated navigation
system formed by combining the two navigation methods may make the best of both
and give play to advantages of both methods, so it is one of the best solutions for
long-distance autonomous positioning and navigation of lunar vehicle[17].
Since the direction of gravity is required to be measured during celestial naviga-
tion of lunar vehicle, and it is hard for existing measuring instrument to determine
direction of gravity accurately due to interference of motion acceleration during its
motion, this section only uses inertial navigation method during motion of lunar
vehicle and SINS/CNS integrated navigation method during rest to correct position,
velocity, and attitude error of inertial navigation by using characteristics of posi-
tion, attitude, and zero velocity during rest obtained from celestial navigation, and
estimate error of inertial component [17] as shown in Fig. 7.9.

7.5.1 Strapdown Inertial Navigation Method of Lunar Vehicle

Measure angular velocity and accelerated velocity of lunar vehicle under the proprio-
coordinate system through IMU, and calculate and track navigation coordinate
252 7 INS/CNS Integrated Navigation Method

system (lunar east, north and vertical geographic coordinate system) through strap-
down algorithm. Obtain position, velocity, and attitude of lunar vehicle through twice
integration [18, 19]. Basic equation of SINS may be abbreviated to:
⎧
⎪
⎨ ṙ = Dv
⎪
v̇ = Rbn f b − (2wim
n
+ wmn
n
) ×v−g (7.21)
⎪
⎪
⎩ n
Ṙb = −Ωbn n
Rbn
 
wherein, D = 1/R
0
m 0
1/(Rm cos L) ; r = [L, λ] , v = [vE , vN ] are respectively
T T

position and velocity vector of lunar vehicle; f b is output vector of the ac-
celerometer; wim n
= [0, wim cos L, wim sin L]T is spin velocity vector of the
moon, wherein, wim = 2.66 × 10– 6 rad/s is lunar angular rate of rotation, which
is small and may be ignored when requirement for precision is low; wmn n
=
T
[−vN /Rm , vE /Rm , vE tan (L)/Rm ] is angular velocity of rotation of navigation coor-
dinate system relative to lunar fixed connection coordinate system, and Rm = 1738 km
is radius of the moon; g = [0, 0, 1.618 m/s2 ]T is gravity vector;
nbn is attitude tran-
b n
sition matrix available to be described by wib and win , wherein, subscripts n and b
respectively represent navigation coordinate system and proprio-coordinate system.
Refer to Bibliography [20–22] for details of SINS.

7.5.2 A Lunar Inertial/Celestial Integrated Navigation Method

Based on UPF

1. State Equation
State equation of SINS/CNS integrated navigation system adopts system error equa-
tion with mathematics platform misalignment angle, velocity, position, and inertial
component error as observation quantities. In view of instability of vertical chan-
nel of SINS, incapability of CNS to provide height information, capacity of lunar
vehicle to drive on lunar surface only, and small height change within short time,
SINS/CNS integrated navigation system excludes height channel. With three mathe-
matics platform misalignment angles, two horizontal velocity errors, two horizontal
position errors and three gyro drifts of SINS selected as state of integrated navigation
system, and lunar geographic coordinate system (similar to east, north and vertical
geographic coordinate system of the earth) as navigation coordinate system [17],
error model of SINS is as follows:
7.5 Inertial/Celestial Integrated Navigation Method of Lunar Vehicle 253

1. Attitude error equation

⎧


⎪
⎪ δvN vE vE
⎪
⎪ φ̇E = − + ωim sin L + tgL φN − ωim cos L + φU + εx
⎪
⎪ Rm Rm Rm
⎪
⎪

⎪
⎪
⎪
⎪ δvE vE vN
⎪
⎨ N φ̇ = − ω im sin LδL − ω im sin L + tgL φE − φU + ε y
Rm Rm Rm


⎪
⎪ δvE vE
⎪
⎪ φ̇U = R tgL + ωim cos L + R sec L δL
2
⎪
⎪
⎪
⎪
⎪

m

m
⎪
⎪
⎪
⎪ vE v N
⎩ + ωim cos L + φE + φN + ε z (7.22)
Rm Rm
2. Velocity error equation
⎧

⎪
⎪ vN vE
⎪
⎪ δ υ̇ E = f φ
y U − f φ
z N + tgLδv E + 2ω im sin L + δvN
⎪
⎪ Rm Rm
⎪
⎪


⎪
⎪
⎪
⎪ v E vN vE
⎨ + 2ωim cos LvN + R sec L δL − 2ωim cos L + R δvU + ∇x
2
⎪
m m


⎪
⎪ vE
⎪
⎪ δ υ̇ = f φ − f φ − 2 ω sin L + tgL δvE
⎪
⎪
N z E x U im
⎪
⎪
Rm
⎪
⎪

⎪
⎪ vE
⎪
⎩ − 2ωim cos L + sec L vE δL + ∇y
2
(7.23)
Rm
3. Position error equation
⎧
⎪ δvN
⎪
⎨ δ L̇ = R
m
(7.24)
⎪
⎪ δv vE
⎩ δ λ̇ = E
sec L + sec LtgLδL
Rm Rm
4. Gyro drifts error equation
⎧
⎪
⎨ ε̇x = 0
⎪
ε̇y = 0 (7.25)
⎪
⎪
⎩
ε̇z = 0

The following system state equation is obtained by solving above attitude error,
velocity error, position error, and inertial component error equations simultaneously:

Ẋ = FX + GW (7.26)

wherein, state quantity X = [ φE φN φU δvE δvN δL δλ εx εy εz ]T;

E, N, and U respectively represent east, north, and vertical directions of lunar
geographic coordinate system.
254 7 INS/CNS Integrated Navigation Method

2. Measurement Equation
Observation quantity of SINS/CNS integrated navigation system includes seven di-
mensions in total, consisting of difference of output position and velocity of SINS
and CNS as well as velocity of SINS during rest (i.e., velocity error) [17].

Z = [ ψ, θ, ϕ, vE , vN , L, λ]T
= [ψI − ψC , θI − θC , ϕI − ϕC , vEI , vN I , LI − LC , λI − λC ]T (7.27)
wherein, ψI , ϕI , θI , νEI , νN I , LI , and λI are output navigation parameters of SINS,
and ψC , ϕC , θC , LC , and λC are output navigation parameters of CNS. Though CNS
cannot provide velocity information, it only operates when lunar vehicle is at rest,
during which velocity of lunar vehicle is zero, so output velocity of SINS is just the
velocity error.
In practice, attitude error angle is not completely equivalent to mathematics plat-
form misalignment angle, there is tiny difference between them, and the specific
relationship is as follows:
p
Rb = Rnp · Rbn (7.28)

⎡ ⎤ ⎡ ⎤ ⎡ ⎤

1 φU −φN c11 c12 c13 c11 c12 c13
⎢ ⎥ n ⎢ ⎥ p ⎢ ⎥
Rnp = ⎢
⎣ −φU 1 φE ⎥ ⎢
⎦, Rb = ⎣ c21 c22 c23 ⎥ ⎢
⎦, Rb = ⎣ c21

c22 ⎥
c23 ⎦

φN −φE 1 c31 c32 c33 c31 c32 c33
p
wherein, Rn is coordinate transformation matrix from lunar geographic coordinate
p
system to SINS platform coordinate system, Rb is coordinate transformation matrix
from proprio-coordinate system of lunar vehicle to SINS platform coordinate system,
and Rbn is coordinate transformation matrix from proprio-coordinate system of lunar
vehicle to lunar geographic coordinate system.
It is obtained from attitude calculation formula of SINS that:

c12 c31
tan (ψI ) = − , sin (θ I ) = c32 , tan (ϕ I ) = − (7.29)
c22 c33

Since tan (ψI ) = tan (ψ + ψ) = tan (ψ) tan ( ψ)

1−tan (ψ) tan ( ψ)
≈ tan (ψ) ψ
1−tan (ψ) ψ
, it is obtained by
using Eq. (7.29) that:

c12 c12 − c32 φN + c22 φU − cc12 ψ
− =− = 22
(7.30)
c22 c22 + c32 φE − c12 φU 1 + − c22 ψ
c12

c12 c32 φE + c22 c32 φN − (c12

2
+ c22
2
)φU
ψ = . (7.31)
c12 + c22 + c22 c32 φE − c12 c32 φN
2 2

Similarly, it is obtained that:

sin (θI ) = sin (θ + θ) = sin θ cos θ + cos θ sin θ ≈ sin θ + cos θ · θ

(7.32)
7.5 Inertial/Celestial Integrated Navigation Method of Lunar Vehicle 255


c32 = c32 − c22 φx + c12 φy = c32 + 1 − c32
2
· θ (7.33)

tan (ϕ) tan ( ϕ) tan (ϕ) ϕ

tan (ϕI ) = tan (ϕ + ϕ) = ≈ (7.34)
1 − tan (ϕ) tan ( ϕ) 1 − tan (ϕ) ϕ

c31 c31 − c21 φE + c11 φN − cc31 ϕ
− =− = 33
(7.35)
c33 c33 − c23 φE + c13 φN 1 + − c33 ϕ
c31

From Eqs. (7.32–7.35), it is worked out that the expressions of θ and ϕ are:
−c22 φE + c12 φN
θ =  (7.36)
1 − c32
2

(c21 c33 − c23 c31 )φE + (c13 c31 − c11 c33 )φN
ϕ = (7.37)
2
c31 + c33
2
− (c21 c31 + c23 c33 )φE + (c11 c31 + c13 c33 )φN

The following measurement equation of SINS/CNS integrated navigation system can

be established by using Eq. (7.28, 7.29, 7.36, and 7.37):

Z = h([X(t), t]) + W (t) (7.38)

3. SINS/CNS integrated Navigation Based on UPF

System state estimation may be conducted by using optimal estimation method in
accordance with state equation and measurement equation established above. Since
the above system state equation is time varying, statistical characteristics of system
noise and measurement noise are also varying, but traditional Kalman filtering is
based on accurate model and given statistical characteristics of random interference
signal, so traditional Kalman filtering algorithm cannot obtain the optimal estimation
effect. UPF method is proposed to solve nonlinear problem, and its core concept is
to approximate state distribution by using determinate sampling points, and select
sampling points to capture real mean value and variance of state distribution, so
its effect is superior to KF for nonlinear system with indeterminate noise. This
subsection adopts UPF for state estimation, referring to Sect. 3.6 of this book for
specific filtering steps.
Flow of SINS/CNS integrated navigation based on UPF is as shown in Fig. 7.10.
4. Computer Simulation
Conduct SINS/CNS integrated navigation of lunar vehicle by using above designed
filtering flow based on UPF, where simulation conditions are: lunar vehicle adopts
landing site of American Prospector No. 3 detector (2◦ 56 N, 336◦ 40 E), and lunar
vehicle stops for 5 min for every 15 min of driving during 80 min of operation process.
When the lunar vehicle is driving, only SINS operates; when it stops, subsystem
SINS and CNS operate simultaneously. Precision of star sensor is 3" (1σ), update
frequency is 5 Hz; constant drift of the gyro is 0.05◦ /h (1σ), and constant null bias
of the accelerometer is 10 μg (1σ). Simulation results are as shown in Table 7.1.
256 7 INS/CNS Integrated Navigation Method

UPF

Initialization

UKF

Importance sampling
Sigma sampling point

Particle updating through

UKF
SINS error model Time updating
Importance weighting
calculation

CNS Measurement updating

-
System resampling
Σ
SINS +
V=0 MCMC movement

Result output
Output

Fig. 7.10 UPF filtering flow diagram of SINS/CNS integrated navigation

Table 7.1 Compare of simulation results of pure SINS navigation and SINS/CNS integrated
navigation method
Maximum Position Velocity Attitude
error Longitude Latitude East North Pitching Roll angle Yaw angle
(◦ ) (◦ ) (m/s) (m/s) angle (◦ ) (◦ ) (◦ )
SINS − 0.5434 0.1610 − 5.9075 − 3.8462 − 0.1115 − 0.1356 0.4702
SINS/CNS − 0.0027 − 0.0022 − 0.4531 − 0.3217 − 0.0628 − 0.0310 0.0576

It is observed from the table above that under pure SINS operation, error of
SINS increases rapidly with time, and maximum longitude and latitude errors reach
− 0.54◦ and 0.16◦ equivalent to 20 km within 80 min of operation time. During
SINS/CNS integrated navigation based on UPF, estimated longitude and latitude
errors are limited within 0.0027◦ and 0.0022◦ , only equivalent to 100 m, that is, error
of SINS is limited within a small scope instead of divergence through continuous
filtering correction for SINS by using CNS, which improves navigation precision of
the lunar vehicle.
7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite 257

7.6 Inertial/Celestial Integrated Attitude Determination

Method of Satellite

With rapid development of deep space exploration technology and satellite technol-
ogy, higher requirement for precision of satellite attitude control is made by humans,
and precondition to realize high-precision attitude control of satellite is to realize
high-precision attitude determination. Solution of attitude determination by combin-
ing star sensor and inertial gyro is a most effective attitude determination solution
[23–25]. This section takes satellite as the background to first introduce attitude
determination model of satellite, and then introduce several satellite high-precision
attitude determination methods based on SINS/CNS.

7.6.1 Satellite Attitude Determination System Equation

1. Gyro Measurement Model

Suppose sensitive axes of the three gyros to be respectively consistent with satellite
proprio-axes x, y, and z, then gyro model is as follows:

ωg = ω + b + n a (7.39)

wherein, ωg is gyro measurement value, ω is true angular rate value, b is constant

drift of the gyro, and na is measurement noise.
2. Star Kinematical Equation Expressed with Quaternion [26]
Suppose navigation coordinate system to be inertial coordinate system, and attitude
of satellite is expressed with quaternion q of star coordinate system relative to inertial
coordinate system, defined as:

q = [e, q]T (7.40)

wherein, e = [e1 , e2 , e3 ] is vector of quaternion, and q is scalar of quaternion. Star

kinematical equation expressed with quaternion is:
1 1
q̇ =
(ω)q = q ⊗ ω (7.41)
2 2
wherein, ω = [ωx , ωy , ωz ]T is angular velocity of rotation of star coordinate system
relative to inertial coordinate system.
⎡ ⎤
⎡ ⎤ 0 −ωz ωy
−[ω × ] ω ⎢ ⎥

(ω) = ⎣ ⎦, [ω×] = ⎢ ωz 0 −ωx⎥ (7.42)
⎣ ⎦
−ω T
0
−ωy ωx 0
258 7 INS/CNS Integrated Navigation Method

3. State Equation
There is norm restriction in quaternion, if four components of quaternion are selected
as state variable, the variance matrix will be singular. To avoid singular variance
matrix problem, error quaternion between real quaternion q defined by multiplicative
quaternion and quaternion calculated value q̂ is adopted as state variable:

δq = q̂ −1 ⊗ q = [δe, δq]T (7.43)

Take vector δe of error quaternion and estimated gyro drift error b as error state
variables, and it is obtained from quaternion kinematical equation that:
1
q̇ = q ⊗ω (7.44)
2
1
q̂˙ = q̂ ⊗ ω̂ (7.45)
2
Take the derivation of Eq. (7.43) and substitute Eqs. (7.44) and (7.45) to obtain:
1
δ q̇ = [δq ⊗ ω − ω̂ ⊗ δq] (7.46)
2
Wherein, δω is defined as:

δω = [ω − ω ] = [(b − b̂) + na ] (7.47)

Substitute Eq. (7.47) into (7.46) to obtain:

1 1
δ q̇ = [δq ⊗ ω̂ − ω̂ ⊗ δq] + δq ⊗ δω (7.48)
2 2
Linearize Eq. (7.48) to obtain:
1
δ q̇ = −ω̂ × δe − ( b + na ) (7.49)
2
So error variable state equation is:

Ẋ(t) = F(t) X + G(t)ε(t) (7.50)

wherein,
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
1 1
−[ω(t) × ] − I3×3 − I 04×3 na
⎢ 2 ⎥ ⎢ 2 3×3 ⎥ ⎢ ⎥
F(t) = ⎣ T⎦, G(t) = ⎣ ⎦, ε(t) = ⎣ ⎦.
03×4 03×3 03×3 I3×3 0

4. Attitude Quaternion Measurement Equation of Satellite

Star sensor determines attitude quaternion q of satellite star coordinate system relative
to inertial coordinate system according to starlight vector information observed;
7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite 259

define error quaternion between output quaternion q of star sensor and calculated
quaternion q̂ as q, so q is expressed as:

q = [ e, q] = q̂ −1 ⊗ q. (7.51)

Take vector e of q as observation quantity and linearize above equation to obtain

linearized error measurement equation;
 T
e = δe + v = H X + v = I3×3 03×12 X + v (7.52)

wherein, v is measurement noise.

7.6.2 An Inertia/Celestial Integrated Attitude Determination

Method of Piecewise Information Fusion Based on EKF

Realize high-precision attitude determination according to above SINS/CNS inte-

grated attitude determination model by combining different characteristics of attitude
determination algorithm based on vector observation and EKF filtering algorithm.
Advantages of optimal REQUEST method are: (1) it is both available for sequen-
tial processing of observed starlight vector and attitude determination under single
star circumstance; (2) introduction of Kalman filtering frame can effectively esti-
mate design process of optimal fading factor and simplified filtering. But it is only
specific to white system noise circumstance, and gyro noise is not pure white noise,
so optimal REQUEST method will diverge rapidly if gyro drift is not compensated.
Advantage of EKF method is that it can estimate gyro drift besides estimating
attitude parameters, but it requires attitude parameters from the star sensor to serve
as observation quantity. It is considered to combine optimal REQUEST method and
EKF method to design a dual filter [26]. Imbed optimal REQUEST filter into EKF
filter, and take quaternion determined through optimal REQUEST method directly
as observation quantity of EKF filter. Gyro drift modeling is not done in optimal
REQUEST method, gyro drift is deemed to be system model error, and system
model error of optimal REQUEST is compensated by gyro drift estimated by EKF
filter.
1. EKF Algorithm
Its filtering performance is equivalent to that of Kalman filtering during processing
of linear system; but for nonlinear system, it requires linearization; refer to Sect. 3.3
of this book for specific principle.
2. Optimal REQUEST Algorithm
The same as standard Kalman filtering process, optimal REQUEST algorithm re-
quires for initialization of matrix K and variance matrix P before time updating and
measurement updating [27].
260 7 INS/CNS Integrated Navigation Method

1) Time updating

Kk+1/k = k Kk/k Tk (7.53)

Pk+1/k = k Pk/k Tk + Qk (7.54)

2) Measurement updating

∗ m2k tr(Pk+1/k )
ρk+1 = (7.55)
m2k tr(Pk+1/k ) + δm2k+1 tr(Rk+1 )

1 ∗ ∗
δmk+1 = δmk − 1 + , mk+1 = (1 − ρk+1 )mk + ρk+1 δmk+1 (7.56)
k
∗ mk ∗ δmk+1
Kk+1/k+1 = (1 − ρk+1 ) Kk+1/k + ρk+1 δKk+1 (7.57)
mk+1 mk+1
 
2
 ∗
 mk 2 ∗ δmk+1
Pk+1/k+1 = 1 − ρk+1 Pk+1/k + ρk+1 Rk+1 (7.58)
mk+1 mk+1

Optimal REQUEST algorithm takes matrix K as state variable, and it is available for
sequential estimation of matrix K at each moment with only inertial angular velocity
and starlight vector of each moment. Optimal quaternion belongs to eigenvector with
maximum eigenvalue for matrix K.
3. EKF + Optimal REQUEST Dual Filter Design
With EKF filter as the external frame, embed optimal REQUEST filter into EKF
filter, detailed process as follows:
1. First, determine initial state value of EKF filter X = [e, b]T , and determine initial
value of matrix K by using q-method according to observed starlight vector.
Extract quaternion from matrix K, take the vector as initial value of eT of EKF
filter, and give initial value and variance matrix P of gyro drift b.
2. Prior to single-step forecast, compensate gyro drift, i.e., subtract gyro drift from
measured angular velocity, and then conduct single-stop forecast for quaternion to
work out qk+1/k , in addition, there is bk+1 = bk , take Xk+1 = [eT k+1/k , bT k+1/k ]T ,
and then work out Kk+1 and Pk+1/k .
3. If there is starlight vector information input, recur matrix K with optimal
REQUEST method, and extract quaternion qm from matrix K to serve as ob-
servation quantity of EKF filter; calculate δ X̂k , and deform the formula into
−1
δ X̂k+1 = Kk+1 ek+1 , wherein, ek+1 is vector of qk+1 = q̂k+1/k ⊗ qm .
4. Calculate X̂k+1 and P̂k+1 , work out attitude of the moment, and return to step (2)
to continue the next cycle.
5. If there is no starlight vector information input, X̂k+1 = X̂k+1/k , P̂k+1 = P̂k+1/k ;
return to step (2) to continue the next cycle.
7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite 261

Fig. 7.11 Operating principle block diagram for attitude estimator of piecewise information fusion

4. Attitude Determination Method of Piecewise Information Fusion [26]

Calculation of Φk in system equation of optimal REQUEST will use angular velocity
measurement value, that is, it will be influenced by system dynamics or kinematic
model error. If estimated gyro drift error is large, precision of quaternion estimated
by optimal REQUEST filter will be influenced, and if the estimated quaternion is
used as observation quantity of EKF, precision of estimated quaternion and gyro drift
will be influenced in reverse.
In view of influence of accurate system error model at start time on precision of
optimal REQUEST filter, an attitude determination method of piecewise information
fusion is introduced here, and its operating principle is as shown in Fig. 7.11.
At start time, utilize vector information observed by star sensor and use attitude
quaternion determined by QUEST algorithm as observation quantity of EKF filter.
Since precision of the quaternion is only relevant to precision of star sensor and
algorithm and not influenced by model error, combination of QUEST and EKF
262 7 INS/CNS Integrated Navigation Method

Fig. 7.12 Comparison of estimated roll angle errors

will rapidly estimate gyro drift, and once estimated gyro drift error is reduced to
certain scope, advantages of recursive optimal REQUEST method will be reflected
compared with QUEST algorithm. At this time, switch to dual filter consisting of
optimal REQUEST method and EKF, and better effect than pure application of EKF
and optimal REQUEST fusion filter will be obtained.
5. Computer Simulation [26]
Specific to microsatellite attitude determination system, conduct integrated attitude
determination by using star sensor based on CMOS APS and MEMS gyro. Suppose
simulation parameters to be: measurement precisions of star sensor 100 (1σ), update
frequency 1 Hz, and suppose that the star sensor can observe two starlight vectors,
and star ephemeris is tycho2. Sampling frequency of the gyro is 10 Hz, constant drift
is 20◦ /h, and random drift is 0.2◦ /h. Suppose that microsatellite proprio-coordinate
system coincides with inertial coordinate system and is resting relative to inertial
coordinate system, initial value of estimated gyro drift error is 10◦ /h, and simulation
time is 500 s. Simulation result is expressed in the form of estimated attitude angle
error.
Adopt two attitude determination methods: one is dual filtering attitude deter-
mination method fusing EKF and optimal REQUEST, and the other is attitude
determination method combining piecewise information fusion of QUEST, that is
combine QUEST and EKF filter during the first 150 s to estimate gyro drift and
compensate; switch to dual filter consisting of optimal REQUEST method and EKF
150 s later. Simulation compare results are as shown in Fig. 7.12 and 7.13; the former
is comparison of estimated roll angle errors and the latter is comparison of estimated
gyro drift errors of axis X; results of estimated error of other attitude angles and gyro
drifts are not shown here since they are similar.
7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite 263

It is observed from Fig. 7.12 and 7.13 that final estimated attitude angle error of
dual filtering method fused by EKF and optimal REQUEST is about 0.01◦ , estimated
gyro drift error is about 0.35◦ /h; but at the start time, estimated roll angle error
presents divergence trend, which is because output angular velocity of the gyro
is required to be used for system equation of optimal REQUEST method, so if
estimated gyro drift error is large at start time, it will directly influence precision
of quaternion estimation through optimal REQUEST algorithm, which will then
influence estimation precision of dual filter; however, with further precise estimation
and compensation of gyro drift, estimated attitude angle error gradually reduces and
converges to level of 0.01◦ finally. Compared with pure EKF + optimal REQUEST
dual filtering method, convergence velocity of attitude determination method of
piecewise information fusion is quicker, and estimation precision of attitude angle
and gyro drift is higher, especially when initial estimated gyro drift error is large,
attitude estimation method of piecewise information fusion can rapidly converge and
reach high attitude determination precision.

7.6.3 Method of Minimum Parameter Attitude Matrix Estimation

of Satellite Based on UKF

Previous subsection introduces a SINS/CNS integrated attitude determination

method of piecewise information fusion based on EKF, and this subsection will in-
troduce a minimum parameter attitude matrix estimation method suitable for satellite
attitude determination system [28].
Minimum parameter method was proposed by Oshman and Markly [29, 30]; it is a
three-order solution adopting orthogonal matrix differential equation to use estimated
minimum three-dimensional vector to express nine parameters of direction cosine
matrix. It requires only angular velocity of rotation of proprio-coordinate system
relative to inertial coordinate system measured by the gyro plus an observation vector
of each moment for sequential determination of attitude matrix of proprio-coordinate
system relative to inertial coordinate system at each moment.
Since minimum parameter method belongs to nonlinear filtering problem, and it
is known from Sect. 3.4 of this book that UKF is an effective filtering method to solve
nonlinear problem, a minimum parameter attitude matrix estimation method based on
UKF will be introduced by combining UKF and minimum parameter attitude matrix
estimation method on the basis of minimum parameter attitude matrix estimation
method proposed by Oshman and Markly [28].
1. Minimum Parameter Problem
As an ordinary attitude parameter, attitude matrix (direction cosine matrix) is often
applied to attitude determination system of gyro—star sensor configuration mode.
Since it is nonsingular in essence, special singular processing is not required. In
addition, its kinetic equation is approximately linear, which is beneficial to design
and realization of filter. However, huge computation is major disadvantage of taking
264 7 INS/CNS Integrated Navigation Method

Fig. 7.13 Comparison of estimated gyro drift errors of axis X

attitude matrix as attitude parameter, since it is required to estimate a 9-dimensional

state vector.
In [29], a minimum parameter attitude matrix estimation method is introduced,
which adopts three-order minimum parameter method to solve orthogonal matrix
differential equation and uses minimum three-dimensional filter to estimate attitude
matrix of each moment in sequence. Therefore, computation burden brought by direct
attitude matrix estimation is reduced. The method only requires angular velocity
information of each moment and an observation vector, so it is simple, stable, and
efficient [31, 32].
A n-dimensional matrix differential equation is given:

V̇ (t) = W (t)V (t), V (t0 ) = V0 (7.59)

wherein, V(t) ∈ Rn,n , V0VT0 = I, W(t) = −WT (t). Minimum parameter problem is
to find out a group of m = n(n − 1)/2 parameters able to clearly define V(t) and
differential equation the m parameters satisfy. Then, find out conversion of the m
parameters matching with matrix V(t), and work out the differential equation the m
parameters satisfy with a simple and effective method and calculate matrix V(t).
Define antisymmetric matrix D(t, t0 ) as:
5 t
D(t, t0 ) = W (τ )dτ (7.60)
t0
7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite 265

Then three-order approximate solution Ṽ (t) of matrix V (t) is defined as:

:
D 2 (t, t0 ) D 3 (t, t0 )
Ṽ (t, t0 ) = I + D(t, t0 ) + +
2! 3!
;
(t − t0 )
+ [D(t, t0 )W0 − W0 D(t, t0 )] V0 (7.61)
3!

wherein, W0 = W(t0 ), Ṽ(t) is three-order approximate solution of orthogonal matrix

V(t), satisfying:

Ṽ (t, t0 )Ṽ T (t, t0 ) = I + O((t − t0 )4 ) (7.62)

The minimum parameter here is n(n − 1)/2 secondary diagonal elements of D(t, t0 ).
For three-dimensional problem, there is a simple geometric interpretation for these
parameters, that is, angle of three components of angular velocity vector ω(t)
obtained through time integral.
 T
ω(t) = ω1 (t) ω2 (t) ω3 (t) (7.63)

ωi is angular velocity component relative to inertial system along axis i, i = 1,2, 3

respectively represents x, y, z. Minimum parameter is secondary diagonal element
of matrix D(t, t0 ), and these parameters satisfy the following differential equation:

Ḋ(t, t0 ) = W (t), D(t0 , t0 ) = 0 (7.64)

wherein, W(t) = [ω(t)×] is corresponding antisymmetric matrix of ω(t).

2. Attitude Matrix Kinematical Equation
For three-dimensional problem, orthogonal matrix refers to attitude matrix or di-
rection cosine matrix, denoted by A(t) and satisfying the following differential
equation:

Ȧ(t) =
(t)A(t), A(t0 ) = A0 (7.65)

(t) = −[ω(t)×] (7.66)

Here, D(t, t0 ) adopts the following form:

D(t, t0 ) = −[θ (t)×] (7.67)

Vector θ (t) is defined as:

 T
θ (t) = θ1 (t) θ2 (t) θ3 (t) (7.68)
5 t
θi (t) = ωi (τ )dτ , i = 1,2, 3 (7.69)
t0
266 7 INS/CNS Integrated Navigation Method

wherein, θ (t) is angular rate integral parameter, three components θ1 (t), θ2 (t), θ3 (t)
of which are respectively obtained through integral of three components
ω1 (t), ω2 (t), ω3 (t) of angular velocity vector ω(t).
3. Method of Minimum Parameter Attitude Matrix Estimation of Spacecraft
Based on UKF
Specific to nonlinear problem of minimum parameter attitude matrix estimation
method, an attitude estimator based on UKF can be designed by combing UKF and
minimum parameter attitude matrix estimation method. The attitude estimator selects
angular rate integral parameter θ defined in Eq. (7.68) as state variable [33].
Satellite attitude determination system uses star sensor and inertial gyro combi-
nation mode, where the star sensor provides direction of starlight on satellite star
coordinate system and inertial gyro measures triaxial inertial angular velocity of
satellite.

1. System Model

With angular rate integral parameter θ (t) as state variable of the filter, make sampling
period T = tk+1 − tk , θ (k) = θ (tk ), and state vector at the time of tk is:
 T
θ (k) = θ1 (k) θ2 (k) θ3 (k) (7.70)

It is implied in formula (7.69) that:

5 tk
θi (k) = ωi (τ )dτ , i = 1,2, 3 (7.71)
t0

wherein, ω(t) is angular rate vector defined in Eq. (7.63); in accordance with formula
(7.71), we have:
5 tk+1
θ (k + 1) = θ (k) + ω(τ )dτ , i = 1,2, 3 (7.72)
tk

The equation above is just system equation, and it is rewritten as follows:

θ̇ (t) = ω(t) + δω(t) (7.73)

wherein, θ = [θ1 , θ2 , θ3 ]T , in addition, angular velocity is obtained through gyro

measurement, denoted by ω̂(t) and satisfying the following equation:

ω̂(t) = ω(t) + δω(t) (7.74)

δω(t) is gyro noise; to simplify the problem, suppose gyro noise to consist of gyro
constant drift and white noise and conform to Gaussian distribution, that is:

δω(t) ∼ N (ω̄, Q(t)) (7.75)

wherein, ω̄ is expectation vector of gyro drift; Q(t) is white noise intensity.

7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite 267

2. System model subject to discretization

5 tk+1
θk+1 = θk + ω̂(τ )dτ (7.76)
tk

3. Measurement equation

If b(k + 1) represents observation value containing noise of starlight vector r(k + 1)

within satellite star coordinate system, b0 (k +1) represents true value, δb is Gaussian
white noise: δb(k)˜N (0, Rb (k)), vector observation model shall be:

bk+1 = A(θk+1 )rk+1 + δbk+1 (7.77)

4. UKF Attitude Estimator Design

1. First select initial state value of UKF: θk−1 = [ 0 0 0 ]T , and define value of
P. Make τ = −1, and calculate each sampling point and corresponding weight
according to the formula.
2. Make each sampling point pass system model (7.76) subject to discretization,
calculate each sampling point θk,k−1 subject to conversion by using the formula,
and work out forecast mean value θ̂k− and forecast variance P− k.
3. Make sampling point θk,k−1 subject to conversion pass nonlinear measurement
model (7.77) again, and work out forecasted measurement value b̂k,k−1 and mea-
−
surement mean value b̂k , corresponding covariance Pb̄k b̄k , Pθk bk and filtering gain
Kk according to the formula.
4. Conduct measurement updating according to latest obtained observation quantity
bk to get final state estimation value θ̂k and covariance Pk .
5. It should be noted that θ̂k−1 should be set as 0 at the beginning of each filtering
period. Corresponding direction cosine matrix calculation formula is:

D̂ 2 (k + 1, k) D̂ 3 (k + 1, k)
Â(k + 1) = I + D̂(k + 1, k) + +
2! 3!
;
T 
+ D̂(k + 1, k)Ŵ (k) − Ŵ (k)D̂(k + 1, k) Â(k). (7.78)
3!

5. Computer Simulation
Simulation conditions: measurement precision of star sensor is 60 (1 σ ), field of
view is 10◦ ×10◦ , and update frequency is 0.5 Hz; suppose optical axis of star sensor
to be along the direction of axis z of satellite proprio-coordinate system, and star
ephemeris adopts tycho2. Constant drift of the gyro is 20◦ /h and random drift is 0.2◦ /h.
Sampling frequency of gyro is 20 Hz. Initial attitude is three Euler angles [12◦ 8◦ 4◦ ]T
268 7 INS/CNS Integrated Navigation Method

compare of the estimated attitude angle error

0.06
EKF
0.04 UKF
roll (°)

0.02

0
0 50 100 150 200 250 300
0.06
EKF
pitch(°)

0.04 UKF

0.02

0
0 50 100 150 200 250 300

0.1 EKF
UKF
yaw(°)

0.05

0
0 50 100 150 200 250 300
time(s)
Fig. 7.14 Compare of performance of UKF and EKF

of satellite star coordinate system relative to inertial coordinate system, suppose

 T
inertial angular velocity to change as per rule of ω(t) = 2◦ 3◦ 4◦ × sin (0.2 × t),
and simulation time is 300 s. Simulation results first convert estimated attitude matrix
into corresponding attitude angle, and then denote estimation precision by estimated
attitude angle error. Figure 7.14 is a comparison of simulation results specific to UKF
and EKF filter of absolute value of estimated attitude angle error.
It is observed from the figure above that convergence rate of UKF is faster than
that of EKF, and fluctuation of UKF is small with high precision. Therefore, min-
imum parameter attitude matrix estimation method based on UKF designed in this
subsection further improves attitude determination precision and reliability of atti-
tude determination system, and it not only can satisfy requirement for precision of
satellite attitude determination, but also has good real-time performance, so it is an
attitude determination method applying to both stable and mobile attitude of satellite.
7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite 269

7.6.4 Interlaced Optimal-REQUEST and Unscented Kalman

Filtering for Attitude Determination

The EKF and UKF methods for high performance attitude determination of strap-
down stellar (A CMOS APS star sensor)-inertial (three low-cost MEMS gyroscopes)
integrated systems can estimate not only attitude parameters, but also gyro drifts.
However, these methods require measurements from the star sensor that are disposed
sequentially. Therefore, a traditional interlaced filtering method based on the EKF,
QUEST, and optimal-REQUEST for micro-spacecraft was presented byYu and Fang
in 2005 [34] and an adaptive interlaced filtering method for attitude determination
of nano-spacecraft was presented by Quan et al. in 2008 [35]. However, the optimal-
REQUEST algorithm is only an approximate algorithm and contains errors beyond
those of the EKF and UKF for attitude information. There is six-dimensional state
variable for filtering models in Refs. [34, 35], which leads to low speed of attitude
determination. However, using the EKF and UKF directly with the quaternion atti-
tude parameterization yields a non-unit norm quaternion estimate. According to the
various features of the UKF and optimal-REQUEST, an interlaced filtering method is
presented by setting the quaternion as the attitude representation, using the UKF and
optimal-REQUEST to estimate the gyro drifts and the quaternion respectively. When
the optimal-REQUEST algorithm estimates the attitude quaternion, the gyro drifts
are estimated by the UKF algorithm only using the three-dimensional state variable
which is much quicker than in the case of six dimensions. Meanwhile, the attitude
quaternion derived from the optimal-REQUEST, is adopted when the UKF estimates
the gyro drifts. Furthermore, the state dimension is only set to three to improve the
speed of attitude determination. This method not only has higher performance of
attitude determination, but also quickly estimates gyro drifts [37].
1. System Model of Integrated Attitude Determination System
Supposing that the three sensitive axes of three MEMS gyroscopes are respectively
parallel to the three body-axes of a spacecraft, a general gyroscope model is given
by

ω̃k = ωk + β k + ηv,k (7.79)

β k+1 = β k + ηu,k (7.80)

where ω̃k is the gyroscope measurement, ωk the attitude angular rate of the gyro
input axis, β k the gyro drift vector, ηv,k and ηu,k are independent zero-mean Gaussian
white-noise processes, and the variance of ηv,k and ηu,k are σv2 and σu2 respectively.
As a high-accuracy vector sensor, the measurement model of the star sensor is
defined by

bk = A(qk )rk + δbk (7.81)

where rk is the stellar reference vector in inertial coordinates, which can be obtained
from the astronomical ephemeris; bk the measurement vector containing noise in the
270 7 INS/CNS Integrated Navigation Method

star sensor coordinates, which are supposed to be the same as the body coordinates
of the spacecraft; δbk the noise of the measurement and satisfied by

E(δb) = 0
(7.82)
E(δb · δbT ) = σ 2 (I 3 − bbT )

where I 3 is the third-order identity matrix and σ 2 the variance of the measurement
noise.
A(qk ) is the attitude matrix from the inertial coordinates to the spacecraft body
 T
coordinates, and can be described as a function of the quaternion qk = q̄Tk q4k :

A(qk ) = ((q4k )2 − q̄Tk q̄k )I 3 + 2q̄k q̄Tk − 2q4k [q̄k ×] (7.83)

 T
where q̄k = q1k q2k q3k and q4k are the vector scale part of the quaternion,
respectively; [q̄k ×] denotes the cross-product matrix associated with the vector q̄k .
Spacecraft attitude is described using the quaternion qk in the body coordinates
relative to the inertial coordinates, and the kinematical equations of a spacecraft are
described as

qk+1 = Ω(ωk )qk (7.84)

 T
where ωk = ωxk ωyk ωzk is the angular velocity in the body coordinates
relative to the inertial coordinates.
Assuming that ωk is constant in the sampling time interval t, the orthogonal
transition matrix Ω(ωk ) is expressed using ωk and computed as follows:
⎡ ⎤
cos (0.5 ωk  t) I 3×3 − [ψ k ×] ψk
⎢ ⎥
Ω(ωk ) = ⎣ ⎦ (7.85)
−ψ kT
cos (0.5 ωk  t)
4×4

sin (0.5 ωk  t)ωk

ψk = (7.86)
ωk 
where [ψ k ×] denotes the cross-product matrix associated with the vector ψ k .
2. Optimal-REQUEST and UKF Based Interlaced Filtering Method
As mentioned above, the optimal-REQUEST method has the advantage of sequen-
tially dealing with the observed starlight vector. Even though there is only one vector,
the spacecraft attitude can be calculated using the optimal-REQUEST algorithm.
However, this method can only be used to estimate the attitude. Considering that
non-Gaussian gyro drifts are inherent for gyroscopes, the UKF method can estimate
spacecraft attitude parameters and gyroscope drifts, although it needs measurement
data from the star sensor.
According to the various features of the UKF and optimal-REQUEST, the
segmented filter is designed for the combination of the algorithms, which uses
7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite 271

Start

Initialization set

Angle velocity data from gyros (100Hz)

Star observation
Observation N
vectors from star
vectors arrive?
sensor (10Hz)
Y

Optimal-REQUEST Optimal-REQUEST
mathematical model for attitude determination

z –1 qˆ k +1 , Switch on UKF z –1 βˆ k +1 , Switch on Optimal -REQUEST

UKF algorithm UKF

mathematical model for the gyro drifts estimation

New observation Y k =k+1

vectors?

End

Fig. 7.15 Flow diagram of the optimal-REQUEST and UKF interlaced filtering method

optimal-REQUEST and UKF algorithms to estimate the attitude quaternion and

the gyro drifts, respectively.
In this section, the optimal-REQUEST and UKF interlaced filtering method is
presented, which uses optimal-REQUEST algorithm and UKF algorithm to estimate
the rotation quaternion and the gyro drifts, respectively, as shown in Fig. 7.15. z−1
indicates a delay.
272 7 INS/CNS Integrated Navigation Method

The optimal-REQUEST algorithm is derived from the REQUEST algorithm,

which takes the K-matrix as state variables, and can sequentially estimate the
K-matrix at any time. The optimal quaternion is the eigenvector corresponding to
the largest eigenvalue of the K-matrix. According to the starlight vector observed by
the star sensor and the reference vector obtained from the astronomical ephemeris
and gyro angular rate information, the attitude quaternion matrix can be obtained at
any moment by the recursive processing using the optimal-REQUEST. Meanwhile,
the gyro drift vector β̂ k estimated at time k is used to determine the quaternion at
time k + 1.
The output of gyro can’t be used for attitude estimation directly for that the gyro
has bias vector, which should be considered in the estimation process. The UKF is
designed to estimate the gyro bias vector.
The system equation of the UKF is built on the basis of the gyro measurement
model and given by:

β k+1 = β k + ηu,k (7.87)

According to the measurement model of the star sensor, the observation equation of
the UKF algorithm is as follows:

bk+1 = A(Ω(ω̃k − β k )qk )rk+1 + δbk+1 (7.88)

It can be seen that the state equation is linear while the observation equation is
nonlinear, and the attitude quaternion estimated at time k is adopted to estimate the
gyro drifts at time k + 1.
3. Hybrid Simulation Results and Analysis
The spacecraft attitude determination system achieves attitude determination by us-
ing a star sensor based on CMOS APS chips and micro-inertial measurement unit
(IMU) based on MEMS devices. Micro-IMU integrates three MEMS gyroscopes
to measure the 3-axis angular velocity of the spacecraft. The star sensor supplies
the starlight direction in the spacecraft’s body coordinates. The hybrid simulation
system diagram for the experiment is depicted in Fig. 7.16.
In Fig. 7.16, Part A is attitude data generator terminal performed by personal
computer (PC), and the standard data of attitude and inertial gyros is created by
Satellite Tool Kit (STK) software, which is installed in the PC A and includes the
dynamics model of the nano-spacecraft. Part B is an integrated star atlas simulator
device, which produces real-time star atlas using the software of star map simulation
and creates parallel light according to standard attitude data and setting matrix of the
star sensor. Part C is a star sensor device simulator used to receive the parallel light,
and outputs star observation vectors which are received by Part E. There are some
algorithms (denoising, distorting, centroid extracting, star recognition, and creating
observation vectors) in Part C. Part D is a micro-IMU and outputs the characteristic
noise data which is equal to the difference of the inertial angular rate data and its
mean in static base. The sum of the characteristic noise data and the standard inertial
gyros data is inputted in Part E which is a special PC with the presented method in
this paper to accomplish the attitude determination of the spacecraft.
7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite 273

Attitude Data B Star Atlas Simulator C Star Sensor Simulator

Generator PC

Z
E
DSP
Y

Integrated Attitude
X D Micro-IMU
Determined PC

Fig. 7.16 Illustration diagram of the hybrid simulation system

In the simulation based on the hybrid simulation system, the presented interlaced
filtering method and the UKF algorithm are studied and compared with respect to
the computational cost and accuracy. The initial conditions of the simulation are
set as follows. The measurement precision of the star sensor is 5" (1σ), the update
frequency is 10 Hz, the astronomical ephemeris is tycho2n star catalog, and the
output is four observation vectors, which is determined by star map recognition (the
pyramid recognition method is adopted in this section). The sampling frequency of
the gyroscope is 100 Hz, the gyro drift rate is 1 (◦ )/h, and the random drift rate is 0.2
(◦ )/h. It is supposed that the Z-axis of the star sensor is in the opposite direction to the
Z-axis of the spacecraft body, and the X-axes are in the same direction. The J2000
coordinate is defined as the navigation coordinate and the duration of the simulation
is set to be 500 s.
The simulation experiment is based on Intel® CoreTM i3 CPU [email protected] GHz
digital processor with 3.24 GB memory, which is installed in the platform of MAT-
LAB R2006b. The sample number is set to be six groups, which are 5000, 7000,
10,000, 15,000, 20,000, and 25,000. For every group, 100 trials are carried out and
the mean value of the execution time of attitude determination is calculated as this
group’s execution time. Finally, the simulation results are averaged. The average
values of the execution time are shown in Table 7.2. The results of the attitude deter-
mination methods based on the UKF, traditional interlaced filtering method [16–17],
and the presented novel method are compared in Table 7.2.
274 7 INS/CNS Integrated Navigation Method

Table 7.2 Comparison of the computation time between the attitude determination methods based
on the UKF and interlaced filtering
Sample number Computational cost
UKF algorithm Traditional interlaced Novel method
filtering method
5000 4.675 5.958 3.444
7000 6.542 9.002 5.016
10,000 9.345 13.880 7.588
15,000 14.006 21.169 10.900
20,000 18.736 27.653 13.236
25,000 23.287 33.918 15.445

From Table 7.2, it can be seen that the computational cost of the traditional
interlaced filtering method is bad because of the sum of computational costs for the
optimal-REQUEST and UKF (the state variables have six dimensions). The UKF
algorithm is better, while the novel method is the best among them because of the
three dimensions of state variables, which makes the computational cost sharp down.
The comparison of simulation results with respect to accuracy is shown in
Fig. 7.17. Figure 7.17a shows a comparison of attitude estimation errors for the
three methods, and Fig. 7.17b shows a comparison of gyro drift estimation for the
three methods. It can be seen in Fig. 7.17 that for the presented method, the final
attitude estimation error reaches about 0.001◦ (four observation vectors and accurate
angle velocity information adopted) and the gyro drift estimation converges gradu-
ally at 1.02 (◦ )/h. For the attitude estimation, the presented method is better than the
UKF method (about 0.0012◦ ), which is better than the traditional interlaced filtering
method (about 0.002◦ ). For the final gyro drift estimation, the presented method is
better than the UKF method (about 1.04 (◦ )/h), which is better than the traditional
interlaced filtering method (about 1.17 (◦ )/h).
There is a noticeable phenomenon. It can be seen in Fig. 7.17b that the presented
novel method is better than the UKF method for the gyro drift estimation, especially
at the Y-axis and Z-axis. Why it is not good at the X-axis will be further researched in
the future. To achieve high performance attitude determination of strapdown stellar-
(A CMOS APS star sensor) inertial (three low-cost MEMS gyroscopes) integrated
systems, an interlaced filtering method is presented, in which the optimal-REQUEST
and UKF algorithms are combined to estimate the attitude quaternion and the gyro
drifts, respectively. The norm constraint for the quaternion is avoided by the optimal-
REQUEST and the disadvantage of the optimal-REQUEST, which cannot estimate
gyroscope drifts, is compensated by the UKF algorithm. The performance of the
presented method is studied and compared with the UKF algorithm and the tradi-
tional interlaced filtering method, which shows that this method has not only lower
computational cost, but also higher estimation precision for the attitude error and
7.7 Chapter Conclusion 275

Attitude Error Estimation Gyro Drift Estimation

UKF 10

X Axis（ °/h）
0.01
Traditional Segmented-f iltering UKF
Traditional Segmented-filtering
Roll(°)

The presented method

0 5
The presented method

-0.01 0
0 100 200 300 400 500
0 100 200 300 400 500
20

Y Axis（ °/h）
0.01 UKF
0 Traditional Segmented-filtering
Pitch(°)

10
-0.01 UKF The presented method
-0.02 Traditional Segmented-f iltering
The presented method
0
-0.03 0 100 200 300 400 500
0 100 200 300 400 500

Z Axis（ °/h）
20 UKF
0
Traditional Segmented-filtering
Yaw(°)

-0.02 UKF 10 The presented method

-0.04 Traditional Segmented-f iltering
The presented method 0
-0.06 0 100 200 300 400 500
0 100 200 300 400 500 Time（ t/s）
Time（ t/s） (t/s)
a (t/s) b
Fig. 7.17 Simulation results of the unscented Kalman filter (UKF) method, the traditional interlaced
filtering method, and the novel method. a Attitude error estimation. b Gyro drift estimation

the gyro drifts in the attitude determination system. The influence of the estimation
errors in the filters will be further researched in the future.

7.7 Chapter Conclusion

SINS/CNS integrated navigation system is a complete autonomous navigation system

with advantages such as high attitude precision and ability to effectively correct
inertial component error, so it has been widely used for high-precision navigation
of the new generation of flying machine including ballistic missile and satellite, etc.
This chapter has systematically discussed principle and methods of inertial/celestial
integrated navigation.
First, it gives a brief introduction to operating mode, combination mode, and
modeling method of inertial/celestial integrated navigation system, and discusses
principle of navigation system error correction based on celestial measurement infor-
mation. Specific to circumstance of large position error of ballistic missile launching
point (such as mobile launching and underwater launching), it introduces principle
of position error correction based on inertial/celestial integrated navigation system.
And specific to precision reduction of SINS/CNS integrated navigation caused by de-
viation of earth gravitational acceleration, it introduces a nonlinear filtering method
of ballistic missile SINS/CNS integrated navigation based on UKF.
Then, specific to autonomous navigation problem of lunar vehicle, it introduces
an INS/CNS integrated navigation method of lunar vehicle based on UPF, which
effectively improves autonomous navigation precision of lunar vehicle. Specific
to requirement of satellite attitude control for high-precision attitude determina-
tion of satellite, it introduces a SINS/CNS integrated attitude determination method
of piecewise information fusion based on EKF by combing satellite attitude de-
termination system model, minimum parameter attitude matrix estimation method
276 7 INS/CNS Integrated Navigation Method

and SINS/CNS integrated attitude determination method of self-adaptive piecewise

information fusion based on nonlinear filtering.
Though SINS/CNS integrated navigation has many advantages, it also has some
inherent shortcomings. SINS/CNS integrated navigation inhibits rapid increase of
velocity and position errors through correcting carrier attitude error, but it cannot cor-
rect velocity and position errors directly, neither can it estimate bias of accelerometer,
which will be fatal for carriers requiring for long-time operation. If GNSS is intro-
duced into SINS/CNS integrated navigation system to constitute SINS/CNS/GNSS
integrated navigation system, this problem will be solved effectively, so the next chap-
ter will introduce principle and methods of SINS/ CNS/GNSS integrated navigation
system in detail.

References

1. Fang JC, Ning XL, Tian YL (2006) The principle and method of the spaceraft autonomous
celestial navigation. National Defence Industry Press, Beijing
2. Fang JC, Ning XL (2006) Principle and application of celestial navigation. Beihang University
Press, Beijing
3. Wu HX,Yu WB, Fang JC (2005) Research on reduced dimension model of SINS/CNS integrated
navigation system. Aerosp Control 23(6):12–16
4. Ali J, Zhang CY, Fang JC (2006) An algorithm for astro/inertial navigation using CCD star
sensors. Aerosp Sci Technol 10(5):449–454
5. Wu HX, Yu WB, Fang JC (2006) Simulation of SINS/CNS integrated navigation system used
on high altitude and long-flight-time unpiloted aircraft. Acta Aeronautica et Astronautica Sinica
27:299–304
6. Li YH, Fang JC, Jia ZK (2002) Simulation of INS/CNS/GPS integrated navigation. J Chin
Inert Technol 10(6):6–11
7. Fang JC, Li XE, Shen GX (1999) Research on INS/CNS/GPS intelligent fault tolerant
navigation system. J Chin Inert Technol 7(1):5–8
8. Zhang GL, Li CL, Deng FL et al (2004) Research on ballistic missile INS/GNSS/CNS integrated
navigation system. Missiles Space Vehicles (2):11–15
9. Xu F, Fang JC (2007) Velocity and position error compensation using sins/star integration based
on evaluation of transition matrix. Aerosp Control 25(6):27–31
10. Ali J, Fang JC (2006) SINS/ANS integration for augmented performance navigation solution
using unscented Kalman filtering. Aerosp Sci Technol 10(3):233–238
11. Quan W, Fang JC, Xu F, Sheng W (2008) Study for hybrid simulation system of SINS/CNS
integrated navigation. IEEE Aerosp Electron Syst Mag 23(2):17–24
12. Guo EZ, Fang JC, Yu WB (2005) Compensation for errors of ballistic missile caused by gravity
abnormality. J Chin Inert Technol 13(3):30–33
13. Ali J, Fang JC (2005) In-Flight alignment of inertial navigation system by celestial observation
technique. Trans Nanjing Univ Aeronaut Astronaut 22(2):132–138
14. Qin YY, Zhang HR, Wang SH (2004) Theory of Kalman filter and integrated navigation.
Northwest Industrial University Press, Xi’an
15. Liu BQ, Fang JC, Guo L (2007)A WNN based Kalman filtering for auto-correction of SINS/Star
sensor integrated navigation system. Dyn Continuous Discrete Impulse Syst 14(S1):559–564
16. Xi XN, Zeng GQ, Ren X et al (2001) Design of lunar probe orbit. National Defence Industry
Press, Beijing
17. Ning XL, Fang JC (2006) Position and pose estimation by celestial observation for lunar rovers.
J Beijing Univ Aeronaut Astronaut 32(7):756–760
References 277

18. Yi GQ (1987) Inertial navigation principle. Aviation Industry Press, Beijing

19. Yuan X (1995) Navigation system. Aviation Industry Press, Beijing
20. Savage PG (1998) Strapdown inertial navigation integration algorithm design part 1: attitude
algorithms. J Guidance Control Dyn 21(1):19–28
21. Savage PG (1998) Strapdown inertial navigation integration algorithm design part 2: velocity
and position algorithms. J Guidance Control Dyn 21(2):208–221
22. Chen Z (1986) Strapdown inertial navigation system. China Aerospace Press, Beijing
23. Lefferts EJ, Markley FL, Shuster MD (1982) Kalman filtering for spacecraft attitude estimation.
AIAA J Guidance Control Dyn 5(5):37–49
24. Brady T, Buckley S, Tillier C (2004) Ground validation of the inertial stellar compass IEEE
aerospace conference proceedings. IEEE Aerosp Conf Proc 1:214–225
25. Lin YR, Deng ZL (2001) Mode-error-based predictive filter for satellite attitude determination.
J Astronaut 22(1):79–88
26. Yu YB, Fang JC (2005) Two-segment information fusion attitude determination method for
spacecraft from vector observations. J Beijing Univ Aeronaut Astronaut 31(11):1254–1258
27. Choukroun D, Bar-Itzhack IY, Oshman Y (2004) Optimal-REQUEST algorithm for attitude
determination. J Guidance Control Dyn 27(3):418–425
28. YuYB, Fang JC (2006) Study of spacecraft minimal-parameter attitude matrix estimation based
on the unscented Kalman filter. J Astronaut 27(1):12–15
29. Oshman Y, Markley FL (1998) Minimal-parameter attitude matrix estimation from vector
observation. J Guidance Control Dyn 21(4):595–602
30. Oshman Y, Markley FL (1999) Sequential attitude and attitude-rate estimation using integrated
parameters. J Guidance Control Dyn 21(3):385–394
31. Bierman GJ, Belzer MR (1985) A decentralized square root information filter/smoother.
Proceedings of 24th IEEE Conference on Decision and Control, Ft. Lauderdate, FL, Dec
32. Kerr TH (1987) Decentralized filtering and redundancy management for multisensor naviga-
tion. IEEE Trans Aerosp Electr Syst 23(1):83–119
33. Wang C, Fang JC (2005) Unscented quatemion particle filter application in micro-satellite
estimating attitude. J Beijing Univ Aeronaut Astronaut 33(5):552–556
34. Yu YB, Fang JC (2005) A two-segment information fusion attitude determination method for
micro-spacecraft from vector observations. J Beijing Univ Aeronaut Astronaut 31(11):1254–
1258
35. Quan W, Fang JC, Guo L (2008) An adaptive segmented information fusion method for
the attitude determination of nano-spacecrafts. In: Fang JC, Wang ZY (eds) Seventh inter-
national symposium on instrumentation and control technology: optoelectronic technology
and instruments, control theory and automation, and space exploration. SPIE, Bellingham, p
71292G-1–71292G-10
36. Quan W, Liu BQ, Gong XL et al (2011) INS/CNS/GNSS integrated navigation technology.
National Defense Industry Press, Beijing
37. Quan W, Xu L, Zhang HJ et al (2013) Interlaced optimal-REQUEST and unscented Kalman
filtering for attitude determination. Chin J Aeronaut 26(2):449–455
Chapter 8
INS/CNS/GNSS Integrated Navigation Method

8.1 Introduction

It is known from the previous chapters that strapdown inertial navigation system
(SINS)/global navigation satellite system (GNSS) integrated navigation corrects the
velocity and position errors of SINS online by using high-precision velocity and po-
sition information measured by GNSS to realize the longtime and high-precision
positioning and navigation, but it is hard to estimate the attitude error rapidly
and accurately, and the GNSS signal is susceptible to external interference. The
SINS/celestial navigation system (CNS) integrated navigation corrects attitude error
and gyro drift online by using high-precision attitude information measured by CNS
and can effectively correct position error of the launching point, which is of great
significance for the mobile launching of missile, but it is unable to completely inhibit
divergence of velocity and position errors of SINS, and CNS is restricted by climate
conditions. Therefore, the combination of SINS, CNS, and GNSS that effectively
constitute the SINS/CNS/GNSS integrated navigation system can realize the correc-
tion of velocity error, position error, and attitude error of SINS simultaneously. At
present, the SINS/CNS/GNSS integrated navigation system has become the most
effective way for high-performance navigation of medium and long-range ballistic
missile and high-altitude long-endurance flying machine, etc.
There is no doubt that the SINS/CNS/GNSS integrated navigation system also
has shortcomings, i. e., the complexity of integrated navigation system, sensitivity to
environmental disturbance, and difficulty of information fusion are further intensi-
fied, which result in instable error model of integrated navigation system, reduction
of navigation precision, and decline of reliability, etc., so that the new challenges are
posed on the SINS/CNS/GNSS integrated navigation method.
For this purpose, this chapter takes the SINS/CNS/GNSS integrated navigation
system as the research object; first, it gives a brief introduction to principle of the
SINS/CNS/GNSS integrated navigation; then, specific to the above difficulties, it in-
troduces the advanced filtering methods such as unscented Kalman filter (UKF) and
genetic algorithm into integrated navigation to focus on the SINS/CNS/GNSS inte-
grated navigation method based on federated UKF and federated filtering optimized
information distribution factor.
© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 279
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_8
280 8 INS/CNS/GNSS Integrated Navigation Method

8.2 Principle of INS/CNS/GNSS Integrated Navigation

8.2.1 Basic Principle of INS/CNS/GNSS Integrated Navigation

The basic principle of SINS/CNS/GNSS integrated navigation is to estimate position

error, velocity error, and attitude error of SINS accurately by using high-precision
attitude information provided by the CNS and high-precision position and velocity
information provided by GNSS and adopting optimal estimator, and correct inertial
component error of SINS to finally realize continuous high-precision navigation of
the carrier.

8.2.2 Combination Mode of INS/CNS/GNSS Integrated

Navigation

The SINS/CNS/GNSS integrated navigation system is a multiple navigation sensor

information fusion system combining three navigation subsystems namely SINS,
CNS, and GNSS. From the perspective of combination mode, the information fusion
system can be classified into the centralized filtering mode and the federated filtering
mode [1].
1. Centralized Filtering Mode of SINS/CNS/GNSS Integrated Navigation
System
The centralized filtering is to use an information fusion filter for centralized re-
ception and processing of information from each navigation sensor, and feed the
result of fusion processing back to the main navigation system. The SINS among the
SINS/CNS/GNSS integrated navigation system is set as the main navigation system
of centralized filter. The state equation of centralized filter consists of error equation
of the SINS and error equation of inertial components, the GNSS provides position
and velocity measurement quantities of the carrier, and the CNS provides attitude
measurement quantities of the carrier. Typical structure for centralized filter of the
SINS/CNS/GNSS integrated navigation system is as shown in Fig. 8.1. The system
measurement quantity consists of two parts: attitude difference between the SINS
and CNS; position difference and velocity difference between the SINS and GNSS.
The centralized filtering mode of the SINS/CNS/GNSS integrated navigation
system may give optimal estimation of error state theoretically [2], but there
are shortcomings such as high state dimension, heavy computation burden, poor
fault-tolerance performance, and adverse to fault diagnosis, etc. [3].
8.2 Principle of INS/CNS/GNSS Integrated Navigation 281

Fig. 8.1 Centralized filtering

structure of strapdown inertial  
6,16
navigation system
Ï6,16 96,1636,16
(SINS)/celestial navigation
system (CNS)/global 
navigation satellite system
&16 Ï&16
(GNSS) integrated navigation ×
system 
central
;3
ized
 filter
*166 9*1663*166
×


2. Federated Filtering Mode of SINS/CNS/GNSS Integrated Navigation System

As a kind of decentralized filtering, federated filtering is a further improvement of
decentralized filtering technology proposed by Carlson based on the researches of
Speyer and Kerr [4]. Each local filter of the federated filter uses observation value of
corresponding subsystem to obtain optimal estimation of local state, and then inputs
local estimation into the main filter for information fusion to obtain global estimation.
The federated filtering adopts the structure of parallel processing by multiprocessor,
and it is easy to realize multilevel fault detection and diagnosis of the system due to
its flexible design, relative less calculation, good fault tolerance, and high reliability.
Therefore, it is deemed to be a new filtering structure and algorithm independent of
decentralized filtering [1].
1) Information distribution of federated filtering
The federated filter has two-level filtering structure as shown in Fig. 8.2. Specific to
the SINS/CNS/GNSS integrated navigation system, since there are only two aided
navigation subsystems namely CNS and GNSS, only subsystem 1 and subsystem 2
are drawn up in the figure. Common reference system in the figure is SINS subsystem,
output Xk of which is provided for the main filter on the one hand, and for each
subfilter (local filter) on the other hand. Output of each subsystem is only passed on
to the corresponding sub-filter. Local estimation value X̂i and its covariance matrix
Pi of each sub-filter are sent to the main filter, and then it is fused with the estimation
value of the main filter to obtain global optimal estimation. If there are N local
state estimations X̂1 , X̂2 , · · ·, X̂N and corresponding estimation covariance matrixes
P11 , P22 , · · ·, PN N , and each local estimation is irrelevant, i. e., Pij = 0(i  = j ),
global optimal estimation may be expressed as:
N
X̂g = Pg Pii−1 X̂i , (8.1)
i=1
282 8 INS/CNS/GNSS Integrated Navigation Method

Ref. Main
System

Time
Sub-
Sub-
Filter1
System1

Sub-
Sub-
Optimization
Filter2
System2

Fig. 8.2 General structure of federated filter

N −1
!
wherein, Pg = Pii−1 .
i=1
Multiple federated filtering structures appear based on whether the estimated
mean square error matrix and state quantity of the sub-filter are reset and different
distribution modes of information factor. Among them, a common method is the
estimation method of feeding back to the sub-filter (denoted by dotted line in Fig. 8.2)
after magnifying the global estimation value Xg and the corresponding covariance
matrix Pg composed by the sub-filter and main filter to reset the sub-filter, i. e.:
X̂i = X̂g , Pii = βi−1 Pg .
Similarly, the covariance matrix of forecast error of the main filter may also be reset as
βm−1 times of global covariance matrix, i. e., βm−1 Pg (βm ≤ 1). βi (i = 1,2, · · ·, N , m) is
called as “information distribution factor,” and determined according to the principle
of “information distribution” [4].
The federated filtering involves two types of information in the SINS/CNS/GNSS
integrated navigation system, i. e., information of state equation and information of
measurement equation [5]. Accuracy of state equation is inversely proportional to the
process noise variance (or covariance matrix). The weaker the process noise is, the
more accurate the state equation is. Therefore, the accuracy of state equation may be
denoted by the inverse of process noise covariance matrix, i. e., Q−1 . In addition, the
accuracy of initial state value section may be denoted by the inverse of the covariance
matrix of initial value estimation, i. e., P −1 (0). The accuracy of the measurement
equation may be denoted by the inverse of measurement noise covariance matrix, i. e.,
R −1 . When the state equation, measurement equation, and P (0), Q and R are selected,
the state estimation X̂ and estimation error P are completely specified, and accuracy
of the state estimation may be denoted by P −1 . Therefore, as previously mentioned,
the distribution relation is generally decided according to certain measurement of
estimated error covariance matrix P of each sub-filter during information factor
distribution determination process.
8.2 Principle of INS/CNS/GNSS Integrated Navigation 283

2) Federated filtering algorithm based on different local models

At present, federated filtering algorithm based on information distribution principle
has become the most common information fusion method of integrated navigation
field due to its flexible design and good fault-tolerance performance, etc. However,
it requires the local filter to have the same state variable, and the state variable
of each local filter must be synthesized when the state variables of local model
are inconsistent, i. e., the state variable of the main filter shall be combination of
the state variable of each local filter; two outstanding problems are bound to be
caused thereby: one is the increased amount of computation, and the other is decline
of algorithm universality. In addition, the traditional information fusion method is
inapplicable since the state variables of each sub-filter are inconsistent. Specific to
federated filtering with different local models, different information fusion methods
shall be adopted. The following is an introduction to the federated-filtering algorithm
based on different local models [6–7]. The specific method is as follows.
1. Relation between local fusion result and global fusion result under different local
models
Local State Space Defining:
 T
Suppose the state estimation X̂i = Xi(1) Xi(2) . . . . Xi(mi ) (i = 1,2,. . ., N)
(j )
of local filter i to be optimal, and its component Xi (j = 1,2,. . ., mi ) is uncorrelated
random variable with limited variance; call mi -dimensional linear space spanned by
(j )
component Xi of state estimation X̂i (i = 1,2,. . ., N) of local filter i as the local state
space, which is denoted by
i . Define the norm in local state space:
3 3   1/2
3 3
3X̂i 3 = traceE X̂i X̂iT .

So
i is Bannch space.
Global State Space Defining:
 T
Suppose the global state estimation X̂ = X (1) X (2) . . . X (m) to be optimal,
(j )
and its component Xi (j = 1,2,. . ., mi ) is uncorrelated random variable with limited
(j )
variance. Call m-dimensional linear space spanned by component Xi of global state
estimation X̂ as global state space, which is denoted by
. Define the norm in global
state space:
3 3
3 3
3X̂ 3 = (traceE(X̂X̂ T ))1/2 . (8.2)

So
is Bannch space.
Relation between local state space and global state space is as follows based on
the two definitions above:
1. Local state space
i (i = 1,2,. . ., N) is subspace of global state space
;
1 N
2
max (dim
i ) ≤ m ≤ dim
i .
i
i=1
284 8 INS/CNS/GNSS Integrated Navigation Method

The global state estimation X̂ is the fusion result of local state estimation X̂i , and
from the perspective of information, information included by X̂ shall be not less than
X̂i ; from the perspective of space, base vector dimension of
i is not greater than
that of
, so there is max i (dim
i ) ≤ m. Meanwhile, since the dimension of space
! N  ! N 
sum of local state space
i is dim
i , there is m ≤ dim
i .
i=1 i=1

2. There is a mapping relation between global state space

and local state space

i ;

Ti :
→
i i = 1, . . ., N .

Since there is necessary linear mapping relation between state space and its subspace,
there is also necessary mapping relation between global state space
and its subspace

i .
2. Global fusion algorithm under different local models
In accordance with the definitions of local-state space and global-state space given
above and the relation between them, the global fusion problem of federated filtering
under different local models has been converted into optimal fusion problem of
different dimension state estimation. Therefore, global fusion may be denoted by:
1 N
2−1

P = Ti Pi−1 Ti . (8.3)
i=1
1 N
2

X̂g = P Ti Pi−1 X̂i . (8.4)
i=1

3. Information distribution problem under different local models

Information distribution theory is the basis of federated filtering algorithm; during
traditional federated filtering, the state variables of each sub-filter are consistent, so
that the information distribution process is required only to consider information con-
servation problem. Under different local models, conversion of global-state space
into local-state space shall be conducted first before distributing according to in-
formation conservation principle. Information distribution formulas under different
local models are:

Pi−1 = βi Ti P −1 . (8.5)

X̂i = Ti X̂ i = 1, . . ., N. (8.6)
8.2 Principle of INS/CNS/GNSS Integrated Navigation 285

4. Comparison of federated filtering algorithm based on different local models and

traditional federated filtering algorithm
Federated filtering algorithm of Carlson requires the synthesis of state variables of
each local filter to substitute state variable of the main filter when the state variables of
local models are inconsistent, but the federated filtering algorithm based on different
local models has no such requirement.
When Ti is the unit matrix, i. e., state variables of each subfilter in the federated
filter are the same, and the local-state space coincides with the global-state space, fed-
erated filtering based on different local models will degrade into traditional federated
filtering. Equations (8.3–8.6) are respectively converted into:
1 N
2−1
P = Pi−1 . (8.7)
i=1

N
X̂g = P Pi−1 X̂i . (8.8)
i=1

Pi−1 = βi P −1 . (8.9)

X̂i = X̂(i = 1,2, . . ., N ). (8.10)

Obviously, the equations above are the global fusion and information distribution
formulas of federated filtering algorithm proposed by Carlson, which indicates that
the federated filtering algorithm based on different local models is consistent with
traditional federated filtering algorithm when the state variables of each sub-filter of
the federated filter are the same.
The state variable of sub-filter is not required for expansion when adopting fed-
erated filtering algorithm based on different local models, and information provided
by the local-state space is less during the global fusion compared with the traditional
federated filtering algorithm, so that the precision of global estimation is somewhat
declined. However, in terms of the amount of computation, since the amount of
computation of Kalman filter is in direct proportion to the state dimension, the re-
duction of state dimension will make the amount of computation decreased greatly.
Therefore, such method is of significant application value on the occasions required
for high real time performance [7].

8.2.3 Modeling of INS/CNS/GNSS Integrated Navigation System

The previous subsection introduced the centralized filtering mode and federated
filtering mode of the SINS/CNS/GNSS integrated navigation system, and the fol-
lowing is an introduction to the modeling method of the SINS/CNS/GNSS integrated
navigation system based on the two filtering modes. The state equation of the
286 8 INS/CNS/GNSS Integrated Navigation Method

SINS/CNS/GNSS integrated navigation system consists of error equation of SINS

and error equation of inertial component here, and the CNS and GNSS, respec-
tively, provide the attitude measurement quantity and position, and the velocity
measurement quantity [8].
Refer to the relevant contents of Sect. 6.3 for the establishment of state equation
of integrated navigation system, the details of which will not be given here.
1. Centralized Filtering Measurement Equation of SINS/CNS/GNSS Integrated
Navigation System
With the navigation coordinate system as the geographic coordinate system, take
15-dimensional
 state variable 
X(t) = φE φN φU δvE δvN δvU δL δλ δh εx εy εz ∇x ∇y ∇z T.
measurement equation of SINS/CNS/GNSS integrated navigation system based on
centralized filtering is:

Z(t) = H (t)X(t) + V (t), (8.11)

wherein,
⎡ ⎤
I3×3 03×3 03×3 03×3 03×3
⎢ ⎥
H (t) = ⎢
⎣ 03×3 I3×3 03×3 03×3 03×3 ⎥
⎦, (8.12)
03×3 03×3 Hp 03×3 03×3

wherein Hp = diag(Rm Rn cos L 1).

 
Measurement quantity Z(t) = φE φN φU δvE δvN δvU δL δλ δh T.
2. Federated Filtering Measurement Equation of SINS/CNS/GNSS Integrated
Navigation System
The federated filter of SINS/CNS/GNSS integrated navigation system mainly in-
cludes sub-filters SINS/CNS and SINS/GNSS; the navigation coordinate system is
still taken as the geographic coordinate system for instance.
The SINS/CNS subsystem adopts mathematics platform misalignment angle ob-
tained subject to conversion as measurement quantity of Kalman filter, and the
following measurement equation is obtained:

Z1 (t) = H1 (t)X(t) + V1 (t), (8.13)

 T  T
wherein Z1 (t) = φE φN φU is measurement quantity; H1 (t) = I3×3 03×12
is measurement matrix.
8.3 INS/CNS/GNSS Integrated Navigation Method Based on Federated UKF 287

The SINS/GNSS subsystem adopts position and velocity differences between

SINS and GNSS as the measurement information of Kalman filter, and the following
measurement equation is obtained:
⎡ ⎤ ⎡ ⎤
Hv V (t)
Z2 (t) = H2 (t)X(t) + V2 (t) = ⎣ ⎦ X(t) + ⎣ v ⎦, (8.14)
Hp Vp (t)

wherein Hv = [ 03×3 diag(1 1 1) 03×9 ],

   
Hp = 03×6 diag RM RN cos L 1 03×9 .

8.3 INS/CNS/GNSS Integrated Navigation Method Based on

Federated UKF

The previous section is an introduction to basic principle, combination mode and

modeling method of the SINS/CNS/GNSS integrated navigation, and the following
will aim at improving the precision of the SINS/CNS/GNSS integrated navigation to
introduce a SINS/CNS/GNSS integrated navigation method based on the federated
UKF.
It is known from Sect. 8.2.2 that the federated filtering is a two-level decentralized
filtering method consisting of two data processing stages. Figure 8.3 is federated
filtering structure of the SINS/CNS/GNSS integrated navigation system based on
UKF filtering [9]. The data of SINS/CNS sub-filter one and the SINS/GNSS sub-
filter two is the output to the main filter for fusion. Each sub-filter is an independent
data processing subsystem. Among them, SINS is the main navigation system. Data
of SINS, CNS, and GNSS is an input to the corresponding sub-filter, and the filtering
result is the output to the main filter for information fusion to realize global state
estimation finally.
It is known from Sect. 3.4 of Chap. 3 of this book that UKF may provide a
state estimation recursive solution according to the state estimation of the previous
moment and new observation. The local filter is generally based on the following
mathematical model in the federated filter design based on UKF:

xj ,k = f (xj ,k−1 ) + Gj ,k−1 wj ,k−1 , j = 1,2. (8.15)

zj ,k = hj ,k (xj ,k ) + vj ,k , (8.16)

wherein j represents the corresponding local UKF filter. Since the estimated state
variables of all local UKF filters are the same, the identical mathematical model can
be adopted. The main UKF filter adopts the information sharing strategy for local
UKF filtering to realize global optimal estimation.
288 8 INS/CNS/GNSS Integrated Navigation Method

Main UKF
filter
CNS Local UKF
filter1 Time updating

Modefication
SINS

Local UKF Measurement

GNSS filter2 updating

Fig. 8.3 Federated filtering structure of signal inertial navigation system (SINS)/celestial navigation
system (CNS)/global navigation satellite system (GNSS) integrated navigation system based on
unscented Kalman filter (UKF) filtering

For the local UKF filter, its time and measurement updating equations are as
follows:
1 2n 2
s

x̂j−,k = Wis χi,k|k−1 . (8.17)

i=0 j
1 2n 2
s
  T
Pj−,k = Wi χi,k|k−1 −
c
x̂k− χi,k|k−1 − x̂k− +Q . (8.18)
i=0 j
1 2n 2
s

ẑj ,k = Wim Zi,k|k−1 . (8.19)

i=0 j
1 2n 2
s
  T
Pj ,ẑk ẑk = Wic Zi,k|k−1 − ẑk Zi,k|k−1 − ẑk +R . (8.20)
i=0 j
1 2n 2
s
  T
Pj−,x̂ − ẑ = Wic χi,k|k−1 − x̂k− Zi,k|k−1 − ẑk . (8.21)
k k
i=0 j
 
Kj ,k = Px̂ − ẑ Pẑ−1
ẑ
k k j
. (8.22)
k k

x̂j ,k = (x̂k− + Kk (zk − ẑk ))j . (8.23)

 
P̂j ,k = Pk− − Kk Pẑk ẑk KkT j , (8.24)

wherein j = 1,2; x̂k− ∈ Rnj is the forecast estimation of xk ; Q ∈ Rnj ×nj is the
covariance matrix of system noise; x̂k ∈ Rnj is the optimal estimation of xk ; Pk − ∈
8.3 INS/CNS/GNSS Integrated Navigation Method Based on Federated UKF 289

Rnj ×nj is the covariance matrix of forecast estimation error; P̂k ∈ Rnj ×nj is the final
covariance matrix of estimation error.
For the main UKF filter:
1 2n 2
s
−
x̂m,k = Wis χi,k|k−1 . (8.25)
i=0 m
⎧
2n 
⎪
⎪ − !s c  −
 
− T
⎪
⎪ Pm,k = Wi χi,k|k−1 − x̂k χi,k|k−1 − x̂k +Q
⎪
⎪
⎪
⎪
i=0 m
⎨ !N
Pf−1 −1
,k = Pm,k + Pj−1 , (8.26)
⎪
⎪ j =1
,k
⎪
⎪
⎪
⎪ !N
⎪
⎪ Pf−1 = −1
+ Pj−1
⎩ ,k x̂f ,k P m,k x̂m,k ,k x̂j ,k
j =1

−1 −1 −1
wherein N = 2, Pm,k = Pm,k|k−1 , Pm,k|k−1 = βm,k Pf−1
,k|k−1 , x̂m,k = x̂m,k|k−1 ,
−1 nf ×nf
x̂m,k|k−1 = x̂f ,k|k−1 , Pf ,k ∈ R is inverse of the fusion covariance matrix,
x̂f ,k ∈ Rnf is the state estimation of fusion, β is the information distribution factor,
subscript m represents the main filter, and subscript f represents the federated filter
output.
Combine the following equation to feed information back to each local UKF filter.
⎧
⎨ P = β −1 P
j ,k f ,k
j
, j = 1,2, m, (8.27)
⎩ x̂j ,k = x̂f ,k

!
2
wherein β must satisfy the following information fusion principle βm + βj = 1.
j =1
It is worth noted that in the SINS/CNS/GNSS integrated navigation, the state vectors
of local UKF filter and the main filter are identical. After the step of combination
and before the next cycle, the global estimation value shall be fed back to each local
UKF filter through the information distribution factor.
Conduct simulation experiment by using the method above. With the inertial
coordinate system of the launching point as navigation coordinate system, CNS and
GNSS begin to operate for 120 s after the missile flying off the atmospheric layer
(about 40 s). Initialized state variable x̂(0) is 0, P (0), Q, R1 , and R2 are initialized
as follows:
Pf (0) = diag[Pφi , Pvi , Pri , Pεi , P∇i ], i = x, y, z.

wherein Pφi = (10−4 )2 , Pvi = (0.01)2 , Pri = (5)2 , Pεi = (0.1◦ / h)2 , P∇i =
(100μg)2 .,
Q = diag[(0.1◦ / h)2 , (0.1◦ / h)2 , (0.1◦ / h)2 , (100μg)2 , (100μg)2 , (100μg)2 ] ,

 2  2  2 
R1 = diag 10 , 10 , 10 ,
290 8 INS/CNS/GNSS Integrated Navigation Method

Table 8.1 Comparison of integrated navigation error results based on federated UKF and federated
EKF
Estimated parameter Estimated error based Estimated error based
on federated UKF on federated EKF
δϕ[ ] Standard deviation 3.0928 4.3218

δθ [ ] 2.9372 3.0733
δγ [ ] 2.7851 3.2869
δvx [m/s] 0.0415 0.1432
δvy [m/s] 0.0455 0.0784
δvz [m/s] 0.0986 0.1239
δx[m] 0.6754 0.7793
δy[m] 1.0784 1.2981
δz[m] 3.1181 4.3272
εx [◦ /h] Mean value 0.0932 0.1232
◦
εy [ /h] 0.1047 0.0847
◦
εz [ /h] 0.1140 0.0740
∇x [μg] 100.0486 102.0883
∇y [μg] 99.9754 98.8659
∇z [μg] 102.3108 103.2160
UKF unscented Kalman filter, EKF extended Kalman filter

R2 = diag[(0.2m/s)2 , (0.2m/s)2 , (0.2m/s)2 , (10m)2 , (10m)2 , (10m)2 ]. Initialized

information distribution factor: β1 (0) = β2 (0) = 0.4, βm (0) = 0.2.
The sub-filter and main filter are initialized into:

Qj (0) = βj−1 (0)Q, Pj (k) = βj−1 (0)Pf (0), x̂j (0) = x̂f (0); j = 1,2, m.

After the filtering is stabilized (about 50 s later), compare the SINS/CNS/GNSS

integrated navigation error based on the federated UKF and the navigation error
based on federated extended Kalman filter (EKF) is as shown in the Table 8.1 below.
It is observed from the table above that the SINS/CNS/GNSS integrated navigation
based on federated UKF has higher estimation precision in terms of velocity error,
position error, attitude error, gyro drift, and null bias of accelerometer compared
with the SINS/CNS/GNSS integrated navigation based on the federated EKF.
8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation . . . 291

8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation

Method Based on the Optimized Information Distribution
Factor

The previous section introduced the SINS/CNS/GNSS integrated navigation method

based on federated UKF. In fact, specific to the distribution of information factor in
federated filtering, there is no conclusion yet at present, and it remains the hotspot
of research at home and abroad. The following is an introduction to two federated
filtering SINS/CNS/GNSS integrated navigation methods based on the optimized
information distribution factor [10–11]. The methods may strengthen fault-tolerant
capability of the system more effectively and improve filtering precision.
Suppose the system state and measurement equation of Kalman filter for a decen-
tralized linear stochastic system consisting of multiple navigation sensors are to be
as follows:

xk = k|k−1 xk−1 + Gk−1 wk−1 . (8.28)

zi,k = Hi,k xk + vi,k , i = 1,2, · · ·, l, (8.29)

wherein i represents the measurement information of each subfilter, l represents the

quantity of subfilter, and the system noise and measurement noise are both assumed
as zero-mean Gaussian white noise here.

8.4.1 Federated Filtering Equation and Information Distribution

Process

The time updating equations of sub-filter and main filter are as follows:

x̂i,k|k−1 = i,k|k−1 x̂i,k−1 , i = 1,2, · · ·, l, m

−1
(8.30)
Pi,k|k−1 = [k|k−1 Pi,k−1 Tk|k−1 + Qi,k ]−1 , i = 1,2, · · ·, l, m.

Measurement updating equations of sub-filter are as follows:

−1 −1 −1
Pi,k = Pi,k|k−1 + HiT Ri,k Hi , i = 1,2, · · ·, l
−1 −1 −1
(8.31)
Pi,k x̂i,k = Pi,k|k−1 x̂i,k|k−1 + HiT Ri,k zi,k , i = 1,2, · · ·, l.

wherein x̂i,k|k−1 ∈ Rni is a priori estimation of xk , Qi ∈ Rni ×ni is the variance

matrix of system noise, x̂i,k ∈ Rni is a posteriori estimation of xk , Pi,k|k−1 ∈ Rni ×ni
−1
is a priori error estimation variance matrix, and Pi,k ∈ Rni ×ni a posteriori error
estimation variance inverse matrix, i. e., the information matrix.
292 8 INS/CNS/GNSS Integrated Navigation Method

The main filter fusion formula is the same as Eq. (8.26), rewritten hereby as
follows:
l
Pf−1 −1
,k = Pm,k +
−1
Pi,k , (8.32)
i=1

l
Pf−1 −1
,k x̂f ,k = Pm,k x̂m,k +
−1
Pi,k x̂i,k , (8.33)
i=1

wherein x̂m,k = x̂m,k|k−1 , x̂m,k|k−1 = x̂f ,k|k−1 ; Pf−1

,k ∈ R
nf ×nf
is the inverse of fusion
covariance matrix, and also fault-tolerant variance matrix; x̂f ,k ∈ Rnf is the fusion
state estimation, also called as fault-tolerant state estimation.
In fact, the state estimation of each sub-filter is correlated, so it is allowed to
adopt the method of upper bound of variance, i. e., setting the process noise and
state-variable variance as their upper bound of variance for decoupling simplified
processing:
−1
Qi,k = βi,k Q, i = 1,2, · · ·, l, m, (8.34)
−1
Pi,k = βi,k Pf ,k , i = 1,2, · · ·, l, m, (8.35)

wherein, βi (≥ 0) is the information distribution factor satisfying the following

condition:

β1 + β2 + · · · + βl + βm = 1. (8.36)

Reset the variance matrixes of sub-filter and main filter upon realization of infor-
mation fusion, and reset the states of the main filter and sub-filter through global
estimation:

x̂i,k = x̂f ,k , i = 1,2, · · ·, l, m. (8.37)

Estimated error variance conservation principle is often used during the filter infor-
mation distribution. Information of estimated error variance can be used to estimate
system error. Ordinary filters fuse the local estimation into a global optimal estima-
tion and feed state back to each subfilter before next cycle begins to guarantee the
amount of information that is consistent before and after feedback.
To maintain optimal filtering, the parameter control of information distribution
must be associated with the parameter control of process noise, i. e., the noise
information must be selected effectively. If there is a process noise, the main filter
must regroup the estimation value of the sub-filter since Kalman filter can only
save the process-noise information of the current cycle. The whole process noise
information is denoted as matrix Qand the real process information can be obtained
by accumulating process information of each sub-filter.
8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation . . . 293

8.4.2 Federated Filtering INS/CNS/GNSS Integrated Navigation

Method Based On Information Distribution Factor
Optimization

In accordance with the Kalman filter, the following state estimation error covariance
matrix is obtained:

Pk = E[(x − x̂k )(x − x̂k )T ]. (8.38)

If a system is observable, all elements of the covariance matrix are definitive. State
estimation error can be calculated directly through covariance matrix Pk ;

k = E[(x − x̂k )T (x − x̂k )] = trace(E[(x − x̂k )(x − x̂k )]T ) = trace(Pk ). (8.39)

Performance of the Kalman filter is evaluated by the state estimation covariance ma-
trix, and the filtering state estimation error will indicate the performance of Kalman
filter. The performance index of Kalman filter may generally be evaluated by using
the following equation:

Ck = E[(x − x̂k )T W (x − x̂k )]. (8.40)

W is a half positive definite weight matrix and the optimal estimation is independent
of W, suppose W to be the unit matrix, and it is obtained that:

Ck = E[(x − x̂k )T (x − x̂k )] = trace(Pk ). (8.41)

The equation above is the sum of mean square error of all the state vector elements.
If there is no uncertain process in the system, the global minimum can be obtained by
using the Kalman filter performance index Ck . However, if the system noise matrix Q
and the measurement noise matrix Rare both uncertain, the minimum of Ck will not
be obtained. Suppose there is no uncertain Q and R here, so the minimized function
is equivalent to the maximum value of trace(Pk−1 ), i. e.:
⎧
⎨ C = trace(P ) → min
k k
. (8.42)
⎩ Ck = trace(P −1 ) → max
k

For regrouping the principle of sub-filter and main filter through information fusion,
it may be abbreviated as:
−1
Pi,k|k−1 = βi,k Pf ,k|k−1 , i = 1,2, · · ·, l, m.
−1
In such case, optimal information distribution factor of βi,k trace(Pf ,k|k−1 ) =
−1
)
trace(Pi,k|k−1 ) or βi,k = trace(Pi,k|k−1 ) trace(Pf ,k|k−1 ) also satisfies the information
conservation principle:
l
βm,k + βi,k = 1. (8.43)
i=1
294 8 INS/CNS/GNSS Integrated Navigation Method

Since the trace of state estimation error covariance matrix is used in federated filtering
of SINS/CNS/GNSS integrated navigation system to realize the effective distribu-
tion of information distribution factor, the fault tolerance of the SINS/CNS/GNSS
integrated navigation system can be enhanced. Such method of optimized infor-
mation distribution factor may realize accurate estimation of gyro drift and null
bias of accelerometer to improve the precision of the SINS/CNS/GNSS integrated
navigation.

8.4.3 Research on FKF Method Based on an Improved Genetic

Algorithm for INS/CNS/GNSS Integrated Navigation
System

The fusion of multi-sensors data is expected to achieve more accurate and more re-
liable navigation. The general federated Kalman filter (FKF) cannot be suited for
rather large changes of some complex nonlinear system parameters and not choose
the optimized information sharing coefficients to estimate the navigation prefer-
ences effectively. This study concerns research on the FKF method for a nonlinear
adaptive model based on an improved genetic algorithm of the SINS/CNS/GPS in-
tegrated multi-sensors navigation system. An improved fitness function avoids the
premature convergence problem of a general genetic algorithm. And the decimal
coding improves the performance of general genetic algorithm. The improved ge-
netic algorithm is used to build the adaptive FKF model and to select the optimized
information sharing coefficients of FKF. And the UKF is used to deal with the non-
linearity of integrated navigation system. Finally, a solution and realization of the
system is proposed and verified experimentally [12].
1. Design of FKF Based on Improved GA
The SINS/CNS/global positioning system (GPS) integrated multi-sensors navigation
system comprises one main system and two subsystems. The SINS acts as a funda-
mental sensor (main system) in the system, and its data is the measurement input
for the MF. The data from the GPS (subsystem 1) and the CNS (subsystem 2) is
dedicated to the corresponding LFs and, after implementation, supply their resulting
solutions to the MF for a time update and optimization fusion[13]. The frame of the
FKF based on the improved genetic algorithm (GA) is shown in Fig. 8.4.
The primary sensor subsystems used in the integrated multi-sensors navigation
system are the SINS, CNS, and GPS. The SINS used in the simulation generates
position, velocity, and attitude information, the CNS provides attitude information,
while the GPS outputs position and velocity solutions. The purpose of the integrated
system is to achieve the increased accuracy and improved estimates of the SINS
error sources. The CNS provides the attitude of the vehicle that is combined with the
SINS attitude information to output an observation to the LF1 (Unscented Kalman
8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation . . . 295

SINS

Time Update
LF 1 Xˆ 1 , P1
CNS Z1 UKF1
Xˆ g Xˆ g , Pg
Xˆ g , β1 Pg
−1
Xˆ m , Pm β m −1 Pg
Xˆ m , Pm
Improved GA

Xˆ g , β 2 Pg
−1

Optimization fuse
LF 2 Xˆ 2 , P2
GPS Z2 UKF2
MF

Fig. 8.4 Frame of the federated Kalman filter (FKF) based on the improved GA. SINS signal
inertial navigation system, CNS celestial navigation system, GPS global positioning system, UKF
unscented Kalman filter

Filter1:UKF1). The velocity and position information available from the GPS in
conjunction with the SINS yield observations to the LF2 (UKF2).
Suppose that the state and measurement equations of the main system are given
by:

Xk = f (Xk−1 ) + Gk−1 Wk−1 , (8.44)

Zk = Hk (Xk ) + Vk , (8.45)

where Wk−1 and Vk are the uncorrelated Gaussian white noise sequences with the
covariance as Qk , Rk .
The state and measurement equations of the sub-system are as follows:

Xi,k = f (Xi,k−1 ) + Gi,k−1 Wi,k−1 , (8.46)

Zi,k = Hi,k (Xi,k ) + Vi,k , (8.47)

where:
i represents the corresponding measurement sources for the LFs, i = 1, 2. . . N.
Wi,k−1 and Vi,k are the uncorrelated Gaussian white noise sequences with
covariance Qi,k , Ri,k .
Suppose that Zk = [Z1,kT T
, Z2,k , · · ·, ZNT ,k ]T and LFs are uncorrelated. Then,

N
1 N
2−1
Xg = Pg Pii−1 Xi , Pg = Pii−1 , (8.48)
i=1 i=1

where: Pg is the covariance matrix of the global FKF.

Pii is the covariance matrix of the ith LF.
296 8 INS/CNS/GNSS Integrated Navigation Method

The covariance matrix and its propagation in time are vital in both describing and
analysing physical test results and comparing them with theoretical predictions. This
number is a significant indication of the overall performance of the system and is often
employed as a “metric to minimize” when making decisions about how to consider
the navigation data. To solve state space system error equations, the covariance matrix
is propagated continuously so that the error estimates and covariance are available
at the discrete time when the measurements transpire [14].
The estimation of the LFs is correlated because they use a common dynamic
system. To eliminate this correlation, the process noise and the state vector covariance
are set to their upper bounds:
N
Q−1 = βi Q−1 −1
i +βm Q , i = 1,2, · · ·, N , m, (8.49)
i=1

N
P −1 = βi Pi−1 +βm P −1 , i = 1,2, · · ·, N , m, (8.50)
i=1

where βi ( ≥ 0) is an information sharing coefficient, and:

β1 + β 2 + · · · + βN + βm = 1. (8.51)

For the time update equations and the measurement update equations for the LFs see
the reference [15]. The information sharing coefficients are selected by the estimated
error covariance of the LFs. The information residing in the estimated error covari-
ance can be construed to be the memory of the filter system. To remain optimal,
the filter must combine the local estimates into a single estimate every cycle. After
the combination step, at the start of the next cycle, the estimate or memory can be
fed back to the LFs with the MF retaining a part or none of the information. At this
point, all estimates in the system are equal and the information is distributed. If the
feedback is implemented, the conservation of information simply dictates that the
net sum of the information in the filter system before and after the feedback operation
must remain the same. However, every feedback operation requires an adjustment
of the covariance to reflect information sharing.
In general, when the information is redistributed about the LFs and MFs, the sum
of the local and MF matrices after feedback must be equal to the MF information
before feedback. The simplest form of the parametric control is to choose parameters
βi and βm with a summation of one, and set the error covariance of the individual
filter and MFs after the feedback equals to βi and βm times the MF covariance before
feedback. In our real-coded GA, individual filters are directly composed of real type
original variables of the regression model (β1 β2 · · · βN ) and have N chromosomes.
The parameter N of the above content is set to 2. In this FKF based on the improved
GA, a quaternion is obtained from the corrected attitude matrix and is fed back for
the attitude error compensation. Similarly, the estimated velocity and position error
states are used for velocity and position error compensation. The effective method
8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation . . . 297

that improved the GA determines the optimized information sharing coefficients for
the FKF to accomplish good navigation results.
Considering the SINS/CNS/GPS-integrated multi-sensors navigation system, we
suppose that the state vectors for the LFs are the same as the MF:

x1 = x2 = xm = [φi , δvi , δri , εi , ∇i ]T , i = x, y, z. (8.52)

We select the information sharing coefficients using the improved GA. Using the
fitness function as follows:

x1 = x2 = xm = [φi , δvi , δri , εi , ∇i ]T , i = x, y, z. (8.53)

where:

β1 + β 2 + βm = 1.

βt is the calculated information-sharing coefficient of the t th generation.

W1 , W2 are the regulated powers dependent on the Q and P.
The algorithms then set the evaluated fitness of each individual filter for the current
generation in ascending order and the relative individual filter is correspondingly
ordered. Consequently the improved GA determines the best fitness. If the best
fitness satisfies the given conditions, the improved GA finds out the relative best
individual filter and exports it as the optimum solution. After that, the calculation is
ended and the present optimum solution becomes the final optimization result of the
improved GA.
2. Experimental Results
Considering the versatility, agility, and transportability of the algorithms, this FKF
based on the improved GA for the SINS/CNS/GPS-integrated multi-sensors naviga-
tion system should have the features of low cost and high efficiency, and also retain the
design and realization ideas of hardware-function modularization and software-flow
integration.
1) Hardware design
Considering the integrated design, the whole hardware system is divided into the de-
vices, component, and system layers. Each layer integrates downward, and supplies
testing and functional verification to the upper layer at the same time. The system
is connected by multifunction ports between the layers. In this way, changing the
elements in each layer, while ensuring the coherence of the interface protocol, will
not affect the other layers, sequentially realizing the versatility of the hybrid simu-
lation system. In addition, both the devices and components layers have pre-formed
interfaces, which enhance the expandability of the system.
298 8 INS/CNS/GNSS Integrated Navigation Method

The entire layers schematic for the hardware system of integrated navigation
Devices
Layer TFT LCLV CCD Star GPS Preformed
OFGA Star Simulator Sensor Receiver Interface

Components
Layer

SINM TGM CNM GPSM PPM PIM

System
Layer
Integrated Navigation and Performance
Testing System

Fig. 8.5 The entire layers of the hardware system. OFGA optical fibre gyro accelerometer, TFT
thin film transistor, CCD charge coupled device, GPS global positioning system, SINM strapdown
inertial navigation, TGM trajectory generator module, CNM celestial navigation module, GPSM
global positioning system module, PPM peripheral processing module, PIM preformed interface
module, LCLV liquid crystal light valve

The devices layer includes the elements of inertia measurement that consist of an
optical fiber gyro accelerometer(OFGA), thin film transistor (TFT) liquid crystal light
valve(LCLV) star atlas simulator [16; 17], charge coupled device (CCD) star sensor
device, GPS receiver and a preformed interface. The components layer consists of a
trajectory generator module (TGM), strapdown inertial navigation module (SINM),
celestial navigation module (CNM), GPS module (GPSM), peripheral processing
module (PPM) and a preformed interface module (PIM).The system layer consists
of an integrated multi-sensors navigation and performance testing system. The entire
layers of the hardware system are shown in Fig. 8.5.
2) Software design
Considering the versatility, flexibility, and portability of the hybrid simulation of
a SINS/CNS/GPS-integrated multi-sensors navigation system, the design of each
part possesses its own characteristics as well as adopting a universal exploitation
environment, which realizes the functional integrated design. In this way, the system
can be exploited expediently and carried out with a hybrid simulation experiment.
The flow diagram of the entire software system is shown as Fig. 8.6.
First, the hardware and software of the system are initialized, and performance-
testing parameters are set for the integrated multi-sensors navigation. Second, after
obtaining data from the SINS, if it is available, the “proceed” performance mode
will be decided; otherwise, the systems wait for the next SINS data. The next step
8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation . . . 299

Fig. 8.6 The flow diagram of

Initialization
the entire software system.
CNS celestial navigation
system, GPS global Set Parameter
positioning system, SIN
strapdown inertial navigation,
Receive Next
UKF unscented Kalman filter, N SINS Data
FKF federated Kalman filter, SINS Data ?
PTR performance testing Y
report, GA genetic algorithm
N
Integrated Mode ?

CNS Data Integrated GPS Data

Navigation

Improved SIN
GA Module Module

Based on UKF of
SIN Algorithm
Adaptive FKF

Feedback
Compensation

performance
testing

Y
Receive Data ?
N
Generate PTR and End
Program

determines whether it is in the “integrated” mode, which depends on whether CNS

and GPS data have been received. The adaptive FKF together with the improved
GA module and the strapdown inertial navigation (SIN) module are then performed
to achieve integrated multi-sensors navigation and feedback compensation for the
system. In the absence of CNS and GPS data, the fine strapdown algorithm is executed
to achieve SIN. Third, the real time performance testing of the navigation information
of system is carried out to determine whether the CNS and GPS data have been
received or not. If not, the system continues to receive the next set of SINS data.
If successful, a performance testing report (PTR) for the integrated multi-sensors
navigation system is generated, and the software flow ends.
300 8 INS/CNS/GNSS Integrated Navigation Method

3) Construction of SINS/CNS/GPS integrated multi-sensors navigation system

To construct the SINS/CNS/GPS-integrated multi-sensors navigation system in a
laboratory, the practical/actual devices and parts are used to build the experimental
integrated multi-sensors navigation system. Based on prior work [18], actual re-
search development and engineering applications of all the various subsystems, a
synthesized approach is carried out on the design of the hardware and software of
the system.
4) Construction of SINS/CNS/GPS integrated multi-sensors navigation system
Based on the error model and the SINS/CNS/GPS-integrated multi-sensors naviga-
tion system as shown in Fig. 4, some experiments were performed [19; 20]. At the
same time, the experiment of the FKF based on the improved GA was performed on
the integrated multi-sensors navigation system. The conditions for the experiment
were:
1. For the improved GA: S = 20, the chromosome number of an individual filter
N = 2, the genetic generation G = 200, the crossover probability Pc = 0.1, the
mutation probability Pm = 0.01, W1 = 0.3, W2 = 0.7.
2. Assuming a long flight-time unmanned plane as an example, the initial latitude
was set at 39.984◦ , the initial longitude at 116.344◦ . The initial error angle for the
head was set at 6 , for the pitch at 20", and for the roll at 20". The gyroscopic drift
was 0.01◦ /h(1σ) and the accelerometer zero offset was 100◦ μg (1σ). The attitude
precision of the CNS was 5" (1σ), the position and velocity precision of the GPS
were 5◦ m(1σ) and 0.1◦ m/s(1σ), and the filter period was 1◦ s.
We assumed the initial value x̂f (0) of state vector xf is zero. Pf (0) and Q are given
by:

Pf (0) = diag[Pφi , Pvi , Pri , Pεi , P∇i ], i = x, y, z, (8.54)

where:
 2
Pφi = 10−4 , Pvi = (0.01)2 , Pri = (5)2 , Pεi = (0.01◦ /h)2 , P∇i = (100μg)2 .
(8.55)
 
Q = diag (0.01◦ /h)2 , (0.01◦ /h)2 , (0.01◦ /h)2 , (100μg)2 , (100μg)2 , (100μg)2 .
(8.56)
      
R1 = diag 5 , 5 , 5 .
2 2 2
(8.57)

R2 = diag[(0.1m/s)2 , (0.1m/s)2 , (0.1m/s)2 , (5m)2 , (5m)2 , (5m)2 ]. (8.58)

8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation . . . 301

12 15 5

Federated KF Federated KF
10
the presented method the presented method
10 0
8
δФx/(″)

δФy/(″)

δФz/(″)
6
5 -5

0 -10
2

0
-5 -15
Federated KF
-2
the presented method
-4 -10 -20
0 200 400 600 0 200 400 600 0 200 400 600
time/s time/s time/s

Fig. 8.7 Attitude errors. KF Kalman filter

Initial values of the information sharing coefficients are assumed to be β1 (0) =

β2 (0) = 0.49 and βm (0) = 0.02. The LFs and the MF are initialized as:

Qi (0) = βi−1 (0)Q, Pi (k) = βi−1 (0)Pf (0), x̂i (0) = x̂f (0), i = 1,2, m.
(8.59)

The experimental results using the FKF and the proposed FKF based on the improved
GA method for the SINS/CNS/GPS-integrated system are depicted in Figs. 8.7, 8.8,
8.9.
Using the UKF and the improved GA for the selection of optimized information
sharing coefficients, the FKF based on the improved GA can estimate the attitude
errors, position errors, velocity errors, gyro drifts, and accelerometer biases better
than for the traditional FKF. Since gyro induced drift errors are the only error variables
that contribute to the attitude errors, the CNS updates are effective in estimating and
compensating for these drift errors, as well as the position and velocity errors that
occur due to misalignments. The velocity and position updates from the GPS can
estimate and compensate for velocity, position, and accelerometer biases.
From Figs. 8.7 and 8.8, it can be seen that the estimated attitude and position errors
are minimal using the proposed method. The estimated 3-axis attitude errors are less
than 2", which is better than the 5" using the traditional FKF. The estimated 3-axis
position errors are less than 1◦ m, which is better than the 4◦ m using the traditional
FKF. The estimated 3-axis velocity errors are better than the traditional FKF, which
are 0.04◦ m/s rather than 0.08◦ m/s.
From Fig. 8.9, it can be seen that the gyro errors and accelerometer biases are
estimated and compensated effectively using the proposed method. Compared with
the FKF, the fluctuating extent of the estimated value of gyro errors and the ac-
celerometer biases are smaller, and the convergence of the estimated value is more
rapid. The proposed method gives estimated gyro errors of up to 0.02◦ /h at about
140◦ s while for the FKF the y are only up to 0.025◦ /h at about 280◦ s. For estimating
302 8 INS/CNS/GNSS Integrated Navigation Method

6 10
Federated KF Federated KF
4 the presented method the presented method
©[P

©\P
5
2
0
0

-2 -5
0 200 400 600 0 200 400 600
WLPHV WLPHV
0.15 0.15
Federated KF Federated KF
©9[PV

©9\PV
0.1 the presented method 0.1 the presented method

0.05 0.05

0 0

-0.05 -0.05
0 200 400 600 0 200 400 600
WLPHV WLPHV
Fig. 8.8 Position errors and velocity errors. KF Kalman filter

the accelerometers biases, the two methods have the same effective convergence
rapidity, but the fluctuating extent for the estimated values is different.
3. Conclusions
In this study, the SINS/CNS/GPS multi-sensors integration using FKF based on the
improved GA for enhancing navigational performance is investigated. First, the in-
formation sharing coefficients of FKF for the multi-sensors navigation system were
optimized using the improved GA, which improved the fitness function avoiding the
premature convergence problem of the general GA, while taking advantage of the
decimal-coding to improve the speed and accuracy of calculation. Second, a solution
and construction of a high performance SINS/CNS/GPS integrated navigation system
was proposed using the design of functional modular hardware and software-flow
integration. Third, the UKF was used to deal with the nonlinearity of the integrated
navigation system. The experimental results show that with the same experimental
conditions, and the estimated errors were minimal using the proposed method. These
errors were 0.25, 0.4, and 0.5 times the corresponding errors estimated by the tradi-
tional FKF. The fluctuating extent for the estimated value of the gyro errors is smaller
than that of the traditional FKF and the convergence of the estimated value was also
quicker. When estimating the accelerometer biases, the two methods had the same
rapidity of convergence, but the fluctuating extent of estimated values was different.
In total, compared with the traditional FKF, the proposed method not only greatly
8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation . . . 303

0.05 0.01
Federated KF
0.04 the presented method 0
εx/（?/h）

εy/（?/h）
Federated KF
0.03 -0.01 the presented method

0.02 -0.02

0.01 -0.03

0 -0.04

-0.01 -0.05
0 100 200 300 400 500 0 100 200 300 400 500
time/s time/s
150 150

100 100
▽x/(ug)

▽y/(ug)

50 50

0 0 Federated KF
Federated KF
the presented method the presented method
-50 -50
0 100 200 300 400 500 0 100 200 300 400 500
time/s time/s
0.02

0.01
εz/（?/h）

-0.01

-0.02 Federated KF
the presented method
-0.03
0 100 200 300 400 500
time/s
Fig. 8.9 Gyros drifts and accelerometers biases

increased the accuracy and reliability of the navigation system, but also resulted
in rapid convergence, which would be appropriate for a long flight-time unmanned
plane or long-range ballistic missile.
304 8 INS/CNS/GNSS Integrated Navigation Method

8.5 Chapter Conclusion

With features such as complete observation quantity and high reliability, the
SINS/CNS/GNSS integrated navigation is an effective integrated navigation method
with strong practicability; it is widely applied in high-performance navigation of
medium and long-range ballistic missile and high-altitude long-endurance flying ma-
chine at present, and an important development direction for integrated navigation
technology.
This chapter first introduced the basic principle, combination mode and the mod-
eling method of the SINS/CNS/GNSS integrated navigation; then, specific to the
decline of integrated navigation precision caused by instable error model and uncer-
tain environmental disturbance of SINS/CNS/GNSS integrated navigation system,
advanced filtering algorithm, and intelligent algorithm are introduced into inte-
grated navigation to focus on introduction of several SINS/CNS/GNSS integrated
navigation methods based on advanced filtering algorithm and SINS/CNS/GNSS
integrated navigation methods based on intelligent algorithm: (1) specific to poor
sub-filter performance of federated filtering, a SINS/CNS/GNSS integrated naviga-
tion method based on the federated UKF is introduced; (2) specific to optimization
difficulty of federated filtering information distribution factor, federated filter-
ing SINS/CNS/GNSS integrated navigation method based on improved genetic
algorithm was introduced.
There is no doubt that real time performance is also a very important index in
engineering application besides precision. Next chapter will introduce methods to
improve real time performance of SINS/CNS/GNSS integrated navigation system
from the perspective of system real time performance.

References

1. Qin YY, Zhang HR, Wang SH (2004) Theory of Kalman filter and integrated navigation.
Northwest Industrial University Press, Xi’an
2. Shen GX, Sun JF (2001) Application of information fusion theory in inertial/ CNS/GPS
astronomical integrated navigation system. National Defence Industry Press, Beijing
3. Fang JC, Li XE, Shen GX (1999) Research on INS/CNS/GPS intelligent fault to lerant
navigation system. J Chin Inert Technol 7(1):5–8
4. Carlson NA (1994) Federated Kalman filter simulation results. Navig J ION 41(3):297–321
5. Li YH, Fang JC (2003) A multi-model adaptive federated filter and it’s application in INS/
CNS/ GPS integrated navigation system. Aerosp Control (2):33–38
6. Yi Y, He Y, Guan X (2002) Federated filtering algorithm based on different local models. J Chin
Inert Technol 10(5):16–19
7. Wu HX,Yu WB, Fang JC (2005) Research on reduced dimension model of SINS/CNS integrated
navigation system. Aerosp Control 23(6):12–16
8. Li YH, Fang JC, Jia ZK (2002) Simulation of INS/CNS/GPS integrated navigation. J Chin
Inert Technol 10(6):6–11
9. Ali J, Fang JC (2005) Multisensor data synthesis using federated form of unscented Kalman
filtering. IEEE 524–529
References 305

10. Ali J, Fang JC (2005) SINS/ANS/GPS integration using federated Kalman filter based on opti-
mized information-sharing coefficients. AIAA Guidance, Navigation, and Control Conference,
6028–6040
11. Quan W, Fang JC (2006) An adaptive federated filter algorithm based on improved GA and
its application. In: Proceedings of 6th international symposium on instrumentation and control
technology 63575C
12. Quan W, Fang JC (2012) Realization of FKF based on improved GAs for SINSCNSGPS
integrated navigation system. J Navig 65(3):495–511
13. Li YH, Fang JC, Jia ZK (2002) Simulation of INS/CNS/GPS integrated navigation. J Chin
Inert Technol 10:6–11
14. Daniel JB (1999) Integrated navigation and guidance systems. AIAA Education Series
15. Ali J, Fang JC (2005) SINS/ANS/GPS Integration using federated kalman filter based on op-
timized information-sharing coefficients. In: Proceedings of the AIAA Guidance, Navigation,
and Control Conference, Monterey, CA, USA
16. Roelof WH, Van B (1994) True-sky demonstration of an autonomous star tracker. In:
Proceedings of the SPIE, Hawaii, USA
17. Quan W, Fang JC (2005) High-precision simulation of star map and its validity check. Opto
Electron Eng 32:22–26
18. Quan W, Fang JC, Xu F, Sheng W (2008) Study for hybrid simulation system of SINS/CNS
integrated navigation. IEEE Aerosp and Electron Syst Mag 23:17–24
19. Ali J, Fang JC (2005) In-flight alignment of inertial navigation system by celestial observation
technique. Trans Nanjing Univ Aeronaut Astronaut 22:132–138
20. Zhang GL, Li CL, Deng FL, Chen J (2004) Research on ballistic missile INS/GNSS/CNS
integrated navigation system. Missiles Sp Veh 4:11–15
Chapter 9
Study for Real-Time Ability of INS/CNS/GNSS
Integrated Navigation Method

9.1 Introduction

Real-time performance is also one of the important indexes to measure its per-
formance besides requirements for navigation precision during the engineering
application of the integrated navigation method of ship inertial navigation system
(SINS), celestial navigation system (CNS), and global navigation satellite system
(GNSS). During the improvement of the SINS/CNS/GNSS integrated navigation
system performance, on one hand, navigation precision may be improved greatly by
making the best of all and through collaborative transcendence due to the diversity of
observation means, but system dimension and measurement dimension of the system
will be increased simultaneously, which will then increase the amount of filtering
computation. On the other hand, it is sometimes required to design several filters
since observation information of a navigation subsystem is nonsynchronous, and the
output frequency is inconsistent, which further increases the difficulty of navigation
computer data processing [1–2].
Data processing of the SINS/CNS/GNSS integrated navigation system is real-
ized generally through Kalman filtering, its computation time is mainly decided
by the state dimension n and the measurement dimension m of the system model,
and the amount of computation for each filtering cycle is in direct proportion to
(n3 + mn2 ) [3]. During the engineering application of the SINS/CNS/GNSS inte-
grated navigation system, real-time data processing is required, but the real-time
performance becomes worse since the system order is generally high and the inte-
grated filtering algorithm is complex. Therefore, dimensionality reduction design
shall be conducted for the high-order system to reasonably reduce the system order,
and integrated algorithm shall be optimized to improve computation speed when the
navigation precision is guaranteed [4]; it is of great significance for the improvement
of real-time performance of the SINS/CNS/GNSS integrated navigation system [5].
System dimensionality reduction design through observability and degree of observ-
ability analysis method is an effective way to improve real-time performance of the
integrated navigation system. Therefore, this chapter first introduces observability

© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 307
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_9
308 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

and the degree of observability analysis theory and method; then, it introduces de-
sign methods of dimensionality reduction filter for SINS/CNS, SINS/GNSS, and
SINS/CNS/GNSS integrated navigation systems based on such an analysis theory
and method.

9.2 Piecewise Constant System (PWCS) Observability Analysis

Theory and Method

Observability analysis for a system generally includes two aspects: one is to de-
termine whether the system is completely observable and the other is to roughly
determine which state variables are observable and which are unobservable for in-
completely observable system. Observability analysis of linear time-invariant system
is easy, but that of linear time-varying system is relatively difficult. Specific to this
problem, Israeli scholars Goshen-Meskin and Bar-Itzhack proposed an observability
analysis method of the PWCS [6–7], which may effectively solve the observabil-
ity analysis problem of linear time-varying systems and is of significant application
value. However, the PWCS method is only available for the qualitative analysis of
the degree of observability of system state variables instead of providing a quanti-
tative value for the observability of each state. Therefore, this section introduces an
improved system observability analysis method based on singular value decomposi-
tion (SVD) [4, 8], which provides an effective way for a quantitative analysis of the
observability of linear time-varying systems.

9.2.1 Observability Analysis Theory of the PWCS

1. Definition of System Observability and Concept of the PWCS

1) Definition of system observability

If the state vector X(ti ) of the system at the time of ti can be determined from the
output function Y[ti ,ti+1 ] (output sequence for a discrete time system) of the system
within the time interval [ti , ti+1 ], the system shall be observable, wherein [ti , ti+1 ] is
the limited time interval here. If the system is observable for any ti and X(ti ), the
system shall be completely observable.
2) Concept of the PWCS
For a linear time-varying stochastic system:
Ẋ(t) = F (t)X(t) + G(t)W (t) (9.1)
9.2 Piecewise Constant System (PWCS) Observability Analysis Theory and Method 309

Fig. 9.1 Measurement series Time frame 1 Time frame 2 Time frame 3
of a discrete PWCS

Z(t) = H (t)X(t) + V (t), (9.2)

wherein X(t) ∈ R n , F (t) ∈ R n×n , G(t) ∈ R n×s , W (t) ∈ R s , V (t) ∈ R m , Z(t) ∈ R m ,

and H (t) ∈ R m×n .
If the variable quantity of matrix F (t) and H (t) of the linear time-varying system
is ignored within a time interval small enough, the time-varying system may be
processed as time-invariant system within the time interval, and such a system is
called piecewise constant system (PWCS); its discretization equations are:
⎧
⎨ X(k + 1) = Fj X(k) + Gj W (k)
(9.3)
⎩ Zj (k) = Hj X(k) + V (k),

wherein j = 1,2, · · · , r, denoting the section gap number of the system.

Matrix Fj , Gi , and Hj are constant within each time frame j, but each matrix is
different during different time frames.
2. Observability Matrix of the PWCS

The following is a description with a discrete system as an example; a circumstance

of a continuous system being similar to a discrete system.
Since the observability of a system is irrelevant to an input, it is allowed to research
the observability of a homogeneous system only, and corresponding homogeneous
system equations of PWCS discretization equations from above are:

X(k + 1) = Fj X(k)
(9.4)
Zj (k) = Hj X(k).

Supposing a series of observation values as shown in Fig. 9.1 to be obtained from

the system, all observation values shall be available to be expresses as a function of
X(1):
310 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

⎧
⎪
⎪ Z1 (1) = H1 X(1)
⎪
⎪
⎪
⎪
⎪
⎪ Z1 (2) = H1 F1 X(1)
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎪ ...
⎪
⎪
⎪
⎪
⎪
⎪ Z1 (n) = H1 F1n−1 X(1)
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎪ Z2 (n) = H2 F1n−1 X(1)
⎪
⎨
Z2 (n + 1) = H2 F2 F1n−1 X(1) (9.5)
⎪
⎪
⎪
⎪
⎪
⎪ ...
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎪ Z2 (2n) = H2 F2n−1 F1n−1 X(1)
⎪
⎪
⎪
⎪
⎪
⎪ Z3 (2n) = H3 F2n−1 F1n−1 X(1)
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎪ Z3 (2n + 1) = H3 F3 F2n−1 F1n−1 X(1)
⎪
⎪
⎪
⎩
...

If the observation matrix remains unchanged during two adjacent time frames, two
identical observation values can be obtained on demarcation points of time frames,
and the total measurement equation can be written in the following matrix form:

Z = N (r) ∗ X(t0 ), (9.6)

9.2 Piecewise Constant System (PWCS) Observability Analysis Theory and Method 311

wherein Z represents all observation values of the observation vector, and N (r) is:
⎡ ⎤
⎡ ⎤
H1
⎢ ⎥ H1
⎢ ⎥ ⎢ ⎥
⎢ H 1 F1 ⎥ ⎢ ⎥
⎢ ⎥ ⎢ H1 F1 ⎥
⎢ ⎥ ⎢ ⎥
⎢ .. ⎥ ⎢ .. ⎥
⎢ . ⎥ ⎢ . ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ H F n−1 ⎥ ⎢ H F n−1 ⎥
⎢ 1 1 ⎥ ⎢ 1 1 ⎥
⎢ ⎥ ⎢ ⎥
⎢. . . . . . . . . . . . . . . . . . . . . . . . . . .⎥ ⎢ . . . . . . . . . . . . . . . . . . . . . . . . . . . ⎥
⎢ ⎥ ⎢ ⎡ ⎤ ⎥
⎢ ⎥ ⎢ ⎥
⎢ H2 F1n−1 ⎥ ⎢ H2 ⎥
⎢ ⎥ ⎢ ⎢ ⎥ ⎥
⎢ ⎥ ⎢ ⎢ ⎥ ⎥
⎢ H 2 F 2 F1 n−1 ⎥ ⎢ ⎢ ⎥ ⎥
⎢ ⎥ ⎢ ⎢ H2 F2 ⎥ n−1 ⎥
⎢ .. ⎥ ⎢ ⎢ .. ⎥ F ⎥
⎢ ⎥ ⎢ ⎢ ⎥ 1
⎥
⎢ . ⎥ ⎢ ⎢ . ⎥ ⎥
N (r) = ⎢ ⎥=⎢ ⎣ ⎦ ⎥.
⎢ ⎥ ⎢ ⎥
⎢ H2 F2n−1 F1n−1 ⎥ ⎢ H2 F2n−1 ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢· · · · · · · · · · · · · · · · · · · · · · · · · · ·⎥ ⎢ · · · · · · · · · · · · · · · · · · · · · · · · · · · ⎥
⎢ ⎥ ⎢ ⎥
⎢ .. ⎥ ⎢ .. ⎥
⎢ . ⎥ ⎢ . ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢· · · · · · · · · · · · · · · · · · · · · · · · · · ·⎥ ⎢ · · · · · · · · · · · · · · · · · · · · · · · · · · · ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢⎡ ⎤ ⎥
⎢ Hr F n−1 F n−1 · · · F n−1 ⎥ ⎢ Hr ⎥
⎢ r−1 r−2 1 ⎥ ⎢ ⎥
⎢ ⎥ ⎢⎢ ⎥ ⎥
⎢ Hr Fr Fr−1 Fr−2 · · · F1
n−1 n−1 n−1 ⎥ ⎢⎢ ⎥ ⎥
⎢ ⎥ ⎢⎢ Hr Fr ⎥ n−1 n−1 n−1 ⎥
⎢ .. ⎥ ⎢⎢ ⎥ F F . . . F ⎥
⎢ ⎥ ⎢⎢ ... ⎥ r−1 r−2 1
⎥
⎢ . ⎥ ⎣⎣ ⎦ ⎦
⎣ ⎦ n−1
Hr Frn−1 Fr−1 Fr−2 · · · F1n−1
n−1 n−1 H r Fr
(9.7)
Suppose the observability matrix corresponding to each time frame j to be defined
as:
 
T
Nj = (Hj )T (Hj Fj )T · · · (Hj Fjn−1 ) . (9.8)

So N (r) can be expressed as:

⎡ ⎤
N1
⎢ ⎥
⎢ ⎥
⎢ N2 F1n−1 ⎥
⎢
N (r) = ⎢ ⎥. (9.9)
⎥
⎢ ··· ⎥
⎣ ⎦
Fr−2 · · · F1n−1
n−1 n−1
Nr Fr−1

Matrix N (r) must be researched to research observability of the PWCS; N (r) is called
total observability matrix of the PWCS, denoted by the TOM (total observability
matrix).
312 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

Theorem 9.1: If the rank of the observability matrix N (r) is n, the discrete PWCS
defined by Eq. (9.3) shall be completely observable.
3. Stripped Observability Matrix of the PWCS

Matrix N (r) is often simplified to research observability of the discreet PWCS, which
is more conveniently. First, two lemmas are given as follows.
Make X an n-dimensional vector, and Mj , Cj , and Gj (1 ≤ j ≤ r) an
n × n-dimensional constant matrix sequence; make:
⎡ ⎤ ⎡ ⎤
G1 G1
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ G2 ⎥ ⎢ G2 M 1n−1 ⎥
⎢ ⎥
G = ⎢ ⎥, Ed = ⎢ ⎢ ⎥,
⎥
⎢. . . ⎥ ⎢ ... ⎥
⎣ ⎦ ⎣ ⎦
n−1
Gr Gr Mr−1 . . . M1n−1
wherein G and Ed are both nr × n-order matrixes.
Lemma 9.1: If Mj X = X, ∀X ∈ null(Gj )(1 ≤ j ≤ r), there is null
(Ed ) = null(G), rank(Ed ) = rank(G), wherein null( • ) represents null space of
the matrix and rank(•) represents the rank of the matrix.
Refer to the References [3] for the proof of the lemma. The condition of Lemma
9.1 is that the eigenvector of the corresponding eigenvalue 1 in Mj belongs to null
space of Gj .
Define Ns (r) as stripped observability matrix (SOM) of the PWCS:
⎡ ⎤
N1
⎢ ⎥
⎢ ⎥
⎢ N2 ⎥
Ns (r) = ⎢
⎢ ⎥.
⎥
⎢· · ·⎥
⎣ ⎦
Nr
If the unobservable space of the PWCS is null space of the TOM, TOM may be
replaced by the SOM for observability research of the PWCS, which is simpler than
direct application of the TOM. Since research of observability includes research of
unobservable subspace of the TOM, if null space of the TOM and SOM is identical,
null space of the SOM may be researched instead of null space of the TOM.
Substitute Lemma 1 as follows:
Mj = Fj , Gj = Nj , G = Ns (r), Ed = N (r).
It is obtained that if Fj X = X, ∀X ∈ null(Gj ) (1 ≤ j ≤ r), there is
null(N (r)) = null(Nd ), rank(Ed ) = rank(G).
9.2 Piecewise Constant System (PWCS) Observability Analysis Theory and Method 313

Thus, it can be seen that the research of observability of a discrete system by sub-
stituting the SOM (i.e., of Ns (r)) of the PWCS with the TOM (i.e., N (r)) may further
simplify the analysis. There is a corresponding relation between the observability
analysis of a continuous time-varying system and a discrete time-varying system,
details of which will not be given here.
4. Steps for Observability Analysis of the PWCS

Following are the steps for an observability analysis of the PWCS; refer to the
References [3] for the detailed derivation process:
1. First consider the first time frame of the PWCS, and make j = 1
2. Define Fj and Hj , and calculate the corresponding observability matrix N(j)
within this time frame
3. Fix Ns (j), and increase j to the current time frame
4. Fix rank R of the current SOM matrix; find out R linearly independent rows in
the SOM to constitute the dimensionality reduced SOM, i.e., Us,0 (j )
5. Calculate P = n-R, and construct the matrix Us,u (j )
6. Convert X to ξ and then to Y by rearranging the state variable sequence of X
7. Check the relation between Y1 (observable part of Y ) and the state variable of X,
as well as the relation between Y2 (unobservable part of Y ) and the state variable
of X
8. Work out the current observability situation of the system based on results of step
7
9. If the current time frame is not the final time frame, j = j + 1, proceed with the
analysis of the next time frame, return to step 2, and continue until the analysis
of all time frames is completed

9.2.2 An Improved System State Degree of Observability Analysis

Method Based on Singular Value Decomposition

For linear time-varying systems not completely observable, through the application
of the PWCS, the observability analysis method can work out which state variables are
observable and which are unobservable. The observable degree for each observable
state variable cannot be obtained, and even for completely observable systems, the
observable degree varies with different state variables. The observable degree of
system state variables is called the degree of observability, research of which is of
important value for the improvement of system real-time performance.
The description of the system degree of observability by using eigenvalue and
eigenvector of the estimated error covariance matrix of Kalman filter is available to
judge the degree of observability of state variables of completely observable systems,
but it is a conducted subject of the Kalman filtering operation (i.e., after the estimated
314 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

state error covariance matrix is calculated). Can a method available to directly use
the observability matrix analysis and judge the system degree of observability be
discovered? It applies to both a completely observable system and a not completely
observable system and has no requirements for a prior Kalman filtering operation.
Specific to this problem, an improved system state degree of an observability analysis
method based on SVD will be introduced below to use a SVD of the observability
matrix to analyze the degree of observability of each state of the system.
Suppose the observability matrix of a dynamical system during a certain time
frame to be N (m1 × n-dimensional), the initial state is X(t0 ) (n-dimensional), and
the measurement value Z (m1-dimensional):

Z = N · X(t0 ). (9.10)

After conducting a SVD for matrix N, there is:

N = U ·S ·VT, (9.11)

wherein U = [u1 u2 · · · um ] and V = [v1 v2 · · · vm ] are both orthogonal


Λr×r 0
matrixes; S = , wherein Λr×r = diag(σ1 σ2 · · · σr ), σ1 > σ2 >
0 0
· · · > σr > 0 are called singular values of matrix Q.
Substitute Eq. (9.11) into Eq. (9.10), and there is:

Z = (U · S · V T) · X(t0 ). (9.12)

Expand Eq. (9.12), and there is:

r
Z= σi (vi T · X(t0 )) · ui . (9.13)
i=1

In accordance with Eq. (9.13), there is:

r

ui T · Z)
Z = X(t0 ) = · vi T . (9.14)
i=1
σi

The traditional analysis method is to calculate the corresponding initial state vector
X0,i of each singular value σi according to Eq. (9.14). From the perspective of the
numerical value, larger singular values can obtain better state estimation; otherwise,
for particularly small singular values, singularity of several X(t0 ) may be caused,
which will finally fall into unobservable space.
During the analysis from the perspective of the linear system theory, observability
of the state variable X(t0 ) shall be only dependent on the system structure and irrel-
evant to the observation quantity Z. In fact, the measurement quantity Z is obtained
when the initial value X(0) of the state variable passes the projection of vector ui and
vi . If certain columns of elements of the matrix ui viT are all zero or very small, the
9.3 Dimensionality Reduction Filter Design of INS/CNS . . . 315

influence of the corresponding state variable of X(0) on the measurement quantity

Z will be very small, in which case it is very hard to estimate the component X(0)
according to the measurement quantity. Therefore, the analysis of matrix ui vi T to
observe the size of each column of the elements is available for direct judgment of
the corresponding initial state vector X0,i of each singular value σi . It is possibly
to prove that results of the analysis by using or not using the observation quantity
are consistent [4]. Compared with the traditional singular value analysis method, the
improved degree of observability analysis method has the following advantages:
1. Since the analysis process puts the measurement quantity aside, the degree of the
observability of the system state variable can be analyzed without test data, and
the computation is simple.
2. Measurement quantity is used in the traditional method, so it will be limited
by a sampling period of observation data when using the PWCS theory to take a
piecewise time interval. Since n observation data are used during each time frame,
if the sampling period of observation data is Tz , each piecewise time interval of
the PWCS shall at least be nT z , but for high-dynamic environment, a small-time
interval is required to ensure a variation of the coefficient matrix available to
be ignored, which is obviously contradictory. Piecewise time interval will not
be limited by the sampling period of measurement data, and only the sampling
period of SINS is required to be considered if the improved analysis method is
adopted. Piecewise time interval can be very small since the sampling frequency
of SINS is usually high.

9.3 Dimensionality Reduction Filter Design of INS/CNS

Integrated Navigation System Based on the Improved
Degree of Observability Analysis

At present, there are many design methods of the dimensionality reduction model
which can be divided into two categories as a whole. One is the reservation of the main
state in the system, such as the aggregation method, model method, Lyapunov func-
tion method, and perturbation method; the other is the identification of input/output
data which are obtained by using a certain input signal to drive the original system.
In terms of SINS/CNS integrated navigation system, the first method is generally
adopted for engineering application, namely to omit some conditions in the system
to reduce the calculated amount.
This section applies the PWCS theory and improved state degree of observabil-
ity analysis method based on SVD to the SINS/CNS integrated navigation system
[9]. According to the degree of the observability analysis result of the system, the
system model dimensionality reduction can be obtained with omission of the state
variables hard to be observed. Finally, it is proved by computer simulation that the di-
mensionality reduction model can guarantee the navigation accuracy, greatly reduce
316 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

50

45

40

35

Singular value
30
奇异值

25

20

15

10

0
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
15个状态变量 15 state variables
Fig. 9.2 Singular values corresponding to state variables

the calculated amount, and improve the real-time performance of the SINS/CNS
integrated navigation system.
1. Degree of Observability Analysis of Full-Order Model of the SINS/CNS
Integrated Navigation System

Refer to Sect. 6.3 of this book for the full-order model of the SINS/CNS integrated
navigation system. Use the improved degree of observability analysis methods based
on SVD to estimate the degree of observability of state variables in the full-order
model. Draw a histogram of singular values corresponding to state variables, as
shown in Fig. 9.2.
It can be seen from Fig. 9.2 that the state variables with best degree of observ-
ability are three gyro drifts ( εx , εy , and εz ) and next are three mathematics platform
misalignment angles ( φE , φN , and φU ). The degree of observability of other state
variables is extremely low so that they can be deemed as unobservable.
9.3 Dimensionality Reduction Filter Design of INS/CNS . . . 317

2. Dimensionality Reduction Model Design of the SINS/CNS Navigation

System

Based on the above analysis conclusion, omit the state variables with poor degree
of observability and take the 6-dimension state variables:
 T
X1 (t) = φE φN φU εx εy εz . (9.15)

The state equation and measurement equation of the dimensionality reduction system
are as follows (geographic coordinate system is taken as the navigation coordinate
system):

Ẋ1 (t) = F1 (t)X1 (t) + G 1 (t)W1 (t)

(9.16)
Z1 (t) = H1 (t)X1 (t) + V1 (t),

wherein the system state transition matrix F1 (t) is:

⎡ ⎤
F2 Cbn
F1 (t) = ⎣ ⎦ , (9.17)
03×3 03×3
6×6

wherein
⎡
⎤
vE tgL vE
⎢ 0 ωie sin L + − ωie cos L +
⎢
 RN + h RN + h ⎥
⎥
⎢ v tgL vN ⎥
F2 = ⎢− ωie sin L + E 0 − ⎥.
⎢ RN + h RM + h ⎥
⎣ vE vN ⎦
ωie cos L + 0
RN + h RM + h
(9.18)

The system noise driving matrix G 1 (t) is:

⎡ ⎤
Cbn
G 1 (t) = ⎣ ⎦ . (9.19)
03×3
6×3

System noise vector

 T
W 1 (t) = wεx wεy w εz , (9.20)

wherein wεx , wεy , and wεz are random errors of the gyro.
The system noise variance matrix is:
 
Q 1 (t) = diag σεx
2 2
σεy σεz2 . (9.21)
318 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

Measurement quantity
 T
Z(t) = φE φN φU . (9.22)

Measurement matrix
 
H1 (t) = I3×3 03×3 . (9.23)

3. Computer Simulation Verification

Simulation condition: Random constant drift of SINS gyro is 0.1◦ /h, random drift
is 0.05◦ /h, random constant bias of accelerometer is 20 μg, random bias is 10 μg,
azimuth misalignment angle is 60 , horizontal misalignment angle is 10 , and CNS
measurement error is 3 .
The initial position of aircraft is 116◦ E, 39◦ N, initial velocity is 200 m/s, course
angle is 45◦ , pitching angle and roll angle are 0◦ , accelerated velocity is 0.2 m/s,
angular velocity of turning is 0.01◦ /s, and simulation time is 900 s. Feedback com-
pensation is adopted in SINS/CNS integrated filtering. The simulation results are
shown in Fig. 9.3.
The calculated amount of filtering is in direct proportion to n3 + mn2 . Ta-
ble 9.1 shows the comparative results of the calculated amount before and after
the dimensionality reduction.
It can be seen from Fig. 9.3 that equal navigation accuracy is obtained by the di-
mensionality reduction model and high-order model. It can be known from Table 9.1
that the calculated amount of filtering is only 8 % of the original amount if the system
model of dimensionality reduction is adopted. Therefore, the real-time performance
is improved.

9.4 Dimensionality Reduction Filter Design of INS/GNSS

Integrated Navigation System Based on the Improved
Degree of Observability Analysis

The degree of the observability analysis method (based on improved SVD) intro-
duced in the above section is also applied to the dimensionality reduction design
of the SINS/GNSS integrated navigation system [10]. The degree of observability
analysis of the SINS/GNSS integrated navigation system based on the improved anal-
ysis method, dimensionality model design, and computer simulation verification are
introduced below.
9.4 Dimensionality Reduction Filter Design of INS/GNSS . . . 319

Fig. 9.3 Simulation error curve of the SINS/CNS dimensionality reduction model
320 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

Table 9.1 Comparison of the calculated amount by two algorithms

System order n Measurement quantity m n3 + mn2
High-order model 15 3 4050
Low-order model 6 3 324

Fig. 9.4 Singular values

corresponding to state
variables

Singular value 15 state variables

1. Degree of Observability Analysis of the SINS/GNSS Integrated Navigation

System

Refer to Section 5.3.1 of this book for the full-order model of the SINS/GNSS
integrated navigation system. Similarly, use the improved degrees of observability
analysis methods [3] based on SVD to estimate the degree of observability of state
variables in the full-order model. Draw a histogram of singular values corresponding
to state variables, as shown in Fig. 9.4.
The sort order of state variables based on the degree of observability from high
to low is [11, 12]: δL, δλ, εy , εx , φN , φE , ∇z , δvU , δvN , δvE , δh, εz , φU , ∇x , ∇y .
2. Dimensionality Reduction Design of the SINS/GNSS Integrated Navigation
System

Based on the above analysis result, omit the horizontal accelerometer biases ∇x and
∇y , mathematics platform misalignment angle ϕz , and vertical direction gyro drift
εz and take the 11-dimension state variables [13, 14]:
 T
X2 (t) = φE φN δvE δvN δvU δL δλ δh εx εy ∇z . (9.24)

The system model of dimensionality reduction is as follows (geographic coordinate

system [3] is taken as the navigation coordinate system):

Ẋ2 (t) = F2 (t)X2 (t) + G 2 (t)W2 (t)

(9.25)
Z2 (t) = H2 (t)X2 (t) + V2 (t),
9.4 Dimensionality Reduction Filter Design of INS/GNSS . . . 321

wherein the state transition matrix of system is:

⎡ ⎤
F 18×8 F 28×3
F2 (t) = ⎣ ⎦ , (9.26)
03×8 03×3
11×11

wherein the elements of F1 correspond to the error equation of the inertial navigation
system [1, 15] and will not be listed here one by one due to space limitation.
⎡ ⎤
F c12×2 02×1
⎢ ⎥
⎢ ⎥
F 2 = ⎢ 03×2 F c23×1 ⎥ , (9.27)
⎣ ⎦
03×2 03×1
8×3

Cbn (1,1) Cbn (1,2)  T
wherein F c1 = , F c2 = Cbn (1,3) Cbn (2,3) Cbn (3,3) .
Cbn (2,1) Cbn (2,2)

Measurement matrix:
⎡ ⎤
03×2 diag(1 1 1) 03×3 03×3
H2 (t) = ⎣ ⎦ . (9.28)
03×2 03×3 diag(RM RN cos L 1) 03×3
6×11

System noise driving matrix:

⎡ ⎤
F c12×2 02×1
⎢ ⎥
⎢ ⎥
G 2 (t) = ⎢ 03×2 F c23×1 ⎥ . (9.29)
⎣ ⎦
06×2 06×1
11×3

System noise vector:

 T
W2 (t) = wεx wεy w∇z . (9.30)

System noise variance matrix:

 
Q 2 (t) = diag σεx
2 2
σεy 2 .
σ∇z (9.31)

3. Computer Simulation Verification

Simulation condition: Random constant drift of SINS gyro is 0.1◦ /h, random drift
is 0.05◦ /h, random constant bias of accelerometer is 100 μg, random bias is 50 μg,
azimuth misalignment angle is 60 , and horizontal misalignment angle is 10 . The
322 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

GNSS adopts differential positioning; the velocity accuracy is 0.005 m/s, and the
positional accuracy is 0.05 m.
The initial position of aircraft is 116◦ E, 39◦ N, initial velocity is 200 m/s, course
angle is 45◦ , pitching angle and roll angle are 0◦ , and the simulation time is 900
s. Feedback compensation is adopted in the SINS/GNSS integrated filtering. The
simulation results are shown in Fig. 9.5.
The calculated amount of filtering is in direct proportion to n3 + mn2 . Ta-
ble 9.2 shows the comparative results of the calculated amount before and after
the dimensionality reduction.
It can be seen from Fig. 9.5 that equal navigation accuracy is obtained by the di-
mensionality reduction model and high-order model. It can be known from Table 9.2
that the calculated amount of filtering is 43.5 % of the original amount if the system
model of dimensionality reduction is adopted.

9.5 Federated Filter Design of the INS/CNS/GNSS Integrated

Navigation System Based on Dimensionality Reduction
Filtering

In the above two sections, the improved degree of observability analysis based
on SVD is used for the dimensionality reduction design of the SINS/CNS and
SINS/GNSS integrated navigation systems. Based on the two dimensionality re-
duction filters, this section introduces a design method of federated filter of the
SINS/CNS/GNSS integrated navigation system based on dimensionality reduction
filtering.
1. SINS/CNS/GNSS Integrated Navigation System Model Based on Federated
Filtering

It can be known from Sect. 7.3 that the SINS/CNS/GNSS integrated navigation
system based on federated filtering contains two subfilters, namely the SINS/CNS
subfilter and SINS/GNSS subfilter.
It can be known from
 Sect. 9.3 that the SINS/CNS
T subfilter takes 6-dimension state
variables, X1 (t) = φE φN φU εx εy εz , and the system state equation
and measurement equation are as follows (geographic coordinate system is taken as
the navigation coordinate system):

Ẋ1 (t) = F1 (t)X1 (t) + G 1 (t)W1 (t)

(9.32)
Z1 (t) = H1 (t)X1 (t) + V1 (t).
9.5 Federated Filter Design of the INS/CNS/GNSS . . . 323

Fig. 9.5 Error curve of the Longitude error (m)

SINS/GNSS dimensionality 0.06

Low-order model
••••
••••
reduction model simulation 0.04 ------High-order model

0.02

-0.02

-0.04

-0.06

-0.08
0 100 200 300 400 500 600 700 800 900
Time (s)
Latitude error (m)
0.25
Low-order model
••••
••••

0.2 ------High-order model

0.15

0.1

0.05

-0.05

-0.1
0 100 200 300 400 500 600 700 800 900
Time (s)

North velocity error (m/s)

0.025
Low-order model
••••
••••

------High-order model
0.02

0.015

0.01

0.005

-0.005
0 100 200 300 400 500 600 700 800 900
Time (s)

East velocity error (m/s)

0.025

Low-order model
••••
••••

0.02 ------High-order model

0.015

0.01

0.005

-0.005

-0.01
0 100 200 300 400 500 600 700 800 900
Time (s)
324 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

Table 9.2 Comparison of the calculated amount by two algorithms

System order n Measurement quantity m n3 + mn2
High-order model 15 6 4725
Low-order model 11 6 2057

According to [16], in order to guarantee the integrity of state variables of the

main filter, the horizontal accelerometer biases are reserved in the dimensionality
reduction model of the SINS/GNSS subfilter, and 13-dimension state variables are
taken:
 T
X2 (t) = φE φN δvE δvN δvU δL δλ δh εx εy ∇x ∇y ∇z .

The system model is:

Ẋ2 (t) = F2 (t)X2 (t) + G 2 (t)W2 (t)
(9.33)
Z2 (t) = H2 (t)X2 (t) + V2 (t),
wherein the system state transition matrix is:
⎡ ⎤
F 18×8 F 28×5
F2 (t) = ⎣ ⎦ , (9.34)
05×8 05×5
13×13

wherein the elements of F1 correspond to the error equation of the SINS [15]. Refer
to Sect. 2.3 of this book for details.
⎡ ⎤
F c2×2 02×3
⎢ ⎥
⎢ ⎥
F 2 = ⎢ 03×2 Cbn ⎥ , (9.35)
⎣ ⎦
03×2 03×3
8×5
⎡ ⎤
Cbn (1,1) Cbn (1,1)
wherein F c = ⎣ ⎦.
n n
Cb (1,1) Cb (1,1)

Measurement matrix:
⎡ ⎤
03×2 diag(1 1 1) 03×3 03×5
H2 (t) = ⎣ ⎦. (9.36)
03×2 03×3 diag(RM RN cos L 1) 03×5

System noise driving matrix:

⎡ ⎤
F c2×2 02×3
⎢ ⎥
⎢ ⎥
G2 (t) = ⎢ 03×2 Cbn ⎥ . (9.37)
⎣ ⎦
08×2 08×3
13×5
9.5 Federated Filter Design of the INS/CNS/GNSS . . . 325

System noise vector:

 T
W2 (t) = wεx wεy w∇x w∇y w∇z . (9.38)

System noise variance matrix:

 
Q 2 (t) = diag σεx
2 2
σεy 2
σ∇x 2
σ∇y 2 .
σ∇z (9.39)

Fifteen dimensions are taken for global state variables:

 T
X(t) = φE φN φU δvE δvN δvU δL δλ δh εx εy εz ∇x ∇y ∇z .
(9.40)

Mapping matrix from global state space to local state space:

⎡ ⎤
I3×3 03×12
T1 = ⎣ ⎦ , (9.41)
03×3 03×12
6×15
⎡ ⎤
1 0 0
⎢ ⎥
⎢ ⎥
⎢ 1 0 0 ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ 0 1 0 ⎥
⎢ ⎥
⎢ ⎥
⎢ 0 1 0 ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ 0 1 0 ⎥
⎢ ⎥
⎢ ⎥
⎢ 0 1 0 ⎥
⎢ ⎥
⎢ ⎥
T2 = ⎢
⎢ 0 1 0 ⎥
⎥ .
⎢ ⎥
⎢ ⎥
⎢ 0 1 0 ⎥
⎢ ⎥
⎢ ⎥
⎢ 0 1 0 ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ 0 1 0 ⎥
⎢ ⎥
⎢ ⎥
⎢ 0 0 1 ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ 0 0 1 ⎥
⎣ ⎦
0 0 1
13×15

(Other unmarked elements are 0) (9.42)

326 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

2. Computer Simulation Verification

Simulation condition: The random constant drift of SINS gyro is 0.1◦ /h, random
drift is 0.05◦ /h, random constant bias of accelerometer is 100 μg, random bias is
50 μg, azimuth misalignment angle is 60 , horizontal misalignment angle is 10 ,
CNS measurement error is 3 , velocity accuracy of GNSS is 0.05 m/s, and positional
accuracy is 5 m.
The initial position of the aircraft is 116◦ E, 39◦ N, initial velocity is 200 m/s,
course angle is 45◦ , pitching angle and roll angle are 0◦ , and simulation time is 900
s. Feedback compensation is adopted in the SINS/CNS/GNSS integrated filtering.
The simulation results are shown in Fig. 9.6.
The error data within 900 s are listed in Table 9.3.
It can be seen from the comparison that the navigation accuracy reduces as the
influence of partial state variables is not considered when the dimensionality reduc-
tion model is adopted, but it is basically at the same order of magnitude with that in
high-order model.
It can be seen from the data listed in Table 9.4 that the calculated amount of the
filter is about 55 % of the original amount if the system model of the dimensionality
reduction is adopted.
In conclusion, use the improved observable degree analysis method to con-
duct the dimensionality reduction design [6, 7] on different partial models of the
SINS/CNS/GNSS integrated navigation system and use the federated filtering algo-
rithm to conduct information integration [17, 18]. The following conclusions can be
obtained through simulation verification:
1. Through observable degree analyses, some unobservable or low observable state
variables in the system are ignored. The filtering accuracy of the dimensionality
reduction model nearly equals the filtering accuracy of the full-order model.
2. Federated filtering algorithm which is designed on the basis of different partial
model dimensionality reduction is used. It is not needed to composite the state
variables of each partial filter; therefore, the amount of calculation is reduced and
real-time performance of the SINS/CNS/GNSS integrated navigation system is
improved. It has important engineering application value.

9.6 Chapter Conclusion

Before engineering application of the SINS/CNS/GNSS integrated navigation sys-

tem, real-time performance is an important content, which shall be considered and
researched, and an important indicator of whether the SINS/CNS/GNSS integrated
navigation system is applicable.
To improve the real-time performance of the SINS/CNS/GNSS integrated navi-
gation method, this chapter introduces a dimensionality reduction filtering method
based on observable analysis; it is applied to the integrated navigation systems
9.6 Chapter Conclusion 327

Longitude error (m) Latitude error (m)

üüLow-order model üüLow-order model

------High-order model ------High-order model

Time (s) Time (s)

East velocity error (m/s) North velocity error (m/s)

üüLow-order model üüLow-order model

------High-order model ------High-order model

Time (s) Time (s)

Azimuth angle error (″/s) Pitching angle error (″/s)

üüLow-order model
------High-order model

üüLow-order model
------High-order model

Time (s) Time (s)

Roll angle error (″/s)

üüLow-order model
------High-order model

Time (s)

Fig. 9.6 Simulation error of the SINS/CNS/GNSS integrated navigation dimensionality reduction
model based on federated filtering
328 9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method

Table 9.3 Comparison of navigation errors by two algorithms (900 s)

Average of Longitude Latitude East North Azimuth Pitching Roll
navigation (m) (m) velocity velocity angle angle angle
errors (m/s) (m/s) ( ) ( ) ( )
High-order model 3.764 4.203 0.020 0.019 − 8.912 − 4.681 7.203
Low-order model 5.255 6.712 0.029 0.031 − 9.457 − 9.812 11.522

Table 9.4 Comparison of the calculated amount by two algorithms

!
SINS/CNS subfilter SINS/GNSS subfilter (n3 + mn2)
n1 m1 n13 + m1n12 n2 m2 n23 + m2n22
High-order 15 3 4050 15 6 4725 7936
model
Low-order 6 3 324 13 6 3211 4374
model

of SINS/CNS, SINS/GNSS, and SINS/CNS/GNSS. The computer simulation re-

sults show that although the filtering accuracy of dimensionality reduction model
is reduced slightly, the amount of calculation is reduced prominently and the real-
time performance of integrated navigation system is improved. It has important
engineering application value.
The method of improving the real-time performance of the SINS/CNS/GNSS in-
tegrated navigation system introduced in this chapter has a certain reference function
on engineering application. If it is used on engineering practice, it shall be tested and
verified strictly. Semiphysical simulation technology is an effective test and verifi-
cation method. The next chapter will focus on semiphysical simulation systems of
the SINS/CNS/GNSS integrated navigation system.

References

1. Yuan X, Yu J, Chen Z (1993) Navigation System. Aviation Industry Press, Beijing

2. Zhang G, Zeng J (2008) The principle and technology integrated navigation. Xi’an Jiaotong
University Press, Xi’an
3. Wan D, Fang J (1998) Inertial navigation initial alignment. Southeast University Press, Nanjing
4. Wu H, Yu W, Fang J (2005) Research on reduced dimension model of SINS/CNS integrated
navigation system[J]. Aerosp Control 23(6):12–16
5. Walton RW (2000) Real time, high accuracy, relative state estimation for multiple vehicle
systems. Department of Mechanical Engineering, University of California, Los Angeles
6. Goshen-Meskin D, Bar-Itzhack IY (1992) Observability analysis of piece-wise constant
system-part I: theroy. IEEE Trans Aerosp Electron Syst 28(4):1056–1067
7. Goshen-Meskin D, Bar-Itzhack IY (1992) Observability analysis of piece-wise constant
system-Part II: application to inertial navigation in-flight alignment. IEEE Trans Aerosp
Electron Syst 28(4):1068–1075
References 329

8. Cheng X, Wan D, Zhong X. (1997) Study on observability and its degree of strapdown inertial
navigation system[J]. J Southeast Univ 27(6):6–11
9. WU H, YU W, Fang J (2006) Simulation of SINS/CNS integrated navigation system used on
high altitude and long-flight-time unpiloted aircraft[J]. Acta Aeronautica et Astronautica Sinica
27(2):299–304
10. Shuai P, Chen D, Jiang Y (2004) Observable degree analysis method of integrated GPS/INS
navigation system[J]. J Astronaut (4):219–224
11. He X, Liu J, Yuan X (1997) Design of the reduced order filter for the integrated GPS/INS[J]. J
Astronaut 5(2):1–3
12. Dong X, Zhang S, Hua Z (1998) Integrated GPS/INS navigation and its applications. National
University of Defense Technology Press, Changsha
13. Huang L, Zhou B, Shan M (2003) MIMU/GPS integrated navigation system based on DSP[J].
J Chin Inert Technol 11(3):12–15
14. He X, Chen Y, Iz HB (1998) A reduced-order model for integrated GPS/INS. IEEE AES Syst
Mag (3):40–45
15. Fu M, Deng Z, Zhang J (2003) Kalman filtering theory and its application in navigation system
16. Yi X, He Y, Guan X (2002) Federated filtering algorithm based on different local model[J]. J
Chin Inert Technol 10(5):16–19
17. Carlson NA (1994) Federated Kalman filter simulation results. Navig J ION 41(3):297–321
18. Julier SJ, Uhlmann JK (1997) A new extension of the Kalman filter to nonlinear systems. The
11th international symposium on aerospace/defense sensing, simulation and controls. Orlando
FL, USA
19. Sheng W, Tan L (2009) Fast data fusion method for MGNC integrated navigation system[J]. J
Beijing Univ Aeronaut Astronaut 35(6):657–660
Chapter 10
Semi-physical Simulation Technology
of INS/CNS/GNSS Integrated Navigation

10.1 Introduction

During the research process of INS/CNS/GNSS integrated navigation technology,

the test and verification of integrated navigation theory, method, and its engineering
realization technology are also very important. At present, three methods, computer
simulation, semi-physical simulation, and physical test, can generally be provided
for test and verification of integrated navigation. Computer simulation can be used
for the verification of theoretical approach correctness with the features such as
easy and convenient operation, etc. But effective verification of engineering appli-
cation technology cannot be achieved. Physical test can effectively test and verify
the engineering application technology of system in real environment, but is has
disadvantages of difficulty, high expense, and long period length, etc. Therefore, the
initial test on SINS/CNS/GNSS integrated navigation technology usually employs
laboratory semi-physical simulation technology to conduct experimental research
[1].
Extensive researches in terms of semi-physical simulation of integrated naviga-
tion system have been carried out abroad. The navigation systems for many severe
environments such as underwater, etc. commonly use semi-physical simulation tech-
nology to conduct tests and experiments [2]. In recent years, NASA has begun to
use semi-physical simulation technology on demonstration [3, 4] of formation flying
of air vehicles. In domestic, semi-physical simulation system researches [5–8] have
been conducted in terms of SINS, GNSS, and CNS, and semi-physical simulation
technology has been used in guidance system of many advanced weapons to test and
verify the algorithm performance [8]. How to build high-performance semi-physical
simulation system of SINS/CNS/GNSS integrated navigation is one challenge of
effectively verifying the algorithm performance of integrated navigation and system
features.
This chapter will introduce a universal semi-physical simulation system of
SINS/CNS/GNSS integrated navigation from four aspects, namely principle, com-
position, realization, and test.

© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 331
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_10
332 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Satellite
Trajectory generating Inertial navigation
Signal acquisition Data processing
subsystem measurement subsystem
module module
unit

Strapdown inertial subsystem

Star map
Integrated
Star Image
map Colli identification
simulati LCLV
Optical processi
and attitude navigation
on mator sensor ng
determination
module module
module subsystem

Star sensor simulator

Display device
Star map simulator

Display device

Celestial subsystem

Fig. 10.1 General block diagram of semi-physical simulation system based on SINS/CNS/GNSS
integrated navigation

10.2 Principle and Composition of Semi-Physical Simulation

System of INS/CNS/GNSS Integrated Navigation

10.2.1 Principle of Semi-Physical Simulation System of

INS/CNS/GNSS Integrated Navigation

The design requirement of SINS/CNS/GNSS integrated navigation system varies

with different application objects [9]. In general, overall design of semi-physical
simulation system based on SINS/CNS/GNSS integrated navigation should be
determined by test requirement of integrated navigation system performance. Re-
fer to Fig. 10.1 for overall design of semi-physical simulation system based on
SINS/CNS/GNSS integrated navigation.
Such semi-physical simulation system of SINS/CNS/GNSS integrated navigation
mainly involves trajectory generating subsystem, SINS subsystem, CNS subsystem,
GNSS subsystem, and integrated navigation system. Its information transmission
and control flow are as shown in Fig. 10.2.
Specific flow is as follows:
1. System initialization: After the system is powered on, initialize star-map
simulation parameter and trajectory generating subsystem parameter.
2. Nominal trajectory data generation: Trajectory generating subsystem generates
nominal trajectory data as reference source of information processing for semi-
physical simulation system of SINS/CNS/GNSS integrated navigation for unified
10.2 Principle and Composition of Semi-Physical Simulation System. . . 333

Static GNSS data receiving

Initial trajectory setting

Mean value removal and true noise acquisition

Optical axis pointing data of standard Generation of nominal trajectory by trajectory generating
Standard trajectory velocity and position
carrier attitude and star sensor subsystem

Static SINS data

Star map simulation Starlight map
SINS component output receiving
initialization simulator
corresponding to trajectory

GNSS
Generation of star map image corresponding to optical Standard output subject to
static SINS removal, true subsystem
axis
noise acquisition

Sensing of star sensor simulator

SINS output with true error
characteristic GNSS output with true error

SINS characteristic
Star map processing and attitude
determination subsystem
CNS
Strapdown calculation velocity and position
subsystem

Integrated navigation subsystem

Integrated navigation data output

Navigation error output

Fig. 10.2 Information transmission and control flow chart of semi-physical simulation system of
SINS/CNS/GNSS integrated navigation

and standard input; such nominal trajectory data is transmitted to three subsystems
on one hand and serves as nominal data of integrated navigation error analysis
on the other hand to be output to integrated navigation system through serial
communication interface.
3. Observation data acquisition of subsystem SINS, CNS, and GNSS:
1. SINS subsystem senses rotational angular velocity of the earth (a constant
value), outputs measurement data of true components through serial commu-
nication interface, obtains true error characteristic data of SINS subsystem
after subtracting the constant value of rotational angular velocity of the earth
subject to data smoothing, and forms observation data of SINS subsystem nec-
essary for integrated filtering subject to superposition of nominal trajectory
data.
2. Produce optical axis point data of star sensor by combining installation ma-
trix of star sensor according to nominal trajectory data. Star-map simulation
module of CNS subsystem generates simulated star-map under specific field
of view according to optical axis pointing at each moment, and then outputs
to liquid crystal light valve (LCLV) through parallel interface to convert dig-
ital information of star map into electrical signal and complete simulation
334 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

of starlight at infinity; the starlight is sensed by optical sensor, generated by

original star-map image through image processing module, subject to star-map
preprocessing and star-map identification processing within star-map identi-
fication and attitude determination module, and combines with quaternion
algorithm to complete attitude determination of three axes of the carrier and
provides integrated navigation system with attitude observation data of CNS
subsystem.
3. GNSS subsystem subtracts the mean value from static measurement data and
then forms observation data of GNSS subsystem necessary for integrated
filtering subject to superposition of nominal trajectory data.
4. Integrated filtering and performance test: Integrated navigation system receives
output data of SINS, CNS, and GNSS subsystem in the way of serial com-
munication interface, completes integrated filtering after time-synchronization
preprocessing according to integrated logical signal, and realizes system perfor-
mance test according to nominal trajectory data generated by trajectory generating
subsystem and output result data of integrated navigation [1].
Utilization of semi-physical simulation system based on SINS/CNS/GNSS inte-
grated navigation can effectively verify experiment and test of SINS, CNS, and
GNSS subsystem, and semi-physical simulation test and verification of SINS/CNS,
SINS/GNSS, and SINS/CNS/GNSS integrated navigation methods and system.

10.2.2 Composition of Semi-Physical Simulation System of

INS/CNS/GNSS Integrated Navigation

Figure 10.3 is hardware block diagram of a universal semi-physical simulation system

of SINS/CNS/GNSS integrated navigation.
Among them, trajectory generating subsystem A is mainly realized by special
PC machine and trajectory generator. CNS subsystem mainly consists of star-map
simulator B and star sensor simulator C. SINS subsystem D mainly consists of IMU
and data processing circuit. GNSS subsystem E mainly consists of satellite navigation
receiver and receiving antenna. Integrated navigation subsystem F is mainly realized
by special PC machine and integrated navigation software.
Software interface of semi-physical simulation system of SINS/CNS/GNSS
integrated navigation is designed according to test performance requirement of
SINS/CNS/GNSS integrated navigation system and functional features of semi-
physical simulation system based on such integrated navigation, as shown in
Fig. 10.4. Information including initial position, initial attitude angle, initial mis-
alignment angle, component error, system noise variance matrix, measurement noise
variance matrix, and initial state variance matrix of integrated navigation may be set
flexibly through the interface.
Trajectory generator and star-map simulator are two important components in the
whole semi-physical simulation system of SINS/CNS/GNSS integrated navigation,
10.2 Principle and Composition of Semi-Physical Simulation System. . . 335

Star map
simulator Graphic card
of OK series
A
Colli Sensitive Star map identification
LCLV probe and attitude determination
Star map simulating mator PC machine C
Trajectory PC machine B Star sensor simulator

PC machine
Receiving antenna
z
F
Data
proces Satellite
navigation
Y sing receiver
circuit
E
X IMU
D Integrated navigation PC machine

Fig. 10.3 Hardware composition structure diagram of semi-physical simulation system of

SINS/CNS/GNSS integrated navigation

Fig. 10.4 A software interface of semi-physical simulation system of SINS/CNS/GNSS integrated

navigation

and as basis of semi-physical simulation research of SINS/CNS/GNSS integrated

navigation, the former is used for motion curve of simulated flying machine and
to generated nominal output of inertial components; the latter provides star sensor
336 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

simulator with real-time and rapid information on parallel starlight at infinity. The
following mainly focuses on brief introduction to the two components.
1 Trajectory Generator

Trajectory generators of different flying machines are different since they adopt
different navigation coordinate systems. The following will take ballistic missile
(inertial coordinate system of launching point), aircraft (geographic coordinate sys-
tem), and satellite (orbital coordinate system) for instance respectively to introduce
corresponding trajectory generators [10–13].
1) Trajectory generator of ballistic missile
The main concept for trajectory generator of ballistic missile is to first obtain tra-
jectory data such as real-time position, velocity, and attitude under gravitational
coordinate system of the launching point through trajectory equation calculation
according to preset initial information (including initial position, velocity, attitude,
launching azimuth angle, and flight time during powered phase, etc.). Then the data
is converted to inertial coordinate system of the launching point to serve as nominal
trajectory data, and finally, obtain output-specific force and angular rate of inertial
component subject to backstepping according to certain kinematical relation to serve
as nominal data output of the accelerometer and gyro. Among them, gravitational
coordinate system of the launching point refers that the origin is the launching point,
axis Y points at external earth’s surface along gravitational reverse direction of the
launching point, axis X is perpendicular to axis Y and points at launching direction,
and axis Z is determined as per right hand rule.
Flight path of ballistic missile generally consists of powered, free, and reentry
phase. Ballistic missile is boosted to required velocity and height during powered
phase, flies along elliptical orbit under gravitational force only during free-flight
phase, and is influenced by aerodynamic force as free reentry body during reentry
phase. Since no control is imposed upon ballistic missile during free flight phase,
its orbit is determined by guidance and control system of powered phase. Therefore,
trajectory generator design of ballistic missile is to design the powered phase primar-
ily. Influences of waggle of propellants and vibration of projectile bodies are ignored
here, and missile is considered as rigid body to establish and describe kinetic equa-
tions with regard to relations among velocity, position, attitude, and rotation angle of
the missile. Simplify the kinetic equations at varying degrees according to practical
situations and the simplified form is as follows:

⎧
⎪
⎨ v̇ = (P − Xd )/m − g sin θ
⎪
ẋ = v cos θ (10.1)
⎪
⎪
⎩
ẏ = v sin θ
10.2 Principle and Composition of Semi-Physical Simulation System. . . 337

⎧ g = g0 (R/r) , r= x 2 + (R + y)2 , H = r − R,
2
Wherein
⎪
⎪ t ≤ t1
⎨ θ0
θ = (θ0 − θk )(t2 − t)2 /(t2 − t1 )2 + θ k t1 t ≤ t2 ,
⎪
⎪
⎩θ t2 t ≤ tk
k

P is the pushing force, Xd is air resistance, θ is pitching angle under gravitational

coordinate system of the launching point, θ0 is pitching angle at the time of launching,
θk is pitching angle at the end of missile powered phase, g is earth gravity, and R is
earths radius.
Velocity, position, and attitude angle of the missile under gravitational coordinate
system of the launching point can be obtained through the equation, and trajec-
tory parameter under gravitational coordinate system of the launching point must be
transferred to inertial coordinate system of the launching point [11, 13] since gravi-
tational coordinate system of the launching point rotates with the earth: (1) Transfer
to local north, vertical, and east coordinate system by multiplying by transfer ma-
trix Cng according to velocity under gravitational coordinate system of the launching
point, and then transfer velocity under north, vertical, and east coordinate system
to inertial coordinate system of the launching point through transfer matrix Cin . (2)
Obtain position of inertial coordinate system of the launching point through velocity
under inertial coordinate system of the calculated launching point according to first-
order algorithm subject to integral. (3) Obtain attitude angle under inertial coordinate
system of the launching point by multiplying by attitude angle under gravitational
coordinate system of the launching point after working out Cig according to attitude
matrix Cic , Cce , Cen and Cng . Among them, subscripts i, c, e, n, and g represent inertial
coordinate system of the launching point, and north, vertical, and east coordinate
system, gravitational coordinate system of the launching point, geocentric inertial
coordinate system, and terrestrial coordinate system, respectively.
In addition, after trajectory parameters under inertial coordinate system of the
launching point are defined, output-specific force data of the accelerometer and out-
put angular velocity data of the gyro can be worked out through algorithm: (1) For
calculation of specific force data, first work out absolute acceleration of the carrier
under inertial coordinate system of the launching point during each moment through
first-order algorithm, and then work out the apparent acceleration according to the
relation between apparent acceleration and absolute acceleration of the missile un-
der inertial coordinate system of the launching point; finally, convert it to projectile
coordinate system to work out output-specific force data of the accelerometer. (2)
For calculation of angular velocity data, work out attitude change rate of the car-
rier through first-order algorithm by using previously obtained attitude angle under
inertial coordinate system of the launching point, and then work out output angu-
lar velocity data of the gyro according to the relation between the change rate and
angular velocity.
Figure 10.5 is a software interface for trajectory generator of ballistic missile,
mainly consisting of initial position parameter setting, trajectory parameter time and
angle setting, integrated correction time setting (for integrated navigation system),
flight end point parameter display, etc.
338 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Fig. 10.5 Software interface for trajectory generator of ballistic missile

Refer to reference [11] for relevant detailed contents of trajectory generator of

ballistic missile.
2) Trajectory generator of aircraft
It is similar to trajectory generator of ballistic missile, and its main concept is also
to calculate according to preset initial information (including initial position, veloc-
ity, attitude and flight time, etc.), but it can obtain nominal trajectory data such as
position, velocity, and attitude under geographic coordinate system through direct
calculation of trajectory equation. Finally, it can obtain output-specific force and
angular rate information of inertial component subject to backstepping according to
certain kinematical relation under geographic coordinate system.
Specifically, it determines the position under geographic coordinate system by
using first-order smoothing algorithm for integral of velocity during different time
frames according to input initial position, velocity, and attitude as well as velocity
and attitude change rate information during different time frames, and conducts
integration for attitude change rate during different time frames to obtain attitude
angle of each moment.
Specific to calculation of angular velocity and specific force, first work out nav-
igation position matrix, displaced angular rate, attitude matrix, and attitude matrix
rate according to previously determined navigation trajectory (velocity, position, and
attitude) parameters, and then obtain output angular velocity of the gyro. Second,
determine measurement value of the accelerator under navigation coordinate system
according to specific force equation in strapdown inertial navigation algorithm ar-
rangement, and obtain output of accelerometer under body coordinate system through
attitude matrix.
10.2 Principle and Composition of Semi-Physical Simulation System. . . 339

Fig. 10.6 Software interface for trajectory generator of aircraft

Figure 10.6 is a software interface for trajectory generator of aircraft, mainly

consisting of initialization, sensor setting, trajectory setting, and assistance.
Refer to reference [13] for relevant detailed contents of trajectory generator of
aircraft.
3) Trajectory generator of satellite
Satellite tool kit (STK) is used to generate nominal orbital data necessary for
computer simulation of satellite autonomous navigation and an essential tool for
numerical simulation of satellite navigation [11, 12]. The following is a main intro-
duction to methods of using STK software to generate satellite orbit and attitude data
to provide basic data and reference standard for simulation and performance analysis
of satellite navigation and control system.
a. Generation of satellite orbital data based on STK software
First start STK software to create an engineering; click Satellite icon in STK
interface to establish a satellite mode; then, carry out essential attribute setting
through pull-down menu item “properties: Basic.” Essential attribute setting win-
dow of satellite consists of eight attribute pages, each of which includes multiple
parameter setting options; enter the Page 1 “Orbit” (satellite orbit parameter setting
interface) hereafter, as shown in Fig. 10.7, and refer to reference [11] for detailed
description of common options.
Orbital data may be generated according to orbit parameters having been set. Cor-
responding orbital data contents may give output in two formats. One is document
with output extension of.e format and available to be opened through notepad con-
veniently. The other is Matlan document with output extension of.mat format and
available to display position information of satellite subject to input of satellite.pos
into the command window after importing Matlab as well as velocity information
340 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Fig. 10.7 Orbit parameter setting interface

of satellite subject to input of satellite.vel. There is no doubt that it is also possible

to display trajectory of sub-satellite point of satellite directly in STK view of STK
software; specific operation is simple, so unnecessary details will not be given here.
b. Generation of satellite attitude data based on STK software
Generation method of satellite attitude data is similar to that of orbital data; enter
Page 2 “Attitude” (satellite attitude parameter setting interface) in essential attribute
setting window of satellite, as shown in Fig. 10.8, referring to reference [11] for
description of common options.
Steps for output of attitude data document are basically the same as those of orbital
data output. Data output of quaternion type or quaternion and Euler angular velocity
type may be selected during output.
Refer to reference [11, 12] for relevant detailed contents of trajectory generator
of satellite.
2 Star-Map Simulator

With regard to the aforementioned general semi-physical simulation system of

SINS/CNS/GNSS integrated navigation, data in SINS and GNSS subsystem is rela-
tively easier to be obtained, and its error characteristics can be gained by collecting
static data; for data in CNS subsystem is not that easy to be obtained, and on the
ground, dynamic simulation is required to be conducted in the fixed star map to
provide sensitive information for the subsystem.
10.2 Principle and Composition of Semi-Physical Simulation System. . . 341

Fig. 10.8 Satellite attitude parameter setting interface

1) Principle of star map simulation

Being a simulation system [14–16] providing simulated star-map data for testing the
function of star sensor and verifying recognition algorithm performance of star map,
star-map simulator can, mainly, be divided into six parts, including star-map analog
calculator, frequency divider, display devices, light source system, liquid crystal
display system, and collimator, in which the star-map calculator is equipped with
simulation algorithm and fundamental catalog. It is as shown in Fig. 10.9.
Main purpose of star-map simulation is to provide far distant parallel starlight for
star sensor. By using it, the transformation matrix between celestial coordinate system
and star sensor coordinate system, as well as the transformational relation between
the star sensor coordinate system and sensor area array, should be confirmed. Since
the star sensor is fixedly installed in the carrier, transformation matrix between the
carrier coordinate system and star sensor coordinate system is clear, and the relation
between celestial coordinate system and star sensor coordinate system can be deemed
as the only factor to be considered. Make O-UVW express celestial coordinate system
and O -XYZ express star sensor coordinate system. Assuming that O overlaps with
O, the relationship between the two coordinate systems can be shown in the formula
below:

[X, Y , Z]T = M[U , V , W ]T (10.2)

wherein, M refers to the rotation matrix.

342 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Area light Liquid-crystal display system

source Liquid-crystal
display panel
Driver board
Collimator
Light source system
Frequency
divider

Display devices

Star map simulation Fundamental

algorithm catalog
Star map analog calculator

Fig. 10.9 Simple diagram for star-map simulator

Rotating as per the sequence of 3–1–3, with a rotation angle of φ, θ, and ψ

respectively, the rotation matrix M, can be shown as follows:
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
cos ψ sin ψ 0 1 0 0 cos φ sin φ 0
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
M=⎢ ⎥ ⎢
⎣− sin ψ cos ψ 0⎦ × ⎣0 cos θ sin θ ⎥ ⎢
⎦ × ⎣− sin φ cos φ 0⎦ =
⎥

0 0 1 0 − sin θ cos θ 0 0 1
⎡ ⎤
cos φ cos ψ − sin φ sin ψ cos θ sin φ cos ψ + cos φ sin ψ cos θ sin ψ sin θ
⎢ ⎥
⎢−cos φ sin ψ − −sin φ cos ψ cos θ −sin φ sin ψ + cos φ cos ψ cos θ cos ψ sin θ⎥
⎣ ⎦
sin φ sin θ −cos φ sin θ cos θ
(10.3)

Assuming that (α0 , δ0 ) is the direction of star sensor optical axis in the celestial
coordinate system, ψ = 90◦ , θ = 90◦ − δ0 (included angle of the optical axis and
OW axis), and Φ = 90◦ + α0 (included angle of OX axis and OU axis); then put the
value of ψ, θ , and φ into the rotation matrix M, the following can be gained:
⎡ ⎤
− sin δ0 cos α0 − sin δ0 sin α0 cos δ0
⎢ ⎥
M=⎢ ⎣ sin α0 − cos α0 0 ⎥ ⎦ (10.4)
cos δ0 cos α0 cos δ0 sin α0 sin δ0

The principle of confirming position coordinates (x, y) of the star in focal plane by
observing its unit vector in the star sensor coordinate system is as shown in Fig. 10.10.
10.2 Principle and Composition of Semi-Physical Simulation System. . . 343

Fig. 10.10 Ideal model for

position of the fixed star in Sky YAxis
focal plane X,Y
XAxis

f
0,0 x Axis
（x,y） （ ）
yAxis
Fig. 10.11 Relation scheme Field of view
for focal length and field of
view

Q
f

P O Ny

Nx R
Sensor area
array

It can be obtained in accordance with the geometrical relationship of similar

triangles:
x × dh X⎫ f ×X⎪
⎫
= ⎪⎪ x= ⎪
f Z⎬ dh × Z ⎬
⇒ (10.5)
y × dv Y⎪ f ×Y ⎪
= ⎪⎭ y= ⎪
⎭
f Z dv × Z
Wherein, (x, y) stands for position of the fixed star in the sensor plane; f refers to
focal length of star sensor in the optical system; dh , dv are width and height of the
pixel respectively.
The field of view is FOVx × FOVy , while the sensor area array is Nx × Ny , in
which Nx , Ny is the number of pixels per line/column in the plane. The focal length
f, from the geometrical triangle relationship in Fig. 10.11, is calculated to be:

N x · dh Ny · dv
f = = (10.6)
2 tan ( FOV
2
x
) 2 tan (
FOVy
2
)
344 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Observing star selected from the fundamental catalog includes the right ascension
and declination, mapping of which into the unit sphere can help obtain the expression
below, namely unit vector of the observing star in the celestial coordinate system:
⎡ ⎤ ⎡ ⎤
U cos αi cos δi
⎢ ⎥ ⎢ ⎥
⎢ V ⎥ = ⎢ sin αi cos δi ⎥ (10.7)
⎣ ⎦ ⎣ ⎦
W sin δi

In conclusion, the expression [16–18] for confirming position (x, y) of the observing
star in the sensitivity plane is:
Nx 1 cos δi sin (αi − α0 ) ⎫
x= × × ⎪
⎬
2 tan (FOVx /2) sin δi sin δ0 + cos δi cos δ0 cos (αi − α0 ) (10.8)
Ny 1 sin δi cos δ0 − cos δi sin δ0 cos (αi − α0 ) ⎪
y= × × ⎭
2 tan (FOVy /2) sin δi sin δ0 + cos δi cos δ0 cos (αi − α0 )

As can be seen from the aforementioned formula, under the condition that direction
of the optical axis (α0 , δ0 ) is confirmed, number of pixels and field-of-view size can
be used to show the coordinate of any observing star within the field of view (αi , δi )
after being mapped into the sensor plane.
2) Method for quick selection of observing star
All observing stars within a specific field of view and specific optical axis should
be confirmed for simulating a complete star map. Let’s take a look at a method for
quick selection of observing stars [15] based on calculating the span of ascension
through a circular section.
Under the condition that direction of the optical axis and field of view is confirmed,
regional area on the unit celestial sphere, in general, is changeless by observing with
optical lens. Given that the optical axis is moved from equator to south (north) pole,
span of ascension within the region can be deemed equal but related to declination
value of the current optical axis. Hence, when the optical axis is certain, in order
to select the observing stars within the field of view, span of right ascension in the
region should be confirmed first. As shown in Fig. 10.12: the line segment O O is
perpendicular to the equatorial plane, as well as the circular section (parallel to the
equatorial plane) with O as the center,  PQO is declination (δ0 ) of the current optical
axis, and longitudinal scope of the circular section is 360◦ . Assuming that radius of
quasidisk for the equator is R, while radius of the circular section is r, in that way,
circumference of the circular section will be L = 2π R cos δ0 , and r = R cos δ0 .
When direction of the optical axis is confirmed, calculate the span range of right
ascension, for which merely the arc length of section covered by the region (S) is
required to be calculated. Arc length of the section covered by Region S may be the
following three conditions as shown in Fig. 10.13.
10.2 Principle and Composition of Semi-Physical Simulation System. . . 345

Fig. 10.12 Section of parallel

equatorial plane in a specific
z
optical axis
P O′ Q
r

O δ0
R y

A B B
A B
E O′ F
G O′
O′
C D
C O D O
O C D
E G F
a b c

Fig. 10.13 Three conditions for arc length of the section covered by S

As shown in Fig. 10.13, assuming that width of the Region S is W = AB = CD,

while the height is H = AC = B; and in the three conditions, namely (a), (b) and (c),
the radius is all r, O is the center of S, O is center of the circular section, and length
of the segment line OO is equal to radius r of the circular section. Now discussion
should be conducted in the following situations.
3) When r > H/2, it is as shown in Fig. 10.13a.
Assuming that  FOG = a, and it is known that OF = r, EF = AB = W = 2GF; in this
case, sina = GF/OF, namely that a = arcsin(W/ 2r), and  EOF = 2a (corresponding
central angle of arc length EO F = y), so y = 2ar, namely that:


W
y = 2 arcsin R cos δ0 (10.9)
2R cos δ0

In this condition, span of the right ascension will be:

360 L L
= = (10.10)
Range y 2 arcsin (W/2R cos δ0 ) · R cos δ0
346 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Namely:


360 × 2 arcsin (W/2R cos δ0 )R cos δ0 360 W
Range = = arcsin
L π 2R cos δ0
(10.11)
(1) When r=H/2, it is as shown in Fig. 10.13b.
It is observed intuitively from Fig. 10.13b that range of right ascension with
Region S is 180◦ .
(2) When r < H/2, calculation of the range of its right ascension is the most
complex, as shown in Fig. 10.13c.
Suppose  FOG to be α, since it is known that OF = r, GO = H/2 = r + OG, there
is cos α = OG/OF, i.e., α = arccos[(H/2 − r)/r], and arc length EO F = y, i.e.:

 
H
y = 2 arccos − R cos δ0 /R cos δ0 · R cos δ0 (10.12)
2
In such case, arc length Rangemin outside Region S is:
360 L L
= =  H   (10.13)
Rangemin y 2 arccos 2 − R cos δ0 /R cos δ0 R cos δ0

 
H
Rangemin = (360/L) · 2 arccos − R cos δ0 /R cos δ0 · R cos δ0
2
360 H /2 − R cos δ0
= arccos (10.14)
π R cos δ0
So the span of right ascension is:
Range = 360 − Rangemin (10.15)
In conclusion, span of the right ascension covered by Region S can be expressed as:
⎧ W 
⎪
⎨360 × arcsin 2 R cos δ0 /π,
⎪ R cos δ0 > H /2
Range = 180, R cos δ0 = H /2
⎪
⎪  H  
⎩
360 − 360 × arccos 2 − R cos δ0 /R cos δ0 /π, R cos δ0 < H /2
(10.16)
Suppose direction of optical axis to be (α0 , δ0 ), range of the right ascension deter-
mined through above method according to declination δ0 to be Range, so selection
method of observing star is: suppose right ascension and declination value of certain
star to be (α, δ), and it may be selected as observing star only if value of α, δ satisfy
the following conditions.



Range <   F OVy Range Range ⎫⎪
|α − α0 | ≤ |δ − δ0 | ≤ , ≤ α0 < 360 − ⎪
⎪

2 

2
2  2 ⎪
⎪
F OVy < Range = Range Range ⎬
|δ − δ0 | ≤ α > α0 − α ≤ α0 + − 360 , α0 ≥ 360 − (10.17)

2 

2 
2  2 ⎪
⎪
F OVy < Range = Range Range ⎪
⎪
⎪
⎭
|δ − δ0 | ≤ α ≤ α0 + α ≥ α0 − + 360 , α0 <
2 2 2 2
10.3 Realization and Test of Semi-Physical Simulation System. . . 347

Select satisfied observing star from the complete fundamental catalog according to
formulas above, and such star should be available to be observed by star sensor.
Factors such as various disturbances, noises, distortions and deviations should be
considered during practical star-map simulation through computer to make their
position and brightness at bright spot of simulated star represented.
4) Design and realization of star map simulator software
Flow of star-map simulator software is as shown in Fig. 10.14.
Write star-map simulator software according to above software design flow; first
design structure of the program according to functional requirements of software,
interactive mode and features of program execution, and complete algorithm imple-
mentation of above theoretical method and software flow. Selection of appropriate
procedural programming language and platform is of great significance for improve-
ment of procedural programming efficiency, compatibility and transportability. To
use existing resources farthest and reduce workload of procedural programming,
object-oriented programming method is generally adopted; Fig. 10.15 is realiza-
tion interface of star-map simulator software (star map with optical axis pointing of
◦ ◦ ◦ ◦
(0 , 90 ) and field of view of 8 × 6 is simulated).
Display parameter setting of star-map simulator software is as shown in Fig. 10.16;
it can set size of field of view of simulated star map, gray value of star point and size
of star conveniently, and join pseudo star randomly or set random missing star.
Basic function of such star-map simulator software includes static simulation
function and dynamic simulation function. Figure 10.17 is static simulation parame-
ter setting interface, including setting right ascension, declination value of simulated
observation center and whether system error addition is required. Whether shaking
optical axis will be set during carrier flight process; shaking scope of optical axis is
◦
generally set as 1 , and shaking frequency is preferred to be less than frequency of
star-map simulation, Fig. 10.18 is dynamic simulation parameter setting interface;
constellation sensed by star sensor during flight process of the carrier can be simu-
lated if fixed installation matrix and orbital data of star sensor installed on the carrier
are given.

10.3 Realization and Test of Semi-Physical Simulation System

of INS/CNS/GNSS Integrated Navigation

It is known from Sect. 10.2 that the semi-physical simulation system of

SINS/CNS/GNSS integrated navigation consists of SINS subsystem, CNS subsys-
tem, GNSS subsystem, integrated navigation computer, trajectory generator and
relevant software, etc. Semi-physical simulation system of SINS/CNS/GNSS inte-
grated navigation adopts SINS, GNSS physical system, star-map simulator, and star
348 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

System initialization

Size of filed of view and initial optical axis

pointing setting

Observing star selection scope calculation

according to preset size of filed of view and
optical axis pointing

Quick selection of observing star from

fundamental catalog
Fundamental catalog

Calculation of position coordinates of each

observing star on sensitive matrix according to
star map simulation principle

Display of each observing star to form star Star display attribute

map according to position coordinates setting

Y Y
Dynamic simulation of star Reading of next moment of
map? optical axis data and not null ?

N N

End

Fig. 10.14 Flow block diagram of star-map simulator software

10.3 Realization and Test of Semi-Physical Simulation System. . . 349

Fig. 10.15 Software interface of star-map simulation

Fig. 10.16 Display parameter

setting window

Fig. 10.17 Static simulation

parameter setting interface

sensor simulator as physical models to obtain true error characteristics. Trajectory

generator generates necessary nominal trajectory data and transits to integrated nav-
igation computer through RS232 serial port together with SINS, CNS, and GNSS
subsystem data to conduct integrated navigation operation and output results.
350 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Fig. 10.18 Dynamic

simulation parameter setting
interface

10.3.1 Realization of Semi-physical Simulation System of

SINS/CNS/GNSS Integrated Navigation

The following is a main introduction to basic composition, data collection, and test
condition of navigation subsystem SINS, CNS, and GNSS.
1 Inertial Subsystem

As shown in Fig. 10.19, SINS subsystem consists of gyro, accelerometer assembly,

power module, temperature control module, rebalance loop (flexible gyro), inertial
navigation computer, I/O communication module, and relevant circuit. After SINS

Fig. 10.19 Composition of

SINS subsystem Power module

Temperature
control module

Gyro,
accelerometer
and
Rebalance loop specific
relevant
to flexible gyro
circuit

Inertial navigation
computer

I/O communication module

10.3 Realization and Test of Semi-Physical Simulation System. . . 351

subsystem is powered on, the gyro and accelerometer output stable signal subject to
a period of preheating, transmit to integrated navigation computer through inertial
navigation computer and I/O communication module, and obtain velocity, position,
and attitude information of the carrier subject to calculation of integrated navigation
computer.
SINS subsystem includes three gyros with single degree of freedom and three ac-
celerometers, which are installed along three orthogonal axial directions to measure
angular rate and acceleration of the carrier along the three axial directions. Original
output data of SINS subsystem in engineering practice is generally output angular
incremental data of the gyro and output specific force incremental data of the ac-
celerometer. The two incremental data is relevant to sampling rate. For instance,
when sampling rate is 100 Hz, angular increment information is angle increment
produced within 0.01s, which is equivalent to 0.01 times of angular rate in theory;
similarly, theoretical value of its specific force increment is 1/100 of specific force
sensed by SINS subsystem.
In theory, when SINS subsystem is at rest, angular rate sensed by the gyro is
just rotational angular rate of the earth, and its output is angular rate or angular in-
crement value influenced by various measurement noises and disturbance torques.
During simulation calculation, simulation for such noises and disturbances is gen-
erally theoretical value with superposition of certain white noise, markoff process,
random constant value or other irregular colored noise. However, output angle in-
crement information of SINS subsystem is influenced by minimum resolution in
practice; angle increment less than minimum resolution can be detected only after
accumulating large enough when sensed by the gyro, so the output angle increment
information includes pulsing jumps instead of being smoothing. Such quantization
error caused by minimum resolution is much greater than measurement noise and
gyro drift error of any type when the system is at rest.
Error amplitude of any axial angle increment data can reach ten times of theoretical
value within 0.01s when it is at rest due to influence of above-mentioned factors, i.e.,
the error magnitude can reach 1000 % for any sampling point. Subject to smoothing
(taking mean value) of sampling data during a certain period, the result is relatively
identical to theoretical value. Take mean value for data of each 1s, and the error is
about 10 %; take mean value for data of each 10 s, and the error is about 1 %. There
is also quantization error caused by minimum resolution for output specific force
incremental data of accelerometer, but its influence on calculation process of SINS
is not great since its minimum resolution is very small.
Figures 10.20 and 10.21 are original data output and data output subject,
respectively, to 10 s of data smoothing for certain type of SINS subsystem.
The statistical result is as shown in Table 10.1 according to output data in
Figs. 10.20 and 10.21.
Data fluctuation of gyro and accelerometer of SINS is generally intense be-
fore reaching steady state, but the data can satisfy use requirement after reaching
steady state. It is observed from Table 10.1 that gyro drift shown in Fig. 10.21 is
352 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Gyro output of axis x ( /h)

Gyro output of axis z ( /h)

Gyro output of axis y ( /h)
°

°
°
Time (s) Time (s) Time (s)

Accelerometer output of axis z

Accelerometer output of axis y
Accelerometer output of axis x

(m/s )
(m/s )
(m/s )

2
2
2

Time (s) Time (s) Time (s)

Fig. 10.20 Original data graph of SINS subsystem output

Gyro average output of
Gyro average output of
Axisx within 10s (°/h)

axis y within 10s ( /h)

Gyro average output of

axis z within 10s ( /h)
°

Time (s) Time (s) Time (s)

Accelerometer average outpu
t of axis y within 10s (m/s2)
Accelerometer average output

Accelerometer average output

of axis x within 10s (m/s2)

of axis z within 10s (m/s )

2

Time (s) Time (s) Time (s)

Fig. 10.21 Data graph of SINS subsystem output (take the mean as per 10 s)

◦
about 0.2 /h(1σ), null bias of accelerometer is about 100 μg(1σ), but gyro drift and
accelerometer bias will vary with time within certain scope.
2 Celestial Subsystem

As shown in Fig. 10.22, CNS subsystem [16] mainly consists of starlight simulator
and star sensor simulator. Starlight simulator consists of star-map simulation termi-
nal and star sensor simulator consists of sensor sensing head, graph acquisition card,
star-map preprocessor and identification terminal. Star-map simulation terminal is
10.3 Realization and Test of Semi-Physical Simulation System. . . 353

Table 10.1 Output data of SINS subsystem

Average within 10 s
Mean value Gyro X − 0.0131 ( /s)
Gyro Y 11.5546 ( /s)
Gyro Z 9.6607 ( /s)
Accelerometer X 0.0028004 (m/s2 )
Accelerometer Y −0.0028004 (m/s2 )
Accelerometer Z 9.80011 (m/s2 )
Mean square error (1σ) Gyro X 0.19535 ( /s)
Gyro Y 0.21603 ( /s)
Gyro Z 0.20482 ( /s)
Accelerometer X 9.8019e-004(m/s2 )
Accelerometer Y 9.5026e-004(m/s2 )
Accelerometer Z 9.4904e-004(m/s2 )

Starlight Star sensor simulator

simulator
Graph acquisition card
Sensor sensing head
Star map simulation
LCLV Star map preprocessor and
terminal identification terminal

Fig. 10.22 Composition of CNS subsystem

equipped with fundamental star catalogue and has function of map generation. Ac-
cording to the trajectory generated by trajectory generator, alternative observational
star along the way can be generated; the gray level of observational star can be chosen
on program interface to simulate star, etc. LCLV is used to simulate paralleled star
starlight at infinity; this starlight is acquired by sensor sensing head and is transmitted
to star-map preprocessor and identification terminal through graph acquisition card.
Star-map preprocessor and identification terminal finish the attitude calculation of
carrier through star-map pretreatment, extraction of center of mass, identification,
and attitude determination.
Starlight simulator is necessary simulating equipments required by semi-physical
simulation system of SINS/CNS/GNS integrated navigation in laboratory. Its core
part is LCLV and TFT LCLV with the resolution of 1024 × 768 pixel. It uses
polycrystalline silicon technology to inlay LCD driver circuit on the base plate.
It enjoys features such as small, light, high reliability, etc; the contrast can reach
354 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

200:1; the highest star magnitude of 7 m can be simulated and the accuracy is ± 0.1
magnitude star.
Star sensor simulator mainly consists of sensor sensing head, graph acquisition
card, star-map identifier and attitude determination processor. On the choice of op-
tical sensor, CCD instead of CMOS camera is chosen here; this is mainly because
the sensitivity of CMOS camera is relatively low, and it can only be sensitive to five
magnitude star, which cannot meet the requirements of verification algorithm and
system function. For the object to which the sensor is sensitive is special star light, this
system employs black and white CCD camera, size 1/2 , resolution 1300 × 1024,
minimum illumination 0.2 lux, progressive scanning method and CCD chip which
can be triggered by random events and performance of 22 frame/s, etc. Graph pro-
cessing module is achieved by black and white graph acquisition card. Domestic
black and white graph acquisition card of OK series employs standard input and
supports synchronized traveling-field recombination and traveling-field segregation.
Progressive/interlacing, number of scanning lines, field frequency, and 8 bits high-
level A/D can be measured. Hardware of 8, 24, and 32 bits can be acquired directly
and the biggest acquisition frequency ≥ 40 M and 256 gray level.
The following graph is collected static test data of CNS subsystem. Static test
◦ ◦ ◦ ◦
conditions: optical axis pointing is fixed at (82.126 , 0 ), field of view is 8 × 6 ,
star magnitude is ≤ 6.95 m, and experiment time is 158 s. The celestial navigation
subsystem used here has relatively big system noise and measurement noise, there-
fore the error of output attitude information is relatively big, and it is difficult to
meet the requirements of semi-physical simulation. For the nominal data of attitude
is known, the noise data output by CNS subsystem which has real characteristic can
be compressed to meet the requirements of integrated navigation system on accu-
racy of CNS subsystem. See Fig. 10.23 for performance curve of compressed static
experiment. Its error mean values of pitching angle, azimuth angle and roll angle are
6.528 , 6.817 , and 5.459 , respectively.
3 Satellite Subsystem

As shown in Fig. 10.24, GNSS subsystem is mainly composed of GNSS receiving

antenna and GNSS receiver. GNSS receiver is composed of frequency conversion
module, code-synchronization circuit, signal demodulation module, positioning and
calculation module, and I/O communication module. In an open environment, GNSS
navigation satellite signal is received through GNSS receiving antenna, and the carrier
velocity and location information can be got through calculation of GNSS receiver
for that signal.
In the semi-physical simulation system of SINS/CNS/GNS integrated navigation
introduced in this chapter, high-accuracy dual-frequency carrier phase GPS receiver
manufactured by JAVAD is used. The following Fig. 10.25 is data under the earth
coordinate system output by that GPS receiver. The measurement time is 400 s and
the GPS data is output one time per second.
Table 10.2 is the output result of the GPS receiver.
10.3 Realization and Test of Semi-Physical Simulation System. . . 355

Azimuth angle error

Second of angle

Pitching angle error t (s)

Second of angle

Roll angle error t (s)

Second of angle

t (s)

Fig. 10.23 Performance curve of static experiment of CNS subsystem

Semi-physical simulation system of SINS/CNS/GNS integrated navigation, as

shown below in Fig. 10.26, is built on basis of aforementioned three navigation sub-
systems of SINS, CNS, and GPS. SINS/CNS/GNS integrated navigation computer
employs PC104 embedded PC machine and is equipped with integrated navigation
algorithm.
In actual system and the semi-physical simulation system, for the SINS, CNS, and
GPS subsystems have different output frequencies, and CNS and GPS subsystems
may have failure or short-time data interrupt problems in actual environment, which
may lead the phenomenon of unmatched SINS and CNS, GPS data, namely desyn-
chronized observed quantity problem; this needs synchronized treatment of various
subsystems. When the optimal filtering technique is used to conduct integrated nav-
igation of SINS and CNS, GPS, the observed quantity of optimal filtering usually
356 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Fig. 10.24 Composition of

CNSS subsystem
GNSS receiving antenna

Frequency conversion
module

Code Signal
synchron demodul
ization ation
circuit module

Positioning and calculation

module

I/O communication
module

takes the difference of navigation parameters calculated by SINS, location measured

by GPS and velocity difference, and difference of navigation platform attitude angle
observed by CNS. The method to solve problem of time-synchronization is usually
the interpolation. For example, consider the general measurement frequency of CNS
and GPS is 1Hz, while sampling frequency of SINS data is 100 Hz, therefore, inter-
polation of SINS data adjacent to the whole second is more reasonable. There are
two kinds of methods of interpolation of SINS data: the first is the original SINS data
package of interpolation, and the second is the navigation parameters calculated by
interpolation pure SINS. Consider that the SINS calculating is an integration pro-
cess while the original single angle increment and the measurement data of force
increment have great quantization error, therefore, no matter consider it from the
complexity of the interpolation calculation procedure or the accuracy of interpola-
tion calculation, interpolate the navigation parameters calculated by SINS and the
resulting navigation parameters at the whole second is more reasonable and effective.
Brief introduction to composition logic problem of desynchronized observed
quantity for time update is as follow (as shown in Fig. 10.27). It should be set that
the navigation subsystem should be considered to be effective when some navigation
subsystem data can be received.
1. The navigation operation will stop when SINS subsystem is invalid.
10.3 Realization and Test of Semi-Physical Simulation System. . . 357

X directional velocity Y directional velocity Z directional velocity

m/s

m/s
m/s

t (s) t (s) t (s)

X directional locatiion Y directional locatio
on Z directional locatio
on
Meter
Meter

Meter
t (s) t (s) t (s)

Fig. 10.25 GPS output data

Table 10.2 GPS data output results

Velocity (m/s) Location (m)
Mean value X − 0.0043 − 2171822.629
Y 0.01081 4385565.634
Z 0.0263 4076644.415
Mean square X 0.06 1.45
deviation Y 0.1 1.9
Z 0.2 4.59

2. The navigation operation will begin when SINS subsystem is valid:

a. SINS/GPS integrated navigation operation will be conducted when GPS
subsystem is valid and CNS subsystem is invalid;
b. SINS/CNS integrated navigation operation will be conducted when GPS
subsystem is invalid and CNS subsystem is valid;
c. SINS/CNS/GPS integrated navigation operation will be conducted when GPS
subsystem and CNS subsystem are valid at the same time;
d. SINS pure strapdown calculating should be conducted when GPS subsystem
and CNS subsystem are invalid at the same time.
358 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

GPS subsystem Celestial subsystem

PC104 integrated
navigation SINS subsystem
computer

Fig. 10.26 Physical map of semi-physical simulation system of SINS/CNS/GNS integrated

navigation

SINS

NO
CNS YES
SINS/CNS
Valid?
SINS
NO
SINS YES GPS YES CNS YES
GPS SINS/CNS/GPS
Valid? Valid? Valid?
NO NO
CNS
Stop SINS/GPS

Fig. 10.27 Logic diagram of integrated navigation procedure

When it is pure SINS, SINS/CNS, SINS/GPS, SINS/CNS/GPS integrated navigation,

the navigation should be completed with the aforementioned corresponding system
state equation and measurement equation in this book to complete navigation.
10.3 Realization and Test of Semi-Physical Simulation System. . . 359

Position curve Longitude (e) Latitude (e)

43 116.8 43

42 116.6 42
Latitude (e)

41 116.4 41

40 116.2 40

39 116 39
116 116.2 116.4 116.6 116.8 0 500 1000 1500 0 500 1000 1500
Velocity of east Longitude (e) Velocity of north Time (second) Vertical velocity Time (second)
direction (m/s) direction (m/s) (m/s)
80 300 0.015

0.01
60
250
0.005
40
0
200
20
-0.005

0 150 -0.01
0 500 1000 1500 0 500 1000 1500 0 500 1000 1500
Azimuth angle (e) Time (second) Pitching angle (e) Time (second) Roll angle (e) Time (second)
-4
x 10
360 4 0.4

0.3
2
355
0.2
0
0.1
350
-2
0

345 -4 -0.1
0 500 1000 1500 0 500 1000 1500 0 500 1000 1500
Time (second) Time (second) Time (second)

Fig. 10.28 SINS/CNS/GNSS integrated navigation trajectory curve

10.3.2 Experiments of Semi-physical Simulation System of

INS/CNS/GNSS Integrated Navigation

Semi-physical simulation experiments should be conducted to test the function of

semi-physical simulation system of SINS/CNS/GNSS integrated navigation and
check the performance of integrated navigation algorithm.
Assume that the simulation trajectory simulates level turning of carrier. The initial
◦ ◦
longitude is 116 , latitude is 39 , orientation misalignment angle is 6 , and level
misalignment angle is 10 ; the drift of gyro, null bias of accelerometer, GPS velocity
and position error, and attitude measurement error of CNS should respectively use the
above error test results; Figs. 10.28 and 10.29 are curve of SINS/CNS/GPS integrated
navigation semi-physical simulation experiment and tendency chart of error curve,
respectively.
From the above figure, it can be seen that SINS/CNS/GNSS integrated navigation
semi-physical simulation conducted here in a large range corrects velocity, location,
and attitude errors caused by misalignment angle, gyro drift, and accelerometer off-
set. The accuracy of attitude angle is equal to accuracy of CNS attitude measurement
360 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

Longitude error Velocity error of east direction

(mile) Latitude error (mile) (m/s)
6 4 1

4 2
0.5
2 0
0
0 -2

-2 -4 -0.5

-4 -6 -1
0 500 1000 1500 0 500 1000 1500 0 500 1000 1500
Velocity error of north Time (second) Vertical velocity error (m/s)Time (second) Azimuth angle error Time (second)
direction (m/s) (second of angle)

1 0.015 20

0.01
0.5 10
0.005
0 0
0
-0.5 -10
-0.005

-1 -0.01 -20
0 500 1000 1500 0 500 1000 1500 0 500 1000 1500
Pitching angle error Time (second) Roll angle error Time (second) Time (second)
(second of angle) (second of angle)
10 40

5
20
0
0
-5
-20
-10

-15 -40
0 500 1000 1500 0 500 1000 1500
Time (second) Time (second)

Fig. 10.29 Curve tendencies of SINS/CNS/GNSS integrated navigation error

when the three combine. The velocity and location accuracies are maintain at the
level of velocity measurement and positioning accuracy of GNSS receiver.
Semi-physical simulation experiment result indicates that the realized semi-
physical simulation system of SINS/CNS/GNSS integrated navigation basically
satisfies performance requirement. Such semi-physical simulation system of
SINS/CNS/GNSS integrated navigation will provide verification platform for high-
performance integrated navigation technology of carrier inside the laboratory. Its
design takes true error characteristics of actual environment into account; it can gen-
erate flight path of various flying machines conveniently through trajectory generator
and has strong university. Theoretical researches and simulation tests such as mod-
eling, data fusion, and integrated algorithm of integrated navigation system can be
conducted on such semi-physical simulation system, which will provide powerful
technology support and practice basis for application and engineering realization of
integrated navigation technology.
References 361

10.4 Chapter Conclusion

Semi-physical simulation technology is an effective method to achieve

SINS/CNS/GNSS integrated navigation method and function verification. This chap-
ter first introduces the principle and composition of semi-physical simulation system
of SINS/CNS/GNSS integrated navigation. Furthermore, to achieve semi-physical
simulation system of high-performance integrated navigation and provide semi-
physical simulation with ideal parts output of navigation system, the principle and
method of corresponding trajectory generator are introduced briefly, respectively, for
three different objects of ballistic missile, aircraft, and satellite. At the same time, for
the special requirements of celestial navigation semi-physical simulation on simula-
tion star map, principle of star-map simulation system, fast selection of simulation
star and verification method, software design and realization are introduced briefly.
Finally, the semi-physical simulation system of SINS/CNS/GNSS integrated nav-
igation is built on the basis of aforementioned principle and method and test and
verification of corresponding algorithm have been conducted on the semi-physical
simulation system.
The establishment of semi-physical simulation system of SINS/CNS/GNSS inte-
grated navigation will help the development of researches on new thought, method,
and plan of high-performance navigation such as missile, aircraft, satellite, etc. as
well as test and verification of navigation system performance. It has important
engineering application value.

References

1. Xu F, Fang JC, Quan W (2008) Hardware in-the-loop simulation of SINS/CNS/GPS. Integrated

navigation system. J Syst Simul 20(2):332–335
2. Gianluca A, Chiaverini S, Finotello R, Schiavon R (2001) Real-time path planning and obstacle
avoidance for RAIS: an autonomous underwater vehicle. IEEE J Ocean Eng 6(2):216–227
3. Williamson W, Rios T, Speyer JL (1999) Carrier phase differential GPS/INS positioning for
formation flight. Proceedings of the American Control Conference
4. Hunt C, Smith C, Hart R, Bums R (2004) Development of a crosslink channel simulator.
Proceedings of aerospace conference
5. WangYD, Huang JX, FanYZ (1999) Research on GPS/INS Integrated navigation semi physical
simulation. J Beijing Univ Aeronaut Astronaut 25(3):299–301
6. Quan W, Fang JC (2006). Hardware in-the-loop simulation of celestial navigation system. J
Syst Simul 18(2):353–358
7. Li T, Li XW (2005) A new hardware-in-the-loop simulation scheme of inertial navigation
system. J Comput Simul 22(1):55–57
8. Quan W, Fang JC, Xu F, Sheng W (2008). Study for hybrid simulation system of SINS/CNS
integrated navigation. IEEE A&E Syst Mag 23(2):17–24
9. Shen GX, Sun JF (2001) Information fusion theory in INS/CNS/GPS integrated navigation
system, Beijing
10. Zhang GL, Li CL, Deng FL, Chen J (2004) Research on ballistic missile INS/GNSS/CNSs
integrated navigation system. J Missiles Space Veh 2:11–15
362 10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation

11. Fang JC, Ning XL, Tian YL (2006) Autonomous celestial navigation principles and methods.
National defense industry Press, Beijing
12. Ying Y, Wang Q (2005) STK IN in the computer simulation. National Defense Industry Press,
Beijing
13. Fang JC, Ning XL (2006) Principles and applications of celestial navigation. Beijing University
of Aeronautics and Astronautics Press, Beijing
14. Tian YL, Wang GJ, Fang JC, Liu J (2004) Semi physical simulation technology star simulation.
J Aerosp Chin 4:25–26
15. Quan W, Fang JC (2005) High-precision simulation of star map and its validity check. J
Opto-Electron Eng 32(7):22–26
16. Quan W, Fang JC (2005) Hardware in-the-loop simulation of celestial navigation system.
Collection of technical papers-AIAA modeling and simulation technologies conference vol 1,
pp 426–429
17. Rao CJ, Fang JC (2004) A way of extracting observed stars for star image simulation. J Opt
Precis Eng 12(2):129–135
18. Quan W, Fang JC (2007) Study on INS/CNS Integrated navigation semi physical simulation
system and its experiment. J Syst Simul 19(15):3414–3418
Chapter 11
Prospects of INS/CNS/GNSS Integrated
Navigation Technology

11.1 Introduction

With the rapid development of space science and growing interest in space exploration
missions, the performance requirement of navigation systems for space vehicles be-
comes demanding; therefore, a single means of navigation has been unable to meet
the needs of engineering applications. The integrated technology of the inertial navi-
gation system (INS), the celestial navigation system (CNS), and the global navigation
satellite system (GNSS) has become an important development direction of naviga-
tion technology due to its high precision, high real-timeliness, high reliability, as
well as integrated and intelligent capabilities.

11.2 Development and Prospect of Integrated Navigation

Technology

Over the past 50 years, the USA, Europe, and other developed countries have paid
much attention to inertial/celestial/satellite integrated navigation technology and car-
ried out productive research on advanced navigation theory and methods. Recently,
there has been some new development in inertial/celestial/satellite navigation tech-
nology. Here, we give a brief introduction about its future development trend from
five aspects.

11.2.1 Accurate Modeling Techniques of the INS/CNS/GNSS

Navigation System

The inertial/celestial/satellite navigation system has great advantages over any single
system. Theoretically speaking, the accuracy of the integrated navigation system is
higher than that of any subnavigation system. However, the accurate model of the

© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015 363
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_11
364 11 Prospects of INS/CNS/GNSS Integrated Navigation Technology

integrated navigation system cannot be built due to the randomness and uncertainty
of the system noise distribution in each subsystem. Consequently, the navigation
accuracy will be reduced. Therefore, designing an appropriate navigation mode and
establishing an accurate system error model are the key to achieve high-precision
integrated navigation.
Concerning the difference of each subsystem in the inertial/celestial/satellite nav-
igation system, in-depth study of the static and dynamic error features of each sensor
in the subsystem and the analysis of environmental factors (such as temperature,
vibration, acceleration, magnetic field, etc.) on the sensor error need to be carried
out in order to reveal how the errors will be affected by the angular motion, linear
motion, mechanics, and temperature, as well as other environmental factors. Mean-
while, the error propagation features of each sensor will be clarified from the linear
model, nonlinear model, time series model, as well as the intelligent forecasting
model to establish a more precise error model of each subsystem [1, 2], and even-
tually, the high-end, high-precision multiparameter error model and higher-order
nonlinear non-Gaussian random error model will be established.

11.2.2 Information Fusion of the INS/CNS/GNSS Navigation

System and the Advanced Filtering Method

Multisensor information fusion is a method to use processed information from a

number of different or the same navigation sensors to obtain the long-term and overall
data fusion of the object in order to achieve high accuracy and high reliability of the
navigation system. This method gives an optimal state estimation of the integrated
navigation system parameters by fusion algorithm based on the measurement data
from various navigation sensors.
With the booming of new-concept, new-mechanism navigation sensors and the
development of modern signal-processing technology, computer technology, arti-
ficial intelligence technology, and information fusion techniques will become an
integral part of the integrated navigation system [3–5]. A high-performance iner-
tial/celestial/satellite navigation system calls for the consideration of many practical
factors of multisensor data fusion, including the type and distribution form of naviga-
tion sensors in the integrated navigation system, multiple rates, computing capability,
and effective algorithm of the system. Development and improvement of the basic
theory of information fusion, advancement of fusion algorithm, as well as research
of system design method for engineering will be an important development direction
of information fusion technology in integrated navigation.
Filtering is to have the recursive state estimation of the system using the mea-
surement data accumulated over time. The filtering method has wide applications in
many fields. For a linear system with Gaussian noise, the optimal estimation method
is the Kalman filter. But usually, the inertial/celestial/satellite navigation system is
11.2 Development and Prospect of Integrated Navigation Technology 365

a nonlinear, non-Gaussian noise system; thus, it is necessary to use an advanced

filtering method to improve navigation accuracy.
In recent years, many scholars have combined an extended Kalman filter (EKF)
[6] with a particle filter (PF) [7, 8] (EKPF), and an unscented Kalman filter (UKF)
[9] with a PF (UPF) [10], that is, a PF that uses an EKF or a UKF to produce impor-
tant density function. This method can make good use of their respective advantages
to get higher navigation accuracy, which is an effective way to solve the system of
nonlinear and non-Gaussian noise. However, the shortcoming of the PF is that it asks
for large computation; therefore, how to improve the real-timeliness of the algorithm
is an important issue to be solved. In addition, neural networks, genetic algorithm,
and other intelligent algorithms, such as genetic PF, have also been introduced to ad-
vanced filtering methods by some scholars [11]. In short, advanced filtering methods
keep on springing up with their own features and specific applications. Exploring
new methods applicable to inertial/celestial/satellite navigation filtering and carrying
out analyses of its performance (e.g., observability, stability, convergence, etc.) are
of vital importance for the development of integrated navigation technology.

11.2.3 INS/CNS/GNSS Navigation Method Based on Advanced

Control Theory

In recent years, with the development of control theory, advanced control methods
have also been used in information fusion of the inertial/celestial/satellite navigation
system, which include the following:
1. Method to Improve Fault Tolerance and Filtering Stability of the Iner-
tial/Celestial/Satellite Navigation System

There are many ways to improve the fault tolerance and filtering stability of the
inertial/celestial/satellite navigation system, among which the fuzzy theory proposed
by Prof. L. A. Zadeh from the University of California, Berkeley, USA, in 1965 is
an effective way. The basic principle is to have a quantitative process of the concept
of human subjective performance roughly in accordance with the approximate logic.
Fuzzy theory is a general term, including fuzzy set theory based on the expanded
set theory, fuzzy measure theory with the expansion of probability, as well as the
fuzzy logic by introducing ordinary logic into fuzzy concept. The integration of
the inertial/celestial/satellite navigation system with fuzzy inference decision can
take advantage of its strong fault tolerance capability, high stability, and reliability
[12, 13] to achieve high-performance navigation.
Currently, the fuzzy theory for the inertial/celestial/satellite navigation system
includes the following types:
366 11 Prospects of INS/CNS/GNSS Integrated Navigation Technology

1) Possibility theory
Possibility, similar to “probability” in mathematical concept, is introduced into the
uncertainty theory, and it is mainly used to measure the degree of certainty. Possibility
theory was proposed in 1978 by L. A. Zadeh, and it sets up a theoretical framework
for fuzzy set theory. The uncertainty and incompletion of information is processed by
means of two fuzzy metrics, that is, likelihood metric P (proposition) and necessity
metric N (proposition). Since the possibility theory has a good theoretical basis and
modest computational complexity, it is widely used in uncertainty reasoning. This
method can also be used for information fusion, integrated navigation, and database
combinations [14, 15].
2) Random set theory
Data fusion research is focused on establishing a framework with a rigorous mathe-
matical foundation, and random set theory is an effective way to solve this problem.
As the general information fusion problems can be constructed with a random finite
set, the navigation state estimation problem can be described as a special random set
estimation problem [16, 17].
3) Stochastic distribution theory
Stochastic distribution estimation and filtering theory is a dynamics model of system
input and controlled output probability density function (PDF) based on measure-
ment output signal or measurement output PDF for stochastic distribution systems in
order to estimate the parameter of stochastic distribution systems, variable statistics,
or internal state. The study of traditional minimum variance estimation and Kalman
filter is focused on linear Gaussian systems theory while the study of stochastic dis-
tribution system estimation and filtering theory is focused on nonlinear non-Gaussian
systems, which broadens the traditional areas of research for filtering stochastic es-
timation theory. In traditional feedback estimation theory, the feedback is the output
value while in stochastic distribution system estimation and filter theory, the mea-
surement information is usually the output statistical information or PDF. Thus, the
stochastic distribution theory can be used to deal with problems like non-Gaussian
noise, nonlinear system, and time lag in the integrated navigation system, which
cannot be solved by traditional methods; meanwhile, the accuracy of the navigation
system and real-timeliness will be improved significantly [18–20].
2. Approaches to Improve the Navigation Precision of the Inertial/Celestial/
Satellite Navigation System
1) Multimodel approach
Aircraft are influenced by external factors, such as uncertainty of environment; thus,
using a single model cannot reach the required accuracy and stability. The multimodel
approach is an effective way to solve this problem through the establishment of a num-
ber of different models to adapt to various changes. Research shows that the use of a
multimodel filter can improve the transient response of the inertial/celestial/satellite
navigation system and cover a wide range of parameter uncertainty to achieve the
11.2 Development and Prospect of Integrated Navigation Technology 367

desired precision navigation, tracking speed, and system stability. However, if the
selected model collection is too large, the complexity of the system will be increased.
Currently, the adaptive multimodel approach and the multimodel adaptive method
based on fuzzy theory are newer methods [21–23].
2) Neural networks theory
The general inertial/celestial/satellite navigation system makes an estimation of the
error and calibrates it through Kalman filtering techniques to ensure the accuracy of
the navigation system. However, this method has strict requirements for the system
model, and the presumption is that the statistical characteristics of the noise for the
state model and measurement model are known; but, the noise of the integrated
navigation system is usually non-priori. To solve this problem, the neural network is
introduced into the INS/CNS/GPS integrated navigation system state estimation to
reduce the external interference on the performance of the navigation system as far
as possible. Neural network has parallel computing capability, high fault tolerance,
and when the hidden layer neurons is enough, sufficient accuracy can be ensured to
approach nonlinear function [24–27].
3) Distributed belief fusion
Distributed belief fusion is a kind of combined logic with different degrees of re-
liability for information from different subsystems. This logic is obtained through
joint multiagency cognitive logic and multisensing reasoning system. The reliability
sequence of each subsystem is indicated by the mode operator, and reasoning can be
carried out under different situations by combined information. By using different
combinations of logic, the final result will be different [28–30].
4) Hybrid estimation
The so-called hybrid estimation method is to deal with state estimation problems of
the dynamic system with both continuous and discrete variables by advanced filtering
[31–33].
5) Genetic algorithm
Genetic algorithm is an efficient exploratory algorithm based on the genetic evolution
of a natural population. It was first proposed by the American scholar Holland in
1975. Genetic algorithm is a calculation model which simulates Darwin’s theory
of natural selection and biological evolution. It uses a simple encoding technique
to express a variety of complex structures and, by simple genetic manipulation and
natural selection of the encoding, to guide learning and determine the direction
of the search. Joining the inertial/celestial/satellite navigation system with genetic
algorithm and optimizing the integration by using of its overall search capability can
improve the accuracy of the system [34–36].
368 11 Prospects of INS/CNS/GNSS Integrated Navigation Technology

6) Ant colony algorithm

Ant colony algorithm, proposed by the Italian scholar Marco Dorigo in 1991, is
a newly developed bionic optimization algorithm which simulates the intelligent
behavior of an ant colony. It has strong robustness, a good distributed operat-
ing mechanism, strong self-learning ability, and it can be easily integrated with
other methods. Therefore, it can reorganize knowledge information and knowledge
structure according to environmental changes and past behavior results in order to
achieve the optimization of the model. The introduction of this algorithm into the
inertial/celestial/satellite navigation system, just like that of genetic algorithm, can
achieve the overall optimization of the navigation system performance faster [37, 38].
7) Coevolution algorithm
It is a genetic evolutionary approach based on genetic algorithm and draws on the ex-
perience of evolutionary ecology. Coevolution is a type of the evolution of species in
the ecosystem, including reciprocal behavior and mutual selection in the ecosystem
among closely related species in the process of life activities, that is, the individual
behavior of one species will influence that of another species, generating the inter-
action between the two species in the course of evolution. Compared with general
genetic algorithm, coevolutionary algorithm can have a reasonable population par-
tition of system variables, and solving large-scale systems can effectively get out of
local optimization to find better solutions [39–42].

11.2.4 Integrated Inertial/Celestial/Satellite Navigation System

Technology Based on Integration

With the continuous development of modern microelectronics, photonics technology,

microelectromechanical systems (MEMS) technology, as well as computer technol-
ogy and control technology, the inertial/celestial/satellite navigation system will be
developed toward precision, miniaturization, and integration.
1. GNC System Integration Technology of High-Performance Navigation,
Guidance, and Control

With the urgent demand for low-cost, precision-guided weapons, the very kind of
it will play a more important role. Miniaturized and guided conventional weapons
are an effective means to improve operational effectiveness and reduce cost; thus,
they have become one of the main development trends. Currently, due to the need of
microsystems and precision-guided conventional weapons, complementary metal-
oxide semiconductor (CMOS)-sensitive devices, silicon MEMS inertial devices,
multisensor inertial measurement unit (MIMU) components, and the guidance,
navigation, and control (GNC) system have been gradually developed toward minia-
turization and high performance. Therefore, GNC system integration technology
11.2 Development and Prospect of Integrated Navigation Technology 369

based on microcomponents and actuators is an important direction for the future

development of the integrated navigation system.
2. Integration Technology of High-Performance Integrated Attitude Determi-
nation System

The attitude measurement system, which consists of gyros and celestial sensors, is
a typical part of the attitude determination system for spacecraft. Celestial sensors
can provide precision measurement without error accumulation, but the output is
not continuous. The inertial measurement unit (IMU) can be used for continuous
measurement of attitude, but the measurement errors accumulate over time due to
gyro drift; so, it cannot be used alone. For this reason, the combination of celestial
sensors with the IMU is the most effective way to achieve precise spacecraft attitude
measurement. In 2003, the inertial stellar compass (ISC) used in microsatellites
has been developed successfully by the American Draper Laboratory. This system
consists of a CMOS star-based active pixel sensor (APS) and three MEMS rate
gyros with the attitude determination accuracy up to 0.1 [43, 44]. It can not only
reduce the cost of the spacecraft attitude determination system, but also improve the
autonomous operation and self-management capabilities of the spacecraft; thus, it
is the future development direction of high-performance attitude determination for
microsatellites.

11.2.5 Applications of the Inertial/Celestial/Satellite Navigation

Technology

The research of precision navigation technology can provide accurate information

for vehicles, and integrated navigation technology is an effective means to achieve
precision navigation. In recent years, thanks to the rapid development of strap-down
inertial navigation technology, miniature celestial sensitive technology, and high-
performance satellite navigation technology, as well as the deepening research of
the advanced information fusion technology, inertial/celestial/satellite navigation
technology has been widely used in the field of aviation and aerospace. With the
development of aerospace science and technology, there will be higher requirement
for inertia/celestial/satellite integrated navigation technology.
Modern wars have increasingly higher requirement for weapon systems on ac-
curacy, responsiveness, mobility, and reconnaissance capabilities; as a result, the
in-depth study of high-precision inertial/celestial/satellite navigation technology will
be of high importance to enhance the operational capabilities of weapon systems.
Spacecraft need to run for several years or even decades in space; therefore, the
positioning and attitude determination system in spacecraft, which is one of the key
technologies of spacecraft research, must feature high precision and long life. How
to ensure high-precision positioning and attitude determination in the long run is the
key issue of spacecraft research. High-precision inertial/celestial/satellite integrated
370 11 Prospects of INS/CNS/GNSS Integrated Navigation Technology

navigation technology, which uses multisensors to get data fusion in order to obtain
high-precision attitude and position information of moving vehicles, is an effective
means of long time, high-precision positioning and attitude determination for space-
craft. The research of high-performance inertial/celestial/satellite positioning and
attitude determination systems applicable to spacecraft will play an important role
to promote the development of space technology.

11.3 Chapter Conclusion

Modern information warfare has developed into a noncontact and nonsymmetrical

one; therefore, precision positioning and navigation, efficient target detection, and
high-resolution earth observation have become a necessary means for high-tech infor-
mation warfare. High-performance inertial/celestial/satellite integrated navigation
technology, as one of the key technologies, can provide high-precision motion infor-
mation for a variety of vehicles; thus, it can meet the urgent need for the development
of modern high-precision navigation and positioning systems.
Looking to the future, there is reason to believe that inertial/celestial/satellite
integrated navigation technology will play a significant role in the high-performance
navigation of new-generation spacecraft, large-scale, high-precision, high-stability
navigation control for earth observation, high-precision ballistic missiles navigation,
autonomous navigation for manned spacecraft, space station, as well as deep space
exploration and other areas.
The pursuit of science is endless. Precision positioning and navigation are always
a goal for scientists to aspire to; this poses both opportunities and challenges for the
development of inertial/celestial/satellite navigation technology. We should closely
follow the new technology and new ideas and carefully analyze the future trends in
order to innovate and promote the development of inertial/celestial/satellite integrated
navigation technology.

References

1. Dong JK, M’Closkey RT (2006) A systematic method for tuning the dynamics of electrostati-
cally actuated vibratory gyros. IEEE Trans Control Syst Technol 14(1):69–81
2. EI-Sheimy N, Hou HY, Niu XJ (2008) Analysis and modeling of inertial sensors using Allan
variance. IEEE Trans Instrum Meas 57(1):140–149
3. Hall LD, Llinas J (2001) Handbook of multisensor data fusion. CRC, Boca Raton
4. Bar-Shalom Y, Li XR (1995) Multitarget-multisensor tracking: principles and techniques. YBS
Publishing, Storrs
5. Macii D, Boni A, De Cecco M, Petri D (2008) Tutorial 14: multisensor data fusion. IEEE
Instrum Meas Mag 11(3):24–33
6. Farina A, Benvenuti D (2002) Tracking a ballistic target: comparison of several nonlinear filters.
IEEE Trans Aerosp Electron Syst 38(3):854–867
7. Arulampalam MS, Maskell S, Gordon N, Clapp T (2002) A Tutorial on particle filter for online
nonlinear/non-Gaussian Bayesian tracking. IEEE Trans Signal Process 50(2):174–188
References 371

8. Kwok C, Fox D, Meila M (2004) Real-time particle filters. Proc IEEE 92(3):469–484
9. Julier SJ, Uhlmann JK (2002) Reduced sigma point filters for the propagation of means
and covariances. Through nonlinear transformations. Proceedings of the American Control
Conference Anchorage, AK, pp 887–892
10. Merwe R, Doucet A, Freitas N, Wan E (2000) The unscented particle filter, technical report
CUED/F-INFENG/TR 380. Cambridge University Engineering Department, pp 1064–1070,
Aug 2000
11. Fang JC, Ning XL (2006) Autonomous celestial navigation for lunar explorer based on genetic
algorithm particle filter. J Beihang Univ 32(11):1273–1276
12. Yang J (2004) Reconfigurable fault tolerant space manipulator design. J Astronaut 25(2):
147–151
13. Funabashi M, Maeda A, Morooka Y, Mori K (1995) Fuzzy and neural hybrid expert systems:
synergetic AI. IEEE Expert 10(4):32–40
14. Zeng AJ, Sha JC (1994) A generalization of possibility theory and its validity. Fuzzy Set Syst
8(2):31–38
15. Dubois D, Prade H (2003) Possibility theory and its applications: a retrospective and prospective
view. Fuzzy Systems. FUZZ ’03. The 12th IEEE international conference, vol 1, pp 5–11, May
25–28
16. DengY, Shi WK (2002) Research on application of random sets theory to data fusion. J Infrared
Laser Eng 31(6):545–549
17. Random sets in data fusion: formalism to new algorithms. Information fusion (2000) Pro-
ceedings of the third international conference, 10–13 July 2000, TUC4/24–TUC4/31 vol 1
18. Wang H (2000) Bounded dynamic stochastic systems, modeling and control. Springer-Verlag,
London
19. Guo H, Wang H (2006) Minimum entropy filtering for multivariate stochastic systems with
non-Gaussian noises. IEEE Trans Autom Control 51:695–700
20. Guo L, Wang H (2005) PID Controller design for output PDFs of stochastic systems using
linear matrix inequalities. IEEE Trans Syst Man Cybern, Part-B 35:65–71
21. Huang ZH, Xing CF (2004) Adaptive multiple model tracking algorithm for maneuvering target
based on fuzzy inference. J Inf Command Control Syst Simul Technol 26(5):42–46
22. Baruch I, Flores JM, Martinez JC, Garrido R (2000) A multi-model parameter and state esti-
mation of mechanical systems. Proceedings of the IEEE International Symposium, vol 2, pp
700–705, 4–8 Dec 2000
23. Ming J, Hanna P, Stewart D, Owens M, Smith FJ (1999) Improving speech recognition
performance by using multi-model approaches. Acoustics, speech, and signal processing.
Proceedings of the IEEE International Conference, vol 1, pp 161–164, 15–19 March 1999
24. Xu LJ, Chen YZ, Cui PY (2004) State estimation of integrated navigation system based on
neural network. J Comp Meas Control 12(7):660–664
25. Chai L, Yuan JP, Fang Q, Huang LW (2005) Neural-network-aided information fusion for
integrated navigation application. J Northwest Polytech Univ 23(1):45–48
26. Qiao H, Peng J, Xu Z, Zhang B Bo (2003) A reference model approach to stability analysis of
neural networks. IEEE Trans Syst Man Cybern, Part-B 33(6):925–936
27. Pei Z, Qin KY, Xu Y (2003) Dynamic adaptive fuzzy neural-network identification and its
application. Systems, Man and Cybernetics, 2003. IEEE International conference, vol 5, pp
4974–4979, 5–8 Oct 2003
28. Han CZ (2000) Information fusion theory and its applications. J Chin Basic Sci Cutting-Edge
Sci 7:14–18
29. Han CZ, Zhu HY (2002) Multi sensor information fusion and automation. J Autom
28(Suppl):117–124
30. Lian CJ (2000) A conservative approach to distributed belief fusion. Information fusion, 2000.
Proceedings of the third international conference, MOD4/3–MOD410 vol 1, 10–13 July
31. Zhou ZH, Ji CX (2003) A variety of filter for maneuvering target tracking and adaptive
comparison. J Inf Command Control Syst Simul Technol 10:25–36
372 11 Prospects of INS/CNS/GNSS Integrated Navigation Technology

32. Hofbaur MW, Williams BC (2004) Hybrid estimation of complex systems. IEEE Trans Syst
Man Cybern, Part-B 34(5):2178–2191
33. Hwang I, Balakrishnan H, Tomlin C (2003) Performance analysis of hybrid estimation algo-
rithms. Decision and Control. Proceedings of 42nd IEEE Conference. vol 5, pp 5353–5359,
9–12 Dec 2003
34. Zhang SY, Jia CY (2004) Optimal celestial positioning constellation combination based on
genetic algorithm. J Traffic Transp Eng 4(1):110–113
35. Fan YZ, Zhang YN, Ma HK (2005) Application of the hybrid BP/ GA mixed algorithms in a
simple integrated navigation system. J Beijing Univ Aeronaut Astronaut 31(5):535–538
36. Chaiyaratana N, Zalzala AMS (1997) Recent developments in evolutionary and genetic algo-
rithms: theory and applications. Genetic algorithms in engineering systems: innovations and
applications. GALESIA 97. Second international conference. 2–4 Sept, pp 270–277
37. Dorig M, Caro GD (1999) Ant colony optimization: a new meta-heuristic evolutionary
computation. CEC 99. Proceedings of the 1999 congress on volume 2, 6–9 July
38. Birattari M, Pellegrini P, Dorigo M (2007) On the invariance of ant colony optimization
evolutionary computation. IEEE Trans 11(6):732–742
39. Liu JC, Wang P (2007) A dynamic model of cooperative coevolutionary algorithm. J China
Water Transp 5(5):163–165
40. Wang JN, Shen QT, Shen HY, Zhou XC (2006) Evolutionary design of RBF neural network
based on multi-species cooperative particle swarm optimizer. J Control Theory Appl 23(2):
251–255
41. Rosin C, Belew R, Morris C et al (1997) New methods for competitive coevolution. Evol
Comput 5(1):1–29
42. Bergh F, Engelbrecht AP (2007) A cooperative approach to particle swarm optimization·IEEE
Trans Evol Comput 8(3):1–15
43. Brady T, Buckley S, Dennehy CJ et al (2003) The inertial stellar compass: a multifunction, low
power, attitude determination technology breakthrough. 26th guidance and control conference,
pp 39–56
44. Brady T, Tillier C, Brown R et al (2002) The inertial stellar compass: a new direction in
spacecraft attitude determination. 16th annual AIAA/USU conference on small satellites