INS/CNS/GNSS Integrated Navigation Technology
Wei Quan • Jianli Li • Xiaolin Gong
Jiancheng Fang
INS/CNS/GNSS Integrated
Navigation Technology
Wei Quan
Beijing University of Aeronautics
and Astronautics
Beijing
China
Xiaolin Gong
Beijing University of Aeronautics
and Astronautics
Beijing
China
Jianli Li
Beijing University of Aeronautics
and Astronautics
Beijing
China
Jiancheng Fang
Beijing University of Aeronautics
and Astronautics
Beijing
China
ISBN 978-3-662-45158-8
DOI 10.1007/978-3-662-45159-5
ISBN 978-3-662-45159-5 (eBook)
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 basis 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 application. Full autonomy, possibility to continuously provide complete motion parameters
and high short-term high precision are the advantages of inertial navigation, and error 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 inertial 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 algorithm, 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 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; 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 information distribution factor of federal filtering optimization. Chapter 9 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 integrated navigation systems. Chapter 10 mainly introduces the
principle, composition, realization, and experiment of semi-physical simulation system 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 expertise 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 suggestions 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 energetic support and assistance; and National Natural Science Foundation of China
and Ministry of Science and Technology for their support and assistance. In addition, 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 operating 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 system. Chapter four mainly introduces gyro error modeling, test and compensation,
calibration methods for strapdown inertial measurement unit and high-dynamic strapdown 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 integrated 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 integrated 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 The History of INS/CNS/GNSS Navigation . . . . . . . . . . . . . . . . . . . . .
1.2 The Current Status of INS/CNS/GNSS Navigation Development . . .
1.2.1 INS/GNSS Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2.2 INS/CNS Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2.3 INS/CNS/GNSS Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1
2
4
4
5
6
8
2
Principle of INS/CNS/GNSS Navigation System . . . . . . . . . . . . . . . . . . .
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2 Coordinate Frames and Earth Reference Model Commonly Used in
Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.1 The Coordinate Frames Used in Navigation . . . . . . . . . . . . . .
2.2.2 The Conversion of Coordinate Systems . . . . . . . . . . . . . . . . . .
2.2.3 Earth Reference Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3 Inertial Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3.1 Work Principle of Inertial Navigation System . . . . . . . . . . . . .
2.3.2 SINS System Error Equation and Error Propagation
Characteristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.4 Satellite Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.4.1 Operating Principle of Satellite Navigation System . . . . . . . .
2.4.2 Analysis of Error Characteristics for Satellite Navigation
System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.5 Celestial Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.5.1 Autonomous Celestial Positioning Principle . . . . . . . . . . . . . .
2.5.2 Celestial Attitude Determination Principle . . . . . . . . . . . . . . .
2.5.3 Star Sensor in CNS and Analysis of Its Error Characteristics
2.6 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
9
9
9
13
15
21
21
23
30
30
32
34
36
44
46
51
51
xi
xii
Contents
3
Filters in Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.3 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.3.1 Mathematical Description of Stochastic Nonlinear System . .
3.3.2 Discrete Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . .
3.4 Unscented Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.5 Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.6 Unscented Particle Filter (UPF) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.7 Predictive Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.8 Federated Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.8.1 Structure of Federated Filter . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.8.2 Fusion Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.9 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
Error Modeling, Calibration, and Compensation of Inertial
Measurement Unit (IMU) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2 Error Modeling and Compensation of Inertial Sensors . . . . . . . . . . . .
4.2.1 Error Model of Gyroscopes . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2.2 Scale Factor Error Modeling of Gyroscope . . . . . . . . . . . . . . .
4.2.3 Temperature Error Modeling of Gyroscope . . . . . . . . . . . . . . .
4.3 Design, Error Calibration, and Compensation of Inertial
Measurement Units . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.3.1 Design of Inertial Measurement Units . . . . . . . . . . . . . . . . . . .
4.3.2 The Optimization Six-Position Hybrid Calibration for SINS .
4.3.3 Integrated Calibration Method for RLG IMU Using a
Hybrid Analytic/Kalman Filter Approach . . . . . . . . . . . . . . . .
4.3.4 Temperature Error Modeling of IMU Based on Neural
Network . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.4 High Dynamic Strapdown Inertial Algorithm . . . . . . . . . . . . . . . . . . .
4.4.1 Error Analysis and Gyro Biases Calibration of Analytic
Coarse Alignment for Airborne POS . . . . . . . . . . . . . . . . . . . .
4.4.2 Conical Motion Analysis and Evaluation Criteria for
Conical Error Compensation Algorithm . . . . . . . . . . . . . . . . .
4.4.3 An Improved Single-Subsample Rotating Vector Attitude
Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
53
53
54
56
56
57
59
61
64
65
68
68
69
70
71
75
75
76
76
78
85
90
90
104
108
117
123
124
131
132
141
142
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
5.2.1 Problem Statements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2.2 Blurred Star Image De-noising . . . . . . . . . . . . . . . . . . . . . . . . .
5.2.3 Blurred Star Image Restoration . . . . . . . . . . . . . . . . . . . . . . . . .
5.2.4 Results and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3 Star Map Identification Method of Star Sensor . . . . . . . . . . . . . . . . . .
5.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3.2 Star Recognition Method Based on AAC Algorithm . . . . . . .
5.3.3 Hybrid Simulation Result and Analysis . . . . . . . . . . . . . . . . . .
5.3.4 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.4 Celestial Navigation Method Based on Star Sensor and
Semi-physical Simulation Verification . . . . . . . . . . . . . . . . . . . . . . . . .
5.4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.4.2 Celestial Navigation Measurements and Orbit Dynamic
Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.4.3 UKF Information Fusion Algorithm . . . . . . . . . . . . . . . . . . . . .
5.4.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.4.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.5 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
INS/GNSS Integrated Navigation Method . . . . . . . . . . . . . . . . . . . . . . . . .
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.2 Principle of Inertial/Satellite Integrated Navigation . . . . . . . . . . . . . .
6.2.1 Combination Mode of Inertial/Satellite Integrated Navigation
6.2.2 Basic Principle for Inertial/Satellite Integrated Navigation . .
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System
6.3.1 Linear Modeling Method of Inertial/Satellite Integrated
Navigation System Based on the Angle . . . . . . . . . . . . . . .
6.3.2 Nonlinear Modeling Method of the Inertial/Satellite
Integrated Navigation System Based on Quaternion Error . .
6.4 High-Precision Inertial/Satellite Integrated Navigation Method . . . .
6.4.1 Inertial/Satellite Integrated Navigation Method Based on
Mixed Correction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.4.2 Self-Adaptive Feedback Correction Filter Method Based
on Observability Normalization Processing Method . . . . . . .
6.4.3 Inertial/Satellite Outlier-Resistant Integrated Navigation
Method Based on Kalman Filtering Innovation
Orthogonality . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.4.4 An Air Maneuvering Alignment Method Based on
Observability Analysis and Lever Arm Error Compensation
6.4.5 SINS/GPS Integrated Estimation Method Based on
Unscented R–T–S Smoothing . . . . . . . . . . . . . . . . . . . . . . . . .
6.5 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
xiii
146
148
150
152
158
159
160
161
167
169
170
171
172
176
178
181
181
181
185
185
186
186
187
189
190
193
199
200
203
209
214
217
231
233
xiv
7
8
Contents
INS/CNS Integrated Navigation Method . . . . . . . . . . . . . . . . . . . . . . . . . .
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.2 Basic Principle of Inertial/Celestial Integrated Navigation . . . . . . . . .
7.2.1 Operating Mode of the Inertial/Celestial Integrated
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.2.2 Combination Mode of Inertial/Celestial Integrated
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.2.3 Principle of Inertial Component Error Correction Based on
Celestial Measurement Information . . . . . . . . . . . . . . . . . . . .
7.3 Modeling Method of Inertial/Celestial Integrated Navigation
System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.3.1 State Equation of Inertial/Celestial Integrated Navigation
System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.3.2 Measurement Equation of Inertial/Celestial Integrated
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.4 New Inertial/Celestial Integrated Navigation Method of Ballistic
Missile . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.4.1 Principle for Initial Position Error Correction of Missile
Launching Point Based on Celestial Measurement
Information . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.4.2 Inertial/Celestial Integrated Navigation Method of Ballistic
Missile Based on UKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.5 Inertial/Celestial Integrated Navigation Method of Lunar Vehicle . . .
7.5.1 Strapdown Inertial Navigation Method of Lunar Vehicle . . . .
7.5.2 A Lunar Inertial/Celestial Integrated Navigation Method
Based on UPF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.6 Inertial/Celestial Integrated Attitude Determination Method of
Satellite . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.6.1 Satellite Attitude Determination System Equation . . . . . . . . .
7.6.2 An Inertia/Celestial Integrated Attitude Determination
Method of Piecewise Information Fusion Based on EKF . . .
7.6.3 Method of Minimum Parameter Attitude Matrix Estimation
of Satellite Based on UKF . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.6.4 Interlaced Optimal-REQUEST and Unscented Kalman
Filtering for Attitude Determination . . . . . . . . . . . . . . . . . . . .
7.7 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
INS/CNS/GNSS Integrated Navigation Method . . . . . . . . . . . . . . . . . . . .
8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8.2 Principle of INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . .
8.2.1 Basic Principle of INS/CNS/GNSS Integrated Navigation . .
8.2.2 Combination Mode of INS/CNS/GNSS
Integrated Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8.2.3 Modeling of INS/CNS/GNSS Integrated Navigation System
237
237
238
238
240
241
242
243
245
245
246
246
250
251
252
257
257
259
263
269
275
276
279
279
280
280
280
285
Contents
INS/CNS/GNSS Integrated Navigation Method Based on
Federated UKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8.4 Federated Filtering INS/CNS/GNSS Integrated Navigation
Method Based on the Optimized Information Distribution
Factor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8.4.1 Federated Filtering Equation and Information
Distribution Process . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8.4.2 Federated Filtering INS/CNS/GNSS Integrated
Navigation Method Based On Information Distribution
Factor Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8.4.3
Research on FKF Method Based on an Improved
Genetic Algorithm for INS/CNS/GNSS Integrated
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8.5 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
xv
8.3
9
10
Study for Real-Time Ability of INS/CNS/GNSS Integrated
Navigation Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9.2 Piecewise Constant System (PWCS) Observability Analysis
Theory and Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9.2.1 Observability Analysis Theory of the PWCS . . . . . . . . . .
9.2.2 An Improved System State Degree of Observability
Analysis Method Based on Singular Value
Decomposition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9.3 Dimensionality Reduction Filter Design of INS/CNS Integrated
Navigation System Based on the Improved Degree of
Observability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9.4 Dimensionality Reduction Filter Design of INS/GNSS Integrated
Navigation System Based on the Improved Degree of
Observability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9.5 Federated Filter Design of the INS/CNS/GNSS Integrated
Navigation System Based on Dimensionality Reduction Filtering
9.6 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Semi-physical Simulation Technology of INS/CNS/GNSS
Integrated Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
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 . . . . . . . . . . . . . .
10.2.2 Composition of Semi-Physical Simulation System of
INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . .
287
291
291
293
294
304
304
307
307
308
308
313
315
318
322
326
328
331
331
332
332
334
xvi
Contents
10.3 Realization and Test of Semi-Physical Simulation System of
INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . . . . . . . . . .
10.3.1 Realization of Semi-physical Simulation System of
SINS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . .
10.3.2 Experiments of Semi-physical Simulation System of
INS/CNS/GNSS Integrated Navigation . . . . . . . . . . . . . .
10.4 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
Prospects of INS/CNS/GNSS Integrated Navigation Technology . . . .
11.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11.2 Development and Prospect of Integrated Navigation Technology .
11.2.1 Accurate Modeling Techniques of the INS/CNS/GNSS
Navigation System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11.2.2 Information Fusion of the INS/CNS/GNSS Navigation
System and the Advanced Filtering Method . . . . . . . . . . .
11.2.3 INS/CNS/GNSS Navigation Method Based on
Advanced Control Theory . . . . . . . . . . . . . . . . . . . . . . . . .
11.2.4 Integrated Inertial/Celestial/Satellite Navigation
System Technology Based on Integration . . . . . . . . . . . . .
11.2.5 Applications of the Inertial/Celestial/Satellite
Navigation Technology . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11.3 Chapter Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
347
350
359
361
361
363
363
363
363
364
365
368
369
370
370
Chapter 1
Introduction
Navigation is a technology to provide real-time position, velocity, and attitude information 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 technologies 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 navigation 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 features 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 information; thus, it has anti-interference ability. Meanwhile, the navigation information
is complete and can be updated thoroughly, etc. [3–5]; therefore, the inertial navigation 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
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_1
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 parameters. 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, highaltitude 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 subsystems 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 integrated 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 navigation 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 technology 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 technology 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 continuous 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 development 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 navigation 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 direction. 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 development 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 system 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 satellite 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 autonomous ability. The former can output navigation information continuously, but the
errors accumulate over time; while the latter can output attitude and position information 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 missiles, 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 metaloxide-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 strapdown 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 integration, 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 navigation 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 diversified 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 navigation 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 integrated 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 information fusion filter is indispensable. There are two ways for the INS/CNS/GNSS
navigation system to realize optimal information fusion by Kalman filtering technique: 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 information 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, reconstruct 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 estimation 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 information 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 position, 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 principle 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
2.2.1
Coordinate Frames and Earth Reference Model Commonly
Used in Navigation
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
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_2
9
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 coordinate 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
Fig. 2.1 Body coordinate
system for airplane
11
zb
Ob
yb
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
2.2.2
13
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 system 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 θ ⎥
⎦
⎣− sin ϕ cos ϕ 0⎦ Tθ = ⎣0 cos θ
0
0
1
0 − sin θ cos θ
⎤
⎡
cos γ 0 − sin γ
⎥
⎢
Tγ = ⎢
1
0 ⎥
⎦
⎣ 0
sin γ 0 cos γ
Cnb = Tγ · Tθ · Tϕ
⎡
cos γ cos ϕ − sin γ sin θ sin ϕ
⎢
⎢
=⎣
− cos θ sin ϕ
sin γ cos ϕ + cos γ sin θ sin ϕ
cos γ sin ϕ + sin γ sin θ cos ϕ
− sin γ cos θ
cos θ cos ϕ
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 quaternion 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
2
2
2
(2.3)
Usually, this type of formula (2.2) is called feature quaternion, referred to as quaternion. 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:
⎤
⎡
2 (q1 q3 + q0 q2 )
2 (q1 q2 − q0 q3 )
q02 + q12 − q22 − q32
⎥
⎢
⎥
⎢
⎥
⎢
b
R = ⎢ 2 (q1 q2 + q0 q3 )
q02 − q12 + q22 − q32
2 (q2 q3 − q0 q1 ) ⎥ R n
⎥
⎢
⎦
⎣
2
2
2
2
2 (q2 q3 + q0 q1 )
q0 − q1 − q2 + q3
2 (q1 q3 − q0 q2 )
(2.5)
2.2 Coordinate Frames and Earth Reference Model Commonly Used in Navigation
So,
⎡
⎢
⎢
⎢
Cnb = ⎢
⎢
⎣
q02 + q12 − q22 − q32
2 (q1 q2 − q0 q3 )
2 (q1 q3 + q0 q2 )
2 (q1 q2 + q0 q3 )
q02 − q12 + q22 − q32
2 (q2 q3 − q0 q1 )
2 (q1 q3 − q0 q2 )
2 (q2 q3 + q0 q1 )
q02 − q12 − q22 + q32
15
⎤
⎥
⎥
⎥
⎥. (2.6)
⎥
⎦
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
Rp2
Re2
(2.7)
Lt is the angle between the normal of point M and scale plane of reference ellipsoid,
we have:
Rp 2 x
dz
= − 2 = − cot Lt
dx
Re z
(2.8)
Rp 2
= 1 − k2
Re2
(2.9)
By the
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
Cg
K
E
M
OE
g
G
L
g
D
g
F
B
Qg
S
a
z
Rp
M
Lt
Lc
OE
N
x
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
x2
(1 − k 2 ) x 2 tan2 Lt
+
=1
Rp2
Re2
2.2 Coordinate Frames and Earth Reference Model Commonly Used in Navigation
17
Then:
Re cos Lt
x=
(2.11)
1/2
(1 − k 2 sin2 Lt )
That is, the expression of the radius curvature can be written as follows:
dz
dx
ρ = 1+
2 3/2
d 2z
dx 2
(2.12)
From the Formula (2.8), we have:
1 dLt
d 2z
=
dx 2
sin2 Lt dx
(2.13)
dx
3/2
= −Re (1 − k 2 ) sin Lt /(1 − k 2 sin2 Lt )
dLt
(2.14)
From Formula (2.11), we have:
Eventually, after finishing, the radius curvature of the Earth in meridian plane can
be obtained as:
RM =
As e =
Re −Rp
,
Re
Re (1 − k2 )
(2.15)
3/2
(1 − k2 sin2 Lt )
obviously, e < 0, k 2 = 2e − e2 , omitting e2 :
RM =
−3/2
Take (1 − 2esin2 Lt )
Re (1 − 2e)
(2.16)
(1 − 2esin2 Lt )3/2
≈ 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:
RN =
(1 −
Re
2
2k sin2 Lt )1/2
(2.19)
−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
Rp2
Re2
(2.22)
The normal of ellipse (geographic vertical line) has a slope of:
tan Lt = −
dx
Re 2 z2
=− 2 2
dz
Rp x
Fig. 2.5 Several vertical lines
and latitude
(2.23)
z
Re
Rp
M
Lc
OE
Lg
Lt
x
2.2 Coordinate Frames and Earth Reference Model Commonly Used in Navigation
Fig. 2.6 Gravity vectors
19
Ω
P
－Ω×（Ω×R）
R
G
g
Geocenter line of the same point (geocentric vertical line) has a slope of:
tan Lc =
tan Lct =
Set R =
x
z
Re2 − Rp2
tan Lt − tan Lc
=
xz
1 + tan Lt tan Lc
2Re2 Rp2
(2.25)
√
x 2 + z2 as the length of geocentric line, we have:
tan Lct =
Re2 − Rp2
2Re2 Rp2
R2
Re2 − Rp2 2
x z
=
R sin 2Lc
zR
2Re2 Rp2
Re − Rp Re + Rp R 2
=
sin Lc ≈ E sin 2Lc
Re
2Rp Rp Re
where
(2.24)
(2.26)
Re − Rp
Re + Rp
R2
≈ 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
From Formula (2.27),
(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:
g=
978.03267714 × (1 + 0.00193185138639sin2 L)
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 measurement, 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:
⎡
⎤
r 2 3
Re 2
z
rx ⎥
1−5
⎢ 1 + J2
⎤
⎡
⎢
⎥
2
r
r
⎢
⎥
Gix
2
⎢
⎥
2
⎥ −μ ⎢
⎢
3
R
r
e
z
⎥
⎥=
Gi = ⎢
(2.30)
J
1
+
r
1
−
5
⎢
⎥
G
2
y
iy
⎦
⎣
⎥
r3 ⎢
2
r
r
⎢
⎥
Giz
⎢
⎥
r 2 3
Re 2
z
⎣
⎦
1 + J2
rz
3−5
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
2.3
21
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 (navigation 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 information, 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 classified into platform inertial navigation system (Gimbaled Inertial Navigation System,
GINS) and SINS [1]. In GINS, the inertial devices are installed on the platform inertial 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 working 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 accelerometer 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 inertial 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 strapdown 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 mainstream 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
V
f ibb
Accelerometer
Transformation
Velocity
Calculating
Error
L, λ , h
ϕ, θ, γ
Quaternion
Rotating
Vector
+
Gyro
Position
Calculating
C en
Attitude
Calculating
Attitude Matrix
Compensation
n
ωen
ωibb +
C bn
b
- ωin
ωinn
n
ω en
+
ωien
Cen
ωiee
Fig. 2.7 Principle of strapdown inertial navigation system (SINS)
During the work process of SINS, a series of navigation parameters, such as position (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):
q̇ =
1
b
q ◦ ωnb
2
(2.31)
b
b
b
b
is the attitude matrix update rate, ωnb
= ωib
− ωin
where the angular velocity ωnb
b
the relationship between ωnb and the other angular velocity is:
n
b
b
n
b
n
ωnb
= ωib
− Cnb ωin
= ωib
− Cnb ωie
+ ωen
(2.32)
b
n
is the gyroscope output; ωie
is angular rate of the Earth in the navigation
where ωib
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
n
can be
from system b to system n to get fibn through Cbn . The relative velocity Ven
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
n
n
aen
× Ven
= fibn − 2ωie
+ ωen
+ 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 = −
⎡
0
n
−ωenz
n
ωeny
n
n
en Ce
(2.34)
⎤
⎥
⎢
n
n ⎥, the position of the vehicle can be obtained
=⎢
0
−ωenx
⎦
⎣ ωenz
n
n
−ωeny ωenx
0
from the corresponding element of Cne .
where
n
en
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:
φ̇E = −
δvN
vE
+ ωie sin L +
tan L φN
RM + h
RN + h
− ωie cos L +
vE
φU + εE
RN + h
φ̇N =
δvE
vE
vN
−ωie sin LδL− ωie sin L +
tan L φE −
φU + εN
RN + h
RN + h
RM + h
φ̇U =
δvE
vE
tan L + ωie cos L +
sec2 L δL
RN + h
RN + h
+ ωie cos L +
vE
vN
φE +
φN + εU
RN + h
RM + h
(2.35)
where, E, N, U represent three directions of east, north, and sky of geographic coordinates; 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
n
n
δ v̇E = fibN
φU − fibU
φN +
+ 2ωie sin L +
vE
δvN
RN + h
+ 2ωie cos LvN +
− 2ωie cos L +
vU
vN
δvE
tan L −
RM + h
RM + h
vE vN
sec2 L + 2ωie sin LvU δL
RN + h
vE
δvU + ∇E
RN + h
n
n
δ v̇N = fibU
φE − fibE
φU − 2 ωie sin L +
vE
vU
tan L δvE −
δvN
RN + h
RM + h
2.3 Inertial Navigation System
−
25
vE
vN
sec2 L vE δL + ∇N
δvU − 2ωie cos L +
RN + h
RM + h
n
n
δ v̇U = fibE
φN − fibN
φE − 2 ωie cos L +
+2
vE
δvE
RN + h
vN
δvU − 2ωie sin Lve δL + ∇U
RM + h
(2.36)
Position error equation
δvN
RM + h
δvE
vE
δ λ̇ =
sec L +
sec L tan LδL
RN + h
RN + h
δ L̇ =
(2.37)
δ ḣ = δ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:
⎤⎡
⎤
⎡ ⎤ ⎡
C11 C12 C13
εx + Wzx
φx
⎥⎢
⎥
⎢ ⎥ ⎢
⎢φ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 misalignment 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
φ
⎢
⎥ ⎢
⎥ ⎢ x ⎥ ⎢ 11
⎢
⎥ ⎢
⎥⎢ ⎥ ⎢
⎢
⎥ ⎢
⎥⎢ ⎥ ⎢
⎢ δay ⎥ = ⎢ −az
0
ax ⎥ ⎢ φy ⎥ + ⎢ C21
⎢
⎥ ⎢
⎥⎢ ⎥ ⎢
⎣
⎦ ⎣
⎦⎣ ⎦ ⎣
δaz
ay −ax
0
φz
C31
⎤⎡
C12
C22
C32
C13
∇x + a∇x
⎤
⎥⎢
⎥
⎥⎢
⎥
⎥⎢
⎥
C23 ⎥ ⎢ ∇y + a∇y ⎥.
⎥⎢
⎥
⎦⎣
⎦
C33
∇z + a∇z
(2.42)
Velocity-position error equation
Simplify the gravitation field model, the velocity-position error equation is:
⎡
⎤ ⎡
⎤⎡
⎤
δ V̇x
δVx
0 0 0 f14 f15 f16
⎢
⎥ ⎢
⎥⎢
⎥
⎢ δ V̇y ⎥ ⎢0 0 0 f24 f25 f26 ⎥ ⎢ δVy ⎥
⎢
⎥ ⎢
⎥⎢
⎥
⎢
⎥ ⎢
⎥⎢
⎥
⎢ δ V̇z ⎥ ⎢0 0 0 f34 f35 f36 ⎥ ⎢ δVz ⎥
⎢
⎥=⎢
⎥⎢
⎥
(2.43)
⎢
⎥ ⎢
⎥⎢
⎥
0
0 ⎥ ⎢ δx ⎥
⎢ δ ẋ ⎥ ⎢1 0 0 0
⎢
⎥ ⎢
⎥⎢
⎥
⎢ δ ẏ ⎥ ⎢0 1 0 0
⎢
⎥
0
0⎥
⎣
⎦ ⎣
⎦ ⎣ δy ⎦
δż
0 0 1 0
0
0
δz
where,
f14 =
x2
∂gx
GM
− 3 1−3 2 ;
∂x
r
r
f16 =
f25 =
GM xz
∂gx
=3 3 2
∂z
r r
∂gy
GM
(R0 + y)2
=− 3 1−3
r
r2
∂y
f15 =
f24 =
∂gx
GM x(y + R0 )
;
=3 3
∂y
r
r2
∂gy
∂x
f26 =
=
∂gx
= f15
∂y
∂gy
GM
=3 3
r
∂z
(R0 + y) z
r2
2.3 Inertial Navigation System
f34 =
f36 =
∂g
∂gz
= x = f16
∂x
∂z
z2
∂gz
GM
=− 3 1−3 2
∂z
r
r
27
f35 =
r=
∂gy
∂gz
=
= f26
∂y
∂z
x2 + (y + R0 )2 + z2
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
0
−g
0
0
2wie sin L
⎢
⎥
⎥
⎡
⎤ ⎢
⎢
⎥
⎢−2wie sin L
⎥
0
0
g
0
0
δ̇Vx
⎢
⎥
⎢
⎥ ⎢
⎥
⎢δ̇Vy ⎥ ⎢
⎥
1
⎢
⎥ ⎢
⎥
⎢
⎥ ⎢
0
0
0
0
0
⎥
⎢δ L̇ ⎥ ⎢
R
⎥
⎢
⎥=⎢
⎥
⎢
⎥ ⎢
1
⎥
⎢φ̇x ⎥ ⎢
0
sin
L
−w
cos
L
0
0
w
⎥
ie
ie
⎢
⎥ ⎢
⎥
R
⎢φ̇ ⎥ ⎢
⎥
⎣ y ⎦ ⎢
⎥
1
⎢
⎥
0
0
0
−wie sin L −wie sin L
⎢
⎥
φ̇ z
R
⎢
⎥
⎣
⎦
tgL
0
0
0
wie cos L wie cos L
R
⎤ ⎡
⎤
⎡
∇x + adx
δVx
⎥ ⎢
⎥
⎢
⎢ δVy ⎥ ⎢ ∇y + ady ⎥
⎥ ⎢
⎥
⎢
⎥ ⎢
⎥
⎢
⎥
⎢ δL ⎥ ⎢ 0
⎥+⎢
⎥
⎢
(2.44)
⎥ ⎢
⎥
⎢
⎢ φx ⎥ ⎢ εx + wdx ⎥
⎥ ⎢
⎥
⎢
⎢φ ⎥ ⎢ε + w ⎥
dy ⎦
⎣ y ⎦ ⎣ y
φz
εz + wdz
28
2 Principle of INS/CNS/GNSS Navigation System
To have Laplace transform for Formula (2.44):
⎡
⎤
0
0
−g
0
0
2wie sin L
⎥
⎡
⎤ ⎢
⎢−2wie sin L
⎥
0
0
g
0
0
sδVx (s)
⎢
⎥
⎢
⎥ ⎢
⎥
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
⎦ ⎢
sin
L
−w
sin
L
0
0
0
−w
ie
ie
⎢
⎥
R
⎢
⎥
sφz (s)
⎣ tgL
⎦
0
0
0
wie cos L wie cos L
R
⎤ ⎡
⎤
⎡
∇x (s) + adx (s)
δVx0 (s)
⎥ ⎢
⎥
⎢
⎢δVy0 (s)⎥ ⎢∇y (s) + ady (s)⎥
⎥ ⎢
⎥
⎢
⎥ ⎢
⎥
⎢
⎥
⎢δL0 ⎥ ⎢0
⎥+⎢
⎥
⎢
(2.45)
⎥ ⎢
⎥
⎢
⎢φx (s) ⎥ ⎢εx (s) + wdx (s) ⎥
⎥ ⎢
⎥
⎢
⎢φ (s) ⎥ ⎢ε (s) + w (s) ⎥
dy
⎦ ⎣ y
⎦
⎣ y
φ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:
(sI − F )−1 =
N (s)
|sI − F |
(2.49)
where denominator is the determinant of the system characteristics matrix, molecular 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 | =
0
0
g
s
2wie sin L
−2wie sin L
s
0
−g
0
1
0
−
s
0
0
R
1
0
0
s
−wie sin L
R
1
−
0
wie sin L
wie sin L
s
R
tgL
−
0
0
−wie cos L −wie cos L
R
2
2
2
2
= s 2 + ωie
s + ωs2 + 4s 2 ωie
sin2 L
0
0
wie cos L
0
s
0
(2.50)
g
is Shula frequency.
where ws2 =
R
The characteristic equation of the system is:
2
s 2 + ωie
2
s 2 + ωie
2
2
+ 4s 2 ωie
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:
Ts =
2π
= 84.4min
ws
30
2 Principle of INS/CNS/GNSS Navigation System
TF =
2π
2π
=
.
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 navigation 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 navigation 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 collectively 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 positioning 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 constellation, 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
three-dimensional positioning
sketch map
Sat2
Sat3
Satn
……
Sat1
ρ2
ρ3
ρ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 pseudorange, 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
+ c t(i = 1,2, 3,4 . . .).
ρi = (x − xsi )2 + (y − ysi )2 + (z − zsi )2
(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 position 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.,:
ρ̇i =
(x − xsi )(ẋ − ẋsi ) + (y − ysi )(ẏ − ẏsi ) + (z − zsi )(ż − żsi )
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 electromagnetic 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 simultaneous observation value to work out the difference, i.e., relative positioning
method.
Tropospheric refraction error: Tropospheric refractive index is irrelevant to frequency or wave length of electromagnetic wave, and reduces gradually with increase
of altitude; it is also closely related to atmospheric pressure, humidity, and temperature. 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 influence 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
4.2
4.2
3.0
3.0
1.0
1.0
0.5
0.5
0.9
0.9
9.6
9.6
2.3
5.0 ∼ 10.0
2.0
2.0
1.2
1.2
0.5
0.5
Subtotal
6.0
8.7 ∼ 13.7
(3) Signal reception
√
Receiver noise
√
Others
1.0
0.5
0.5
7.5
Subtotal
1.5
8.0
Total
17.1
26.3 ∼ 31.3
(1) Satellite
√
Ephemeris error and model error
√
Clock error and stability
√
Satellite perturbation
√
Uncertainty of phase position
√
Others
Subtotal
(2) Signal propagation
√
Ionospheric refraction
√
Tropospheric refraction
√
Multipath error
√
Others
angle of celestial body relative to the horizon is generally used and real-time positioning 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 satellite. 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
O
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 contoured 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 centerof-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 celestial navigation methods of direct sensitive horizon and indirect sensitive horizon
38
2 Principle of INS/CNS/GNSS Navigation System
Sun
i1
恒星1
Star 1
近天体1轨道
Orbit of nearby
celestial body 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 equation 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 simple, 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 measured 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 satellite 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
⎪ dvx = −μ x 1 − J2 Re
⎪
⎪
⎪
r3
r
⎪ dt
⎪
⎪
⎪
⎪ dvy
Re
y
⎪
⎪
= −μ 3 1 − J2
⎪
⎪
dt
r
r
⎪
⎪
⎪
⎪
⎪
Re
z
dvz
⎪
⎪
= −μ 3 1 − J2
⎩
dt
r
r
r=
z2
−
1.5
+ FX
r2
z2
7.5 2 − 1.5 + Fy
r
z2
7.5 2 − 4.5 + Fz
r
(2.60)
7.5
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 vector 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:
⎧
2
da
⎪
⎪
⎪
⎪ dt = n√1 − e2 [e(S sin f + T cos f ) + T ]
⎪
⎪
⎪
√
⎪
⎪
⎪
1 − e2
de
⎪
⎪
⎪
=
[(S sin f + T cos f ) + T cos E]
⎪
⎪ dt
na
⎪
⎪
⎪
⎪
⎪
di
r cos (w + f )
⎪
⎪
⎪
⎨ dt = na 2 √1 − e2 W
d
r sin (w + f )
⎪
⎪
⎪
W
=
√
⎪
⎪
dt
⎪
na 2 1 − e2 sin i
⎪
⎪
√
⎪
⎪
⎪
dw
1 − e2 d
r
⎪
⎪
sin f · T − cos i
−cos f · S + 1 +
=
⎪
⎪
⎪
dt
nae
p
dt
⎪
⎪
⎪
⎪
2
⎪
r
1−e ⎪ dM
⎪
S + 1 + pr sin f · T
− cos f − 2e
=n−
⎩
dt
nae
p
(2.61)
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
1
2
⎪
= √
(1 + 2e cos f + e2 ) 2 U
⎪
⎪
⎪ dt
n 1 − e2
⎪
⎪
⎪
√
⎪
⎪
2
⎪
√
1
⎪
⎪ de = 1 − e (1 + 2e cos f + e2 ) 2 [2( cos f + e)U − 1 − e2 sin E · N ]
⎪
⎪
⎪
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
⎪
dw
1 − e2
d
⎪
⎪
=
(1 + 2e cos f + e2 ) 2 [2 sin f · U + ( cos E + e)N ] − cos i
⎪
⎪
⎪
dt
nae
dt
⎪
⎪
⎪
⎪
⎪
1
⎪
dM
2e2
1 − e2
⎪
⎩
sin E)U +( cos E −e)N ]
= n−
[(1 + 2e cos f +e2 ) 2 (2 sin f + √
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
+ vα
Z(k) = α + vα = arccos −
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
− arcsin
γ = arccos −
r
r
s · r
Re
Z(k) = γ + vγ = arccos −
− arcsin
r
r
+ vγ
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 spacecraft, 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
u
s
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 geocentric direction; vector uup is the unit vector perpendicular to the starlight inside
(s×rs )×s
the plane comprising starlight and spacecraft position vector, i.e., uup = |(s×r
.
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
B
us
A
Z
R
ha hg
a
ha
S
·
Bottom radius b
Spacecraft
Re
u up
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 attitude 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 reference 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 performance (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 reference 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 noncollinear 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 vectors, 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 pseudoinverse 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:
f (G) =
∗
2
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 :
Aopt = Gopt =
2.5.3
1 ∗
G ((3I − (G∗ )T G∗ ).
2
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
of star sensor
Star
Lens hood
Sensitive
surface
Sensory system
Lens
Sensitive star map data
Star map matching
identification
Extraction of center
of mass for star
Attitude
determination
Data processing system
Star map preprocessing
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, position 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 attitude 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 facula 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
− (x−xc ) − (y−yc )
I0
2
2
l (x, y) =
e 2σP SF e 2σP SF
2
2πσP SF
(2.64)
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
!
x̄ =
2m
!
xij Iij
i,j
2m
!
i,j
, ȳ =
Iij
yij Iij
i,j
2m
!
(2.65)
Iij
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:
ŵiT ŵj =
N
= Gij x̂0 , ŷ0 , fˆ
D1 D2
(2.66)
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 ij
ij
ij
y0 viT vj = Gij x̂0 , ŷ0 , fˆ − (2.67)
,
,
∂x0 ∂y0 ∂f f ∂Gij ∂Gij ∂Gij Rij = viT vj − Gij x̂0 , ŷ0 , fˆ = − ,
,
∂x0 ∂y0 ∂f x0 y0 f (2.68)
Therefore for i = 1,2 . . . n − 1, following equation can be obtained:
R=A Z
(2.69)
wherein,
∂G12
∂x0
∂G13
∂x0
A = −
..
.
∂Gn−1,n
∂x0
∂G12
∂y0
∂G13
∂y0
..
.
∂G12
∂f
∂G13
∂f
..
.
∂Gn−1,n
∂y0
∂Gn−1,n
∂f
T
R = R12 , R13 · · · R23 · · · Rn−1,n
Z=
x0
y0
f
T
50
Fig. 2.16 Two-dimensional
conversion sketch map
2 Principle of INS/CNS/GNSS Navigation System
Star b
Sky
Star a
.
f
p
.
o
Lens
Y-axis
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 correspondingly converted into:
∂G ∂G x0 ij
ij
Rij = viT vj − Gij x̂0 , fˆ = (2.70)
,
∂x0 ∂f f wherein, domain of definition of variable quantity also changes correspondingly, and
it can be obtained in such case that:
⎧
2
⎪
⎪
⎪ N = xi xj + f
⎪
⎪
⎨
(2.71)
D1 = xi2 + f 2
⎪
⎪
⎪
⎪
⎪
⎩ D = x2 + 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
(2.72)
4f xi2
∂Gij
⎪
⎪
=
⎪
2
2
⎩ ∂f
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 generation of flying machine such as the guided missile, aircraft, and satellite. Starting
with basic knowledge of inertial/celestial/satellite integrated navigation, this chapter first introduces coordinate systems commonly used during navigation and their
mutual relations as well as reference model of the Earth. Then, it outlines operating 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.
2.
3.
4.
5.
6.
7.
8.
9.
10.
Yi GQ (1987) Inertial navigation principle. Aviation Industry press, Beijing
Deng ZL (1994) Inertial navigation system. Harbin Institute of Technology press, Harbin
Yuan X (1995) Navigation system. Aviation Industry press, Beijing
Gai E (2000) The century of inertial navigation technology. IEEE proceedings of aerospace
conference. Big sky: IEEE, 3(1), pp. 59–60
Barbour N, Schmidt G (2001) Inertial sensor technology trends. IEEE Sens J 1(4):332–339
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
Bar-Itzhack IY, Goshen-Meskin D (1992) Unified approach to inertial navigation system error
modeling. J Guid Control Dyn1 15(3):648–653
Xu YW (1991) Analysis and design of ballistic missile and launch vehicle system. China
astronautic publishing house, Beijing
Wang HN (2003) Principle and application of GPS navigation. Beijing Science & Technology
press, Beijing
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. Cptoelectron 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 application 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
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_3
53
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 probability 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 ⎪
⎪
⎬
,
(3.3)
E[Vk ] = 0, Cov Vk , Vj = E Vk Vj T = Rk δkj
⎪
⎪
⎭
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)
Kk = Pk/k−1 Hk T (Hk Pk/k−1 Hk T + Rk )−1
(3.6)
Kk = Pk/k−1 Hk T Rk −1
(3.7)
Filtering gain:
Or
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)
Pk = (I − Kk Hk )Pk/k−1
(3.10)
−1
Pk−1 = Pk/k−1
+ HkT Rk−1 Hk
(3.11)
Or
Or
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 information 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 information 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 timeupdating 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 applications [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, Sunahara, 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 system 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 application 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 restrictions are usually imposed on above model, and several assumptions complying with
the reality and convenient for mathematical treatment are provided for statistical characteristics 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) = U (k)
X̂(k|k), 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 X̂(k + 1|k) = Z(k + 1)
h X̂(k + 1|k), 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
⎪
⎪
'
⎪
(
⎪
⎪
X̂(k
+
1|k),
k
+
1
+
K(k
+
1)
Y
(k
+
1)
−
h
⎪
⎪
⎪
⎪
⎪
⎨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]
,
|
∂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)
[k + 1, k] =
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 calculation. 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 covariance 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 sampling 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 + τ )
⎪
⎪
⎪
⎪
⎨
√
√
(3.22)
Wi = 1/[2(n + τ )]
χ i,k = x̂ k + n + τ P (k|k) i
⎪
⎪
⎪
⎪
√
√
⎪
⎩ χ
n + τ P (k|k) i Wi+n = 1/[2(n + τ )]
i+n,k = x̂ k −
√
P (k |k ) i is row i of A, and when
wherein, τ ∈R; when P (k|k) = AT A,
√
P (k|k) = AAT , P (k |k ) i is column i of A. Standard UKF algorithm is as follows:
1. Initialization
x̂ 0 = E[x 0 ],
P0 = E
x 0 − x̂ 0
x 0 − x̂ 0
T ,
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)
x̂ −
k =
(3.25)
2n
Wi χ i,k|k−1
(3.26)
T
Wi χ i,k|k−1 − x̂ k̄ χ i,k|k−1 − x̂ k̄ + Qk
(3.27)
i=0
Pk− =
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)
T
Wi y i,k|k−1 − ŷ k̄ y i,k|k−1 − ŷ k̄ + Rk
(3.30)
T
Wi χ i,k|k−1 − x̂ k̄ y i,k|k−1 − ŷ −
k
(3.31)
i=0
4. Measurement Updating
2n
Pỹk ỹk =
i=0
2n
Pxk yk =
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 estimation 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:
p̂(x0:t |y1:t ) =
1
N
N
δx (i) (dx0:t ),
0:t
(3.35)
i=1
wherein, δ is Dirac function; when N is large enough, the posterior estimation probability 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)
, y1:t = p xt(i) x0:t−1
, y1:t
(3.36)
q xt(i) x0:t−1
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)
= p y1:t | x0:t
(3.37)
w̃t x0:t
p x0:t /q x0:t |y1:t
Normalize the weight and we get:
)
(i)
(i)
= w̃t x0:t
wt x0:t
N
i=1
(i)
w̃t x0:t
(3.38)
3.5 Particle Filter
63
Calculate valid particle size Neff :
Neff =
N
!
i=1
1
(i)
wt x0:t
2
(3.39)
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
u to make u ∼ U[0,1] ;
i
) from Markov chain;
Sample xk∗(i) ∼ p(xk xk−1
If u ≤ min 1,
∗(i)
p yk xk
(i∗)
p yk xk
:
∗
(i )
i
Adopt MCMC movement, x0:k
= xk−1
, xk∗(i) ,
∗
(i )
i
= x0:k
Otherwise: 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)
T
wki xki − x̂k xki − x̂k
(3.41)
i=1
N
pk =
i=1
As recursive Bayes filter based on posterior probability distribution, PF may
effectively solve optimal estimation problem of integrated navigation system 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 measurement 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
(3.42)
, y1:k = N x̄k(i) , Pk(i) , i = 1, · · · , N ,
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)
⎬
(3.43)
T ⎪
P0(i) = E (x0(i) − x̄0(i) )(x0(i) − x̄0(i) ) ⎭
2. Steps Are as Follows When t ≥ 1:
– Sampling:
(
'
(
'
(i)
(i)
, Pk−1
by using UKF to get x̄k(i) , Pk(i) , and sample
Update particle xk−1
(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
(i)
w̃k(i) = w̃k−1
(3.44)
(i)
q x̂k(i) x0:k−1
, y1:k
Normalize the weight and we get:
wk(i) = w̃k(i)
)
N
w̃k(i)
i=1
(3.45)
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)
T
wki xki − x̂k xki − x̂k
(3.47)
i=1
N
pk =
i=1
Applying to nonlinear and non-Gaussian noise system, UPF is an advanced
nonlinear non-Gaussian filter method which can greatly meet filtering demands 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 development 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
to be zero-mean Gaussian white noise, and the covariance is
% and assumed
&
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 appearing 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) ≈ ŷi (t) +
= ŷi (t) +
t
t
∂ ŷi (t)
t 2 ∂ 2 ŷi (t)
t pi ∂ pi ŷi (t)
+
+
·
·
·
+
∂t
2! ∂t 2
pi ! ∂t pi
∂hi ∂ x̂
t2 ∂
+
∂ x̂ ∂t
2! ∂t
∂hi
∂t
+ ··· +
t pi ∂
pi ! ∂t
∂ pi −1 hi
∂t pi −1
(3.52)
In accordance with k -order Lie derivative Lkf (hi ) in Lie derivative definition:
L0f (hi ) = hi
∂Lk−1
f (hi )
f (x̂(t), t)
Lkf (hi ) =
∂ x̂
(3.53)
k≥1
Since the minimum order of any component of D(t) first appearing in hi differential
i
is pi , when the differential order is less than pi , ∂h
G(x̂(t), t)D(t) = 0, therefore in
∂ x̂
accordance with Lie derivative definition, Eq. (3.52) may be written as:
ŷi (t +
t) ≈ ŷi (t) +
tL1f (hi ) +
t2 2
L (hi ) + · · ·
2! f
p −1
+
i
t pi pi
t pi ∂Lf (hi )
Lf (hi ) +
G(x̂(t), t)D(t)
pi !
pi !
∂ x̂
(3.54)
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:
λii =
t pi
,
pi !
i = 1, . . . , m
(3.56)
Record first-order Lie derivative of scalar function Lkf (hi ) relevant to the vector field
gj x̂ (t), t as Lgj Lkf (hi ):
Lgj Lkf (hi ) =
∂Lkf (hi )
∂ x̂
gj (x̂(t), t)
(3.57)
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 ) ⎥
(3.58)
U x̂ (t) = ⎢
⎥,
⎥
⎢
···
···
···
⎦
⎣
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
Si (x̂(t), t) =
k=1
tk k
L (hi ), i = 1, . . . q
k! f
(3.59)
Define the performance index function according to operating principle of predictive
filter:
T
1
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:
'
(−1
T
D̂k = − ( t) U x̂k R −1 ( t) U x̂k + W
(3.61)
T
· ( t) U x̂k R −1 S x̂k , t − zk+1 + ẑk
Mk =
(−1
T
( 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 decentralized 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 filtering 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 subsystems, 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
Subsystem 1
Z1
Local filter 1
−1
Xˆ g , β 2 Pg
Subsystem 2
Z2
Local filter 2
M
M
Subsystem 1
ZN
Local filter M
Master filter
Xˆ 1 , P1
Time updating
Xˆ g
Xˆ 2 , P2
−1
Xˆ g , β N Pg
Xˆ N , PN
Xˆ g , Pg
Xˆ m , Pm
β m −1 Pg
Optimal fusion
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)
X̂i (k) = X̂f (k)
i = 1,2, · · · , n, m,
(3.64)
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 twostage 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 collaborative 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 covariances 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 programming. 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 satellites 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 equations 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 using 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 errors. Inertial component error is just an error of the gyro and accelerometer; for
instance, for electromechanical gyro, such error is mainly caused by texture structure, 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
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_4
75
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 performance 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 components. 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 unbalance, 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 operation 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, lowpower 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 inhomogeneity, 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 designed by Draper Lab in 1988. The device is a monolithic-silicon gyroscope and
consists of a vibrating mass, electrostatic drive electrodes, electrostatic pickoff electrodes, 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:
θy =
MG
I1 Sθx
·
= 2
Iy S 2 + D y S + K y
Iy S + 2ζy ωy S + ωy2
=
Iy ωy2
I1 ωx θx 90
·
2 2
2
1 − (ωx /ωy) + (2ζy ωx /ωy)
(4.6)
engineering unit (rad), Dy and ζy are
where θy is torsion amplitude in output axis in
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 amplitude (θ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 ⎜
-
⎝
) 2 2 ) 2 ⎠
Iy ωy2 1 − ωx ωy
+ 2ζy ωx ωy
(4.7)
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 ⎝
) 2 2 ) 2 ⎠⎠
2
Iy ωy 1 − ωx ωy
+ 2ζy ωx ωy
(4.8)
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:
⎞⎞
⎛ ⎛
⎟⎟ 1
⎜ ⎜
e f θy
I1 ωx θx 90
⎟⎟ . (4.9)
⎜
= e⎜
S=
-
⎝f ⎝
2 2 2 ⎠⎠
+ 2ζy ωx /ωy
Iy ωy2 1 − ωx /ωy
4.2 Error Modeling and Compensation of Inertial Sensors
81
The manufacturing tolerance, material inhomogeneity will result in variation the resonant 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 microstructure 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 frequencies 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 mechanical asymmetrical error of scale factor. The error of demodulation circuit will
result in electrical asymmetrical error of scale factor too. Compared with the conventional 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:
C = f (θy ) = −
2
εhp z02 − (l + b) θy2
ln 2
θy
z0 − (l · θy )2
(4.10)
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:
C ≈ P (θy ) =
εhp b(2s + b)
· θy
z02
(4.11)
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 (ζ ) · θy2
2
(4.12)
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 wellknown 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 )
⎪
⎪
· ( − n-1 ), n-1 < ≤ n
⎪S( n-1 ) +
⎪
h
⎪
⎪
⎪
⎪
⎪
S( 1-n ) − S( -n )
⎪
⎩S( 1-n ) +
(4.14)
· ( − 1-n ), -n < ≤ 1-n
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
.
S( )
(4.15)
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
1th order fitting
0.0198
0.0197
(
(
S g / V⋅ (°) ⋅ s −1
)
−1
)
0.0199
0.0196
0.0195
0.0194
-100
-80
-60
-40
-20
(
0
Ω / (°) ⋅ s −1
20
)
40
60
80
100
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 factor 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,
4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)
15000
10000
5000
0
-5000
-100
−1
Ef
−1
Ω / (°) ⋅ s−1
(
0
)
50
100
Ω / (°) ⋅ s−1
(
0
)
50
100
(
0
)
50
100
)
50
100
0
-50
1000
0
-1000
-100
((°)⋅ h )
Ec
-50
2000
-2000
-100
((°)⋅h )
En
−1
((°)⋅h )
Ey
−1
((°) ⋅h )
84
-50
Ω / (°) ⋅ s−1
100
0
-100
-100
-50
0
Ω / (°) ⋅ 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 asymmetry 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 compensation 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 compensation 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 thermocouples 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 manufacturing 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 temperature 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 K0 [1 − (λE + λσ αE) T ]
1 K
=
,
(4.23)
f =
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
zc = Lz α T ,
(4.24)
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:
uf =
Iy
S2
Q
k V ME
= kV
ME
2
K
+ 2ζy ωy S + ωy
y
(4.26)
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 analyzed 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 resulting 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 subtracting 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 gyroscope. 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.3
4.3.1
4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)
Design, Error Calibration, and Compensation of Inertial
Measurement Units
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 position, 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 sinusoidally dithering the gyroscope, a continuous input rate is maintained on the
gyroscope to eliminate the lock-in effect. However, the mechanical dither will become 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 compensation 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 module, 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 module. 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
part in the
dithered
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)
2
B
B
2
B
i=1 + mGi RGi ω̇IS + ωIS × mGi RGi ωIS + mGi (g + a)RGi
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
B
body frame, aIB
= 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
MA =
B
B
2
2
ω̇IS + mAi (g + a)RAi + ωBIS × IAi + mAi RAi
IAi + mAi RAi
ωIS
i=1
(4.30)
2
2
MS = IS + mS RS ω̇BIS + mS (g + a) RS + ωBIS × IS + mS RS ωBIS ,
(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
ˆ cos (ωb t), with ωb
the inertial frame, expressed in the body frame, ωBIB = = 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 magnitude 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
2
2
2
˙ + θ̈
IS + mS RS +
IGi + mGi RGi + IAi + mAi RAi
i=1
3
+
3
(mGi RGi + mAi RAi ) + mS RS (g + a) +
i=1
IGi ϕ̈i + Dθ̇ + Tθ = 0,
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
2
2
+
I
+
m
R
+
m
R
=
I
,
M
where I = IS + mS RS2 +
Gi
Ai
Gi
Ai
0
Gi
Ai
i=1
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
˙ + K = 0,
(4.41)
IS
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,
is the
accelerometers, and ISA structure, m̄ = ms + 3i=1 (mGi + mAi ), and aIS
linear acceleration of ISA frame with respect to the inertial frame, expressed in the
body frame and can be expressed as
2
B
¨ + 2θ̇ × ,
˙
= aBIB + g + aBBS = a + g + θ̈δS + θ̇ δS + aIS
(4.42)
B
where aIB
is the linear acceleration of the body frame with respect to the inertial
B
B
frame, expressed in the body frame, aIB
= a, and aBS
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 dur2
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
˙ + K = 0.
m̄(a + g + )
(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
m̄(ax + gx ) + m̄ ¨ x +
k=1
n
n
ckx Yk θ̇z +
k=1
kkx
x
n
+
k=1
kkx Zk θy −
k=1
n
m̄(ay + gy ) + m̄ ¨ y +
n
k=1
y
kky Zk θx +
k=1
n
n
k=1
n
kkz
k=1
kkz Yk θx = 0
(4.46)
ckz Xk θ̇y
k=1
n
ckz Yk θ̇x +
k=1
n
ckz ˙ z −
k=1
+
(4.45)
cky Zk θ̇x
n
−
k=1
m̄(az + gz ) + m̄ ¨ z +
kky Xk θz = 0
n
n
kky
(4.44)
k=1
n
cky Xk θ̇z +
kkx Yk θz = 0
k=1
cky ˙ y −
k=1
+
ckx Zk θ̇y
k=1
n
−
n
ckx ˙ x +
z −
n
kkz Xk θy +
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
cky ˙ y Zk +
Ix θ̈x − Ixy θ̈y − Ixz θ̈z −
k=1
n
+
n
ckz Yk2 + cky Zk2 θ̇x −
n
n
ckz Yk θ̇y Xk −
k=1
kkz Yk2 + kky Zk2 θx −
k=1
n
n
kkz Yk θy Xk −
n
kky
y Zk
+
k=1
kkz
z Yk
n
+ IGx ϕ̈x + Ix ˙ x + M0x = 0
ckx ˙ x Zk −
k=1
n
n
ckz Xk2 + cky Zk2 θ̇y −
kkz Xk2 + kky Zk2 θy −
n
n
ckz Xk θ̇x Yk −
x Zk
−
k=1
kkz
n
kkz Xk θx Yk −
z Xk
n
+ IGy ϕ̈y + Iy ˙ + M0y = 0
ckx ˙ x Yk +
k=1
cky Xk2 + ckx Yk2 θ̇z −
kky Xk2 + kkx Yk2 θz −
cky ˙ y Xk
n
n
cky Xk θ̇x Zk −
n
n
kky Xk θx Zk −
n
kkx
x Yk +
kky
ckx Yk θ̇y Zk
k=1
k=1
n
k=1
n
k=1
k=1
−
(4.48)
k=1
k=1
n
kkx Zk θz Yk
k=1
k=1
Iz θ̈z − Ixz θ̈x − Iyz θ̈y −
+
ckx Zk θ̇z Yk
k=1
n
n
kkx
n
ckz ˙ z Xk
k=1
n
+
n
k=1
k=1
+
(4.47)
k=1
k=1
+
kky Zk θz Xk
k=1
k=1
Iy θ̈y − Ixy θ̈x − Iyz θ̈z +
+
cky Zk θ̇z Xk
k=1
k=1
n
−
ckz ˙ z Yk
k=1
k=1
+
n
y Xk
kkx Yk θy Zk
k=1
+ IGz ϕ̈z + Iz ˙ z + M0z = 0
(4.49)
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
to symmetrical
integral
which
!n
!n
!nprinciple, there
!nexist two equations,
!n
are
k
Y
=
k
Z
=
k
X
=
k
Z
=
k
Xk =
kx
k
kx
k
ky
k
ky
k
kz
k=1
k=1
!n k=1
!k=1
!n
!n k=1
n
k
Y
=
0,
k
Y
Z
=
k
Z
X
=
k
X
Y
=
0.
Hence,
kz
k
kx
k
k
ky
k
k
kz
k
k
k=1
k=1
k=1
k=1
the spring force and torque of absorbers along every axis can be uncoupled. For the
same absorbers, the parameter kkj /c
the integral
kj is constant. Therefore,
!n
!n damping
!
n
c
Y
=
c
Z
=
force
and
torque
are
zero,
that
is
kx
k
kx
i
k=1
!n
!n
!n
!n k=1
!n k=1 cky Xk =
c
Z
=
c
X
=
c
Y
=
0,
c
Y
Z
=
ky
k
k
k
kz
k
kx
k
k
k=1
k=1
k=1
k=1 cky Zk Xk =
!k=1
n
=
0.
As
the
centroid
and
the
original
point
of
the
reference
frame are
c
X
Y
kz
k
k
k=1
superposition, the centroid of ISA with inertial sensors can be expressed as
!
3
+
m
R
+
m
R
R
(m
)
Gi
Gi
Ai
Ai
S
I
i=1
!
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
2
3
M0 =
m S RS +
1
(mGi RGi + mAi RAi ) (g + a)
i=1
2
3
(mGi + mAi ) + mS (g + a) = 0.
= Pco
(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
m̄ ¨ x + Cx ˙ x + Kx
x
= −m̄(ax + gx )
m̄ ¨ y + Cy ˙ y + Ky
m̄ ¨ z + Cz ˙ z + Kz
y
z
= −m̄(ay + gy )
(4.52)
= −m̄(az + gz )
Ix θ̈x + Dx θ̇x + Tx θx = −IGx ϕ̈x − Ix ˙ x
Iy θ̈y + Dy θ̇y + Ty θy = −IGy ϕ̈y − Iy ˙ y
(4.53)
Iz θ̈z + Dz θ̇z + Tz θz = −IGz ϕ̈z − Iz ˙ z
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
⎡ ⎤ ⎡
⎤ ⎡
⎤
⎢
⎢
⎣
⎥ ⎢
⎥ ⎢
y⎦ = ⎣
0x
⎥ ⎢
⎥ ⎢
0y ⎦ + ⎣
ax
z
0z
az
x
⎥
⎥
(4.54)
ay ⎦
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
θ0x
θbx
θx
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢θy ⎥ = ⎢θ0y ⎥ + ⎢θby ⎥
⎣ ⎦ ⎣ ⎦ ⎣ ⎦
θz
θ0z
θbz
(4.55)
T T
,
represent the linear and angular diswhere
θ
θ
θ
0x
0y
0z
0x
0y
0z
placement solutions of kinetics models for static environment where there exist only
T T
, θbx θby θbz represent
mechanical dither of RLG, and
ax
ay
az
the linear and angular displacement solutions of kinetics models for the case where
there exists only inputting external linear and angular vibration. In static environment, there is no external inputting linear and angular acceleration, that = ˙ = 0,
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
⎡
⎡
⎤
⎤
m̄gx /Kx
0x
⎢
⎢
⎥
⎥
⎢ 0y ⎥ = − ⎢m̄gy /Ky ⎥.
(4.56)
⎣
⎣
⎦
⎦
m̄gz /Kz
0z
With the mechanical dither of three RLGs, the rotational kinetics model Eq (4.50)
can be solved as
⎡ ⎤ ⎡ θ̂ cos ω t + φ ⎤
x
px
px
θ0x
⎥
⎢ ⎥ ⎢
⎥
(4.57)
⎣ θ0y ⎦ = ⎢
⎣ θ̂y cos ωpy t + φpy ⎦,
θ0z
θ̂z cos ωpz t + φpz
2
2
2 2
2
2
2 2
2
where θ̂j = −IGjˆϕ j ωpj
/Ij (ω0j
− ωpj
) + 4μ2j ωpj
= −IGjˆϕ j ωpj
/ (Tj − Ij ωpj
) + Dj2 ωpj
,
2
2
2
φpj = arctg 2μj ωpj / ω0j
− ωpj
, ω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 ϕ̂
Gj j
3 3
≤ δ.
3θ̂j 3 =
Ij
(4.58)
According to Eq. (4.58), the design requirement of moment of inertia for ISA is
given by
Ij ≥
IGj ϕ̂j
δ
.
(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 2
2 2
(ω0j − ωpj ) + 4μj ωpj
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
4
2
2
ω
−2I
ϕ̂
ω
ω
−
ω
3
3
Gj
j
0j
pj
0j
pj
d MTj
=
(4.61)
3 2 .
2
dω0j
2
2
2 2
ω0j − ωpj + 4μj ωpj
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
⎡
⎤ ⎡
⎤
ˆ x cos (ωax t + ψx )
ax
⎢
⎥ ⎢
⎥
⎢ ay ⎥ = ⎢ ˆ y cos (ωay t + ψy )⎥
(4.62)
⎣
⎦ ⎣
⎦
ˆ z cos (ωaz t + ψz )
az
2
2 2
2
2
2
,
− ωaj
) + 4ξ 2 ωaj
, ψj = arctan 2ξj ωaj / ω0aj
− ωaj
where ˆ j = −âj / (ω0aj
2
with ω0aj being the linear inherent resonance circular frequency, ω0aj
= 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
⎥
⎢ ⎥ ⎢
⎥,
(4.63)
θ̂
cos
ω
t
+
φ
⎣ θby ⎦ = ⎢
by
by
by
⎣
⎦
θbz
θ̂bz cos (ωbz t + φbz )
2
2 2
2
where θ̂bj = − ˆ j ωbj / (ω0j
− ωbj
) + 4μ2j ωbj
, the phase φbj of angular response
2
2
with respect to , φbj = arctg 2μj ωbj / ω0j
+π/2. The less the rotational
− ωbj
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
2
2
2
−2 ˆ j ωbj
− ωbj
ω0j
d 3θbj 3
=
(4.64)
3 2 .
2
dω0j
2
2
2 2
ω0j − ωbj + 4μj ωbj
3 3
Assuming that the first-order differential equation equal to zero, d 3θTj 3/dω0j = 0,
where the solutions of function existing minimum
3 3 and maximum are that ω0j = 0,
ω0j = ωbj . When ωbj > ω0j > 0, then d 3θj 3/dω0j ≥ 0, which represents the
amplitude of angular vibration monotonously
with the inherent resonance
3
3 increases
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 measured 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
4
4
ˆ j ω0j
− ωbj
d 3θ̂bj 3
=
(4.65)
3 2 .
2
dωbj
2
2
2 2
ω0j − ωbj + 4μj ωbj
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 cos (ωhj t), ω0j = qωhj . According to Eq. (4.63), the angular amplitude
hj =
can be expressed as
3 3
3 3
3θ̂bj 3 = ˆ hj ωhj
=
2
2
2
2
(q 2 ωhj
− ωhj
) + 4μ2j ωhj
ˆ hj
2
(q 2 − 1)2 ωhj
+ 4μ2j
≤ δ.
(4.66)
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
⎛ ⎜
⎜
qj ≥ ⎜
⎝
ˆ hj /δ
2
⎞1/2
1/2
−
4μ2j
ωhj
⎟
⎟
+ 1⎟
⎠
(4.67)
.
Taking account of the mechanical dither of RLG and the external high-frequency
vibration, the optimal resonance frequency should be determined as
⎡
ω0j = qωhj = ⎣ωhj
1
2
hj
δ2
21/2
− 4μ2j
⎤1/2
2 ⎦
+ ωhj
.
(4.68)
With the external vibration frequency reducing, the maximal angular amplitude measured 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
&&
θa
f ryb
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
⎤ ⎡
⎤
⎡
rxx rxy rxz
RA1
⎥ ⎢
⎥
⎢
⎢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 ⎤
⎡
xx
z xy
y xz
arxx
x
y
⎥
2
⎥ ⎢
⎢
2
⎢
(4.70)
arx = ⎣arxy ⎦ = ⎣ θ̈z rxx − θ̇x + θ̇z rxy − θ̈x rxz ⎥
⎦
2
2
arxz
−θ̈y rxx + θ̈x rxy − θ̇x + θ̇y rxz
⎤ ⎡ θ̇ 2 + θ̇ 2 r − θ̈ r + θ̈ r ⎤
⎡
yx
z yy
y yz
aryx
y
z
⎥
2
⎥ ⎢
⎢
2
⎥
ary = ⎣aryy ⎦ = ⎢
(4.71)
⎣ θ̈z ryx − θ̇x + θ̇z ryy − θ̈x ryz ⎦
2
2
aryz
−θ̈y ryx + θ̈x ryy − θ̇x + θ̇y ryz
⎤ ⎡ θ̇ 2 + θ̇ 2 r − θ̈ r + θ̈ r ⎤
⎡
zx
z zy
y zz
arzx
y
z
⎥
2
⎥ ⎢
⎢
2
⎥
arz = ⎣arzy ⎦ = ⎢
(4.72)
⎣ θ̈z rzx − θ̇x + θ̇z rzy − θ̈x rzz ⎦.
2
2
arzz
−θ̈y rzx + θ̈x rzy − θ̇x + θ̇y rzz
102
4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)
Taking account of the installing misalignment αj , βj , γj between the jth accelerometer 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 = ⎢
(4.73)
⎣ fry ⎦ = ⎣ aryx cos αy + aryy cos βy + aryz cos γy ⎦.
arzx cos (αz ) + arzy cos (βz ) + arzz cos (γz )
frzb
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 imaging 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 acceleration, 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 lockin effects which occur at low rotational rates. The dither causes the signal noise,
nonlinear error, disturbance, and a coupling measurement error. The structure optimization 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
sturcture
accelerometer
5/*
103
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 observation 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 performance 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 outage, 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 multiposition [15], rotation rate [16], and dynamic-static hybrid calibration methods, and
so on. Among all the methods, the static multiposition calibration method can accurately 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 traditional 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 fourdirection 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 Temperature
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
⎢
⎢K
⎢ ax
K=⎢
⎢Mxy
⎣
Myx
Mxz
Myz
Kay
∇z
⎤
⎥
Mzx ⎥
⎥
⎥.
Mzy ⎥
⎦
Kaz
(4.77)
106
4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)
; ;ಬ
=7
=7
<ಬ
=ಬ
;ಬ
<
Position 2
=7
=ಬ
Position 3
=7
;ಬ
<ಬ
;ಬ
=ಬ
<
; ;ಬ
<
;
=
<ಬ
;ಬ
;
Position 1
=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 ⎥
x2
x3
x4
⎥
⎢ x1
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)
0
ax
ωie cos (φ) sin ϕ(t)
⎢
⎥ ⎢ ⎥ ⎢ ⎥
⎥ ⎢
(4.80)
⎣ ωy (t)⎦ = ⎣ ωie cos (φ) cos ϕ(t)⎦, ⎣ ay ⎦ = ⎣ 0⎦,
ωz (t)
ω(i) + ωie sin (φ)
az
g
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units
;
± ωi
± ωi
<
=7
=
=
<
;
=
Position 1
± ωi
=7
=7
<
107
;
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 ⎥ = ⎢
⎥.
(4.81)
0
⎣ ⎦ ⎣
⎦
ω(i) + sin (φ)
ωz
Therefore, the state equation of the gyro channel can be modeled as:
⎡
ω̄x1
⎢
⎢ω̄x2
⎢
⎢
⎢ω̄x3
⎢
⎢
⎢ω̄x4
⎢
⎢ω̄
⎣ x5
ω̄x6
ω̄y1
ω̄y2
ω̄y3
ω̄y4
ω̄y5
ω̄y6
⎡
1 ωi+
⎥ ⎢
⎢
ω̄z2 ⎥
⎥ ⎢1 ωi−
⎥ ⎢
ω̄z3 ⎥ ⎢1
0
⎥=⎢
⎥ ⎢
ω̄z4 ⎥ ⎢1
0
⎥ ⎢
⎢
⎥
0
ω̄z5 ⎦ ⎣1
ω̄z6
1
0
ω̄z1
⎤
0
0
⎤
0
⎥⎡
0 ⎥
⎥ ⎢ εx
⎥
0 ⎥⎢
Kx
⎥⎢
⎥⎢
⎢
0 ⎥ ⎣Exy
⎥
ωi+ ⎥
⎦ Exz
0
ωi−
0
ωi+
ωi−
⎤
εy
εz
Eyx
Ky
Eyz
⎥
Ezx ⎥
⎥
⎥,
Ezy ⎥
⎦
Kz
(4.82)
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
⎢
⎢Eyx
⎣
Ezx
⎤
⎡
Ky
⎥
Eyz ⎥
⎦=
Ezy
Kz
⎢
1
·⎢
ω̄ − ω̄y2
2 · ω(i) ⎣ y1
ω̄z1 − ω̄z2
Exy
Exz
ω̄x1 − ω̄x2
ω̄x3 − ω̄x4
ω̄x5 − ω̄x6
⎤
ω̄y3 − ω̄y4
⎥
ω̄y5 − ω̄y6 ⎥
⎦. (4.84)
ω̄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)⎥
⎢
⎥
⎥ ⎢
⎣
⎦
⎦ ⎣
+ ωie sin φ
ωz (t)
(4.85)
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
⎢
⎥ ⎢ ⎥
⎢
⎥ ⎢ ⎥
⎢
⎥ ⎢ ⎥
(4.86)
⎢ay (t) ⎥ = ⎢ 0 ⎥,
⎢
⎥ ⎢ ⎥
⎣
⎦ ⎣ ⎦
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/
dt
ω̄x
ω
(t)
x
⎥ ⎢
⎢ ⎥
⎢ 0
⎥ ⎢
⎢ ⎥
⎢4
⎥ ⎢
⎢ ⎥
⎢ 2π n/
⎢ ω̄y ⎥ =
⎢ 0
ωy (t) dt ⎥ = ⎢
⎥ ⎢
⎢ ⎥ 2π n ⎢
⎦ ⎣
⎣ ⎦
⎣4
2π n/
dt
ω
ω̄z
(t)
z
0
⎤
0
⎤
⎡
0
⎥ ⎢ ⎥
⎥ ⎢ ⎥
⎥ ⎢ ⎥
⎥ = ⎢ 0 ⎥.
0
⎥ ⎢ ⎥
⎦ ⎣ ⎦
+ ωie sin φ
ω̄
(4.87)
The mean value of inputting accelerations in three sensing axes can be given by:
⎤ ⎡ ⎤
⎡ ⎤
⎡4
2π n/
0
dt
āx
a
(t)
x
⎥ ⎢ ⎥
⎢ ⎥
⎢ 0
⎥ ⎢ ⎥
⎢ ⎥
⎢4
⎥ ⎢ ⎥
⎢ ⎥
⎢ 2π n/
(4.88)
⎢ āy ⎥ =
⎢
ay (t) dt ⎥ = ⎢ 0 ⎥.
⎥ ⎢ ⎥
⎢ ⎥ 2πn ⎢ 0
⎦ ⎣ ⎦
⎣ ⎦
⎣4
2π n/
g
āz
az (t) dt
0
The system status equation of angular rate channel of RLG IMU during optimization
analytic calibration can be given by:
⎡
⎤⎡
⎤ ⎡
⎤
Gx1 Gy1 Gz1
Gbx Gby Gbz
1 ω̄ 0 0
⎢
⎥⎢
⎥ ⎢
⎥
⎢
⎥⎢
⎥ ⎢
⎥
⎢
⎥⎢
⎥ ⎢
⎥
⎢ Gx2 Gy2 Gz2 ⎥ ⎢ 1 −ω̄ 0 0 ⎥ ⎢ Sx Exy Exz ⎥
⎢
⎥⎢
⎥ ⎢
⎥
(4.89)
⎢
⎥⎢
⎥=⎢
⎥,
⎢
⎥⎢
⎥ ⎢
⎥
⎢ Gx3 Gy3 Gz3 ⎥ ⎢ 1 0 ω̄ 0 ⎥ ⎢ Eyx Sy Eyz ⎥
⎢
⎥⎢
⎥ ⎢
⎥
⎣
⎦⎣
⎦ ⎣
⎦
1 0
0 ω̄
Gx4 Gy4 Gz4
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
Abx Aby Abz
1 g 0 0
⎢
⎥⎢
⎥ ⎢
⎥
⎢
⎥⎢
⎥ ⎢
⎥
⎢
⎥⎢
⎥ ⎢
⎥
⎢ Ax2 Ay2 Az2 ⎥ ⎢ 1 −g 0 0 ⎥ ⎢ Kx Myx Mzx ⎥
⎢
⎥⎢
⎥ ⎢
⎥
(4.90)
⎢
⎥⎢
⎥=⎢
⎥,
⎢
⎥⎢
⎥ ⎢
⎥
⎢ Ax3 Ay3 Az3 ⎥ ⎢ 1 0 g 0 ⎥ ⎢ Mxy Ky Mzy ⎥
⎢
⎥⎢
⎥ ⎢
⎥
⎣
⎦⎣
⎦ ⎣
⎦
1 0 0 g
Ax4 Ay4 Az4
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⎢
) ⎥
⎢
⎥
⎢
⎥
(4.91)
⎢ Gby Eyx ⎥ = ⎢ Gy2 + Gy1 Gy1 − Gy2 ω̄ ⎥
⎢
⎥ 2⎢
⎥
⎣
⎦
⎣
⎦
)
Gz2 + Gz1 (Gz1 − Gz2 ) ω̄
Gbz Ezx
4.3 Design, Error Calibration, and Compensation of Inertial Measurement Units
⎡
Exy
⎢
⎢Sy
⎣
Ezy
⎤
⎡
Exz
Gx3 − Gbx
⎥
⎢
1
⎢
Eyz⎥
⎦ = ω̄ ⎣Gy3 − Gby
Sz
Gz3 − Gbz
⎤
Gx4 − Gbx
⎥
Gy4 − Gby⎥
⎦.
Gz4 − Gbz
111
(4.92)
According to Eq. (4.90), the error coefficients of acceleration channel of RLG IMU
can be calculated as:
⎤
⎤
⎡
⎡
g (Ax1 + Ax2 ) g Ay1 + Ay2 g (Az1 + Az2 )
Abx Aby Abz
⎥
⎥
⎢
⎢
⎥
⎥
⎢
⎢
⎥
⎥
⎢
⎢
−
A
A
−
A
A
−
A
A
⎥
⎢
⎢ Kx Myx Mzx ⎥
x1
x2
y1
y2
z1
z2
1 ⎢
⎥
⎥
⎢
⎥.
⎥=
⎢
⎢
⎥
⎥ 2g ⎢
⎢
⎢ 2 (Ax3 − Abx ) 2 Ay3 − Aby
⎢ Mxy Ky Mzy ⎥
2 (Az3 − Abz ) ⎥
⎥
⎥
⎢
⎢
⎦
⎦
⎣
⎣
2 (Az4 − Abz )
Mxz Myz Kz
2 (Ax4 − Abx ) 2 Ay4 − Aby
(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 installation 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 ψ
δGnib = Cbn δGi =⎣
cos θ sin ψ
− cos γ sin ψ − sin γ sin θ cos ψ − sin γ cos θ
cos θ cos ψ
sin γ cos ψ − cos γ sin θ sin ψ − sin γ sin ψ − cos γ sin θ cos ψ
⎡
⎡
⎤
⎤
sin θ ⎦
cos γ cos θ
⎡
⎤ 1
⎤⎡
⎤
εgx δSx δExy δExz ⎢ ⎥
T11 T12 T13
δGx
⎢ω ⎥
⎢ ε δE δS δE ⎥ ⎢ x ⎥ ⎢
⎥⎢
⎥
⎥ = ⎣ T21 T22 T23 ⎦ ⎣ δGy⎦,
yx
y
yz ⎦ ⎢
⎣ 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 transformation 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:
⎤⎡
⎡
T
⎢ 11
⎢
⎢
δAnib = Cbn δAbib = ⎢ T21
⎢
⎣
T31
⎤
T32
ε
⎥ ⎢ ax
⎥⎢
⎥⎢
T23 ⎥ ⎢ εay
⎥⎢
⎦⎣
T33
εaz
T12
T13
T12
T22
T13
⎡
⎤⎡
T11
⎢
⎢
⎢
= ⎢ T21
⎢
⎣
T31
T22
T32
δKx
δMxy
δMyx
δKy
δMzx
δMzy
⎤
⎤
⎡
1
⎢ ⎥
⎢ ⎥
⎥⎢ ⎥
⎥ ⎢ ax ⎥
⎥⎢ ⎥
δMyz ⎥ ⎢ ⎥
⎥⎢ ⎥
⎦ ⎢ ay ⎥
⎢ ⎥
δKz ⎣ ⎦
az
δMxz
δAx
⎥⎢
⎥
⎥⎢
⎥
⎥⎢
⎥
T23 ⎥ ⎢ δAy ⎥.
⎥⎢
⎥
⎦⎣
⎦
T33
δAz
(4.98)
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
⎪
⎪
⎪
⎪
⎪
VE
⎪
⎪
− (2ωie cos L +
)δ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
δA
+
T
δA
+
T
δA
21
x
22
y
23
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
⎪
) − φU (ωie cos L +
) + T11 δGx
⎪
⎪φ̇ E = − Re + φN (ωie sin L +
⎪
Re
Re
⎪
⎪
⎪
⎪
⎪
⎨ + T12 δGy + T13 δGz
.
VN
δVE
VE tan L
⎪
φ̇N =
− φE (ωie sin L +
) − φU
+ T21 δGx + T22 δGy + T23 δGz
⎪
⎪
R
R
R
⎪
e
e
e
⎪
⎪
⎪
⎪
V
δV
V
⎪
E
E
N
⎩φ̇U =
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:
w = wax
T
way
waz
wgx
wgy
wgz
(4.102)
.
The dynamic noise distribution matrix G can be given by:
⎡
⎤
Cbn
03×3
⎢
⎥
⎢
⎥
⎢
⎥
n
G = ⎢ 03×3 Cb ⎥.
⎢
⎥
⎣
⎦
024×3 024×3
The dynamic coefficient matrix F denotes a
matrix, which is expressed as follows:
⎡
A
C3×12
⎢ 3×6
⎢
⎢
F = ⎢ B3×6 03×12
⎢
⎣
024×6 024×12
⎡
VN
Re
A3×6 = ⎣−2ωie sin L +
2ωie cos L
⎡
0
B3×6
⎢
⎢ 1
=⎢
⎢ Re
⎣1
Re
⎡
−
2ωie sin L +
tan L
1
0
Re
0
tan L 0
− VRUe
2VE tan L
Re
E
+ 2V
Re
⎤
03×12
⎥
⎥
⎥
D3×12 ⎥
⎥
⎦
024×12
VE tan L
Re
VE
ωie cos L +
Re
(4.104)
−2ωie cos L −
− VRNe
2VN
Re
0 −ωie sin L −
0
30 × 30 dimension state transition
VE tan L
Re
0
(4.103)
0
VE
Re
0
⎤
0
g
−g
0
0⎦
0
0
0
(4.105)
⎤
VE tan L
VE
−ωie cos L −
Re
Re ⎥
VN
⎥
0
−
⎥
Re
⎥
⎦
VN
0
Re
ωie sin L +
(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
and ωtb
are the rotation rate of the turntable in the geographic and
(otxtytzt). ωtb
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:
rb = [rx ry rz ]
T
(4.109)
T
t
b
= Cbn ωtb
= Cbn [ωx ωy ωz ] .
ωtb
The lever arm effect can be written as:
drb drb t
b
= ωtb × rb +
= Cbn ωtb
× rb +
dt t
dt b
(4.110)
drb ,
dt b
(4.111)
where (drb /dt)|t and (drb /dt)|b are the body velocity in the geographic and body
t
coordinate system separately, ωtb
× 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 b
= Cbn ωtb
× rb .
(4.112)
dt t
The velocity error caused by the lever arm effect is expressed in the geographic
coordinate system as follows:
drb b
= Cbn (ωtb
× rb ).
(4.113)
dVln =
dt t
116
4 Error Modeling, Calibration, and Compensation of Inertial Measurement Unit (IMU)
The velocity error in the navigation frame is given by:
⎤ ⎡
⎤⎡
⎤
⎡
n
T11 T12 T13
ωy rz − ωz ry
dVlE
⎥ ⎢
⎥⎢
⎥
⎢
n ⎥
⎢
⎥⎢
⎥
dVln = ⎢
⎣dVlN ⎦ = ⎣T21 T22 T23 ⎦ ⎣ ωz rx − ωx rz ⎦
n
dVlU
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)⎦.
T31 (ωy rz − ωz ry) + T32 (ωz rx − ωx rz ) + T33 (ωx ry − ωy rx)
(4.114)
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:
⎤ ⎡ ⎤ ⎡
⎤ ⎡
⎤
⎡
n
n
VE
dVlE
VE − dVlE
δVE
⎥ ⎢ ⎥ ⎢
⎥ ⎢
⎥
⎢
n ⎥
⎥ ⎢ ⎥ ⎢ n⎥ ⎢
(4.116)
Z=⎢
⎣δVN ⎦ = ⎣VN ⎦ − ⎣dVlN ⎦ = ⎣VN − dVlN ⎦.
n
n
δVU
VU
dVlU
VU − dVlU
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:
⎤ ⎡
⎤ ⎡
⎤
⎡
Gbx Gby Gbz
εgx
Ḡbx Ḡby Ḡbz
εgy
εgz
⎥ ⎢
⎥ ⎢
⎥
⎢
⎥ ⎢
⎥ ⎢
⎥
⎢ S̄
⎢ x Ēyx Ēzx ⎥ ⎢ Sx Eyx Ezx ⎥ ⎢ δSx δEyx δEzx ⎥
⎥=⎢
⎥+⎢
⎥. (4.119)
⎢
⎢ Ēxy S̄y Ēzy ⎥ ⎢ Exy Sy Ezy ⎥ ⎢δExy δSy δEzy ⎥
⎦ ⎣
⎦ ⎣
⎦
⎣
Exz Eyz Sz
δExz δEyz δSz
Ēxz Ēyz S̄z
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 influencing 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 measurement 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
of RLG
Spherical Light Path
mirror
Shell T3
Anode
T1
Anode
Cathode
T2
XGi of gyroscope in the ith axes is given as:
⎤ ⎡
⎤
⎡
(TaGi + TcGi + TsGi )/3
TGi
⎥ ⎢
⎥
⎢
⎢ Ṫ ⎥ ⎢(Ṫ + Ṫ + Ṫ )/3⎥
cGi
sGi
⎥
⎢ Gi ⎥ ⎢ aGi
X Gi = ⎢
⎥=⎢
⎥.
⎥
⎢∇T1Gi ⎥ ⎢
−
T
T
aGi
sGi
⎦ ⎣
⎦
⎣
∇T2Gi
TcGi − TsGi
(4.120)
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 temperature 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 zerocrossings 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 envelope 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 conditions 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 network. 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 nonlinear 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
, (4.125)
wj Gi hj Gi =
wj Gi exp −
δ ḠT i = f (XGi ) =
2σj2Gi
j =1
j =1
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 assembly 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 ] ).
The RBF neural network is given in Fig. 4.17.
(4.126)
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
,
(4.127)
hj Ai = exp −
2σj2Ai
where CjAi is the basis function center of the jth node in the hidden layer for accelerometer 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
, (4.128)
wj Ai hj Ai =
wj Ai exp −
δ ĀT i = f (XAi ) =
2σj2Ai
j =1
j =1
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
2
⎪
⎪
=
(f(XGi (n)) − δ ḠT i (n))
E
⎪ dGi
⎪
N n=1
⎨
,
(4.129)
N
⎪
⎪
1
⎪
2
⎪
⎪
(f(XAi (n)) − δ ḠT i (n))
⎩ EdAi = 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
⎪
⎪
=
w2
E
⎪ wGi
⎪
MG k=1 j Gi
⎨
.
(4.130)
MA
⎪
⎪
1
⎪
⎪
⎪
wj2Ai
⎩ EwAi = M
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 overemphasis 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 probably 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
αGi,
MP parameters
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 ⎧
(k) + αGi (k)∇ 2 EwGi wGi
(k)
HGi (k) = βGi (k)∇ 2 Ed Gi wGi
⎪
⎪
⎪
⎪
⎪
⎪
γGi (k) = MG − 2αGi (k)tr(HGi (k))−1
⎪
⎪
⎪
⎨
γGi (k)
(4.132)
MP αGi (k + 1) =
⎪
2E
⎪
wGi wGi (k)
⎪
⎪
⎪
⎪
⎪
N − γGi (k)
⎪
⎪
MP ⎩ βGi (k + 1) =
2EdGi wGi
(k)
MP MP ⎧
2
2
⎪
⎪HAi (k) = βAi (k)∇ ED Ai wAi (k) + αAi (k)∇ EwAi wAi (k)
⎪
⎪
⎪
⎪ γAi (k) = MA − 2αAi (k)tr(HAi (k))−1
⎪
⎪
⎪
⎨
γAi (k)
(4.133)
MP αAi (k + 1) =
⎪
w
2E
(k)
⎪
wAi
Ai
⎪
⎪
⎪
⎪
⎪
(k)
N
−
γ
Ai
⎪
⎪
MP ⎩βAi (k + 1) =
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 regularization 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 ⎫⎞
⎪
∗ 3 ⎪
MG
⎨ 3
3XGi − Cj Gi 3 ⎬⎟
⎜
Gbi = Goi − δ ḠT i = Goi − ⎝
(4.135)
wj∗Gi exp −
⎠
⎪
⎪
2(σj∗Gi )2
⎭
⎩
j =1
⎧ 3
32 ⎫⎞
⎪
∗ 3 ⎪
⎨ 3
3XAi − Cj Ai 3 ⎬⎟
⎜
= Aoi − ⎝
wj∗Ai exp −
⎠.
⎪
⎪
2(σj∗Ai )2
⎭
⎩
j =1
⎛
MA
Abi = Aoi − δAT i
(4.136)
Bayesian regularization method reduces the network effective weight values under the condition of minimizing network training error by new evaluation criterion.
Therefore, the opportunity of overfitting decreases rapidly, which eventually improves 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 affecting 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 remote sensing. It can successively provide position, velocity, and attitude of the
airborne sensor and has become one of the key technologies for airborne direct georeferencing. 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 condition 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 doubleposition 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
(4.137)
gn = 0 0 −g , ωnie = 0
(L)
sin(L)
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 alignment 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.
⎤⎡ ⎤
⎡ ⎤ ⎡
c11 c12 c13
0
gxb
⎥⎢ ⎥
⎢ ⎥ ⎢
b
b n
b
⎢
⎢
⎥
⎥
⎢
(4.138)
g = Cn g , ⎣gy ⎦ = ⎣c21 c22 c23 ⎦ ⎣ 0 ⎥
⎦
b
−g
gz
c31 c32 c33
⎡
ωbie
=
Cnb ωnie ,
ωxb
⎤
⎤⎡
⎡
c11
⎢ ⎥ ⎢
⎢ ⎥ ⎢
⎢ b⎥ ⎢
⎢ ωy ⎥ = ⎢ c21
⎢ ⎥ ⎢
⎣ ⎦ ⎣
ωzb
c31
c12
c22
c32
c13
⎥⎢
⎥⎢
⎥⎢
c23 ⎥ ⎢
⎥⎢
⎦⎣
c33
⎤
0
⎥
⎥
⎥
cos (L) ⎥.
⎥
⎦
sin (L)
(4.139)
In practical implementation, ωbie and gb are replaced by the gyros or accelerometers
T
b
b
b ] , f = [ b
outputs ωbib = [ωibx
ωiby
ωibz
fx fyb fzb ] of SINS at some fixed location,
b
respectively. The body attitude matrix with respect to the navigation frame can be
expressed explicitly as:
⎤ ⎡
⎤
⎡
c22 c33 − c32 c23 c12 c13
c11 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
b
fyb ωibz
− fzb ωiby
b
ωibx
fb
+ x tan (L)
cos (L)
g
b
ωiby
fyb
+
tan (L)
cos (L)
g
⎢
g cos (L)
⎢
⎢ b b
⎢ f ω − f b ωb
z ibx
x ibz
=⎢
⎢
g cos (L)
⎢
⎢ b b
b
⎣ fx ωiby − fyb ωibx
b
ωibz
fb
+ z tan (L)
cos (L)
g
g cos (L)
fb
− x
g
fyb
−
g
⎤
⎥
⎥
⎥
⎥
⎥.
⎥
⎥
⎥
b ⎦
f
− z
g
(4.140)
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 (θ )
⎥
⎥
⎥
(4.141)
⎥.
cos (θ ) cos (φ)
sin (θ)
⎥
⎦
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:
2
1
fyb
−1
−1
θ = sin (c23 ) = sin
(4.142)
−
g
γ = tan−1 −
ϕ = tan
−1
c21
−
c22
= tan
c13
c33
1
−1
= tan−1
fxb
fzb
b
b
fxb ωibz
− fzb ωibx
b
gωiby
+ fyb
sin (L)
(4.143)
2
.
(4.144)
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:
dθ =
∂θ
−1
· ∇y = b
2 ∇y
∂fy
g 2 − fyb
b
b
fz ∇x − fxb ∇z
fz ∇x − fxb ∇z
∂γ
∂γ
dγ =
· ∇x + b · ∇z = 2 2 ≈ 2 ∂fxb
∂fz
g 2 − fyb
fzb + fxb
(4.145)
(4.146)
∂ϕ
∂ϕ
∂ϕ
∂ϕ
∂ϕ
∂ϕ
· εx +
· εy +
· ε z + b · ∇x + b · ∇y + b · ∇z
b
b
b
∂fx
∂fy
∂fz
∂ωibx
∂ωiby
∂ωibz
b b
b b
fz ωibx − fx ωibz (gεy + sin (L)∇y )
=
2 2
b
b
b
gωiby
+ fyb sin (L) + fxb ωibz
− fzb ωibx
b
b
b
gωiby
+ fyb sin (L) fxb εz + ωibz
∇x − fzb εx − ωibx
∇z
+
,
2 2
b
b
b
gωiby
+ fyb sin (L) + fxb ωibz
− fzb ωibx
dϕ =
(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 stationary 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 < 2g(∇z + ∇x )/g 2 ≈ 40μrad are less,
dγ = gzb ∇x − gxb ∇z
g − gy
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
0
ωiex
⎢
⎢ ⎥
⎥
⎢ωb ⎥ = C b ⎢ cos (L)⎥
n⎣
⎣ iey⎦
⎦
b
ωiez
sin (L)
⎤
⎡
=⎣
cos (γ ) sin (ϕ) + sin (γ ) sin (θ ) cos (ϕ) cos (L) − sin (γ ) cos (θ )
cos (θ ) cos (ϕ) cos (L) + sin (θ ) sin (L)
sin (γ ) sin (ϕ) − cos (γ ) sin (θ ) cos (ϕ) cos (L) + cos (γ ) cos (θ)
sin (L)
⎦.
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
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
dωiex
=
b
=
dωiey
b
∂ωiey
∂ϕ
dϕ +
b
∂ωiey
∂θ
dθ +
b
∂ωiey
∂γ
dγ
= − cos (θ ) sin (ϕ) cos (L)dϕ + cos (θ ) sin (L)
− sin (θ ) cos (ϕ) cos (L) dθ
(4.150)
b
b
b
∂ωiez
∂ωiez
∂ω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)
b
dωiez
=
Thereinto, the errors of Earth rotation rate in body frame caused by Euler angles error
dθ, dγ can be calculated 3using the
to (4.151). Since dθ
3 (4.149)
3 Eqs.
3 14μrad,
)
) <
b 3
b
b
dγ < 40μrad, so that 3dωiex
= 3dθ · ∂ωiex
∂θ + dγ · ∂ωiex
∂γ 3 < 1.98×
4.4 High Dynamic Strapdown Inertial Algorithm
129
3
3
3
)
) 3
3
3 b 3
3
b
b
∂θ + dγ · ∂ωiey
∂γ 3 < 3.63 × 10−4◦ / h and
10−3◦ / h, 3dωiey
3 = 3dθ · ∂ωiey
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
spect to the Earth frame, expressed in navigation frame, ωnen
)
)
)
T
−VN (RN + h)VE (RE + h)VE tan L (RE + h) ,
re=
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
b
n
b
b
b
b
= ωibx
− ωieb
= (ω̄ibx
+ εx ) − (ω̄iex
+ dωiex
) = εx − dωiex
ωnbx
= εx − ( cos (γ ) cos (ϕ) − sin (γ ) sin (θ) sin (ϕ)) cos (L)dϕ
(4.153)
b
b
b
b
b
b
b
ωnby
= ωiby
− ωiey
= (ω̄iby
+ εyb ) − (ω̄iey
+ dωiey
) = εyb − dωiey
= εyb + cos (θ ) sin (ϕ) cos (L)dϕ
(4.154)
b
b
b
b
b
b
b
ωnbz
= ωibz
− ωiez
= (ω̄ibz
+ εz ) − (ω̄iey
+ dωiez
) = εz − dωiez
= εz − ( sin (γ ) cos (ϕ) + cos (γ ) sin (θ) sin (ϕ)) cos (L)dϕ,
(4.155)
b
b
, ω̄iej
are theoretic values of gyros outputs and the Earth rotation rate of
where ω̄ibj
b
b
= ω̄iej
. According to
three axes j = x, y, z in the body frame, noting the fact ω̄ibj
Eqs. (4.153) and (4.154), the gyro biases equation in X- andY -axes can be expressed:
εx +
cos (γ )
b
b
.
= ωnbx
cot (ϕ) − sin (γ ) tan (θ) εy − ωnby
cos (θ )
(4.156)
According to Eqs. (4.154) and (4.155), the gyro biases equation in Y - and Z-axes can
be expressed:
εz +
cos (γ )
b
b
cot (ϕ) − sin (γ ) tan (θ) εy − ωnby
.
= ωnbz
cos (θ )
(4.157)
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:
b1
ωnbx
= εx +
cos (γ1 )
b1
cot (ϕ1 ) − sin (γ1 ) tan (θ1 ) εy − ωnby
cos (θ1 )
(4.158)
b2
ωnbx
= εx +
cos (γ2 )
b2
.
cot (ϕ2 ) − sin (γ2 ) tan (θ2 ) εy − ωnby
cos (θ2 )
(4.159)
Finally, by subtracting Eq. (4.158) from (4.159), then formula transforming, the
gyros biases in X- and Y -axes can be calibrated as:
b1
1
b2
b1
b2
+ Aωnby
− Bωnby
ωnbx − ωnbx
(A − B)
(4.160)
AB
1 b1
1 b2
b1
b2
− ωnby
+ ωnbx
− ωnbx
ωnby
(B − A)
A
B
(4.161)
εy =
εx =
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:
b1
ωnbz
= εz +
sin (γ1 )
b1
cot (φ1 ) + cos (γ1 ) tan (θ1 ) εy − ωnby
cos (θ1 )
(4.162)
b2
ωnbz
= εz +
sin (γ2 )
b2
.
cot (φ2 ) + cos (γ2 ) tan (θ2 ) εy − ωnby
cos (θ2 )
(4.163)
By subtracting Eq. (4.162) from (4.163), then formula transforming, the gyro bias
along Z-axis is calibrated as:
εz =
CD
1 b1
1 b2
b1
b2
,
ωnby
− ωnby
+ ωnbz
− ωnbz
(D − C)
C
D
(4.164)
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 environment 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 section 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 algorithm. 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
(4.166)
Φ = 0 α cos ( t) α sin ( t) .
In accordance with the relationship between quaternion and rotating vector, it is
known that rotating quaternion of the system is
α α α T
0 sin
cos ( t) sin
sin ( t) .
Q(t) = cos
(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
⎥.
t+
Φ=⎢
(4.168)
⎢−2 sin (α) sin 2 sin
2 ⎥
⎢
⎥
⎣
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 equation, 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 sin (α) sin
⎢
⎣
2 sin (α) sin
α 2
h
sin
2
h
cos
2
⎤
⎥
⎥
h ⎥
⎥
t+
2 ⎥
⎥
h ⎦
t+
2
(4.170)
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:
(4.172)
φε = Φ x − Φ̂ x = ( Φ − θ)x − Cpx .
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 updating 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 vector 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 rotating vector attitude algorithm researched in this section only conducts one sampling
within each attitude updating cycle h, and uses angular increment θ within current 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 frequency. 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
!
(i)
wherein, N
× θ) is conical error compensation term C p , θ (i) is ani=1 Gi ( θ
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)
Δθ2… ΔθN
Δ θ1
…
Δ
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 ⎥
⎥.
t−
=⎢
⎢ −2 sin (α) sin 2 sin
⎥
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) ×
i=1
θ) |x = 4
Gi sin2 (α)sin2 ( h/2) sin (i h).
i=1
(4.175)
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)).
2
(4.176)
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
Gi sin2
− sin ( h) + ( h) − 8
2
i=1
h
sin (i h) .
2
(4.177)
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⎥
⎢ . ⎥
⎢ ⎥
3
5
2N
+1
( h) − sin ( h) = λ λ · · · λ
(4.178)
· · · · ⎢ .. ⎥
⎢ ⎥
⎢ ⎥
⎢CN ⎥
⎣ . ⎦
..
m+1
, m = 1,2, 3 . . . , constituting column vector C.
wherein, coefficient Cm = (−1)
(2m+1)!
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
Gi sin2
8
i=1
= λ3
λ5
h
sin (i h) =
2
· · · λ2N +1
N
Gi (4 sin (i h) − 2 sin ((i + 1) h)
i=1
−2 sin ((i − 1) h))
⎤
⎡
A11 A12 · · · A1N
⎡ ⎤
⎥
⎢
G1
⎥
⎢A
⎢ 21 A22 · · · A2N ⎥ ⎢ ⎥
⎥
⎢
⎢ .
.. ⎥ ⎢ G2 ⎥
..
⎥
⎢
. ⎥·⎢ . ⎥
.
···
· · · · ⎢ ..
⎥ ⎢ .. ⎥
⎢
⎥ ⎣ ⎦
⎢
⎢AN 1 AN 2 · · · AN N ⎥
⎣ .
.. ⎦ GN
..
..
.
.
···
(4.179)
m
(−1)
[4 · n2m+1 − 2 · (n + 1)2m+1 − 2 · (n − 1)2m+1 ],
wherein, coefficient Amn = (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
2
N
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,
→0
|F ( )| ≤ Fmax
(4.181)
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 algorithm taking characteristics of the filter into account. Assume first-order lowpass
1
filter W (s) = 1+RCs
, and T = RC is filter time constant. Its amplitude-frequency
characteristic is:
F( ) = √
It obviously satisfies with Eq. (4.181).
1
1+T2
2
.
(4.183)
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
⎥
t+
Φ=⎢
⎢−2F ( ) sin (α) sin 2 sin
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
⎢
⎢
⎢
=⎢
⎢−2F ( ) sin (α) sin
⎢
⎣
2F ( ) sin (α) sin
α 2
h
sin
2
h
cos
2
⎤
⎥
⎥
h ⎥
⎥.
t+
2 ⎥
⎥
⎦
h
t+
2
Similarly, angular increment of gyro output within previous I attitude updating cycles
subject to filtering is
5
θ
(i)
t−(i−1)h
=
ω(τ )dτ
t−ih
α ⎤
−2F (0)( h)sin2
⎢
2
⎥
⎢
h
(2i − 1)h ⎥
⎢−2F ( ) sin (α) sin
⎥
sin
t
−
=⎢
⎥.
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
Cpx =
N
Gi ( θ (i) ×
θ) |x = 4F 2 ( )
i=1
Gi sin2 (α)sin2
i=1
h
sin (i h).
2
Therefore, algorithm error expression is:
φε =
N
α2 −F 2 ( ) sin ( h) + ( h) −8F 2 ( )
Gi sin2
2
i=1
h
sin (i h) .
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 2
F ( ) − sin ( h) + ( h) +
2
T
h
N
2
( h)3 − 8
Gi sin2
i=1
h
sin (i h) .
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
T
T 2
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 threeorder 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 environment, 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 rotating 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 singlesubsample algorithm updates the attitude for once after each sampling, but threesample 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
a
Roll angle
Roll angle
Three-subsample algorithm
Quaternion algorithm
Three-order single-subsample algorithm
b
Time t (s)
Time t (s)
Fig. 4.20 Roll angle drift curve
Roll angle drift curve
Roll angle
Quaternion algorithm
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 algorithm 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 algorithm 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 amplitudefrequency 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 current and angular increments within previous N attitude updating cycles and deduces
correcting algorithm of single-subsample rotating vector attitude algorithm by combining 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 attitude 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 autonomous 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 achievements of the author in terms of strapdown inertial navigation technology in recent
years.
First, error modeling, measurement and compensation methods for inertial components 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, dynamic 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 problem 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 affecting 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 characterization 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 inertial 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 temperature. 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 navigation 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 highprecision 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
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_5
145
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 pretreatment, 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 coordinate 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
f
lens
α
Yp
Xp
Autonomous
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
parameters of CMOS star
sensor [4]
5.2.2
Performance parameters
Value
Unit
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
◦
Aperture
40
mm
SNR after defocus
5.5
–
/s
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 approximation 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
Fig. 5.2 Subbands of the 2-D
orthogonal wavelet transform
LL3
HL3
LH3
HH3
149
HL2
HL1
LH2
HH2
LH1
HH1
Lakhwinder Kaur et al. [8] appears more suitable, in which threshold choice is:
TN =
βσ 2
,
σy
(5.3)
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:
Thl = β
σ2
,
σlJ2
(5.6)
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
,
(5.7)
x̂ =
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
H (u, v) =
0
T
e−
j 2π[ux(t)+vy(t)]
dt.
(5.9)
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 γ
e
πul cos γ
T sin (πvl sin γ ) −j π lv sin γ
+
.
e
πvl sin γ
H (u, v) =
Then, the expression of H(u, v) in the time domain is:
1/ l 0 ≤ x ≤ l cos γ , y = x tan γ
h(x, y) =
.
0
else
(5.11)
(5.12)
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 leastsquares 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:
F̂ (u, v) =
H ∗ (u, v)
G(u, v),
H (u, v)H ∗ (u, v) + a
(5.13)
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
Fixed
threshold
(Th = 128)
BayesShrink SureShrink Lowpass AdaptThr
filter
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
σ = 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
σ = 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
w=1
w=5
Noise
image
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 deblurring 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
Fig. 5.6 The original real star image and its gray distribution
155
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
80
100
60
40
50
20
0
0
0
0
10
5
10
15
10
20
a
20
15
30
5
25
b
40
5
10
15
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
(pixel)
Coordinates of blurred
star image (pixel)
Coordinates of
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 different 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. Besides, 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 conditions 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 assess 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)
before(w=20)
after(w=5)
after(w=10)
after(w=20)
Extract error of x+ y (pixel)
3.5
3
2.5
2
1.5
1
0.5
0
0
2
4
6
8
10
Star point
12
14
16
18
Fig. 5.10 Extraction error of star centroid with different w
according to angular velocity in before/after using the restoration method with different 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 dimension, and other aspects of sensitive star map matching and identification. What’s
more, redundant matching and mismatching will lead to significant reduction of identification 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 spacecraft 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 characteristic 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 intelligencebased. For example, the triangular [13–14], the quadrilateral [15] and the Delaunay
triangulation [16] fall into the first group. Planar triangles [17], grids [18], identification 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 genetic 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)]β
k
,
pij (t) = s ∈ dk
⎪
⎪
⎪
⎩
(5.16)
0,
j∈
/ dk
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
k
Lk
τij (t) =
⎪
⎩ 0, not T
(5.19)
(Lavg − Lk )/(Lavg − Lopt ), Lk < Lavg
,
λk =
0,
Lk ≥ Lavg
Lavg =
1
N
N
Lk , Lopt =
k=1
min (Lk ),
k=1,2,...,N
(5.20)
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.
r > θ = arccos
ī · j¯
,
|ī||j¯|
(5.21)
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
number
Angular
distance 1
Angular
distance 2
Star point 1
Degree/◦
Star point 2
Star point 3
(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
Davg =
1
·
Dsj .
Q j =1
(5.22)
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.
D > Davg
6
φ+r ≤
F ov
.
2
(5.23)
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, respectively, 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 . . .
1
(A)
The Successful Rate of Identification
0.95
0.9
0.85
(C)
0.8
0.75
(B)
(A)ant colony identification
(B)Delaunay cutting recognition
(C)improved triangle identification
0.7
0.65
0
20
40
60
80
The Error of Star Position(")
100
120
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 identification [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 identification success rate of this method is about 99.5 % and equivalent to the Delaunay
identification method. Because the Delaunay identification adopts the angular distance 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 identification 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 identification 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 Guidancestar 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
(B)Delaunay cutting recognition
(C)improved triangle identification
The time of Identification(ms)
250
(C)
200
(B)
150
100
(A)
50
0
50
60
70
80
90
The Error of Star Position(")
100
110
120
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 measurement of the Earth’s direction is the most important measurement and the horizon
sensing accuracy is the most important factor which effects celestial navigation accuracy. 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 celestial measurement formulas of celestial navigation system are nonlinear, an advanced
UKF instead of traditional extended Kalman filter (EKF) and the corresponding information 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 validated 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 attractive 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 horizon 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
ha
R
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 exponential 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]:
ha (R, ρ) = h0 − H ln (R) + H ln k(λ)ρ0
2π Re
H
1
2
+R
H Re
2π
1
2
,
(5.27)
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 twobody 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
⎪
⎪
⎪
⎪
3
2 2
⎪
dvx /dt = −μx/r · [1 − J2 (Re /r)(7.5z /r − 1.5)] + ax + wvx ⎪
⎪
⎬
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
r=
(5.30)
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 gravitational 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 perturbing 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 perturbations, 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.4.3
5 Star Map Processing Algorithm of Star Sensor and Autonomous . . .
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 ;
χi,k = x̂k +
√
n+τ
W0 = τ/(n + τ ),
P (k|k) ;
i
Wi = 1/ 2(n + τ ) ,
√
χ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
x̂0 = E[x0 ],
T P0 = E x0 − x̂0 x0 − x̂0 .
Calculate Sigma Points
√
χk−1 = x̂ k−1 x̂ k−1 + n + τ √Pk−1
i
i = 1,2, . . ., n.
(5.32)
√
√
x̂ k−1 − n + τ
Pk−1 i ,
(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)
T
Wi χi,k|k−1 − x̂k− χi,k|k−1 − x̂k− + Qk ,
(5.36)
i=0
2n
Pk− =
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
Pẑk ẑk =
T
Wi Zi,k|k−1 − ẑk− Zi,k|k−1 − ẑk− + Rk ,
(5.39)
T
Wi χi,k|k−1 − x̂k− Zi,k|k−1 − ẑk− ,
(5.40)
i=0
2n
Px̂k ẑk =
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 covariance 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
· P1 (k)X1 (k) + P2−1 (k)X2 (k) ,
X̂g (k) = P1−1 (k) + P2−1 (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 . . .
Z1
Star-earth angle
measurement
X 1 , P1
X 1 , P1
X g , Pg
Orbital dynamics
If there is refracted
star measurement?
Z2
Yes
Indirectly sensing horizon
local UKF filter 2
X 2 , P2
Information fusion
Master filter
Directly sensing horizon
local UKF filter1
X g , Pg
No
Fig. 5.18 Information fusion algorithm
Horizon sensor measurement
Directly sensing horizon
measurement
Orbit and
Attitude
A
Star Chart Simulator
Star Sensor Simulator
B
D
Position,Velocity
Trajectory
Generator
Orbit and
Attitude
Navigation
Computer
Star Chart Simulator
Indirectly sensing horizon
measurement
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 characterizing 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 . . .
Position Estimation Error
directly sensing horizon
indirectly sensing horizon
new method
1800
3.5
1400
3
velocity error/(m/s)
position estimation error/m
directly sensing horizon
indirectly sensing horizon
new method
4
1600
1200
1000
800
2.5
2
1.5
600
1
400
0.5
200
0
a
Velocity Estimation Error
4.5
2000
179
0
50
100
150
200
time/min
250
300
350
0
400
b
0
50
100
150
200
250
300
350
400
time/min
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: semimajor 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 horizon 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 errors 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
400
Velocity Estimation Error
1.8
UKF
EKF
UKF
EKF
1.6
350
1.4
300
velocity error/(m/s)
position error/m
1.2
250
200
150
100
0.6
0.4
50
0
1
0.8
0.2
0
50
100
a
150
200
250
300
350
0
400
time/min
0
50
100
150
200
250
300
350
400
time/min
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
EKF-based method
UKF-based method
Position
estimation
error/m
Velocity
estimation
error/(m/s)
Position
estimation
error/m
Velocity
estimation
error/(m/s)
Directly sensing
horizon
method
220–270
0.23–0.28
180–220
0.19–0.23
Indirectly
sensing
horizon
method
180–300
0.21–0.33
70–110
0.14–0.19
Information
fusion method
110–150
0.10–0.15
40–70
0.07–0.10
Navigation
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 information 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 information 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
5.4.5
181
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 elaborated 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 navigation 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. Proceeding 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: thinning 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 satellites 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 advantages 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 susceptibility 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 navigation to truly achieve complementary advantages of the two is a problem during
practical application of the navigation system. Position and velocity or pseudorange 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 position 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 application problem of the SINS/GNSS integrated navigation system. First, it discusses
© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_6
185
186
6 INS/GNSS Integrated Navigation Method
briefly the basic principle and operating mode of the SINS/GNSS integrated navigation 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, microminiature, 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 highprecision 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 correcting 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 information combination to use error equation of the SINS as the state equation of the
integrated navigation system, and the respective output position and velocity difference 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 divergence; 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 measurement 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
calculated by SINS
Motion of the carrier
SINS
Carrier
Kalman Filter
GNSS
Optimal
estimation of
navigation
parameter
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
Navigation parameter calculated by SINS
Motion of the carrier
SINS
-
+
Kalman
Filter
Carrier
GNSS
+
Optimal
estimation of
navigation
parameter
Output
correction
Navigation parameter obtained from GNSS
Feedback
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
+
X1
SINS
×
X2
GNSS
+
Kalman Filter
_
ΔX̂ 1
189
×
_
ΔX 1 − ΔX 2
Fig. 6.3 Schematic diagram of the output correction. SINS ship inertial navigation system, GNSS
global navigation satellite system
ΔX̂ 1
SINS
X1
X1
×
GNSS
X2
+
_
Kalman Filter
Δ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 according 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 coordinate 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
⎢
⎥
n ⎥
G(t) = ⎢
.
⎣ 03×3 Cb ⎦
09×3 09×3
15×6
System noise vector consists of the random error of the gyro and accelerometer with
expression as follows:
w = wεx
T
wε y
w εz
w∇x
w∇ y
w∇z
.
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System
State transfer matrix F (t) of the system is:
⎡
FN
F (t) = ⎣
06×9
191
⎤
FS
⎦,
FM
wherein FN is corresponding to a 9-dimensional basic navigation parameter system
matrix [8, 9], and its nonzero element is:
F (1,2) = ωie sin L +
vE
tgL;
RN + h
F (1,3) = − ωie cos L +
1
;
RM + h
vN
F (2,3) = −
;
RM + h
F (2,1) = −ωie sin L −
F (2,7) = −ωie sin L;
F (3,1) = ωie cos L +
F (1,5) = −
F (3,2) =
F (2,4) =
vN
;
RM + h
F (3,7) = ωie cos L +
F (3,4) =
vE
sec2 L;
RN + h
vE
;
RN + h
vE
tgL;
RN + h
1
;
RN + h
vE
;
RN + h
1
tgL;
RN + h
F (4,2) = −fU ;
vU
vN
tgL −
;
RM + h
RM + h
vE
;
F (4,6) = − 2ωie cos L +
RN + h
F (4,3) = fN ;
F (4,4) =
vE
tgL;
RN + h
vE vN
F (4,7) = 2ωie cos L · vN +
sec2 L + 2ωie sin L · vU ;
RN + h
F (4,5) = 2ωie sin L +
F (5,3) = −fE ;
F (5,1) = fU ;
F (5,4) = − 2ωie sin L +
F (5,6) = −
vN
;
RM + h
vE
tgL ;
RN + h
F (6,7) = −2vE ωie sin L;
F (8,4) =
sec L
;
RN + h
F (9,6) = 1.
vU
;
RM + h
F (5,7) = − 2ωie cos L +
F (6,1) = −fN ;
F (6,4) = 2 ωie cos L +
F (5,5) = −
vE
sec2 L vE ;
RN + h
F (6,2) = fE ;
vE
;
RN + h
F (6,5) =
2vN
;
RM + h
1
;
RM + h
vE
F (8,7) =
sec LtgL;
RN + h
F (7,5) =
192
6 INS/GNSS Integrated Navigation Method
FS and FM are:
⎡
Cbn
⎢
FS = ⎢
⎣ 03×3
03×3
03×3
⎤
⎥
Cbn ⎥
⎦ and FM = [06×6 ],
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
⎥
⎢
⎥
(6.2)
Zv (t) = ⎢
⎣ vIN − vGN ⎦ = Hv X(t) + Vv (t),
vIU − vGU
wherein
Hv = 03×3
diag 1
Vv = vGE
1
1
03×9 ,
(6.3)
.
(6.4)
T
vGN
vGU
Position measurement vector Zp is:
⎤
⎡
LIE − LGE
⎥
⎢
⎥
Zp (t) = ⎢
⎣ λIN − λGN ⎦ = Hp X(t) + Vp (t),
hIU − hGU
wherein
Hp = 03×6
diag RM
Vp = NGE
RN cos L
NGN
1
NGU .
03×6 ,
(6.5)
(6.6)
(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:
⎤ ⎡ ⎤
⎤
⎡
⎡
Hv
Vv (t)
Zv (t)
⎦ = ⎣ ⎦ X(t) + ⎣
⎦ = H (t)X(t) + V (t).
(6.8)
Z(t) = ⎣
Zp (t)
Vp (t)
Hp
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System
6.3.2
193
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 between the mathematics platform coordinate system and the navigation coordinate
system. The misalignment angle is often described by adopting the additive quaternion 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 substitutes 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
b
)=
Qb ωnb =
Qb (ωib − ωin
Qb ωib −
Qb ωin
2
2
2
2
1 7 b 8 n 1 n n −1 n n = ωib
(6.10)
Qb −
Qb ωin Qb
Qb
2
2
17 b8 n 1 n n
= ωib
Qb − [ωin ]Qb ,
2
2
⎤
⎡
−q1 −q2 −q3
q0
⎥
⎢
⎢ q1
q0 −q3 q2 ⎥
⎥. ωb represents the projection of the
wherein Qnb = ⎢
⎥ ib
⎢
q3
q0 −q1 ⎦
⎣ q2
q3
−q2 q1
q0
angular velocity of system b relative to the geocentric inertial system (system i) on
Q̇nb =
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 ⎢
0
ωz
−ωy ⎥
⎥
⎢ ωx
(6.11)
ωib = ⎢
⎥,
⎥
⎢ ωy
−ω
0
ω
z
x
⎦
⎣
ωz
ωy −ωx
0
⎡
n
ωin
0
⎢
⎢ ω
⎢ E
=⎢
⎢ ωN
⎣
ωU
−ωE
−ωN
0
−ωU
ωU
0
−ωN
ωE
−ωU
⎤
⎥
ωN ⎥
⎥
⎥.
−ωE ⎥
⎦
0
(6.12)
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 ,
2
2
(6.13)
b
b
b
b
= ωib
+ δωib
is the measurement value of the gyro, δωib
is the measurewherein ω̂ib
n
n
n
ment error of the gyro; ω̂in = ωin + δωin is the angular velocity of system n relative
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]:
δ Q̇ =
17 b8 n 1 n n 17 b8 n 1 n n
ω̂ 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
Qb + ωib δQb + δωib Q̂b
= ωib
2
2
2
1 n n 1 n
1
17 b8 n 1 n n
n
n
Qb + [ωin ]Qb
− [ωin ]Qb − [ωin ] δQb − [δωin
]Q̂nb − ωib
2
2
2
2
2
8
7
17 b8 n 1 n
1
1
b
n
Q̂nb − [δωin
= ωib δQb − [ωin ] δQnb + δωib
]Q̂nb .
2
2
2
2
(6.14)
It is known that
7 b8 n
b
δωib Q̂b = U Q̂nb δωib
⎡
wherein U
Q̂nb
−q̂1
⎢
⎢ q̂
⎢ 0
=U=⎢
⎢ q̂3
⎣
−q̂2
−q̂2
−q̂3
q̂0
q̂1
,
n
n
[δωin
]Q̂nb = Y Q̂nb δωin
,
−q̂3
⎤
⎥
q̂2 ⎥
⎥
⎥.
−q̂1 ⎥
⎦
q̂0
(6.15)
6.3 Modeling Method of Inertial/Satellite Integrated Navigation System
195
From
UT U = I3×3 , YT U = Ĉnb
(6.16)
1
b
n
δ Q̇ = MδQ + (Uδωib
− Yδωin
),
2
(6.17)
it is obtained that
wherein
M≡
17 b8 1 n
ω − [ωin ].
2 ib
2
(6.18)
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
n
V̇tn = Cnb f b − 2ωie
× Vtn + gn ,
+ ωen
(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
n
V̇cc = Ĉnb f̂ b − 2ω̂ie
+ ω̂en
(6.20)
× Vcc + gc ,
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:
n
n
n
n
n
n
f̂ b = f b + ∇ b , Vcc = Vtn + δV, ω̂ie
= ωie
+ δωie
, ω̂en
= ωen
+ δωen
to obtain
δ V̇ =
n
n
n
n
n
Cnb f̂ b + Cnb ∇ b − ωie
+ ωin
+ δωen
× δV − 2δωie
× Vt + δV + δgn ,
(6.21)
wherein Cnb = Ĉnb − Cnb is the calculation error of the attitude matrix, and
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).
Cnb f̂ b
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
n
Ignore small quantity of second order (2δωie
+ δωen
) × δV, and from the equation
(6.24)
2YT (δQ) U Q̂ f̂ b = −2 Ĉnb f̂ b × YT Q̂ δQ + 2Ĉnb f̂ b Q̂T δQ
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
n
n
n
+ Cnb ∇ b − 2ωie
+ ωen
+ δωen
× δV − 2δωie
× 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:
δ Q̇ =
17 b8
1 n
1
b
n
]δQ + (Uδωib
− Yδωin
).
ω δQ − [ωin
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
n
n
n
+ Cnb ∇ b − 2ωie
+ ωen
+ δωen
× δV − 2δωie
× Vn .
Position error equation:
L̇
1
δh +
δvN .
RM + h
RM + h
λ̇
sec L
δ λ̇ = λ̇ tan L · δL −
δh +
δvE .
RN + h
RN + h
δ ḣ = δvU .
δ L̇ = −
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 .
xa = δL
T
δλ
δh
δvE
xe = ∇x
δvN
∇y
∇z
δvU
δq0
δq1
δq2
δq3
.
T
ε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)
xe
06×10
ẋe
06×6
198
6 INS/GNSS Integrated Navigation Method
⎡
03×3
03×3
⎥
⎥
03×3
⎥;
⎦
1
U Q̂nb
2
⎢
n
⎢
wherein FS = ⎢ Cb
⎣
04×3
⎡
F11
⎢
coefficient matrix FN = ⎢
⎣ F21
F31
⎡
03×1
⎤
⎤
⎡
F12
03×4
⎤
⎥
F23 ⎥
⎦; q(x, t) is the nonlinear part,
F33
F22
F32
03×1
⎤
⎢
⎥ ⎢
⎥
T
b ⎥
⎥ ⎢
q (x, t) = ⎢
⎣ f1 (x, t) ⎦ = ⎣ −Y (δQ) U (δQ) f̂ ⎦;
010×1
010×1
⎡
⎤
03×3
⎢
⎢ Cn
⎢
b
⎢
the system noise transfer matrix G = ⎢
⎢ 04×3
⎢
⎣
06×3
03×3
03×3
1 n
U Q̂b
2
⎥
⎥
⎥
⎥
⎥;
⎥
⎥
⎦
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 = ⎢
0
0 ⎥
⎦,
⎣ sec L (RN + h)
0
0
1
⎡
⎤
vE
sec2 L + 2 cos L vN + 2 sin L · vU 0 0
⎢ RN + h
⎥
⎢
⎥
vE
⎥
2
F21 = ⎢
⎢
−
0 0 ⎥,
sec L + 2 cos L vE
⎣
⎦
RN + h
−2 sin L · vE
0 0
6.4 High-Precision Inertial/Satellite Integrated Navigation Method
⎡
F22
F23
F31
F32
)
)
vN tan L (RN + h) − vU (RN + h)
⎢
⎢
=⎢
−2
+ λ̇ sin L
⎣
2
+ λ̇ cos L
+ λ̇ sin L
)
−vU (RM + h)
2
199
− 2
⎤
+ λ̇ cos L
2L̇
= −2 Ĉnb f̂ b × YT Q̂ + 2Ĉnb f̂ b Q̂T ,
⎤
⎡
−q̂1 −q̂2 −q̂3 ⎡
⎥
⎢
0
0 0
⎥
⎢
q̂3 −q̂2 ⎥ ⎢
⎢ q̂0
⎥⎢
1⎢
⎥⎢
− sin L
0 0
=− ⎢
⎥⎢
⎢
2 ⎢ −q̂3 q̂0
q̂1 ⎥ ⎢
⎥⎣
⎢
vE
⎦
⎣
sec2 L 0 0
cos L +
R
+
h
N
q̂2 −q̂1 q̂0
⎤
⎡
−q̂1 −q̂2 −q̂3 ⎡
⎥
⎢
0
−1/(RM + h)
⎥
⎢
q̂3 −q̂2 ⎥ ⎢
⎢ q̂0
⎢
⎥⎢
1⎢
⎥ ⎢ 1/(RN + h)
=− ⎢
0
⎥
⎢
2 ⎢ −q̂3 q̂0
q̂1 ⎥ ⎢
⎣
⎥
⎢
⎦ tan L/(R + h)
⎣
0
N
q̂2 −q̂1 q̂0
F33 =
⎥
⎥
⎥,
⎦
−L̇
0
⎤
⎥
⎥
⎥
⎥,
⎥
⎦
⎤
0
⎥
⎥
⎥
0 ⎥,
⎥
⎦
0
17 b8 1 n
ω − [ω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 toward 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 introduces the SINS/GNSS integrated navigation method based on mixed correction,
self-adaptive feedback correction filter method based on the observability normalization processing method, the SINS/GNSS outlier-resistant integrated navigation
method based on Kalman filtering innovation orthogonality, air maneuvering alignment 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
9k/k−1 H T Hk P
9k k−1 H T + Rk −1 .
K
k
k
/
9k/k−1 ).
9k/k−1 + K
9k (Zk − Hk X
9k = X
X
9k/k−1 = φk,k−1 X
9k−1 .
X
9k Hk )P
9k/k−1 (I − K
9k Hk )T + K
9k Rk (K
9k )T .
9k = (I − K
P
T
9k−1 φ T
9k/k−1 = φk,k−1 P
P
k,k−1 + k−1 Qk−1 k−1 .
(6.29)
6.4 High-Precision Inertial/Satellite Integrated Navigation Method
201
Navigation error correction
(Misalignment angle, position,
velocity)
correction)
Navigation data
SINS
mechanical layout
IMU
(Feedback
(Output
correction)
+
GNSS receiver
Kalman
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 Zk .
9k = K
X
9k/k−1 H T + Rk )−1 .
9k = P
9k/k−1 H T (Hk P
K
k
k
T
9k−1 φ T
9k/k−1 = φk,k−1 P
+
k−1 Qk−1 k−1
.
P
k,k−1
9k Hk )P
9k/k−1 (I − K
9k Hk )T + K
9k Rk (K
9k )T .
9k = (I − K
P
(6.30)
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 Correction 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 correction 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 navigation, 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 introduced previously improves the SINS/GNSS integrated navigation precision, but
it can only improve the precision of the observable state within the system. Generally, 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
,
σo
(6.31)
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 integrated 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 observability 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 Observability
Some researchers have researched the influences of various maneuvering modes on
the observability of the SINS/GNSS integrated navigation system according to analysis 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 observability 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
,
(6.32)
ηk =
σ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 Observability Based on the Improved Normalization Processing Method
In the SINS/GNSS integrated navigation system, the feedback quantity of each system 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 observability 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
SINS
205
Output position, velocity, attitude and other
navigation information of the carrier
+
Kalman filter
X̂
GNSS
η k Xˆ
η
k
Weighted feedback factor
(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) selfadaptive 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 carrierphase 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 selfadaptive 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
6.4.3
209
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 navigation 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 introduces 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 Orthogonality
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 filtering [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
(6.33)
E Zk ZTk = E ek eTk + E Ẑk Ẑk ,
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.
(6.37)
X̂ k = X̂ k/k−1 + K k Bk − H k X̂ k/k−1 ,
T
wherein
Bk = Z1 (k) f1 (r1 ) Z2 (k) f2 (r2 ) · · · Zm (k) fm (rm ) ,
fi (ri )(i = 1,2 · · ·, m) is the activation function with the following properties:
and
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 correction, 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
i = 1,2 · · ·, m,
fi (ri ) =
⎩ 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 following 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 Novatel 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 filter (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, altitude, 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
Correction KF
Latitude (deg)
DGPS
Standard KF
Correction KF
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
Correction KF
DGPS outlier
Longitude (deg)
Standard KF
Correction KF
Time (s)
Fig. 6.15 Longitude comparison among correction Kalman filter (KF), differential global positioning system (DGPS), and standard KF
Correction KF
Standard KF
DGPS
Standard KF
Correction 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 innovation process is lost, and then weight an activation function to the measurement
quantity to make the innovation process subject to correction to maintain its orthogonality. 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 precision 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
SINS
215
Position, velocity, attitude and other navigation
information of the output carrier
+
Self-adaptive
feedback filter
GNSS
ηk X̂
Lever arm
error
compensation
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 maneuvering alignment method based on system state variable observability analysis and
lever arm effect error compensation [37] to compensate the lever arm effect error between 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 observability 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 heading misalignment angle, two horizontal alignment angles, three gyro drifts, three
accelerometer biases, and other errors by adopting the self-adaptive feedback correction 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
system (SINS)
z
A
r
SINS
measurement
x
y
O
center
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 navigation 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:
⎤ ⎡
⎤
⎡
b
b
ωnby
rz − ωnbz
ry
vbx
⎥ ⎢
⎥
⎢
b
⎥ ⎢ b
⎥
(6.38)
Vb = ⎢
⎣ vby ⎦ = ⎣ ωnbz rx − ωnbx rz ⎦.
b
b
vbz
ωnbx ry − ωnby rx
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 compensation, 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, accelerometer 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
10
5
-5
-10
-15
0
-5
-20
-10
400
0
a
800
1200
Time (t)
1600
2000
-25
b
0
400
Heading misalignment angle
20
0.04
15
0.02
εx(°/h)
Δγ('')
5
0
2000
-0.02
-0.04
-0.08
-10
-0.1
-15 0
400
c
800
1200
Time (t)
1600
2000
-0.12 0
d
400
0.01
0
-0.01
-0.02
-0.03
-0.04
-0.05
-0.06
-0.07
-0.08
-0.09
0
-0.02
εz(°/h)
-0.04
-0.06
-0.08
-0.1
-0.12
0
400
e
800
1600 2000
1200
800 1200
Time (t)
1600 2000
Gyro drift of axis x
Roll misalignment angle
0.02
εy(°/h)
1600
-0.06
-5
f
Time (t)
0
400
800
Time (t)
1200
1600 2000
Gyro drift of axis z
Gyro drift of axis y
140
120
100
80
60
40
20
0
-20
120
100
80
60
40
20
0
-20
∇y(ug)
∇x(ug)
1200
Time (t)
0
10
g
800
Pitching misalignment angle
0
400
800
1200
1600 2000
Time (t)
0
400
Accelerometer bias of axis x
800
1200
1600
2000
Time (t)
h
Accelerometer bias of axis y
140
120
∇z(ug)
100
80
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 becomes 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 ,
T
T
P−
k = F k−1 P k−1 F k−1 + Γk−1 Qk−1 Γk−1 ,
−1
− T
T
K k = P−
,
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
(6.45)
x̂Sk = x̂k + KkS x̂Sk+1 − x̂−
k+1 ,
S
S T
−
Kk ,
PkS = Pk + KkS Pk+1
− Pk+1
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]:
√
√
j = 1,2, ..., n
χ̂k−1 = x̂k−1 x̂k−1 + n + λ Pk−1 x̂k−1 − n + λ Pk−1
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, respectively; 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− .
x̂−
k =
P−
k =
Ck =
2n
!
i=0
2n
!
i=0
2n
!
i=0
Wi(m) χ̂ −
k,i ,
−
−
− T
T
Wi(c) (χ̂ −
k,i − x̂k )(χ̂ k,i − x̂k ) + Γ k−1 Qk Γk−1 ,
(6.49)
−
− T
Wi(c) (χ̂ k−1,i − x̂−
k−1 )(χ̂ k,i − x̂k ) .
• Propagate the sigma points through the measurement model and compute the
predicted measurement ŷk .
γ̂k = h(χ̂ −
k , k),
ŷk =
2n
!
i=0
(6.50)
Wi(m) γ̂k,i .
• Compute the covariance of the predicted measurement P yy , and the crosscovariance P xy between x̂−
k and ŷk .
P yy =
P xy =
2l
!
i=0
2l
!
i=0
Wi(c) (γ̂ k,i − ŷk )(γ̂k,i − ŷk )T ,
(6.51)
Wi(c) (χ̂ −
k,i
−
x̂−
k )(γ̂ k,i
− ŷk ) ,
T
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)
T
Pk = P−
k − K k P yy K k .
Part II: Backward recursion process (for k = N − 1, N − 2, . . ., 0)
• Compute the smoother gain K Sk .
−1
K Sk = C k+1 (P −
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 ,
S T
P Sk = P k + K Sk P Sk+1 − P −
.
k+1 K k
(6.54)
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
n
ˆ = I − Cnp ωin
+ δωin
− Ĉbn ε b ,
(6.55)
T
n
where = [ E
N
U ] is the attitude error vector, ωin is the rotation velocity
n
of the n-frame relative to the i-frame expressed in the n-frame, and δωin
is the error
T
n b
of ωin .ε = [ εx εy εz ] is the gyroscope’s random constant drifts in the b-frame
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
n
n
n
δ V̇n = I − Cpn Ĉbn f̂ b − 2δωie
× Vn − 2ωie
× δVn + Ĉbn ∇ b ,
+ δωen
+ ωen
(6.56)
where Vn = [VE
VN VU]T is the velocity of the b-frame relative to the e-frame expressed 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 velocity 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 T
n b
p
n
Eq. (6.55) and I − Cp Ĉb f̂ in Eq. (6–56), and the nonlinear matrix Cn = Cp
is expressed as follows:
⎡
cos N cos U − sin E sin N sin U
⎢
p
⎢
Cn = ⎣ − cos E sin U
sin N cos U + sin E cos N sin U
cos
N
sin
U
cos
E
cos
U
sin
N
sin
U
+ sin
− sin
E
E
sin
cos
N
N
cos
cos
U
U
− cos
E
sin
E
cos
E
⎤
sin
cos
N
⎥
⎥.
⎦
(6.57)
N
The differential equation of position error is given as follows [48]:
VN
1
δh +
δVN ,
2
RM + h
(RM + h)
VE sin L
VE
1
δ λ̇ = −
δL −
δVE , (6.58)
δh +
2
2
(RN + h)cos L
(RN + h) cos L
(RN + h) cos L
δ ḣ = δVU ,
δ L̇ = −
where δL, δλ, and δh represent the latitude, longitude, and altitude errors, respectively. 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
⎥
⎢
n
⎥
(6.59)
G=⎢
⎣ 03×3 Ĉb ⎦.
09×3 09×3
The measurement vector is defined by:
z = [ δVE δVN
δλ δH ]T . δVE , δVN , and δVU represent
δL
δVU
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
⎦. (6.61)
H=⎣
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,
T
P yy = P −
k Hk ,
T
P xy = H k P −
k H k + Rk .
(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ˆ
δ Xˆ Fk
Strapdown Navigator
S
k
δ Xˆ Fk +1
δ Xˆ Sk + 1
Strapdown Navigator Strapdown Navigator
k
Smoother Solution
Ture Trajectory
Time Epoch 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 segments) 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
Gyroscope
Accelerometer
GPS(velocity)
GPS(position)
Characteristics
Magnitude (1 σ )
Drift
0.2◦ /h
White noise
0.2◦ /h
Bias
100 μg
White noise
50 μg
Horizon
0.03 m/s
Height
0.05 m/s
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
a
b
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 integration 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 University 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, respectively [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
Ground
33.78
Latitude(deg)
33.772
113.16
113.12
113.2
113.24
Longitude(deg)
Fig. 6.24 Flight trajectory
33.78
Flight Trajectory
Imaging Points
Imaging
Area
Imaging Segment 1
Latitude(deg)
33.778
Imaging Segment 2
33.776
Imaging Segment 3
33.774
Imaging Segment 4
33.772
113.14
113.15
113.16
113.17
Longitude(deg)
113.18
113.19
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 relationship 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 methods 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 introduced; the program effectively combines the advantages of output correction and
feedback correction and improves estimation precision of observable state in integrated 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 adjustment 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 estimate 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 generally 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 application 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 highprecision navigation parameters through celestial measurement information and
inertial measurement information; it has features such as strong autonomy and highattitude precision, and can effectively correct gyro drift error, so it has become an important 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 dependence on the inertial navigation system is bound to bring great technical difficulty
and sharp increase of cost. SINS/CNS integrated application to realize complementary 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 component 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 exploration. 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
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_7
237
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 navigation 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 correction 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 introduction 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 operating 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 vibration 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 application 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 measurement error; and since the star sensor is directly connected to the carrier fixedly, vibration 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
Inertial navigation
Position, velocity
and attitude
+
Position, velocity
and attitude
Star sensor
Attitude
Fig. 7.2 Simple combination mode
advantages such as low cost and high reliability, etc., but its requirement for performance of gyro and accelerometer is high. With appearance of various new gyros and
accelerometers, SINS is more competitive. However, the system still has shortcomings 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 dynamic 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 perspective of future development trend, inertial/celestial integrated navigation system
of full strapdown mode has better development prospect and is one of the important 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 determining 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
Inertial navigation
system
Star sensor
241
Position, velocity and attitude
Attitude +
Attitude
Filter
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 highprecision 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=⎢
(7.1)
⎣ aφ ⎦ = ⎣ φ − φ 0 ⎦
aγ
γ − γ0
242
7 INS/CNS Integrated Navigation Method
εx, εy , εz
3 gyros
3 accelerometers
Star sensor
x, y, z
SINS
algorithm
CNS
attitude
calculation
+
Attitude
error
∆a
Mathematics
platform
∆ a'
Optimal
estimation
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 mathematics 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 navigation 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
7.3.1
243
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
⎢ b
⎢
⎢
⎢ 03×3
⎢
⎢
⎢
G(t) = ⎢ 03×3
⎢
⎢
⎢
⎢ 03×3
⎢
⎣
03×3
03×3
Clib
03×3
03×3
03×3
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(7.6)
15×6
244
7 INS/CNS Integrated Navigation Method
⎤
⎤
⎡
⎡
f14 f15 f16
0
ȧz
−ȧy
⎥
⎥
⎢
⎢
⎥
⎢
Wherein, Fa = ⎢
0
ȧx ⎥
⎦, referring to Sect. 2.3
⎣f24 f25 f26⎦ , Fb = ⎣−ȧz
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:
2
2
2
2
2
2
Q(t) = diag σεx
(7.8)
σεy
σεz
σ∇x
σ∇y
σ∇z
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 error, 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
alignment angles, [ δvE
δvN
three position errors, [ εx
εy
φN
φU ] are three mathematics platform mis-
δvU ] are three velocity errors, [ δL
δλ
δh] are
ε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 ⎥
⎢
⎥
⎣
⎦
09×3 09×3 I9×9
(7.12)
7.4 New Inertial/Celestial Integrated Navigation Method of Ballistic Missile
W = wεx
245
T
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
⎢
⎢
⎢
G = ⎢03×3
⎢
⎣
09×3
W = wεx
⎤
03×3
⎥
⎥
⎥
Cbn ⎥
⎥
⎦
09×3
(7.14)
T
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) = ⎢
(7.16)
⎣ φy ⎦ = H X(t) + V (t)
φz
⎡
XS
⎤
⎥
⎢
⎥
wherein, H = [I3×3 03×3 03×9 ]; V = ⎢
⎣ YS ⎦; XS , YS , and ZS are measureZS
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 missileborne 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 information, 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 reduction 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
S
z
x
θ′
Δθ
θ
x′
S
z′
z
θ
x
C
x′
A
R
Δθ
O
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 calculation. 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
navigation system
3-axis
IMU
Ephemeris
Processin
g circuit
data processor
Star
sens
or
Error
compensate
Strapdown
calculation
State
estimation
Observation
quantity
Optimal
filter
For optimal filter, one of the important problems affecting its filtering performance 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 perturbation influence factor affecting ballistic missile orbit is gravitational acceleration
deviation caused by Earth ellipsoid model, and this problem can be solved by adopting 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
δ ṙ = δv
li
(7.18)
li
wherein, φ is mathematics platform misalignment angle, Cbli is relevant coordinatetransformation 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:
⎡
⎤
i 2 i
3
2
rx ⎥
⎢ 1 + 2 J2 (re /r) 1 − 5 rz /r
⎢
⎥
⎢
⎥
3
μ
2
⎢
⎥
g li = −Cili 3 ⎢ 1 + J2 (re /r)2 1 − 5 rzi /r
(7.19)
ryi ⎥
⎥
r ⎢
2
⎢
⎥
⎣
2 i ⎦
3
1 + J2 (re /r)2 3 − 5 rzi /r
rz
2
⎡
⎤⎡
⎤
∂rxi
∂gxi /∂rxi ∂gxi /∂ryi ∂gxi /∂rzi
⎢
⎥⎢
⎥
i
i ⎥
i
i
i
i
i ⎥⎢
δg li = Cili ⎢
(7.20)
⎣ ∂gy /∂rx ∂gy /∂ry ∂gy /∂rz ⎦ ⎣ ∂ry ⎦
∂gzi /∂rxi ∂gzi /∂ryi ∂gzi /∂rzi
∂rzi
7.4 New Inertial/Celestial Integrated Navigation Method of Ballistic Missile
Initial
variance
and
state
variable
Samplin
g point
creation
Samplin
g point
spread
State,
measure
ment
value
and
variance
forecast
Updatin
g state
and
variance
249
K+1
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 coordinatetransformation 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 simulation 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 accumulated 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
During motion
Navigation
information output
IMU
SINS
Celestial sensor
CNS
251
Platform error angle , position error , velocity error and
inertial component error
Navigation
Position, velocity and attitude
Position and
attitude
Position, velocity and attitude
(during rest )
SINS/CNS
integrated
navigation system
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 realtime 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 navigation. 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 navigation 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 position, 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 propriocoordinate 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 strapdown algorithm. Obtain position, velocity, and attitude of lunar vehicle through twice
integration [18, 19]. Basic equation of SINS may be abbreviated to:
⎧
⎪
⎪
⎨ ṙ = Dv
n
n
(7.21)
v̇ = Rbn f b − (2wim
+ wmn
) ×v−g
⎪
⎪
⎩ n
n
Ṙb = −Ωbn
Rbn
m
wherein, D = 1/R
0
0
T
T
1/(Rm cos L) ; r = [L, λ] , v = [vE , vN ] are respectively
position and velocity vector of lunar vehicle; f b is output vector of the acn
celerometer; wim
= [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
n
=
is small and may be ignored when requirement for precision is low; wmn
T
[−vN /Rm , vE /Rm , vE tan (L)/Rm ] is angular velocity of rotation of navigation coordinate 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 tranb
n
and win
, wherein, subscripts n and b
sition matrix available to be described by wib
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 equation with mathematics platform misalignment angle, velocity, position, and inertial
component error as observation quantities. In view of instability of vertical channel 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 mathematics 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
⎪
⎪
φU + εx
+ ωim sin L +
tgL φN − ωim cos L +
φ̇E = −
⎪
⎪
⎪
Rm
Rm
Rm
⎪
⎪
⎪
⎪
⎪
δvE
vE
vN
⎪
⎪
⎪
⎨ φ̇N = R − ωim sin LδL − ωim sin L + R tgL φE − R φU + εy
m
m
m
⎪
δvE
vE
⎪
2
⎪
⎪
⎪ φ̇U = R tgL + ωim cos L + R sec L δL
⎪
m
m
⎪
⎪
⎪
⎪
⎪
vE
vN
⎪
⎪
+ ωim cos L +
φN + ε z
(7.22)
φE +
⎩
Rm
Rm
2. Velocity error equation
⎧
vN
vE
⎪
⎪
δvN
tgLδvE + 2ωim sin L +
δ υ̇E = fy φU − fz φN +
⎪
⎪
⎪
R
R
m
m
⎪
⎪
⎪
⎪
⎪
v E vN
vE
⎪
2
⎪
δvU + ∇x
⎪
⎨ + 2ωim cos LvN + R sec L δL − 2ωim cos L + R
m
m
⎪
vE
⎪
⎪
δ υ̇N = fz φE − fx φU − 2 ωim sin L +
tgL δvE
⎪
⎪
⎪
R
m
⎪
⎪
⎪
⎪
⎪
vE
⎪
⎪
sec2 L vE δL + ∇y
(7.23)
⎩ − 2ωim cos L +
Rm
3. Position error equation
⎧
δvN
⎪
⎪
⎨ δ L̇ = R
m
⎪
δv
vE
E
⎪
⎩ δ λ̇ =
sec L +
sec LtgLδL
Rm
Rm
(7.24)
4. Gyro drifts error equation
⎧
⎪
⎪
⎨ ε̇x = 0
ε̇y = 0
⎪
⎪
⎩
ε̇z = 0
(7.25)
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 dimensions 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 platform misalignment angle, there is tiny difference between them, and the specific
relationship is as follows:
p
Rb = Rnp · Rbn
⎡
1
⎢
Rnp = ⎢
⎣ −φU
φN
−φN
φU
1
−φE
⎤
⎡
c11
⎥ n ⎢
⎢
φE ⎥
⎦, Rb = ⎣ c21
1
c31
c12
c22
c32
(7.28)
c13
⎤
⎡
c11
⎥ p ⎢
⎢
c23 ⎥
⎦, Rb = ⎣ c21
c33
c31
c12
c13
⎤
c22
⎥
c23 ⎥
⎦
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:
tan (ψI ) = −
c12
c
, sin (θI ) = c32 , tan (ϕI ) = − 31
c22
c33
tan (ψ) ψ
,
1−tan (ψ) ψ
it is obtained by
− cc12
ψ
c12
c12 − c32 φN + c22 φU
22
−
=−
=
c12
c22
c22 + c32 φE − c12 φU
1 + − c22 ψ
(7.30)
Since tan (ψI ) = tan (ψ +
using Eq. (7.29) that:
ψ=
ψ) =
tan (ψ) tan ( ψ)
1−tan (ψ) tan ( ψ)
(7.29)
≈
2
2
+ c22
)φU
c12 c32 φE + c22 c32 φN − (c12
.
2
2
c12 + c22 + c22 c32 φE − c12 c32 φN
(7.31)
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
c32 = c32 − c22 φx + c12 φy = c32 +
tan (ϕI ) = tan (ϕ +
ϕ) =
2
1 − c32
·
255
θ
tan (ϕ) tan ( ϕ)
tan (ϕ) ϕ
≈
1 − tan (ϕ) tan ( ϕ)
1 − tan (ϕ) ϕ
− cc31
ϕ
c31 − c21 φE + c11 φN
c31
33
=−
=
−
c31
1 + − c33 ϕ
c33
c33 − c23 φE + c13 φN
From Eqs. (7.32–7.35), it is worked out that the expressions of
θ=
ϕ=
2
c31
θ and
−c22 φE + c12 φN
2
1 − c32
(c21 c33 − c23 c31 )φE + (c13 c31 − c11 c33 )φN
2
+ c33
− (c21 c31 + c23 c33 )φE + (c11 c31 + c13 c33 )φN
(7.33)
(7.34)
(7.35)
ϕ are:
(7.36)
(7.37)
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
Time updating
SINS error model
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
error
Longitude Latitude
(◦ )
(◦ )
SINS
− 0.5434
SINS/CNS − 0.0027
Attitude
Pitching
angle (◦ )
Roll angle
(◦ )
Yaw angle
(◦ )
− 5.9075 − 3.8462
− 0.1115
− 0.1356
0.4702
− 0.0022 − 0.4531 − 0.3217
− 0.0628
− 0.0310
0.0576
0.1610
Velocity
East
(m/s)
North
(m/s)
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
7.6
257
Inertial/Celestial Integrated Attitude Determination
Method of Satellite
With rapid development of deep space exploration technology and satellite technology, 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 combining 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:
q̇ =
1
1
(ω)q = q ⊗ ω
2
2
(7.41)
wherein, ω = [ωx , ωy , ωz ]T is angular velocity of rotation of star coordinate system
relative to inertial coordinate system.
⎡
⎤
⎡
⎤
0
−ωz ωy
⎢
⎥
−[ω × ] ω
⎦, [ω×] = ⎢ ωz
(ω) = ⎣
(7.42)
0
−ωx⎥
⎣
⎦
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 ⊗ω
2
(7.44)
1
q̂˙ = q̂ ⊗ ω̂
2
(7.45)
q̇ =
Take the derivation of Eq. (7.43) and substitute Eqs. (7.44) and (7.45) to obtain:
δ q̇ =
1
[δq ⊗ ω − ω̂ ⊗ δq]
2
(7.46)
Wherein, δω is defined as:
] = [(b − b̂) + na ]
δω = [ω − ω
(7.47)
Substitute Eq. (7.47) into (7.46) to obtain:
δ q̇ =
1
1
[δq ⊗ ω̂ − ω̂ ⊗ δq] + δq ⊗ δω
2
2
(7.48)
Linearize Eq. (7.48) to obtain:
1
δ q̇ = −ω̂ × δe − ( b + na )
2
(7.49)
So error variable state equation is:
Ẋ(t) = F(t) X + G(t)ε(t)
(7.50)
wherein,
⎤
⎡
1
1
− I
−[ω(t) × ] − I3×3
⎥
⎢ 2 3×3
⎢
2
T⎦, G(t) = ⎣
F(t) = ⎣
03×4
03×3
03×3
⎤
⎡
04×3
I3×3
⎤
⎡
⎥
⎢
⎦, ε(t) = ⎣
na
⎥
⎦.
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;
e = δe + v = H X + v = I3×3
T
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 integrated 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 sequential processing of observed starlight vector and attitude determination under single
star circumstance; (2) introduction of Kalman filtering frame can effectively estimate 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 requires 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
∗
=
ρk+1
m2k tr(Pk+1/k )
m2k tr(Pk+1/k ) + δm2k+1 tr(Rk+1 )
1
∗
∗
)mk + ρk+1
δmk+1
δmk+1 = δmk − 1 + , mk+1 = (1 − ρk+1
k
mk
∗ δmk+1
Kk+1/k + ρk+1
δKk+1
mk+1
mk+1
2
mk 2
∗
∗ δmk+1
= 1 − ρk+1
Pk+1/k + ρk+1
Rk+1
mk+1
mk+1
∗
Kk+1/k+1 = (1 − ρk+1
)
Pk+1/k+1
(7.55)
(7.56)
(7.57)
(7.58)
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 observation 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 determination 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 introduce 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
W (τ )dτ
(7.60)
D(t, t0 ) =
t0
7.6 Inertial/Celestial Integrated Attitude Determination Method of Satellite
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
3!
265
(7.61)
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 direction cosine matrix, denoted by A(t) and satisfying the following differential
equation:
Ȧ(t) =
(t)A(t), A(t0 ) = A0
(t) = −[ω(t)×]
(7.65)
(7.66)
Here, D(t, t0 ) adopts the following form:
D(t, t0 ) = −[θ (t)×]
(7.67)
T
θ (t) = θ1 (t) θ2 (t) θ3 (t)
(7.68)
Vector θ (t) is defined as:
5
θi (t) =
t
t0
ωi (τ )dτ , i = 1,2, 3
(7.69)
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 combination 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
(7.70)
θ (k) = θ1 (k) θ2 (k) θ3 (k)
It is implied in formula (7.69) that:
5
θi (k) =
tk
ω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
ω(τ )dτ , i = 1,2, 3
(7.72)
θ (k + 1) = θ (k) +
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))
wherein, ω̄ is expectation vector of gyro drift; Q(t) is white noise intensity.
(7.75)
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)
+
2!
3!
;
T
+ D̂(k + 1, k)Ŵ (k) − Ŵ (k)D̂(k + 1, k) Â(k).
3!
Â(k + 1) = I + D̂(k + 1, k) +
(7.78)
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
roll (°)
0.06
EKF
UKF
0.04
0.02
0
0
50
100
150
200
250
300
pitch(°)
0.06
EKF
UKF
0.04
0.02
0
0
50
100
150
200
250
yaw(°)
0.1
300
EKF
UKF
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, minimum parameter attitude matrix estimation method based on UKF designed in this
subsection further improves attitude determination precision and reliability of attitude 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
7.6.4
269
Interlaced Optimal-REQUEST and Unscented Kalman
Filtering for Attitude Determination
The EKF and UKF methods for high performance attitude determination of strapdown 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 optimalREQUEST 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 attitude 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:
⎤
⎡
ψk
cos (0.5 ωk t) I 3×3 − [ψ k ×]
⎥
⎢
Ω(ωk ) = ⎣
(7.85)
⎦
T
−ψ k
cos (0.5 ωk t)
4×4
ψk =
sin (0.5 ωk t)ωk
ωk (7.86)
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 sequentially 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
vectors from star
sensor (10Hz)
Observation
vectors arrive?
N
Y
Optimal-REQUEST
mathematical model
z –1 qˆ k +1
, Switch on UKF
UKF algorithm
mathematical model
Optimal-REQUEST
for attitude determination
z –1 βˆ k +1 , Switch on Optimal -REQUEST
UKF
for the gyro drifts estimation
New observation
vectors?
Y
k =k+1
N
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 using 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
A
B
Attitude Data
Generator PC
C
Star Atlas Simulator
Z
Star Sensor Simulator
E
DSP
Y
X
D
Micro-IMU
Integrated Attitude
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 M380@2.53 GHz
digital processor with 3.24 GB memory, which is installed in the platform of MATLAB 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 determination 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
filtering method
Novel method
5.958
3.444
6.542
9.002
5.016
9.345
13.880
7.588
5000
4.675
7000
10,000
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 gradually 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 optimalREQUEST 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 traditional 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
0.01
0
-0.01
-0.02
-0.03
0
100
200
300
Traditional Segmented-f iltering
The presented method
0
100
200
300
400
500
UKF
Traditional Segmented-f iltering
The presented method
0
100
200
300
Time（ t/s）
400
(t/s)
20
500
UKF
0
-0.02
-0.04
-0.06
a
400
X Axis（ °/h）
0
10
Y Axis（ °/h）
Roll(°)
Pitch(°)
Traditional Segmented-f iltering
The presented method
-0.01
Yaw(°)
Gyro Drift Estimation
UKF
Z Axis（ °/h）
Attitude Error Estimation
0.01
500
b
UKF
Traditional Segmented-filtering
The presented method
5
0
0
100
200
10
0
0
100
200
400
500
300
400
500
UKF
Traditional Segmented-filtering
The presented method
20
10
0
300
UKF
Traditional Segmented-filtering
The presented method
0
100
200
300
Time（ t/s）
400
500
(t/s)
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 information. 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 deviation 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 determination of satellite, it introduces a SINS/CNS integrated attitude determination method
of piecewise information fusion based on EKF by combing satellite attitude determination 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 correct 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 introduced into SINS/CNS integrated navigation system to constitute SINS/CNS/GNSS
integrated navigation system, this problem will be solved effectively, so the next chapter 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 navigation. 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 international 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 position 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 correction 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 intensified, 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 introduces the advanced filtering methods such as unscented Kalman filter (UKF) and
genetic algorithm into integrated navigation to focus on the SINS/CNS/GNSS integrated navigation method based on federated UKF and federated filtering optimized
information distribution factor.
© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_8
279
280
8.2
8.2.1
8 INS/CNS/GNSS Integrated Navigation Method
Principle of INS/CNS/GNSS Integrated Navigation
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 reception 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
Fig. 8.1 Centralized filtering
structure of strapdown inertial
navigation system
(SINS)/celestial navigation
system (CNS)/global
navigation satellite system
(GNSS) integrated navigation
system
281
6,16
Ï6,16
96,1636,16
&16
Ï&16
*166
×
9*1663*166
×
central
;3
ized
filter
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
Pii−1 X̂i ,
X̂g = Pg
i=1
(8.1)
282
8 INS/CNS/GNSS Integrated Navigation Method
Main
Ref.
System
Time
SubSub-
Filter1
System1
Sub-
Sub-
Filter2
System2
Optimization
Fig. 8.2 General structure of federated filter
wherein, Pg =
N
!
i=1
Pii−1
−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:
Suppose the state estimation X̂i = Xi(1)
Xi(2)
. . . . Xi(mi )
T
(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
(8.2)
3X̂ 3 = (traceE(X̂X̂ T ))1/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
max (dim
i
i)
2
N
≤ m ≤ dim
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
i;
Ti :
→
and local state space
i = 1, . . ., N .
i
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
2−1
N
Ti Pi−1 Ti
P =
(8.3)
.
i=1
1
2
N
Ti Pi−1 X̂i
X̂g = P
.
(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 conservation problem. Under different local models, conversion of global-state space
into local-state space shall be conducted first before distributing according to information conservation principle. Information distribution formulas under different
local models are:
Pi−1 = βi Ti P −1 .
X̂i = Ti X̂
i = 1, . . ., N.
(8.5)
(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, federated filtering based on different local models will degrade into traditional federated
filtering. Equations (8.3–8.6) are respectively converted into:
1
2−1
N
Pi−1
P =
.
(8.7)
Pi−1 X̂i .
(8.8)
i=1
N
X̂g = P
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 federated 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 reduction 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 following 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, respectively, 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,
⎡
⎢
H (t) = ⎢
⎣ 03×3
03×3
03×3
I3×3
03×3
03×3
⎥
03×3 ⎥
⎦,
03×3
03×3
Hp
03×3
03×3
wherein Hp = diag(Rm
03×3
⎤
03×3
I3×3
Rn cos L 1).
Measurement quantity Z(t) = φE φN φU δvE δvN δvU δL
(8.12)
δλ δ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 includes 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 obtained 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:
⎤
⎤
⎡
⎡
V (t)
Hv
⎦ X(t) + ⎣ v ⎦,
Z2 (t) = H2 (t)X(t) + V2 (t) = ⎣
(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 subfilter 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 ,
zj ,k = hj ,k (xj ,k ) + vj ,k ,
j = 1,2.
(8.15)
(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
filter2
GNSS
Measurement
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:
2
1 2n
s
x̂j−,k =
Wis χi,k|k−1
1 2n
Pj−,k
c
s
=
(8.17)
.
i=0
j
Wi χi,k|k−1 −
x̂k−
χi,k|k−1 −
T
x̂k−
2
+Q
.
i=0
1 2n
2
s
ẑj ,k =
Wim Zi,k|k−1
i=0
1 2n
s
Pj ,ẑk ẑk =
Wic
(8.19)
.
j
Zi,k|k−1 − ẑk
Zi,k|k−1 − ẑk
T
2
+R
.
i=0
k k
(8.20)
j
1 2n
Pj−,x̂ − ẑ =
(8.18)
j
s
T
Wic χi,k|k−1 − x̂k− Zi,k|k−1 − ẑk
i=0
2
.
(8.21)
j
Kj ,k = Px̂ − ẑ Pẑ−1
.
ẑ
k k j
(8.22)
x̂j ,k = (x̂k− + Kk (zk − ẑk ))j .
(8.23)
P̂j ,k = Pk− − Kk Pẑk ẑk KkT j ,
(8.24)
k k
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:
2
1 2n
s
−
x̂m,k
=
Wis χi,k|k−1
i=0
⎧
⎪
−
⎪
⎪
Pm,k
=
⎪
⎪
⎪
⎪
⎪
⎨
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎩
2ns
!
i=0
(8.25)
.
m
T
Wic χi,k|k−1 − x̂k− χi,k|k−1 − x̂k− + Q
−1
Pf−1
,k = Pm,k +
N
!
m
Pj−1
,k
j =1
N
!
−1
Pf−1
,k x̂f ,k = Pm,k x̂m,k +
j =1
,
(8.26)
Pj−1
,k x̂j ,k
−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
is inverse of the fusion covariance matrix,
x̂m,k|k−1 = x̂f ,k|k−1 , Pf ,k ∈ R
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
wherein β must satisfy the following information fusion principle βm +
2
!
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 ] ,
R1 = diag
10
2 2 2 , 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
on federated UKF
Estimated error based
on federated EKF
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
3.1181
4.3272
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
δϕ[ ]
Standard deviation
δz[m]
εx [◦ /h]
Mean value
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 . . .
8.4
291
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 decentralized linear stochastic system consisting of multiple navigation sensors are to be
as follows:
xk =
k|k−1 xk−1
zi,k = Hi,k xk + vi,k ,
+ Gk−1 wk−1 .
i = 1,2, · · ·, l,
(8.28)
(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 =
−1
Pi,k|k−1
i = 1,2, · · ·, l, m
i,k|k−1 x̂i,k−1 ,
=[
k|k−1 Pi,k−1
T
k|k−1
+ Qi,k ]−1 ,
i = 1,2, · · ·, l, m.
(8.30)
Measurement updating equations of sub-filter are as follows:
−1
−1
−1
Pi,k
= Pi,k|k−1
+ HiT Ri,k
Hi ,
−1
x̂i,k
Pi,k
=
−1
Pi,k|k−1
x̂i,k|k−1
+
i = 1,2, · · ·, l
−1
HiT Ri,k
zi,k ,
i = 1,2, · · ·, l.
(8.31)
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
−1
Pf−1
,k = Pm,k +
−1
Pi,k
,
(8.32)
i=1
l
−1
Pf−1
,k x̂f ,k = Pm,k x̂m,k +
−1
Pi,k
x̂i,k ,
(8.33)
i=1
nf ×nf
is the inverse of fusion
wherein x̂m,k = x̂m,k|k−1 , x̂m,k|k−1 = x̂f ,k|k−1 ; Pf−1
,k ∈ R
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,
−1
Pi,k = βi,k
Pf ,k ,
i = 1,2, · · ·, l, m,
i = 1,2, · · ·, l, m,
(8.34)
(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 information 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 information distribution. Information of estimated error variance can be used to estimate
system error. Ordinary filters fuse the local estimation into a global optimal estimation 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 . . .
8.4.2
293
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 matrix, 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
Pf ,k|k−1 ,
Pi,k|k−1 = βi,k
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.
i=1
(8.43)
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 distribution of information distribution factor, the fault tolerance of the SINS/CNS/GNSS
integrated navigation system can be enhanced. Such method of optimized information 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 reliable 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 preferences 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 integrated 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 genetic 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 nonlinearity 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 fundamental 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
CNS
LF 1
UKF1
Z1
Time Update
Xˆ 1 , P1
−1
Xˆ g , β1 Pg
Xˆ m , Pm
Improved GA
−1
Xˆ g , β 2 Pg
GPS
LF 2
UKF2
Z2
Xˆ g
Xˆ m , Pm
Xˆ g , Pg
β m −1 Pg
Optimization fuse
Xˆ 2 , P2
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 .
T
T
Suppose that Zk = [Z1,k
, Z2,k
, · · ·, ZNT ,k ]T and LFs are uncorrelated. Then,
1
N
Pii−1 Xi , Pg
Xg = Pg
i=1
2−1
N
Pii−1
=
i=1
where: Pg is the covariance matrix of the global FKF.
Pii is the covariance matrix of the ith LF.
,
(8.48)
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 =
−1
βi Q−1
i +βm Q ,
i = 1,2, · · ·, N , m,
(8.49)
βi Pi−1 +βm P −1 ,
i = 1,2, · · ·, N , m,
(8.50)
i=1
N
P −1 =
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 covariance 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 navigation 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 devices, 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 simulation 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
OFGA
TFT LCLV
Star Simulator
CCD Star
Sensor
GPS
Receiver
Preformed
Interface
Components
Layer
SINM
System
Layer
TGM
CNM
GPSM
PPM
PIM
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 performancetesting 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 . . .
Fig. 8.6 The flow diagram of
the entire software system.
CNS celestial navigation
system, GPS global
positioning system, SIN
strapdown inertial navigation,
UKF unscented Kalman filter,
FKF federated Kalman filter,
PTR performance testing
report, GA genetic algorithm
299
Initialization
Set Parameter
Receive Next
SINS Data
N
SINS Data ?
Y
N
Integrated Mode ?
Y
CNS Data
Integrated
Navigation
GPS Data
SIN
Module
Improved
GA Module
Based on UKF of
Adaptive FKF
SIN Algorithm
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 research 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 navigation 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)
2
2
2
R1 = diag 5 , 5 , 5
.
(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 . . .
12
15
Federated KF
the presented method
10
δФy/(″)
δФx/(″)
8
6
4
2
0
5
Federated KF
the presented method
0
δФz/(″)
10
301
5
0
-5
-5
-10
-15
Federated KF
the presented method
-2
-4
0
200
400
600
-10
0
time/s
200
400
600
-20
0
time/s
200
400
600
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 accelerometer 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
the presented method
©\ P
©[ P
4
2
0
-2
0
200
400
5
0
-5
600
Federated KF
the presented method
0
WLPHV
600
0.15
Federated KF
the presented method
0.1
©9\ PV
©9[ PV
400
WLPHV
0.15
0.05
0
-0.05
200
0
200
400
600
Federated KF
the presented method
0.1
0.05
0
-0.05
0
WLPHV
200
400
600
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 information 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 traditional 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 . . .
0.05
0.01
Federated KF
the presented method
0
εy/（?/h）
εx/（?/h）
0.04
0.03
0.02
0.01
0
-0.01
0
100
200
300
400
-0.02
-0.03
-0.04
-0.05
500
Federated KF
the presented method
-0.01
0
100
150
150
100
100
50
0
Federated KF
the presented method
0
100
200
200
300
400
500
time/s
▽y/(ug)
▽x/(ug)
time/s
-50
303
300
400
500
50
0
-50
Federated KF
the presented method
0
100
time/s
200
300
400
500
time/s
εz/（?/h）
0.02
0.01
0
-0.01
Federated KF
the presented method
-0.02
-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.5
8 INS/CNS/GNSS Integrated Navigation Method
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 machine at present, and an important development direction for integrated navigation
technology.
This chapter first introduced the basic principle, combination mode and the modeling method of the SINS/CNS/GNSS integrated navigation; then, specific to the
decline of integrated navigation precision caused by instable error model and uncertain environmental disturbance of SINS/CNS/GNSS integrated navigation system,
advanced filtering algorithm, and intelligent algorithm are introduced into integrated 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 navigation method based on the federated UKF is introduced; (2) specific to optimization
difficulty of federated filtering information distribution factor, federated filtering 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 optimized 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 optimized 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 performance 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 realized 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 integrated navigation system, real-time data processing is required, but the real-time
performance becomes worse since the system order is generally high and the integrated 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 observability 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
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_9
307
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 design 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 determine whether the system is completely observable and the other is to roughly
determine which state variables are observable and which are unobservable for incompletely 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 observability 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 quantitative value for the observability of each state. Therefore, this section introduces an
improved system observability analysis method based on singular value decomposition (SVD) [4, 8], which provides an effective way for a quantitative analysis of the
observability of linear time-varying systems.
9.2.1
1.
Observability Analysis Theory of the PWCS
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
Fig. 9.1 Measurement series
of a discrete PWCS
Time frame 1
Time frame 2
Z(t) = H (t)X(t) + V (t),
309
Time frame 3
(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)
Zj (k) = Hj X(k).
(9.4)
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)
⎪
⎪
⎪
⎪
⎪
⎪
...
⎪
⎪
⎪
⎪
⎪
⎪
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)
⎪
⎪
⎪
⎪
⎪
⎩
...
(9.5)
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
⎢
⎥ ⎢
⎥
⎢
⎥ ⎢
⎥
..
..
⎢
⎥
⎢
⎥
.
.
⎢
⎥ ⎢
⎥
⎢
⎥
⎢
⎥
n−1
n−1
⎢
⎥
⎢
⎥
H
H
F
F
1 1
1 1
⎢
⎥ ⎢
⎥
⎢
⎥ ⎢
⎥
⎢. . . . . . . . . . . . . . . . . . . . . . . . . . .⎥ ⎢ . . . . . . . . . . . . . . . . . . . . . . . . . . . ⎥
⎢
⎥ ⎢
⎥
⎤
⎡
⎢
⎥ ⎢
⎥
H2
H2 F1n−1
⎢
⎥ ⎢
⎥
⎥
⎢
⎢
⎥ ⎢
⎥
⎥
⎢
⎢
⎥
⎢
⎥
n−1
⎥
⎢
⎢
⎥
⎢
⎥
H 2 F 2 F1
⎢ 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
⎢
⎥ ⎢
⎥
⎢
⎥
⎢
⎥
⎢
⎥
n−1 n−1
n−1 ⎥
⎢
⎥
⎢ Hr Fr Fr−1
⎢
⎥
Fr−2 · · · F1
⎢
⎥ ⎢⎢ Hr Fr ⎥ n−1 n−1
n−1 ⎥
F
F
.
.
.
F
⎢
⎥
⎢
⎥
⎢
⎥
1
..
⎢
⎥ ⎢⎢ ... ⎥ r−1 r−2
⎥
.
⎢
⎥ ⎣⎣
⎦
⎦
⎣
⎦
n−1
n−1 n−1
H r Fr
Fr−2 · · · F1n−1
Hr Frn−1 Fr−1
(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) = ⎢
⎥
···
⎢
⎥
⎣
⎦
n−1 n−1
Fr−2 · · · F1n−1
Nr Fr−1
(9.9)
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
⎥
⎢ ⎥
⎢
⎥
⎢ ⎥
⎢
n−1
⎥
⎢ G2 ⎥
⎢
G2 M 1
⎥
⎥,
⎢
⎢
G = ⎢ ⎥, Ed = ⎢
⎥
...
⎥
⎢. . . ⎥
⎢
⎦
⎣ ⎦
⎣
n−1
. . . M1n−1
Gr
Gr Mr−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 substituting 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,
wherein U = [u1
u2
· · · um ] and V = [v1
v2
(9.11)
· · · vm ] are both orthogonal
Λr×r 0
, wherein Λr×r = diag(σ1
0
0
· · · > σr > 0 are called singular values of matrix Q.
Substitute Eq. (9.11) into Eq. (9.10), and there is:
matrixes; S =
σ2
Z = (U · S · V T) · X(t0 ).
· · · σr ), σ1 > σ2 >
(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
Z = X(t0 ) =
i=1
ui T · Z)
σi
· vi T .
(9.14)
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 irrelevant 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 function 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 observability 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 dimensionality 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
Singular value
35
奇异值
30
25
20
15
10
5
0
1
2
3
4
5
6
7
8
9
15个状态变量
10
11
12
13
14
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 observability 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:
X1 (t) = φE
T
φ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) + G1 (t)W1 (t)
Z1 (t) = H1 (t)X1 (t) + V1 (t),
(9.16)
wherein the system state transition matrix F1 (t) is:
⎡
Cbn
F2
F1 (t) = ⎣
03×3
wherein
⎡
0
⎢
⎢
⎢
v tgL
F2 = ⎢− ωie sin L + E
⎢
R
N +h
⎣
vE
ωie cos L +
RN + h
03×3
⎤
⎦
,
(9.17)
6×6
⎤
vE
− ωie cos L +
RN + h ⎥
⎥
⎥
vN
⎥.
−
⎥
RM + h
⎦
0
vE tgL
ωie sin L +
RN + h
0
vN
RM + h
(9.18)
The system noise driving matrix G1 (t) is:
⎡
Cbn
⎤
⎦
G1 (t) = ⎣
03×3
(9.19)
.
6×3
System noise vector
W 1 (t) = wεx
T
wεy
w εz
,
wherein wεx , wεy , and wεz are random errors of the gyro.
The system noise variance matrix is:
2
2
Q1 (t) = diag σεx
σεy
σεz2 .
(9.20)
(9.21)
318
9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method
Measurement quantity
Z(t) = φE
Measurement matrix
3.
T
φN
H1 (t) = I3×3
φU
.
03×3 .
(9.22)
(9.23)
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 compensation 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 . Table 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 dimensionality 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) introduced 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 analysis method, dimensionality model design, and computer simulation verification are
introduced below.
9.4 Dimensionality Reduction Filter Design of INS/GNSS . . .
Fig. 9.3 Simulation error curve of the SINS/CNS dimensionality reduction model
319
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
Singular value
Fig. 9.4 Singular values
corresponding to state
variables
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]:
X2 (t) = φE
φN
δvE
δvN
δvU
δL
δλ
δh
εx
εy
∇z
T
.
(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) + G2 (t)W2 (t)
Z2 (t) = H2 (t)X2 (t) + V2 (t),
(9.25)
9.4 Dimensionality Reduction Filter Design of INS/GNSS . . .
wherein the state transition matrix of system is:
⎤
⎡
F 18×8 F 28×3
⎦
F2 (t) = ⎣
03×8
03×3
321
,
(9.26)
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
⎢
⎢
F 2 = ⎢ 03×2
⎣
03×2
wherein F c1 =
Cbn (1,1)
Cbn (2,1)
Cbn (1,2)
,
Cbn (2,2)
Measurement matrix:
⎡
03×2 diag(1 1
H2 (t) = ⎣
03×2
03×3
02×1
⎤
⎥
⎥
F c23×1 ⎥
⎦
03×1
,
(9.27)
8×3
F c2 = Cbn (1,3) Cbn (2,3)
1)
03×3
diag(RM
03×3
1)
RN cos L
03×3
Cbn (3,3)
T
.
⎤
⎦
. (9.28)
6×11
System noise driving matrix:
⎡
F c12×2
⎢
⎢
G2 (t) = ⎢ 03×2
⎣
06×2
02×1
⎤
⎥
⎥
F c23×1 ⎥
⎦
06×1
.
(9.29)
11×3
System noise vector:
W2 (t) = wεx
T
wεy
w∇z
.
(9.30)
System noise variance matrix:
2
Q2 (t) = diag σεx
3.
2
σεy
2 .
σ∇z
(9.31)
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 . Table 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 dimensionality 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 reduction 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
T subfilter takes 6-dimension state
Sect. 9.3 that the SINS/CNS
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) + G1 (t)W1 (t)
Z1 (t) = H1 (t)X1 (t) + V1 (t).
(9.32)
9.5 Federated Filter Design of the INS/CNS/GNSS . . .
Fig. 9.5 Error curve of the
SINS/GNSS dimensionality
reduction model simulation
323
Longitude error (m)
0.06
••••
Low-order model
••••
------High-order model
0.04
0.02
0
-0.02
-0.04
-0.06
-0.08
0
100
200
300
400
500
Time (s)
600
700
800
900
Latitude error (m)
0.25
••••
Low-order model
••••
------High-order model
0.2
0.15
0.1
0.05
0
-0.05
-0.1
0
100
200
300
400
500
Time (s)
600
700
800
900
North velocity error (m/s)
0.025
••••
Low-order model
••••
------High-order model
0.02
0.015
0.01
0.005
0
-0.005
0
100
200
300
400
500
600
700
800
900
Time (s)
East velocity error (m/s)
0.025
••••
Low-order model
••••
------High-order model
0.02
0.015
0.01
0.005
0
-0.005
-0.01
0
100
200
300
400
500
Time (s)
600
700
800
900
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) + G2 (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) = ⎣
05×8
05×5
,
(9.34)
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
⎥
⎢
⎥
⎢
(9.35)
F 2 = ⎢ 03×2
Cbn ⎥ ,
⎦
⎣
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
H2 (t) = ⎣
03×2
03×3
1)
03×5
03×3
diag(RM
RN cos L
1)
⎤
⎦.
(9.36)
03×5
System noise driving matrix:
⎡
F c2×2
⎢
⎢
G2 (t) = ⎢ 03×2
⎣
08×2
02×3
⎤
⎥
⎥
Cbn ⎥
⎦
08×3
13×5
.
(9.37)
9.5 Federated Filter Design of the INS/CNS/GNSS . . .
325
System noise vector:
W2 (t) = wεx
T
wεy
w∇x
w∇y
(9.38)
.
w∇z
System noise variance matrix:
2
Q2 (t) = diag σεx
2
σεy
2
σ∇x
2 .
σ∇z
2
σ∇y
(9.39)
Fifteen dimensions are taken for global state variables:
X(t) = φE φN φU δvE δvN δvU δL
T
δλ δh εx εy εz ∇x ∇y ∇z .
(9.40)
Mapping matrix from global state space to local state space:
⎤
⎡
I3×3 03×12
⎦
T1 = ⎣
,
03×3 03×12
(9.41)
6×15
⎡
1
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
T2 = ⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
⎤
0
0
1
0
0
0
1
0
0
1
0
0
1
0
0
1
0
0
1
0
0
1
0
0
1
0
0
1
0
0
0
1
0
0
1
0
0
(Other unmarked elements are 0)
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
1
.
13×15
(9.42)
326
2.
9 Study for Real-Time Ability of INS/CNS/GNSS Integrated Navigation Method
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 reduction 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 conduct the dimensionality reduction design [6, 7] on different partial models of the
SINS/CNS/GNSS integrated navigation system and use the federated filtering algorithm 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 system, 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 navigation 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
------High-order model
üüLow-order model
------High-order model
Time (s)
Time (s)
East velocity error (m/s)
North velocity error (m/s)
üüLow-order model
------High-order model
üüLow-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
navigation
errors
Longitude Latitude
(m)
(m)
East
velocity
(m/s)
North
velocity
(m/s)
Azimuth
angle
( )
Pitching
angle
( )
Roll
angle
( )
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
model
15
3
4050
15
6
4725
7936
Low-order
model
6
3
324
13
6
3211
4374
of SINS/CNS, SINS/GNSS, and SINS/CNS/GNSS. The computer simulation results show that although the filtering accuracy of dimensionality reduction model
is reduced slightly, the amount of calculation is reduced prominently and the realtime 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 integrated 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 verification 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 application 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 navigation system have been carried out abroad. The navigation systems for many severe
environments such as underwater, etc. commonly use semi-physical simulation technology 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, composition, realization, and test.
© National Defense Industry Press, Beijing and Springer-Verlag Berlin Heidelberg 2015
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_10
331
332
10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation
Inertial
measurement
unit
Trajectory generating
subsystem
Signal acquisition
module
Data processing
module
Satellite
navigation
subsystem
Strapdown inertial subsystem
Star
map
simulati
on
module
LCLV
Colli
mator
Optical
sensor
Image
processi
ng
module
Star map
identification
and attitude
determination
module
Integrated
navigation
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
10.2.1
Principle and Composition of Semi-Physical Simulation
System of INS/CNS/GNSS Integrated Navigation
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. Refer 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 semiphysical 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
carrier attitude and star sensor
subsystem
Standard trajectory velocity and position
Static SINS data
Star map simulation
Starlight map
initialization
simulator
receiving
SINS component output
corresponding to trajectory
GNSS
Standard output subject to
static SINS removal, true
Generation of star map image corresponding to optical
axis
subsystem
noise acquisition
Sensing of star sensor simulator
SINS output with true error
GNSS output with true error
characteristic
CNS
subsystem
Star map processing and attitude
SINS
determination
subsystem
characteristic
Strapdown calculation velocity and position
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 communication 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 necessary for integrated filtering subject to superposition of nominal trajectory
data.
2. Produce optical axis point data of star sensor by combining installation matrix 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 digital 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 identification 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 communication interface, completes integrated filtering after time-synchronization
preprocessing according to integrated logical signal, and realizes system performance 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 integrated 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 semiphysical simulation system based on such integrated navigation, as shown in
Fig. 10.4. Information including initial position, initial attitude angle, initial misalignment 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. . .
Star map
simulator
335
Graphic card
of OK series
A
LCLV
Star map simulating
B
Trajectory
PC machine
Colli
mator
Sensitive
probe
Star map identification
and attitude determination
PC machine C
Star sensor simulator
PC machine
Receiving antenna
z
Data
proces
sing
circuit
Y
F
Satellite
navigation
receiver
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 system), 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 trajectory 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 primarily. 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 equations 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 θ
⎪
⎪
⎩
ẏ = v sin θ
(10.1)
10.2 Principle and Composition of Semi-Physical Simulation System. . .
2
Wherein
⎧ g = g0 (R/r) ,
r=
x 2 + (R + y)2 ,
⎪
⎪
⎨ θ0
θ = (θ0 − θk )(t2 − t)2 /(t2 − t1 )2 + θ k
⎪
⎪
⎩θ
k
337
H = r − R,
t ≤ t1
t1 t ≤ t2 ,
t2 t ≤ tk
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 trajectory parameter under gravitational coordinate system of the launching point must be
transferred to inertial coordinate system of the launching point [11, 13] since gravitational coordinate system of the launching point rotates with the earth: (1) Transfer
to local north, vertical, and east coordinate system by multiplying by transfer matrix 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 firstorder 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 output 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 under 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 carrier through first-order algorithm by using previously obtained attitude angle under
inertial coordinate system of the launching point, and then work out output angular 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, velocity, 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 navigation 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 arrangement, 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 introduction 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 window 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. Corresponding 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 conveniently. 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 relatively 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
wherein, M refers to the rotation matrix.
(10.2)
342
10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation
Area light
source
Liquid-crystal display system
Liquid-crystal
display panel
Driver board
Collimator
Light source system
Frequency
divider
Display devices
Star map simulation
algorithm
Star map analog calculator
Fundamental
catalog
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 θ ⎥
⎦ × ⎣− sin φ cos φ 0⎦ =
⎣− sin ψ cos ψ 0⎦ × ⎣0 cos θ
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=⎢
(10.4)
sin α0
− cos α0
0 ⎥
⎣
⎦
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. . .
Fig. 10.10 Ideal model for
position of the fixed star in
focal plane
343
YAxis
X,Y
XAxis
Sky
f
0,0
x Axis
（x,y） （ ）
yAxis
Fig. 10.11 Relation scheme
for focal length and field of
view
Field of view
Q
f
O
P
Nx
Ny
R
Sensor area
array
It can be obtained in accordance with the geometrical relationship of similar
triangles:
x × dh
X⎫
= ⎪
⎪
f
Z⎬
y × dv
Y⎪
⎭
= ⎪
f
Z
⎫
f ×X⎪
⎪
dh × Z ⎬
f ×Y ⎪
⎪
⎭
y=
dv × Z
x=
⇒
(10.5)
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:
f =
N x · dh
x
2 tan ( FOV
)
2
=
Ny · dv
2 tan (
FOVy
)
2
(10.6)
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:
⎤
⎡ ⎤ ⎡
cos αi cos δi
U
⎥
⎢ ⎥ ⎢
⎢ 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 )
⎪
×
×
⎬
2
tan (FOVx /2) sin δi sin δ0 + cos δi cos δ0 cos (αi − α0 )
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 )
x=
(10.8)
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. . .
Fig. 10.12 Section of parallel
equatorial plane in a specific
optical axis
345
z
O′
P
Q
r
δ0
O
y
R
x
A
E
B
O′
G
C
a
F
D
O
B
A
B
O′
C
O′
O
D
b
C
c
O
E
G
F
D
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:
y = 2 arcsin
W
2R cos δ0
R cos δ0
(10.9)
In this condition, span of the right ascension will be:
360
L
L
= =
Range
y
2 arcsin (W/2R cos δ0 ) · R cos δ0
(10.10)
346
10 Semi-physical Simulation Technology of INS/CNS/GNSS Integrated Navigation
Namely:
Range =
360 × 2 arcsin (W/2R cos δ0 )R cos δ0
W
360
=
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
(10.12)
− R cos δ0 /R cos δ0 · R cos δ0
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
=
H /2 − R cos δ0
360
arccos
π
R cos δ0
(10.14)
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
⎪
R cos δ0 > H /2
⎪
⎨360 × arcsin 2 R cos δ0 /π,
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 determined 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.
F OVy
Range
Range < Range ⎫
⎪
|δ − δ0 | ≤
,
≤ α0 < 360 −
⎪
⎪
⎪
2
2
2
2
⎪
F OVy <
Range =
Range
Range ⎬
|δ − δ0 | ≤
α > α0 −
α ≤ α0 +
− 360
, α0 ≥ 360 −
⎪
2
2
2
2
⎪
⎪
F OVy <
Range =
Range
Range ⎪
⎪
⎭
|δ − δ0 | ≤
α ≤ α0 +
α ≥ α0 −
+ 360
,
α0 <
2
2
2
2
|α − α0 | ≤
(10.17)
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 implementation of above theoretical method and software flow. Selection of appropriate
procedural programming language and platform is of great significance for improvement 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 realization 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 parameter 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 simulated 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 subsystem, GNSS subsystem, integrated navigation computer, trajectory generator and
relevant software, etc. Semi-physical simulation system of SINS/CNS/GNSS integrated 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
map according to position coordinates
Dynamic simulation of star
map?
Y
Star display attribute
setting
Reading of next moment of
optical axis data and not null ?
N
End
Fig. 10.14 Flow block diagram of star-map simulator software
N
Y
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 navigation 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
relevant
circuit
Rebalance loop specific
to flexible gyro
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 accelerometers, 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 accelerometer. 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 increment value influenced by various measurement noises and disturbance torques.
During simulation calculation, simulation for such noises and disturbances is generally theoretical value with superposition of certain white noise, markoff process,
random constant value or other irregular colored noise. However, output angle increment 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 before 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 z ( /h)
°
°
°
Gyro output of axis y ( /h)
Gyro output of axis x ( /h)
Time (s)
Time (s)
Time (s)
Accelerometer output of axis z
2
(m/s )
Accelerometer output of axis y
2
(m/s )
Accelerometer output of axis x
2
(m/s )
Time (s)
Time (s)
Time (s)
Fig. 10.20 Original data graph of SINS subsystem output
Time (s)
Time (s)
Time (s)
Accelerometer average output
2
of axis z within 10s (m/s )
Accelerometer average outpu
t of axis y within 10s (m/s2)
Accelerometer average output
of axis x within 10s (m/s2)
Time (s)
Gyro average output of
°
axis z within 10s ( /h)
Gyro average output of
°
axis y within 10s ( /h)
Gyro average output of
Axisx within 10s (°/h)
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 terminal 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
Mean square error (1σ)
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 )
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
simulator
Star sensor simulator
Graph acquisition card
Sensor sensing head
Star map simulation
terminal
LCLV
Star map preprocessor and
identification terminal
Fig. 10.22 Composition of CNS subsystem
equipped with fundamental star catalogue and has function of map generation. According 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 optical 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 processing 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 highlevel 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, therefore 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 accuracy 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
Second of angle
Azimuth angle error
t (s)
Roll angle error
t (s)
Second of angle
Second of angle
Pitching angle error
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 subsystems 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 desynchronized observed quantity problem; this needs synchronized treatment of various
subsystems. When the optimal filtering technique is used to conduct integrated navigation 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
synchron
ization
Signal
demodul
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, interpolation 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 process 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 interpolation 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. . .
Y directional velocity
t (s)
X directional locatiion
t (s)
Z directional locatio
on
t (s)
Y directional locatio
on
t (s)
Meter
Meter
Meter
Z directional velocity
m/s
m/s
m/s
X directional velocity
357
t (s)
t (s)
Fig. 10.25 GPS output data
Table 10.2 GPS data output results
Mean value
Mean square
deviation
Velocity (m/s)
Location (m)
X
− 0.0043
− 2171822.629
Y
0.01081
4385565.634
Z
0.0263
4076644.415
X
0.06
1.45
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
Celestial subsystem
GPS subsystem
PC104 integrated
navigation
computer
SINS subsystem
Fig. 10.26 Physical map of semi-physical simulation system of SINS/CNS/GNS integrated
navigation
SINS
SINS
GPS
CNS
SINS
Valid?
YES
NO
CNS
Valid?
YES
NO
GPS
Valid?
YES
SINS/CNS
CNS
Valid?
NO
Stop
YES
SINS/CNS/GPS
NO
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. . .
Latitude (e)
Position curve
359
Latitude (e)
Longitude (e)
43
116.8
43
42
116.6
42
41
116.4
41
40
116.2
40
39
116
116.2
116.4
116.6
116.8
116
Velocity of east Longitude (e)
direction (m/s)
80
0
500
1000
Velocity of north
direction (m/s)
39
1500
300
500
Vertical velocity
(m/s)
1000
1500
Time (second)
0.015
0.01
60
250
0.005
40
0
200
20
0
0
Time (second)
-0.005
0
500
1000
Azimuth angle (e)
1500
150
Time (second)
0
500
1000
Pitching angle (e)
1500
-0.01
0
Time (second)
500
1000
1500
500
1000
1500
Roll angle (e)
Time (second)
-4
360
4
x 10
0.4
0.3
2
355
0.2
0
350
345
0
0.1
-2
500
1000
1500
Time (second)
-4
0
0
500
1000
1500
Time (second)
-0.1
0
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 offset. 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
(mile)
6
4
4
2
2
0
0
-2
-2
-4
-4
0
500
Velocity error of north
direction (m/s)
1000
1500
0.5
0
-0.5
0
500
1000
1500
0.01
500
Pitching angle error
(second of angle)
1000
1500
Time (second)
-0.01
0
1000
1500
-10
-0.005
0
500
0
0
-0.5
0
Azimuth angle error Time (second)
(second of angle)
10
0.005
0
-1
20
0.015
0.5
500
Roll angle error
(second of angle)
1000
1500
-20
0
Time (second)
500
1000
1500
Time (second)
40
10
5
20
0
0
-5
-20
-10
-15
-6
1
Vertical velocity error (m/s)Time (second)
Time (second)
1
-1
Velocity error of east direction
(m/s)
Latitude error (mile)
0
500
1000
1500
Time (second)
-40
0
500
1000
1500
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 semiphysical 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 highperformance integrated navigation technology of carrier inside the laboratory. Its
design takes true error characteristics of actual environment into account; it can generate flight path of various flying machines conveniently through trajectory generator
and has strong university. Theoretical researches and simulation tests such as modeling, 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
10.4
361
Chapter Conclusion
Semi-physical simulation technology is an effective method to achieve
SINS/CNS/GNSS integrated navigation method and function verification. This chapter 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 semiphysical 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 simulation 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 navigation 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 integrated 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 becomes demanding; therefore, a single means of navigation has been unable to meet
the needs of engineering applications. The integrated technology of the inertial navigation system (INS), the celestial navigation system (CNS), and the global navigation
satellite system (GNSS) has become an important development direction of navigation 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 carried out productive research on advanced navigation theory and methods. Recently,
there has been some new development in inertial/celestial/satellite navigation technology. 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
W. Quan et al., INS/CNS/GNSS Integrated Navigation Technology,
DOI 10.1007/978-3-662-45159-5_11
363
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 navigation 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. Meanwhile, 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 eventually, 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, artificial intelligence technology, and information fusion techniques will become an
integral part of the integrated navigation system [3–5]. A high-performance inertial/celestial/satellite navigation system calls for the consideration of many practical
factors of multisensor data fusion, including the type and distribution form of navigation 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 measurement 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 important 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 advanced 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 Inertial/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 mathematical 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 measurement 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 distribution system estimation and filtering theory is focused on nonlinear non-Gaussian
systems, which broadens the traditional areas of research for filtering stochastic estimation theory. In traditional feedback estimation theory, the feedback is the output
value while in stochastic distribution system estimation and filter theory, the measurement 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 number 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 reliability 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 operating 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 experience 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 interaction between the two species in the course of evolution. Compared with general
genetic algorithm, coevolutionary algorithm can have a reasonable population partition 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 technology 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 metaloxide 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 miniaturization 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 Determination 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 highperformance 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 accuracy, 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 spacecraft. 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 information warfare. High-performance inertial/celestial/satellite integrated navigation
technology, as one of the key technologies, can provide high-precision motion information 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 electrostatically 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) Proceedings 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 estimation 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 algorithms. 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 algorithms: 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