Jun 22, 2018 - AUTHORS DECLARATION. We take full responsibility of the research work conducted during the FYP thesis titled. âDevelopment of Low Cost ...
DEVELOPMENT OF LOW COST MEMS BASEDINS/GNSS INTEGRATED NAVIGATION SYSTEM
Prepared by:
Supervised By:
Fatima Rasool
Dr. Umar Iqbal Bhatti
140101041
Dr. Najam Abbas Naqvi
Amna Ahmed
Dr. Muhammad Ushaq
140101080 Merium Fazal Abbasi 140101036
Submission Date: June 22, 2018
1
A report submitted to the Institute of Space Technology In partial fulfillment of the requirements For the degree of Bachelor of Engineering in Aerospace Engineering
by Merium Fazal Abbasi Fatima Rasool Amna Ahmed
Supervisor Dr. Umar Iqbal Bhatti Dr. Muhammad Ushaq Dr. Najam Abbas Naqvi
Department of Aeronautics and Astronautics Institute of Space Technology, Islamabad June, 2018
I
Institute of Space Technology Department of Aeronautics and Astronautics
APPROVAL PAGE
DEVELOPMENT OF LOW COST MEMS BASEDINS/GNSS INTEGRATED NAVIGATION SYSTEM
by Merium Fazal Abbasi Amna Ahmed Fatima Rasool
APPROVAL BY BOARD OF EXAMINERS
_________________________ Dr. Umar Iqbal Bhatti
II
AUTHORS DECLARATION We take full responsibility of the research work conducted during the FYP thesis titled “Development of Low Cost MEMS Based INS/GNSS Integrated Navigation System". We solemnly declare that the research and development work presented in the FYP is done solely by us with no significant help from any other person: however, small help wherever taken is duly acknowledged. We have also written the complete thesis ourselves. Moreover, we have not presented this thesis or any part of this thesis previously to any other degree awarding institution within Pakistan or abroad. We understand that the management of IST has zero tolerance policy towards plagiarism. Therefore, we as the authors for the above-mentioned thesis solemnly declare that no portion of our thesis has been plagiarized and any material used in the Thesis from other sources is properly referenced. Moreover, the thesis does not contain any literal citing (verbatim) of more than 70 words (total) even by giving a reference unless. We have obtained the written permission of the publisher to do so. Furthermore, the work presented in the thesis is our own original work and we have positively cited the related work of the other researchers by clearly differentiating our work from their relevant work. We further understand that if we are found guilty of any form of plagiarism in our thesis work even after our graduation the Institute reserves the right to revoke our BS degree. Moreover, the Institute will also have the right to publish our names on its website that keeps the record of the students who plagiarized their thesis work.
III
__________________________ Merium Fazal Abbasi 140101036 ___________________________ Fatima Rasool 140101041 ___________________________ Amna Ahmed 140101080 I hereby acknowledge that the submitted thesis is the final version and should be scrutinized for plagiarism as per IST policy.
________________________ Dr. Umar Iqbal Bhatti
Dated: __________________
________________________ Verified by Plagiarism Cell Officer Dated: __________________
IV
Copyright © 2018 This document is jointly copyrighted by the author(s) and the Institute of Space Technology (IST). Both the author(s) and IST can use, publish or reproduce this document in any form. Under the copyright law no part of this document can be reproduced by anyone, except copyright holders, without the permission of the author(s).
V
ABSTRACT Satellite Navigation is one of the most common type of navigation that is used extensively during these days. This type of navigation suffers from outages due to signal blockage, jamming, multipath and interference. In critical applications, satellite navigation alone cannot serve the purpose. In modern days, for crucial applications, MEMS based inertial navigation is integrated with satellite navigation to obtain the optimal navigation solution, as the two techniques have complementary properties. Although, MEMS based inertial navigation has advantage of low cost, small size, light weight but it has poor accuracy. Thus, sophisticated algorithms are developed to improve its accuracy. In this project we will develop algorithm for integration of Satellite and inertial navigation through Kalman filter.
VI
PROJECT OBJECTIVE STATEMENT Our final year project focus is on providing better navigation solution by using Kalman filter. The problem is that the present navigation system is exposed to various errors. Occasionally, inertial navigation system and Global Navigation System are used in standalone configuration in the industry and both have complimentary errors. What our project will do is that, it will provide a solution that eliminates these errors to deliver improved navigation solution.
VII
PROJECT SUMMARY In this project, INS and GNSS are integrated to overcome the problems of both types of navigation. For this purpose, the data of accelerometer, gyroscope and GNSS receiver is required. The data of accelerometer and gyroscopes is processed by MATLAB code, which give complete navigation solution. This INS solution is corrected by integrating it with GNSS data using extended close loop Kalman filter. Moreover, in this project, calibration of magnetometer is also performed by using a new and automatic technique to improve heading.
VIII
ACKNOWLEDGEMENTS
We would like to pay our gratitude to our supervisor, Dr. Umar Iqbal Bhatti, for his guidance, support and motivation. It was he, who was always ready to help us, who patiently answered all our queries and who guided us from the understanding of basic concepts of the subject to implementation of most sophisticated algorithms, while working with the hardware. We are also thankful to our external supervisor Dr. Muhammad Ushaq for his support and guidance. We are grateful to Dr. Najam Abbas Naqvi for supervising and guiding us. We sincerely thank faculty of Department of aeronautics and astronautics, especially, Mr. Asim Shahzad for his support and motivation. We are thankful to Mr. Shoaib Mansoor, Mr. Imran Kamal, Ms. Freha Imran, Mr. Amir Malik for their help in hardware as well as in the development of algorithms. Finally, we would like to pay our special gratitude to our families, for their support and cooperation.
IX
DEDICATIONS We would like to dedicate this report to Dr. Umar Iqbal Bhatti.
X
Table of Contents Approval Page
II
AUTHORS DECLARATION 1.
2.
III
INTRODUCTION
1
1.1
Background
1
1.2
Previous Works
3
1.3
Our Work
4
1.4
Aims and Objectives
6
1.5
Thesis Outline
6
LITERATURE REVIEW 2.1
7
GNSS
7
2.1.1
GNSS Architecture
7
2.1.2
GNSS Constellations
9
2.1.3
GNSS Error Resources
18
2.1.4
Applications of GNSS
21
2.2
INS Overview
23
2.2.1
History
23
2.2.2
INS
26
2.2.3
Strapdown INS
27
2.2.4
Principle
28
2.2.5
Reference Frames
29
2.2.6
Axis Transformations
31
1. 2.3
Direction Cosine
31
Inertial Measurement Units
33
2.3.1
Magnetometer
33
2.3.2
Gyroscope
49
2.3.3
Accelerometers
52
Overview of Integration
56
2.4
2.4.1
Integration
56
2.4.2
Kalman Filter
57 XI
2.4.3 3.
HARDWARE IMPLEMENTATION 3.3
4.
64 67
RTK Emlid Reach Recievers
71
3.3.1
RTK Technique
72
3.3.2
Hardware Integration
72
3.3.3
Updating
73
3.3.4
Settings
74
3.3.5
Output
76
3.3.6
Post-Processing
77
3.3.7
Results
78
GNSS ALGORITHM 4.1
85
RINEX:
85
4.1.1
Why we are using RINEX:
85
4.1.2
RINEX Observation File:
85
4.1.3
RINEX Navigation File:
88
4.1.4
RINEX files Names:
89
DATA Acquisition Algorithm:
90
4.2
4.2.1
Observation File:
90
4.2.2
Navigation File:
90
4.3
5.
Integration of INS and GNSS by Kalman Filter
Position Estimation
90
4.3.1
Coordinate Systems
90
4.3.2
Satellite Orbit Computation
91
4.3.3
Least Square Method for position Estimation
93
4.3.4
Flow Chart of GNSS Algorithm
98
4.3.5
Results:
98
INS ALGORITHM
102
5.1
Inertial Navigation System Algorithm:
102
5.2
Algorithm
102
5.2.1 Transformation from Earth Centered Inertial Frame (ECIF) to North East Up (ENU) Frame: 103 5.2.2
Position Matrix Update
104 XII
5.2.3
Earth Spin Rate in ENU Frame
104
5.2.4
Transport Rate
105
Position Update from Updated Velocity Components
5.2.5
5.2.6
Gravity Vector Update
106
5.2.7
Velocity Computation
106
5.2.8
Transformation of Navigation frame (geographic) to Strapdown Frame: 107
5.2.9
Updating Attitude Direction Cosine Matrix (DCM) Cgb
107
5.2.10
Computation of ωgbb .
108
5.2.11
Updating Attitude DCM Cgb Using Quaternion
108
5.2.12
Updating Quaternion
110
5.2.13
Attitude Computation
110
5.3
6.
105
Magnetometer Algorithm
112
5.3.1
Algorithm
112
5.3.2
Inputs
113
5.3.3
Process
114
5.3.4
Check Routines
117
INTEGRATION ALGORITHM
119
6.1
Algorithm
119
6.2
Calculation of Matrices
120
RESULTS AND DISCUSSION
123
7.
7.1
INS and GNSS integration by Kalman filter
123
7.2
Magnetometer calibration
131
8.
CONCLUSION
133
9.
REFERENCES
134
10.
APPENDIX
138
10.1
Codes
138
10.1.1
GNSS code
138
10.1.2
INS code
149
10.1.3
KALMAN FILTER CODE
158
10.1.4
MAGNETOMETER CALIBRATION
172
XIII
LIST OF FIGURES Fig. 1.1 Work Flow Fig. 2.1 GNSS Architecture Fig. 2.2 Antipodal Satellites Fig. 2.3 GNSS Errors Fig. 2.4 Gimbal INS [8] Fig. 2.5 Strap Down INS [9] Fig. 2.6 Basic mechanism of INS Fig. 2.7 Reference Frames Fig. 2.8 Magnetic sensing [12] Fig. 2.9 fluxgate magnetometers [12] Fig. 2.10 Magneto resistive effect [12] Fig. 2.11 Magneto resistors magnetometer [12] Fig. 2.12 Giant magneto resistive devices [12] Fig. 2.13 Earth's magnetic field measurement [15] Fig. 2.14 Levelling of magnetometer readings [16] Fig. 2.15 Heading determination using magnetic sensor [16] Fig. 2.16 Ideal circle and hard iron error [16] Fig. 2.17 Soft iron errors [16] Fig. 2.18 Compass swinging Fig. 2.19 MEMS Gyros [21] Fig. 2.20 Potentiometric Accelerometer Fig. 2.21 Capacitive Accelerometer Fig. 2.22 Piezo-resistive Accelerometer Fig. 2.23 Working of Kalman filter Fig. 2.24 Process of Kalman filter Fig. 2.25 Uncoupled integration [28] Fig. 2.26 loosely coupled integration [28] Fig. 2.27 Tightly coupled integration [28] Fig. 2.28 Ultra-tightly coupled integration [28] Fig. 3.1 Trajectory Generator Software Interface Fig. 3.2 Sensors error Interface Fig. 3.3 Generate trajectory Interface Fig. 3.4 Hardware integration of RTK reach receivers Fig. 3.5 Updating of RTK Reach receivers Fig. 3.6 Updated Fig. 3.7 Settings of RTK reach receiver Fig. 3.8 Setting of base Fig. 3.9 Coordinates of base Fig. 3.10 Setting of Rover XIV
5 8 15 21 27 28 29 30 34 37 38 39 39 40 43 43 45 46 48 51 54 55 55 61 62 65 65 66 66 68 70 71 73 74 74 75 75 76 76
Fig. 3.11 Real time solution of RTK reach receivers Fig. 3.12 Results of RTK Reach Fig. 3.13 Variation of latitude with time Fig. 3.14 Variation of longitude with time Fig. 3.15 Variation of altitude with time Fig. 3.16 Trajectory through Post processing Fig. 3.17 Position variation Fig. 3.18 Velocity Variation Fig. 3.19 Acceleration Variation Fig. 3.20 Showing number of satellite Fig. 3.21 Variation of Velocity with time Fig. 3.22 Variation of Position with time Fig. 4.1 Header Section of observation file Fig. 4.2 Data Section of observation file Fig. 4.3 Header Section of Navigation file Fig. 4.4 Data Section of Navigation file Fig. 4.5 Flow Chart of GNSS Fig. 4.6 Section of RINEX File Fig. 4.7 Variation in Coordinates using P1 observable Fig. 4.8 Plot of longitude verses latitude data from P1 Fig. 4.9 Variation in Coordinates using P2 observable Fig. 4.10 Plot of longitude verses latitude data from P2 Fig. 5.1 Scheme of Algorithm of INS [1] Fig. 5.2 Transformation in ENU Scheme [12] Fig. 5.3 Structure of Algorithm of Magnetometer [35] Fig. 6.1 INS/GNSS integration Fig. 7.1 Position Fig. 7.2 Velocity Fig. 7.3 Attitude Fig. 7.4 Trajectory Fig. 7.5 INS Position Error Fig. 7.6 INS Velocity error Fig. 7.7 INS Attitude Error Fig. 7.8 GNSS Position Error Fig. 7.9 GNSS Velocity Error Fig. 7.10 Combined Calibrated un-calibrated heading Fig. 7.11 Calibrated Heading
XV
77 78 79 79 80 81 81 82 82 83 84 84 86 87 89 89 98 99 99 100 100 101 102 103 113 119 123 124 125 126 127 128 129 130 130 131 132
LIST OF TABLES Table. 1.1 Properties of INS and GNSS [1]........................................................................3 Table. 2.1 Space Segment of GPS.....................................................................................12 Table. 2.2 Space Segment of GLONASS.........................................................................14 Table. 2.3 Signal of GLONASS.........................................................................................14 Table. 2.4 Space Segment of GALILEO..........................................................................17 Table. 4.1 Parameters of World Geodetic System [33]......................................................93
XVI
List of Abbreviations and symbols INS
Inertial Navigation System
IMU
Inertial Measurement Unit
GNSS
Global Navigation Satellite System
MEMS
Micro-Electro-Mechanical System
GPS
Global Positioning System
GLONASS
Global Navigation Satellite System
RTK
Real Time Kinematics
KF
Kalman Filter
EKF
Extended Kalman Filter
1. INTRODUCTION This chapter provides brief overview of the Project. It comprises of background, significance, aims and objectives, and workflow.
1.1 Background To estimate the position, velocity and attitude of a moving object in real time with respect to some reference is called Navigation. it is very ancient art. Its history started with the history of travel of mankind. Initially the term was coined for marine navigation, but with the passage of time it has been generalized and now it encompasses all imaginable forms
XVII
of transportations as well as other applications like location-based services, law and enforcement, intelligence gathering, fleet management and search and rescue. There are two basic methods of navigation: 1. Dead Reckoning: determining position by knowing initial position and keeping record of direction and velocity. 2. Position fixing: finding location using external source of known location. Many different types of navigation have been used. Some of the common types are: 1. Landmark navigation: it is based upon observation of some distinct features of environment which may be artificial or natural 2. Celestial navigation: it depends upon the sight or angle measurement between horizon and celestial bodies. 3. Inertial Navigation: it uses inertial sensors like accelerometers and gyroscopes. 4. Satellite Navigation: it is based upon the signals transmitted by satellite at known location in space. All of these types of navigation have passed through different stages of development in history. Each type has some advantages as well as some drawbacks as compare to the other. Nowadays, the most common types of navigation that is being used is Global satellite navigation system (GNSS). It has wide range of applications and it give accuracy that ranges from centimeter to meter level depending upon the method being used. It is inexpensive and gives very good long-term accuracy. Along with these advantages it also has some serious drawback associated with it i.e., it requires clear line of sight between satellite and receiver for position determination. This requirement cannot meet in urban areas crowded with buildings and in mountainous regions, where problems of multipath, interference and signal blockage are experienced and reliability of GNSS is significantly XVIII
reduced. Although some methods like differential techniques and carrier phase observable are being used to overcome this problem, but it has not been eliminated yet. So, in critical applications where GNSS outages are not tolerate able, it is integrated with other types of navigation. Most commonly GNSS is integrated with inertial navigation system (INS), as the two types have complementary properties.
Table. 1.1 Properties of INS and GNSS [ CITATION Abo13 \l 1033 ] Characteristics Accuracy
Inertial Navigation It has good short-term accuracy. It has high output data rate.
Satellite Navigation It has good long-term accuracy. Output data rate It has slow output data rate. Requirement of external It does not require external It requires external source. source source. Attitude information It gives attitude It usually does not give information. attitude information. Initial conditions It requires initial It does not require initial conditions. conditions. So, to get improved navigation solution, INS and GNSS are integrated together With the advancement of Micro-Electro-Mechanical-System (MEMS), inertial sensors have been introduced which have small size, light weight and cheap prices but they have degraded performance, so their use in integrated navigation is a research problem.
XIX
In this project, we will use MEMS inertial sensors along with the differential GNSS receivers to get integrated navigation solution.
1.2 Previous Works A lot of research has been conducted on integration of INS and GNSS e.g. Angrisano (2010) integrated MEMS INS and GNSS (consisting of only GPS and GLONASS) while working with GPS as single point mode. He used least square as well as Kalman filter for integration. Autor (2015) also developed INS and GNSS simulator and then he integrated the navigation solution from two sources using Kalman filter. Integration of MEMS INS and GNSS (using low cost GNSS receivers) has also been performed through tightly coupled as well as loosely Kalman filter by Muhammad Shafique (2014) in urban areas while collecting the data on land vehicle. Research of Yueming Zhao (2011) is also focused on integration of INS and GPS for land vehicle navigation. He performs integration through Kalman filter using all three methods, lose coupling, tight coupling and ultra-tight coupling. Viktor Elisson’s (2014) research work is aimed at minimizing the cost of INS and GNSS integrated Navigation system for ground vehicle applications. Loosely coupled INS filter was developed by Eduardo Infante (2016) for INS and GNSS integration along with the aid of magnetometer for smartphone application.
XX
Zhouzheng Gao et al (2015) integrated INS with raw GPS data through ionospheric constrained (IC), precise point positioning (PPP) technique. Research has also been conducted by Gianluca Falco (2017) for assessment of INS and GNSS integrated solution through loose and tight coupling.
1.3 Our Work In this Project, RTK reach receivers, IMU and trajectory generator software were used for data collection. We got output of accelerometer and gyroscope as well as position and velocity of GNSS. Output of accelerometer and gyroscope is processed by MATLAB code to give INS position, velocity and attitude. The difference between position and velocity from two sources, is fed to Kalman filter, to correct INS position, velocity and attitude.
XXI
Fig. 1.1 Work Flow Moreover, a novel technique is employed for magnetometer calibration to give improved heading.
1.4 Aims and Objectives The purpose of this project is to analyze the performance of: 1. Standalone INS 2. Standalone GNSS 3. Results of magnetometer calibration XXII
4. INS and GNSS integration by Kalman filter
1.5 Thesis Outline This thesis comprises of 8 chapters. Chapter 1 gives the brief introduction and background. It also describes significance, objective and workflow of the research. Chapter 2 gives the detailed literature review of INS, GNSS and integration techniques for INS and GNSS. Chapter 3 is about data collection. Chapter 4 deals with the development of Algorithms for GNSS, its implementation and analysis of its results. Chapter 5 comprises of Algorithm of INS and magnetometer calibration, its implementation and discussion. Chapter 6 is about development of Algorithm of Kalman filter for integration of INS and GNSS. Chapter 7 consist of results and discussions. Whereas, chapter 8 will conclude the thesis.
2. LITERATURE REVIEW 2.1 GNSS The GNSS stands for Global Navigation Satellite System consists of diverse constellations of satellites. The NAVSTAR GPS is the first satellite navigation system, consisting of constellation of 30+ satellites. With the passage of time, the other systems
XXIII
were invented including Russian Global Navigation System (GLONASS), Chinese BEIDOU and European Union Galileo, Indian Regional Navigation Satellite System (IRNSS) and Quasi-Zenith Satellite System (QZSS).
The American constellation
NAVSTAR GPS and the Russian constellation GLONASS (Russia) are operational whereas BEIDOU by china and GALILEO by Europe are the constellations in developing phase. Initially, the GNSS receivers were used for military purposes but with modernization the receivers have found applications in Air navigation, Automobile, Mobile phones, Boats and Ships. Moreover, in the recent years the GNSS receivers are using in spacecraft’s. The Global Navigation Satellite System is a most widely used Navigation system. GNSS is a collection of positional satellites which are operational or planned. These satellites broadcast signals which allow determination of absolute position using receivers. 2.1.1
GNSS Architecture
The basic architecture of GNSS system includes three segments: 1. Space segment 2. Control segment 3. User segment 2.1.1.1 Space segment Space segment is the constellation of satellites; orbit the earth in different planes that are inclined at various angles. Each orbit have different orbital period. Space segment’s satellites broadcast positioning and timing information. These satellites use medium earth orbit for orbiting the earth[ CITATION Abo13 \l 1033 ].
XXIV
2.1.1.2 Control segment Control segment consists of master station, monitoring stations and the ground antennae. The master station controls monitoring stations and have the responsibility of maintenance of satellite’s orbital parameters and clock biases. The monitoring stations are located at various geographical locations; these stations observe the clock integrity and satellite signals and guide the information to master stations[ CITATION Abo13 \l 1033 ]. 2.1.1.3 User segment User segment consists of GNSS receivers which receives the signals transmitted by the satellites in the constellation and process the signals for absolute positioning. These GNSS receivers are ranged from handheld receivers to specialized receivers[ CITATION Abo13 \l 1033 ].
Fig. 2.2 GNSS Architecture
2.1.2
GNSS Constellations
Among different GNSS satellite systems there are four main systems: 1. Global positioning System 2. Global Navigation Satellite System XXV
These two systems are currently developed and operational. 3. Galileo 4. BeiDou Navigation Satellite System These systems are in developing phase not operational but are fully planned. 2.1.2.1 Global Positioning System (GPS) 2.1.2.1.1 Introduction: GPS is Global Positioning System developed in 1970’s by US Department of Defence (DoD). The idea was to establish the satellite base navigation system which determines the position in three dimensions. In 1960’s it was decided that the optimum positioning system should have the high accuracy, global coverage, ability to serve high dynamic platform and should operate in all weather conditions[ CITATION Kap \l 1033 ]. GPS consists of 24-staellites arranged in 6-planes i-e 4 satellites in each plane[ CITATION Kap \l 1033 ]. The satellites orbit around the earth in circular orbits of radius 26,560km. Each satellite transmits navigation data and pseudo random noise code (PRN). PRN code contains transit time information which is primarily used for calculating the range[ CITATION Abo13 \l 1033 ]. To completely determine the receiver’s three-dimensional location usually four satellites are required; if the receiver clock is synchronized with satellite clock than the complete position can be determined using three satellites because the forth satellite is used to remove receiver’s clock bias[ CITATION Abo13 \l 1033 ].
XXVI
2.1.2.1.2 Observables GPS has three observables; Pseudo-range measurements, Doppler measurements and carrier phase measurements[ CITATION Abo13 \l 1033 ]. Pseudo-range is the distance between satellite and receiver that is measured, using time information from the signal. The navigation massage gives transmission time and replica of PRN code generated by receives gives reception time. Difference between transmission time and reception time are than multiplied with speed of light to get pseudo-range[ CITATION Abo13 \l 1033 ]. Range can also be calculated using carrier phase. Initially GPS receiver calculates the fractional phase of a cycle but cannot calculate the total number of complete cycle; once this measurement is done than range can be calculated by multiplying signals’ wavelength to total cycles (complete cycles plus fractional part). The range calculated from carrier phase is more accurate than that measured from pseudo-random code[ CITATION Abo13 \l 1033 ]. Another important observable is Doppler measurements. Doppler Effect is the outcome of relative motion of receiver and satellite. Doppler Effect is basically the frequency shift of signal transmitted by the satellites which gives the user velocity[ CITATION Abo13 \l 1033 ]. 2.1.2.1.3 Architecture The complete GPS structure includes space segment, control segment and the user segment.
XXVII
Space segment is the constellation of 31 satellites; orbit the earth in 6 planes inclined at 55º. The orbits have orbital period of 12hours[ CITATION Abo13 \l 1033 ]. Control segment monitors the health states of satellites and is responsible for the maintenance of the system. Control segment consists of master station, monitoring stations and the ground antennae. The master station (near Colorado springs) controls monitoring stations, the monitoring stations observe the clock integrity and satellite orbits and send the information to master stations. The master station than estimate the clock parameters and the ephemeris. The ground antennas at four monitoring stations receive data from satellites through s-based; it is also linked with Master Control Station (MCS) which enables MCS to upload commands for navigation massage[ CITATION Abo13 \l 1033 ]. User segment consists of devices known as GPS receiver not GPS itself. The receiver receives radio signals from GPS satellites and calculates user position and velocity. The typical receiver composed of antenna, data record unit, display device, the amplifiers and the power supply. The antenna receives the signals from satellites and the receivers determine the location using the data encoded in the signals. For complete deduction of location signals from at least 4 different satellites are required[ CITATION Abo13 \l 1033 ]. Table. 2.2 Space Segment of GPS Parameters No of satellites Orbital planes Orbital inclination
Values 31 6 55 degrees XXVIII
Orbit radius Orbital period
26560 km 12 hours
2.1.2.1.4 Signal Characteristics More precisely the navigation massage is in terms of binary data which contain Pseudo random code, ephemeris, almanac data, signal timing data and satellite health massage. Pseudo random code gave identification of satellite transmitting the signals. Broadcast ephemeris describes the clock correction and orbits parameters. Ephemeris description varies overtime and each satellite transmits unique ephemeris data. Processing of this data compute the coordinates of satellites, which is then used to compute receiver’s coordinate. Satellite Almanac data is similar to ephemeris data but this data is not much accurate to be used for determining the location of satellite rather it is used for estimating the satellites that would be visible at given location[ CITATION Moh07 \l 1033 ]. Signal timing data provides propagation time that is used for range calculations. The signals also contain information of satellite health which alerts the receiver, whether to use this satellite data for navigation or to ignore it[ CITATION Moh07 \l 1033 ]. 2.1.2.1.5 Services GPS provides two services: 1. Standard Positioning service: It is open service available to all the worldwide consumers free of cost. 2. Precise Positioning service: it is provided to military and authorized users. XXIX
2.1.2.1.6 Current status GLONASS is operational with 31 satellites to provide navigation services worldwide. 2.1.2.2 GLONASS 2.1.2.2.1 Introduction GLONASS is Global Navigation Satellite System developed by Soviet Union in 1982. With the dissolution of Soviet Union that project was continued by the federation of Russia and it became operational with 24 satellites in 1995. After its completion, the system falls into decay due to fall of Russian economy and lack of funding. In 2000, Russian government decided to restore the system by 2011 with cooperation of India. The purpose of GLONASS is to provide more opportunities to GNSS application developers give free asses to civilian consumers without any limitations and free of cost. 2.1.2.2.2 Architecture It’s control system consist of system control and command tracking solutions throughout the Russia, which monitor the status of satellites, find satellite and clock offset and upload navigation data to the satellite twice a day. The space segment consists of constellation of satellites with following characteristics: Table. 2.3 Space Segment of GLONASS Parameters No of satellites Orbital planes Orbital inclination Orbit radius Orbital period
Values 21 + 3 spares 3 64.8 degrees 19140 km 8/17 of sidereal day XXX
2.1.2.2.3 Signal characteristics GLONASS signal consists of information of location of satellite, time and almanac. The radio signals. Table. 2.4 Signal of GLONASS Designation L1
Frequency 1598.0625-1609.3125
Description Modulated by High precision (HP) and
L2
MHz 1242.9375-1251.6875
standard precision (SP) signals. Modulated by HP and SP. SP is same as in
MHz
the case of L1.
GLONASS transmits P-code on L1 frequency and C/A code on all L1 as well as on L2 frequency. It transmits same code at different frequencies by using the technique known as frequency division multiple asses (FDMA). The system operates by having 24 satellites using 12 frequencies. It is achieved by using the concept of antipodal satellites, which are the satellites located in same plane and separated by 180 degrees. As antipodal satellites can never appear on the same view, they can use the same frequency 2.1.2.2.4 Services The GLONASS system provides two services: 3. Standard Positioning service: It is open service available to all the worldwide consumers free of cost. 4. Precise Positioning service: it is provided to military and authorized users.
XXXI
2.1.2.2.5 Current status GLONASS is operational with 24 satellites to provide navigation services worldwide, covering 100% of the globe.
Fig. 2.3 Antipodal Satellites
2.1.2.3 GALILEO Navigation System 2.1.2.3.1 Introduction The GALIEO is satellite navigation system, named after Italian scientist Galileo that is being developed by European Union. It is funded by Germany, France, Great Britain and Italy. The purpose of the project is to provide the navigation services down to the meter level and to make European nations independent in the field of satellite navigation.
XXXII
2.1.2.3.2 Architecture The control segment of Galileo comprises of 2 Ground control centers (GCC), 5telemetry, tracking and control (TTC) stations, several mission data uplink stations (ULS) and some Galileo sensor stations (GSS). Data recovered by GSS will be sent to the GCC which will synchronize satellite clock with ground station clocks and monitor integrity of satellites. The control centers will communicate to the satellites through ULS. The space segment of Galileo has following characteristics: Table. 2.5 Space Segment of GALILEO Parameters No of satellites Orbital planes Orbital inclination Orbit radius Orbital period
Values 27 + 3 spares 3 56 degrees 23,616 km 10 days
Galileo’s ground segment consists of receivers to receive the signals from satellite and to compute their position. 2.1.2.3.3 Signal Characteristics The Galileo signals have selected four frequency bands named as E5a, E5b, E6 and E1. These bands are placed in the allocated spectrum for Aeronautical Radio Navigation Services (ARNS). 2.1.2.3.4 Services Galileo provides following services:
XXXIII
1. Open services: This service is provided free of cost using E1 and E5 signals. 2. Commercial services: It provides accuracy down to centimeter range, so charges will be applied on users. It broadcast encrypted signals on E1, E5 and E6 signals. 3. Safety of Life services: It is a precise service that is used for critical applications like aircraft landing. It will use E1 and E5 signals. 4. Public regulated services: its signal modulation and encryption provide the capabilities of anti-jamming and anti-spoofing. It will be used by government agencies. 5. Search and Rescue: it will make it possible to locate the people and vehicles in distress. 2.1.2.3.5 Current Status It is in development phase yet and is expected to become operational till 2020. 2.1.3
GNSS Error Resources
GNSS receiver uses the principle of trilateration for calculating the range using four satellites. The range measured from this is plagued with errors from various sources. To get accurate measurements these errors must be catered in calculations. More typical errors encountered by GPS receiver includes; satellite clock error, receiver clock error, ionospheric delay, tropospheric delay, multipath errors, satellite orbital errors and receiver noise error[ CITATION Abo13 \l 1033 ].
XXXIV
2.1.3.1 Satellite Clock Error: Satellite clock error referred to drift in satellite clock from GNSS system time. The control segment monitors the clock bias, formulate the error and send the correction formula to the user. The user uses these correction parameters to correct transit time; before estimating transmission time that is used for range calculation. Use of inexpensive clocks in the receivers for affordability raises receiver clock bias. Receiver clock error effects all the range measurements in similar fashion. The receiver clock bias can be estimated using the pseudo-random measurements from four satellites, using Kalman filtering or alternatively using the difference of two simultaneous range measurements taken from same receivers[ CITATION Kap \l 1033 ]. 2.1.3.2 Receiver Clock Error Receiver clock error is the error of variation in time. It effects the simultaneous range measurements in the same manner. If simultaneous range measurements from four satellites are known, the position and clock bias can easily be determined. The three methods to handle receiver clock bias are: a) Taking difference of two simultaneous range measurements of the same receiver b) Independently determining the clock bias c) Through dynamic model for clock bias 2.1.3.3 Atmospheric delay While traveling through the atmosphere GNSS signals get distorted in various ways which delay the signal and effect the range measurements. The atmospheric delay can be categorize into one of the two types; non-dispersive and dispersive. The non-dispersive XXXV
portion is related with troposphere whereas the dispersive portion is related with ionosphere. Atmosphere is divided into various segments; the lower part of atmosphere is the troposphere which extends from 8 to 40km above the surface of earth and the layer of atmosphere above 50km is referred as ionosphere. The tropospheric segment of atmosphere experience large change of temperature, pressure and humidity which effect the speed of light and thus range measurements. The tropospheric delay can be modelled as dry component and wet component[ CITATION Kap \l 1033 ]. The wet component is accounted for 10% of atmospheric delay and is difficult to model whereas the 90% of tropospheric delay is because of dry component and is better modelled[ CITATION Abo13 \l 1033 ]. Since the tropospheric delay depends upon the local variables (receiver position, satellite-user line of sight etc.) thus the differential techniques for compensating the tropospheric delay will depend upon the user position relative to the base station. Ionosphere consists of ionized gases; the level of ionization varies with solar activities. The variation in ionization affects the refractive indices of layers which effect the travel time of signal. Ionospheric delay is difficult to model one basic model is Klobuchar model which compensate approximately 50% of the delay[ CITATION Kap \l 1033 ]. 2.1.3.4 Receiver Noise The error receiver makes in transit time measurement is referred as receiver noise error. The error is caused by thermal noise and non-linearity of components. Technology incorporated in receiver as well as the elevation of satellite determines the magnitude of error.
XXXVI
2.1.3.5 Multi-path Error The environmental obstacles such as buildings, trees etc. also offer resistance to the GNSS signals. The signal reaches the receiver by different paths. These include reflected signals and direct signals. At receiving antenna the interference of reflected signals distorts the original signal; this introduces error in position and is called as multipath error[ CITATION Abo13 \l 1033 ]. Certain precautions taken by the user reduce the multipath error as; a) Through receiver setting avoid signals from satellites at low elevation b) Place the receiver antenna above certain height to avoid reflection. 2.1.3.6 Selective Availability Selective availability errors are control by government and are artificially add to the satellite signal to degrade the position and velocity precision attained with GNSS receivers. Modeling such error involves a lot of assumptions.
XXXVII
Fig. 2.4 GNSS Errors 2.1.4
Applications of GNSS
2.1.4.1 Consumer Consumer market has greatly adopted the GNSS technology in a range of systems. Smart phones have integrated GNSS receivers for supporting applications that display the location on the map and guide the person on road[ CITATION NOV \l 1033 ]. 2.1.4.2 Precision Agriculture GNSS support applications that use for farm planning, soil sampling, crop assessment, field mapping and tractor guidance. It ensure precise application of pesticides, herbicides and fertilizers that reduces expenses. It enables the operation of farm machinery at higher speed and accuracy without any environmental constrain. Moreover, it reduce the labor required[ CITATION NOV \l 1033 ]. XXXVIII
2.1.4.3 Survey Surveying position of a point on earth surface using precise GNSS technique had reduced the equipment and labor requirements as compare to the previously used techniques, even a single surveyor can accomplish the task[ CITATION NOV \l 1033 ]. 2.1.4.4 Ground Mapping Systems have been industrialized for capturing 360-degree panoramic photographs that use record data from IMU and GNSS. Attitude and position data from the GNSS and IMU is programmed into the system such as in cameras that determines the position of particular object in the system[ CITATION NOV \l 1033 ]. 2.1.4.5 Marine Applications GNSS technique has drastically improved the marine navigation. GNSS is applied to a wide range of applications such as underwater pipeline and cable inspection and installation, oilrig positioning and pipeline, recovery and rescue[ CITATION NOV \l 1033 ]. 2.1.4.6 Aviation In aviation industry the GNSS has brought evolutionary changes, it enables the aircraft to fly thought the most fuel-efficient route. It also enables the aircraft to minimize the carbon emissions and noise. In aviation industry it basically ensures optimized vehicle performance. Hence assists performance-based navigation[ CITATION AnI \l 1033 ]. 2.1.4.7 Unmanned Vehicles Unmanned vehicles are unoccupied but are under control, whether automatically guided by GNSS or radio-controlled. In the present era the market for unmanned vehicle has XXXIX
diversified and its commercial use has grown. The civilian use of these unmanned vehicles includes: aerial photograph, crop monitoring, environmental research, search and rescue, bathymetry, infrastructure inspection and landmine detection[ CITATION NOV \l 1033 ]. 2.1.4.8 Defense In defense sector GNSS has broader applications in navigation, reconnaissance and map creation, search and rescue, munitions guidance and unmanned vehicles[ CITATION NOV \l 1033 ].
2.2 INS Overview 2.2.1
History
In the past times, moving from one place to another, required navigation. Bible first gave the oblique reference of inertial navigation[CITATION 1 \l 1033 ]. Bible reference is used to navigate on land. After that navigation by observation (using landmarks) is used for travelling. Polynesians used position fix techniques after the understanding of landmarks and celestial bodies but these techniques were only suitable for clear weather conditions. In thirteenth century, Chinese fabricated compass after the discovery of properties of lodestone (magnetized piece) but it was not suitable in rough weather. For long distance travel, an instrument sextant was developed. During seventeenth century, Sir Isaac Newton discovered the laws of gravitations and mechanics. So, the fundamentals principles of inertial system were developed. During the era of eighteen centuries, inertial navigation sensors were made. Stabilized sextant was renewed by Serson[ CITATION 2 \l 1033 ]. Harrison demonstrating a chronometer. XL
Further researched on these instruments enable to take reference of celestial objects without using horizon reference and accurate longitude determination. These instruments provided us to navigate accurately using charts and reference table of location of celestial bodies of visible objects. In 1852, Foucault discovered the gyroscopic effect. Bohneberger, Johnson and Lemarle, researched on rotational dynamics of earth. They used the gyroscopic effects to investigate rotational motion of earth. In the end of nineteenth century, many gyroscopic instruments were produced. Professor G.H. Bryan, described about the Solid state gyroscopes in 1890. Gyrocompass basic principle is to identify the true north. It establishes the equilibrium between gyrocompass’s effect and the base angular momentum that caries the compass. A device with vertical erection system was made by Professor Max Schuler [ CITATION SCH24 \l 1033 ]that defines an accurate reference system and its was tuned to undamped natural using the radius and acceleration of earth. Later, this technique was named as ‘Schuler tuning’[ CITATION WRI69 \l 1033 ] by Dr Walter Wrigley of MIT. This instrument became successful for directional measurements but it was insensitive to acceleration measurement at sea level. Elmer, Lawrence Sperry, Brown and Perry improved and refined the design of gyrocompass. The major significance of these instruments was the navigation in all weather conditions. In early twentieth century, The Sperry Brothers applied the gyroscopic effect to develop the autopilot equipment and gyroscopes for use in aircrafts and torpedoes. In 1920s directional gyroscopes, artificial horizons and directional gyroscopes were made. Side slip sensors and open loop accelerometers were being produced. Schuler
XLI
made a north seeking device for land that had an accuracy of 22 seconds of arc. These were the revolutionary developments in the beginning of the 20 th century. Fire control system of guns for stable planforms was also developed using these sensors. Boykow produced the Full inertial navigation system by describing the combined use of gyroscopes and accelerometers but the quality of sensors was not enough for the demonstration of INS. During World War II, German scientists produced the inertial guidance with a feedback system for accurate guidance. These systems were used in the Vl and V2 rockets. During this time, new type of inertial sensors had been produced with more accuracy. In 1949, concept of Strapdown inertial navigation had taken into account. In 1950s, many accurate sensors with more accuracy had been made with a reduction in error about 15°/hour to about 0.01 °/hour. Professor Charles Stark Draper and his coworker took a forward step to demonstrate the floated rate integrating gyroscopes[ CITATION 3 \l 1033 ].Accurate accelerometer were produced by using the principle of force feedback. Unites States of America used the full navigation inertial system in aircraft through the fabrication of stabilized inertial system in early 1950s. After that the stable platform inertial navigation systems were widely used as an equipment in aircraft and ships. Further important developments were made on the miniaturization of these instruments and understanding of the ring laser gyroscopes. During this period, inertial navigation applications were applied to ballistic missile projects and the exploration of space. One of the advance progress in twentieth century was the used of microcomputers that enabling the strapdown navigation principle to be used thought the developments of
XLII
gyroscopes with large dynamics ranges. So, the sizing complexity had been reduced that increased the applications of inertial navigation system. Small, rugged, reliable, accurate and inexpensive inertial sensor were produced by using novel method. Silicon accelerometers and optical fibre gyroscopes sensors were also produced during this era. In recent years, the strapdown navigation system has taken the place of stable platform inertial navigation. Advancements in the technology of gyroscopes, increases the application of strapdown inertial navigation system. The developments in miniature of rate integrating gyroscopes, ring laser gyroscopes, fibre optic rate sensors, dynamically tuned gyroscopes and vibratory gyroscopes have expand the need of inertial navigation system. 2.2.2
INS
Inertial Navigation System (INS) is a one of the navigation system that uses the motion sensors to determine all the navigation states of a moving vehicle. The navigation states include position, velocity and attitude of a vehicle. Rotational Sensors(Gyroscopes), Linear sensors (Accelerometers) and magnetic sensors (Magnetometers) are used in inertial Navigation System. It is referred as self-contained system as it doesn’t require an external source for its computation. 2.2.2.1 Types of Inertial Navigation System There are two main type of Inertial Navigation System 1. Stabilized Planform INS 2. Strapdown INS
XLIII
2.2.2.2 Stabilized Planform INS This type of INS is driven by sensors (accelerometers and gyros) that maintain its orientation with the specified axes regardless of any rotation and movement of aircraft. Acceleration and velocity can be computed using integration form the output taken form accelerometers and gyroscopes. [ CITATION Sky1 \l 1033 ]
2.2.2.2.1 Advantages
Requires less computation time as no axes transformation method is used in computation.
2.2.2.2.2 Disadvantages
Costly
Complex
Require maintenance
Suffers from wear and tear due to mechanical parts
Output drifts over time
XLIV
Fig. 2.5 Gimbal INS [8]
2.2.3
Strapdown INS
This type of INS is driven by sensors (accelerometers and gyros) that are strap down to the body frame of the vehicle. As the vehicle move, INS planform also move. The gyroscopes and accelerometer sense rotational rates and acceleration along each vehicle axis. Nowadays strapdown INS are commonly used in Navigation aid. 2.2.3.1 Advantages
Low Cost
Reduced Size and weight
No mechanical part
Require no maintenance and reliable over time
XLV
2.2.3.2 Disadvantages
Require greater computation time as axes transformation method are also used in Computation Process.
Uses Accurate and precise Gyroscopes
Fig. 2.6 Strap Down INS [9] 2.2.4
Principle
INS consist of three gyroscopes and three accelerometers. In Inertial Navigation System, the acceleration, angular rates and heading provided by accelerometers, gyroscopes and magnetometers are transformed into the required axis by using transformation methods. After that acceleration are integrated twice and angular rates are integrated once to get the position and velocity of the vehicle.
XLVI
Fig. 2.7 Basic mechanism of INS 2.2.5
Reference Frames
In INS, many Cartesian co-ordinate reference frames have been used according to the requirement. Every reference frame is an orthogonal and right-handed co-ordinate frame. The basic frame of references used in Inertial Navigation System are: 1. The Inertial frame (i-frame) 2. The Earth frame (e-frame) 3. The navigation frame (n-frame) 4. The wander azimuth frame (w-frame) 5. The body frame (b-frame)
1. The Inertial Frame (i-frame) The inertial frame (i-frame) is non-rotating reference frame with respect to fixed stars. Its origin is located at the center of the earth and the other axes Oxi, Oyi, and Ozi are defined with respect to fixed stars. 2. The Earth Frame (e-frame) XLVII
The Earth frame (e-frame) origin is also located at the center of the earth. Its axes Oxe, Oye and Oze are fixed with respect to the Earth. The e-frame rotates at a rate of Ω about the polar axis Ozi with respect to i-frame. 3. The Navigation Frame (n-frame) It is a local geographic frame with origin lies at the location of navigation system. Its axes are aligned along the direction of north, east and vertical down. The n-frame rotates at a rate of ωen with respect to earth. 4. The Wander Azimuth Frame (w-frame) It is similar like the navigation frame. Its axes are rotated about the vertical axes through the wander angle. The w-frame is used to avoid singularities. 5. The Body Frame (b-frame) It is aligned with the axes of the vehicle.
Fig. 2.8 Reference Frames
XLVIII
2.2.6
Axis Transformations
Different method can be used to transformation data from one coordinate axes to another co-ordinate system. 1. Direction Cosines 2. Euler angles 3. Quaternions 1. Direction Cosine It uses a 3x3 Matrix cosine matrix. The columns of the cosine matrix represent a unit vectors in a body frame axes projected alone the reference frame axes.
[
C 11 C12 C 13 C = C 21 C22 C 23 C 31 C33 C 33 n b
Where Cnb
]
is cosine matrix used for transformation from body frame to navigation
frame. 2. Euler Angles It uses a three-different rotation about different axes. The purpose is to transform from one co-ordinate system to another co-ordinate axes. The rotations are Roll, Pitch and Yaw. These rotations can be expressed mathematically as
Rotation Ψ
about z-axis,
Rotation θ about z-axis,
XLIX
[ [
cosΨ C1= −sinΨ 0
C2 =
sinΨ 0 cos Ψ 0 0 1
cos θ 0 −sin θ 0 1 0 sin θ 0 cos θ
]
]
Rotation ∅
about z-axis,
[
1 0 C3 = 0 cos ∅ 0 −sin ∅
0 sin ∅ cos ∅
]
The transformation from reference frame to body frame can be done by n
Cb =C3 C 2 C 1
The main disadvantages is singularity in pitch at ±90∘. They are used for three-
dimensional computation. 3. Quaternions It is a four-element vector representation that uses a single rotation about a vector that is defined in the reference frame that allow transformation from co-ordinate to another reference co-ordinate.
Where q is quaternion matrix and vector
μx , μ y and
μz are the components of the
μ that is defined in the reference frame.
Quaternions can also be expressed as
Where a, b, c and d are the components of complex number with a as real part and b, c and d as imaginary parts.
L
2.2.6.1 Choice of Transformation We have selected quaternion because of following advantages and disadvantages: [ CITATION BcI \l 1033 ] 2.2.6.1.1 Advantages Quaternions only uses four differential equations. Quaternions can be easily computed and have no singularities issues. 2.2.6.1.2 Disadvantages Initial computation is required for further computation. Initial transformation matrix can be computed from derivatives.
2.3 Inertial Measurement Units 2.3.1
Magnetometer
2.3.1.1 Introduction “Magnetometer is a device that measure magnetic field.” Magnetic field is a region in space where magnetic force acts. Magnetic force is a force that is exerted by moving charges. Origin of magnetic force lies in the structure of an atom. In an atom, there are negatively charged electrons revolving around the positively charged nucleus and spinning about their axis. This rotational motion of each negatively charged electron produce a magnetic force. So, magnetism is characteristic of all the materials. In some materials number of electrons and their motion is such that, the magnetic forces cancel each other, these are called diamagnetic materials like copper, bismuth and antimony. while in other materials, the magnetic forces of electrons are LI
summed up and each atom behave like a small magnet, such materials are called paramagnetic materials. There are also some solid substances like iron, where magnetic field of atoms cooperate with each other to produce strong magnetic effect, such substances are called ferromagnetic substances. In these substances, there are small regions called domains, within which magnetic field of all the electrons are aligned, so each domain behaves like a small magnet. When there is no external magnetic field, all of the domains are randomly oriented and net magnetic effect is zero. In the presence of external magnetic field, these domains are aligned in the direction of magnetic field and entire specimen become saturated making a powerful magnet, called electromagnet. The materials which require very strong external magnetic field to align their domain, and maintain their alignment for long time are called hard magnetic materials, whereas those materials whose domains are oriented easily are called soft iron materials.[ CITATION Pro13 \l 1033 ] 2.3.1.2 Working Principle Magnetometers are usually not used to measure the magnetic field, rather they measure the parameters which are otherwise difficult to determine. The parameters to be measured, create or modify the magnetic field. The magnetometer measures the change or new magnetic field, and after some signal processing convert it into desired parameter value.
LII
Fig. 2.9 Magnetic sensing [12] 2.3.1.3 Structure Structure of magnetometers vary depending upon types. They use some mechanism to detect magnetic field, that magnetic field is converted into voltage output, which is then processed to get the desired parameter. 2.3.1.4 Applications Magnetometers are used to: 1. Determine wheel speed 2. Detect presence of magnetic ink 3. Detect Vehicle 4. Determine heading 2.3.1.5 Types Magnetometers are divided into different types on the basis of different considerations. On the basis of magnitude and direction of measurement, magnetometers are categorized as:
LIII
2.3.1.5.1 Scalar Sensors Sensors which measure magnitude of magnetic field but not direction are called scalar sensors. 2.3.1.5.2 Omni-Directional Sensors Sensors which measure only magnitude of component of magnetic field, along their sensitive axis are called omni-directional sensors. 2.3.1.5.3 Bi-Directional Sensors Sensors which measure magnitude as well as direction of magnetic field components are called bi-directional sensors On the basis of sensing range, magnetometers are classified as low, medium and high field sensors: 2.3.1.5.4 Low Field Sensing They sense magnetic fields less than 1 microguass. Examples of low field sensing magnetometers are: 2.3.1.5.5 Search Coil They are based upon faraday’s law of induction, according to which changing magnetic field induce voltage in the coil, this induce voltage produces current in the coil which is measure of change in magnetic field. These sensors cannot detect the static or slowly changing fields. They are normally used in traffic control signals on roads.
LIV
2.3.1.5.6 Medium Field sensing They sense magnetic field ranging from 1 microguass to 10 microguass. They are also called earth field sensors. 2.3.1.5.7 Fluxgate They consist of two coils, primary and secondary coils, which are wounded on the same ferromagnetic core. An alternating current is applied to the primary coil, which produce changing magnetic field. This changing magnetic field produce alternating current in secondary coil, which is constantly measured. On the application of external magnetic field, current of secondary coil is changed. This change is measure of applied magnetic field.[ CITATION how17 \l 1033 ]
Fig. 2.10 fluxgate magnetometers [12] 2.3.1.5.8 Anisotropic Magneto-Resistive It is a resistive strip, which consist of silicon wafer, deposited by a thin film of nickeliron perm alloy. In order to measure the magnitude as well as direction of magnetic field, four of such resistors are connected in Wheatstone bridge configuration. During fabrication, the film is deposited in the strong magnetic field, to align its magnetization vector M, parallel to the length of resistor. Suppose a current I flowing in
LV
the resistor, makes an angle theta i.e. 45 degrees, with M. The properties of perm alloy are such that there is relationship between M, I, theta and resistance. The resistance is maximum when theta is zero and minimum when theta is 90 degrees. When external magnetic field is applied perpendicular to the side of resistor, it causes the magnetization vector to rotate and thus changing the angle theta, which results in the change in resistance and thus producing voltage output in the Wheatstone bridge, which is measure of applied magnetic field.[ CITATION Kav13 \l 1033 ]
Fig. 2.11 Magneto resistive effect [12] 2.3.1.5.9 High Field Sensing They sense magnetic field greater than 10 microguass. 2.3.1.5.10 Magneto Resistor They utilize the Lorentz force, which is a force exerted on moving electric charge, by magnetic field, it is given by: FL = q (v x B) As this force is cross product of velocity and magnetic field, its direction is perpendicular to them, so it will curve the path of the charge. LVI
Suppose a slab of semiconductor of given length and width. If a magnetic field is applied normal to the slab, the path of charge carrier will be deflected due to the presence of Lorentz force. Due to this deflection, length of path and thus electrical resistance of slab increases. This increase in resistance is measure of applied magnetic field. [ CITATION Mic \l 1033 ]
Fig. 2.12 Magneto resistors magnetometer [12] 2.3.1.5.11 Giant Magneto Resistive Devices It consists of two thin layers of ferromagnetic material, which are separated by nonmagnetic but a conducting material. The resistance of ferromagnetic material is changed by changing its moments. If moments are oriented parallel, there will be less scattering at interface, longer mean free path and smaller resistance. Reverse of this will happen, if moments are oriented antiparallel.
LVII
Fig. 2.13 Giant magneto resistive devices [12] They are called giant magneto resistive devices because of the fact that, there is very large change in resistance (about 70%) with change in magnetic field. In navigation, magnetometers are used to determine heading, by measuring earth’s magnetic field. Earth’s magnetic field has intensity of 0.5 to 0.6 gauss and it has a component that is parallel to the earth surface that always points towards magnetic north. By measuring this component, heading of vehicle with respect to the magnetic north can be determined. To find heading with respect to true north, declination angle has to be added in west and subtracted in east from the obtained values. Declination angle is basically an angle between axis of rotation (geographic north) and magnetic north. It has been mapped for entire globe and for a given location it can be determined from GPS, IGRF model or geomagnetic declination map. Thus, heading can be determined by:
ψ=arctan
( hyhx )± Dec angle LVIII
Fig. 2.14 Earth's magnetic field measurement [15]
To measure hx and hy, compass consisting of magneto resistive sensors are used. These sensors are used because they are mass produced as integrated circuits, they come in solid state packages. Moreover, they also have sensitivities less than 0.1 mill gauss, response time less than 1 micro second, and can collect readings in moving vehicle at rate of 1000 times per second. There are many types of compass which use these sensors. Two main types of compass are basic eight-point compass and one-degree compass. Eight-point compass is comparatively simpler and show only cardinal points (E, N, W,S) and midway points (NE, NW, SE, SW), whereas one degree compass are more sophisticated and can measure heading accurately up to 1 degree, which require magnetometer that can resolve angular changes to 0.1 degrees.
LIX
In this way, heading can be determined by using above mentioned formula. But due to the fact that, tangent function is valid over 180˚ and it does not allow y=0 division calculation, the following equations can be used: (hx0, hy0, hy >0)
ψ=360−arctan
( hyhx ) ± Dec angle
for (hx =0, hy 0) ψ=270 deg ± Dec angle If a strapdown compass is used to determine heading, then it gives hx, hy and hz in body frame, whereas to determine heading we need hx and hy in horizontal plane, that is a plane normal to the earth gravity vector. To get, hx and hy in horizontal plane, we have to perform rotations mathematically using following equations:
LX
hx_level = hx*cos(φ) + hy*sin(θ)*sin(φ) - hz*cos(θ)*sin(φ) hy_level= hy*cos(θ) + hz*sin(θ)
Where: hx, hy, hz are magnetic field components measured in body axis hx_level, hy_level are magnetic fields components in horizontal plane φ
is the roll angle, which is angle about longitudinal axis
θ
is pitch angle, which is angle between longitudinal axis and horizontal
plane
Fig. 2.15 Levelling of magnetometer readings [16] Where pitch and roll angles can be obtained from gyroscopes or tilt sensors. Thus, in order to determine heading using magnetometers we need readings of magnetometers in LXI
body frame which are rotated into horizontal plane using pitch and roll angles. These levelled readings are processed to get heading. [ CITATION Mic2 \l 1033 ][ CITATION Mic1 \l 1033 ]
Fig. 2.16 Heading determination using magnetic sensor [16] 2.3.1.6 Errors For Navigation, we need to measure magnetic field of earth. But in our surrounding, there are always many other sources of magnetic field, which are also measured by magnetometers. Moreover, these magnetic fields also interact with earth’s magnetic field and distort it. Apart from this, there are also some limitations of sensor like manufacturing issues, environmental effect and alignment problems. Thus, heading given by magnetic compass is most of the time erroneous. Following are the common errors in magnetometers. 2.3.1.6.1 Null Shift Errors If a sensor gives some output on zero input, then it means that it has null shift errors. It is also called DC offset or zero bias. It offset the output by a constant amount. This error can be modelled by CZb matrix, which is 2x1 for 2D case and 3x1 for 3D case.
LXII
2.3.1.6.2 Scale Factor Errors A sensor senses some input and on the basis of its sensitivity, scale it to some output. As no two sensors can have exactly same sensitivity, so their output will never be exactly same even if the input is common. This mismatch of outputs due to difference in the sensitivities of different sensors is called scale factor error. It can be modelled by 3x3 C sf matrix.
[
C sf =
]
(1+ Sfx) 0 0 0 (1+ Sfy) 0 0 0 (1+ Sfz)
2.3.1.6.3 Hard Iron Errors Magnetometers cannot distinguish between the magnetic field of earth or that of any other source. The error that appears in the heading due to the measurement of some unwanted magnetic field is called hard iron error. It causes a constant offset in the value of output just like null shift error. If there is no disturbing effect, and magnetometer is rotated in 360 degrees and resulting data is plotted in x-y plane, a circle is obtained with center at origin (0,0). In case of hard iron errors, the center of circle will offset from origin.
LXIII
Fig. 2.17 Ideal circle and hard iron error [16] As hard iron error produces constant offset, it must be due to the magnetic field of something attached to the same vehicle on which magnetometer is mounted. It can be modelled by δBb, which is 3x1 matrix. 2.3.1.6.4 Soft Iron Errors There are some materials which on exposure to the magnetic field produce their own magnetic field, which interact with the magnetic field of the earth and distort it, thus producing errors in heading determination called soft iron error. These errors can be identified as perturbation of ideal circle into an ellipse.
LXIV
Fig. 2.18 Soft iron errors [16] Assuming no hysteresis, soft iron can be modelled by Csi matrix.
[
α xx α xy α xz C si= α yx α yy α yz α zx α zy α zz Where α xy
]
is effective soft iron co-efficient, which relates magnetic field generated in
x-direction due to the applied field in y-direction. 2.3.1.6.5 Misalignment Errors In ideal case, axis of magnetometers are aligned with the axis of the body on which it is mounted. In practice, ideal alignment cannot be achieved, which produce errors in heading determination called misalignment error. This error can be modelled as C m matrix.
[
1 −ϵ z ϵ y C si= ϵ z 1 −ϵ x −ϵ y ϵ x 1
] LXV
Where
ϵx ,
ϵy ,
ϵz
represent small rotations which align the magnetometer axis
with body axis.[CITATION Dem \l 1033 ][ CITATION CCF08 \l 1033 ] Combining all of the errors we have:
Bb = Cm Csf Csi(Bb + δBb) + Czb Where Bb is estimated magnetic field and Bb is measured magnetic field. 2.3.1.7 Elimination of errors Heading determined with the help of magnetometer has errors. Different techniques have been used for the elimination of these errors. Some of the techniques eliminated errors in magnetic field domain, some in sensor domain and some in heading domain. Following are some of the commonly used approaches: 2.3.1.7.1 Compass swinging It is heading domain calibration that had been widely used in aviation and marine industry for compass calibration. The process involves the levelling and rotation of vehicle, containing compass, through a series of known headings. By comparing the heading measured by compass and known heading values, error in heading could have been calculated. The main draw backs of this scheme are that it involves the rotations of entire vehicle, involvement of user and requirement of large 3-D space and external heading source. These drawbacks had limited the scope of this method.
LXVI
Fig. 2.19 Compass swinging 2.3.1.8 Ellipse/Ellipsoid Fitting It is calibration in magnetic field domain, which is based upon the fact that, in an undisturbed environment, if magnetometer is rotated in 2D (or 3D) space it gives circle (or sphere) with its center at origin. Whereas, due to perturbations this circle is converted into ellipse (or ellipsoid). By fitting magnetometer measurements to an ellipse or ellipsoid calibration parameters can be calculated using algebraic or geometric techniques. These calibration parameters are then applied to the measured magnetic field values to get actual magnetic field and hence actual heading. 2.3.1.8.1 Sensor Rotation This is calibration in sensor domain, which involves the rotation of a leveled sensor in horizontal plane and collection of measurements. From these measurements scale factor and biases can be calculated, which are applied to the magnetometers readings to get correct heading. This approach also involves, user and sensor motion which make the calibration process slow. LXVII
2.3.2
Gyroscope
2.3.2.1 Introduction It is a device used for measuring and maintain orientation. It consists of rotating wheel or disc mounted that can spin about axis and the axis of rotation can assume any orientation by itself. The orientation of the axis are unaffected by rotation or tilting of the mounting, so it can be used in maintaining a reference direction in navigation system, automatic pilots and stabilizers. 2.3.2.2 Working Principle Gyroscope works on two basic principle 1. Rigidity in Space It is the ability of a rotating wheel to remain in its position and to resist being moved when it is rotation or spinning.
2. Gyroscopic Precession When a force is applied to a spinning wheel or rotor, the rotor will resist the force where it is applied. The force will act 90 degrees from the point of application in the direction of rotation. 2.3.2.3 Applications 1. In Aviation and Aeronautics Industry a. Remote control flying devices
LXVIII
b. Helicopters and some hovercraft c. Spacecrafts and spaceships d. Aircraft and aircraft autopilots 2. In Naval Field a. It has been used in ships for maintaining stability in: i.
Steering
ii.
Pitching
iii.
Rolling
3. In Automobiles a. Car Industry b. In motorbikes wheels for balancing 4. In Electronics and Gadgets a. Smartphones b. Computer mouses c. Video game controllers. 2.3.2.4 MEMS Gyroscope In MEMS gyroscope, silicon micro-machining techniques are used. They have low part count as compared to mechanical and optical gyroscope that has still high part counts and relatively cheaper to manufacture. LXIX
In MEMS gyroscope Coriolis effect is used. According to this effect “The frame of reference rotating at angular velocity ω, with mass m moving with velocity v experiences a force”, FC = −2m (ω × v) Vibrating elements such as tuning fork and vibrating wheel are used in MEMS Gyros to measure Coriolis effect. Rotation of gyros produces secondary vibrations due to Coriolis force. By measuring the secondary vibration, angular velocity can be measured. At present, optical gyroscope has better accuracy than MEMS gyroscope however, in future MEMS gyroscope are expected to do so.
MEMS gyroscopes have many
advantages compared to other gyroscopes, some are listed below: • Small size • Low weight • Short start-up time • Rugged construction • Inexpensive to produce • Low power consumption
LXX
Fig. 2.20 MEMS Gyros[ CITATION Hap \l 1033 ]
2.3.3
Accelerometers
2.3.3.1 Definition “Accelerometer is an electro-mechanical device that measure proper acceleration forces, that may be static like constant force of gravity pulling on the feet or dynamic like forces due to motion or vibrations” 2.3.3.2 Working Principle There are large number of different types of accelerometer having different types of working principles. The Common principles are: 1. Piezoelectric effect (due to accelerative forces) 2. Displacement sensing (based on displacement of mass) Measurement of acceleration in accelerometers is based upon Newton’s 2nd Law: F=ma LXXI
a=
F m
Accelerometers measures the force, from which acceleration can be determined. 2.3.3.3 Structure Accelerometers are made up of at least three basic components: 1. A mass called the proof mass 2. A suspension acting as the link between mass and accelerometer case. It locates the mass 3. A sensing mechanism 2.3.3.4 Applications Accelerometers are used for: 1. Velocity and position inertial measurement 2. Vibration and shock measurements 3. Gravity measurement to determine the orientation[ CITATION Mti04 \l 1033 ] They have applications in: 1. Engineering 2. Industry 3. Medical Application 4. Navigation 5. Safety airbags in automobiles 6. Gamming 7. Cell phones and laptops etc.[ CITATION Shy \l 1033 ] LXXII
2.3.3.5 Types There are large number of different types of accelerometer. The 4 basic types are: 1. Mechanical 2. Piezo-electric 3. Capacitive 4. Micro-chip These types are also further subdivided into different categories on the basis of construction, applications and other characteristics. Some of the major types of accelerometer are discussed below: 2.3.3.5.1 Potentiometric Accelerometer It measures the motion of the spring mass, by attaching the proof mass with wiper end of the potentiometer. Thus, it converts changing acceleration into changing resistance. These accelerometers have small natural frequency of about 30Hz, so they can measure small vibrations and have limited applications.
LXXIII
Fig. 2.21 Potentiometric Accelerometer 2.3.3.5.2 Capacitive Accelerometer In these capacitor, a diaphragm is used which move due to the acceleration. This diaphragm is placed between two fixed plates and act as moveable plate. As acceleration is produced, diaphragm moves and thus capacitance is changed. Thus, change in acceleration is measured in terms of change in capacitance. Capacitive sensing is most commonly used in MEMS accelerometer. Their advantage is that they have true DC response. Their disadvantage is that, they have limited dynamic range.
LXXIV
Fig. 2.22 Capacitive Accelerometer 2.3.3.5.3 Piezo-Resistive Accelerometer They employ piezo-resistive materials or strain gauges which change electrical resistance on the application of mechanical strain. In piezo-electric accelerometer, electrical resistance of piezo-electric material is changed on the application of force (due to acceleration). Thus, change in acceleration is measured in terms of change in resistance. They are used in sudden and extreme vibrating conditions. Piezo-resistive sensing is used in micro-machined structures. Their advantage is that, they have true DC response and they can measure acceleration up to +/-1000g.
Fig. 2.23 Piezo-resistive Accelerometer LXXV
2.3.3.5.4 Resonance Micromechanical Accelerometer They are also called vibrating beam accelerometer. They have the proof mass loaded on the opposite sides of the vibrating flexures. When proof mass is loaded, one point of attachment is in tension, while other is in compression. These points of attachment are excited to the frequency from 100 to 1000 of the hertz, when unloaded. When force is applied due to acceleration, frequency of one point of attachment is increased, while that of other is decreased. This difference of frequency provides measure of acceleration. 2.3.3.5.5 Surface Micromachined Accelerometer It consists of spring, mass and motion sensing components. The sensors are made with the standard IC processing techniques. Over the layers of oxide and poly-silicon, the IC photolithography and selective etching is used to create sensor as a 3D structure suspended over the substrate with 3 degrees of freedom. The core of the sensor is the surface micromachined by the poly-silicon structure or mass suspended over the substrate with the springs. Both the mass and wafers have fixed plates, which form differential capacitor. The plates on wafer are 180 degrees out of phase. Any movement of the mass changes the capacitance by the amount proportional to the acceleration.[ CITATION Acc08 \l 1033 ]
2.4 Overview of Integration 2.4.1
Integration
GNSS is most common type of navigation that is being used nowadays. It is cheap, easily accessible and give good long-term accuracy, but its applications are limited due to its outages in tunnels, bad weather or anywhere, where view to the satellites is limited. To LXXVI
improve the reliability of GNSS, it is often integrated with INS, as the two techniques have complementary properties. There are different methods for integration of INS and GNSS, but the two commonly used methods are: a) Least square b) Kalman filtering Kalman filtering is more advanced and intelligent form of least square, so it is used more commonly. 2.4.2
Kalman Filter
Kalman filter is a mathematical tool that is used for stochastic estimation from noisy measurement. It is considered to be optimal estimator as it minimizes estimated error covariance. Kalman filter is used to estimate the instantaneous state of linear dynamic system that is perturbed by white noise. It assumes Gaussian distribution and was originally for the linear system, but now linearized and extended Kalman filter is also used for non-linear problems. Kalman filter was developed by Rudolf Kalman in 1960 for control problems. It experienced immense criticism, that’s why it was originally published in mechanical engineering journal, instead of that of electrical engineering. Its earliest application was in Apollo project by NASA for trajectory estimation and control problems. Nowadays, its applications are spanning various fields i.e., prediction of flow of rivers during flood, trajectories of celestial bodies, global navigation system and prices of trade commodities. [ CITATION Gre01 \l 1033 ] It is used for smoothing the noisy data and estimating the parameters of interest. Kalman gained popularity because of less computational complexity, stability and generality. It has small computational requirements, elegant LXXVII
recursive properties, and is optimal estimator for linear system with Gaussian errors [ CITATION Far12 \l 1033 ]. 2.4.2.1 Modelling Kalman filter is based upon state space model. It deals with the problems that involve the estimation of state of discrete time-controlled process, that is modeled by linear stochastic differential equation. Continuous time model: For continuous system, state space can be written as: x´ t=f ' ( x (t ) , u (t )) + w(t ) y t=h ( x ( t ) , u (t )) + v (t) Where: x t=State vector y t=Measurement vector
u=Input vector w=Process noise vector v =sensor noise vector For linear systems, above equations can be written as: x´ t= A ' x ( t )+ B ' u(t)+ w(t) y t=Cx ( t )+ Du(t)+v (t)
LXXVIII
Discrete time model: For computer handling, above equations are discretized using finite difference method according to which:
x´ t ≅
x k+1−x k T
And thus: x k =( I +T A ' ) x k−1 +TB ' uk−1+T w k−1 x k = A x k−1 + Bu k−1 +T wk−1 y k =C x k + D uk + v k
Given the state space, we assume gaussian noise, where: p ( w ) N (0 , Q) p ( v ) N (0 , R)
Where: Qk =E [wk wtk ]
Qk
is process noise covariance matrix, and it vary with each step. It should be selected
in such a way that stability of filter is not affected. And: t
Rk =E [v k v k ]
LXXIX
Rk
is a sensor noise covariance matrix.
Rk
and Qk
are uncorrelated.
[ CITATION Sha14 \l 1033 ] 2.4.2.2
Process
Kalman filter is two-step process, in first step it predicts the state of system, and in second step it updates the state using measurements. The equations of first step are called predictor equation and that of second step are called corrector equations. Thus, Kalman filter is predictor-corrector algorithm. The predictor equations are: −¿= A x k−1 + Bu k−1 +w k−1 ¿ xk
−¿= A Pk−1 A t +Q k−1 P¿k The corrector equations are: −¿ C + R ¿−1 C P¿k −¿ Ct ¿ K k =P¿k −¿ z k −C x¿k −¿+ K k ¿ x k =x ¿k −¿ Pk =(I−K k C) P¿k Where: LXXX
−¿=Priori state of the system x ¿k −¿=Priori processcovariance ¿ P¿k Q=Process Noise covariance ¿ R=Sensor noise covariance ¿ K k =Kalman gain z k =Masurement ¿
x k =State of the system Pk =Process covariance ¿
uk =Control ¿ w k =Processnoise vector
A , B , C=Mappingmatrices It works in the following way:
LXXXI
Fig. 2.24 Working of Kalman filter
In simple words, it can be described as:
LXXXII
Fig. 2.25 Process of Kalman filter
2.4.2.3 Types The type of Kalman filter, discussed above is very basic. For real world non-linear problems, we use following types: 2.4.2.3.1 Linearized Kalman Filter It linearizes the non-linear equations in state space, about a nominal trajectory, that is independent of measurement data. It is based upon assumption that, trajectory is known in advance and thus Kalman gain is computed offline and stored on computer.
LXXXIII
2.4.2.3.2 Extended Kalman Filter It linearizes the non-linear equations in state space, about a best current estimate, by using the Taylor series at each time step. In this case, functions ‘f’ and ‘h’ are nonlinear: x k =f ( x k−1 ,u k , w k−1 )
y k =h (x k , v k ) In practice, one may not know the values of
vk
x k =f ( x k−1 ,u k , 0) y k =h ( x k , 0)
Now, the predictor equations are: −¿=f ( x k−1 ,u k−1 ) x¿k x (¿¿ k−1)+Qk−1 −¿=F x ( x k−1 ) Pk−1 F x t ¿ P¿k The corrector equations are:
LXXXIV
and w k at each time step. So,
−¿ x ¿k ¿ −¿ −¿ ¿ x k + R ¿−1 t −¿ H x ¿ ¿ ¿ x k Pk Hx¿ t −¿ H x ¿ K k =P¿k −¿ z k−H x x ¿k −¿+ K k ¿ x k =x ¿k −¿ x ¿k ¿ ¿P ¿ I−K k H x ¿ ¿ P k =¿ Where:
F x=
∂f ¿ ∂ x x= x , u=u
H x=
∂h ¿ ∂ x x=x
k
k
k
,u=uk
Extended Kalman filter, is quite useful for most of the applications, but it has certain limitations i.e., it is difficult to calculate Jacobins analytically, numerical computation of Jacobins incurs high computational cost, it works only for differentiable systems, and it is not optimal for highly non-linear systems.[ CITATION Gre011 \l 1033 ]
LXXXV
2.4.2.3.3 Unscented Kalman Filter It is based upon evaluation of function, rather than its linearization. It is more stable and complex and thus it can handle higher order non-linearities. It uses unscented transform, in which it approximates the mean and covariance of transformed variable. A fixed number of sigma points are generated about point x, which have same mean and covariance as in the case of x. The sigma points are then transformed through non-linear model for estimation of mean and covariance of transformed variables.[ CITATION VIK14 \l 1033 ] 2.4.3
Integration of INS and GNSS by Kalman Filter
There are several approaches for integration of INS and GNSS. 2.4.3.1 Uncoupled This is one of the simplest integration technique, that uses linearized filters. In this Case, the solution of Ins and GNSS remain independent. Errors are estimated, when GNSS measurements are received. Its drawback is that, during GNSS outages, for low cost MEMS sensors, errors are propagated largely in small time interval. Due to high error propagation, the assumptions about linearization will not remain valid and the performance of system will degrade.
LXXXVI
Fig. 2.26 Uncoupled integration [28] 2.4.3.2 Loosely Coupled It is the least complicated form of close loop integration. Here INS errors are corrected through integration filter, by a feedback loop. It uses processed output of GNSS. The output of GNSS remain independent, whereas that of INS is corrected to give integrated solution.
Fig. 2.27 loosely coupled integration [28] 2.4.3.3 Tightly Coupled It is more robust integration and is referred to as ‘centralized integration’. It uses raw data from GNSS to correct errors in INS, as a result of which, it can use the GNSS measurements even if less than 4 satellites are locked. It requires more computation within the Kalman filter.
LXXXVII
Fig. 2.28 Tightly coupled integration [28] 2.4.3.4 Ultra-Tightly coupled The devices of INS and GNSS are coupled in this case. Measurements from GNSS are used to estimate INS errors and INS’s measurements are used for estimation of GNSS errors.
Fig. 2.29 Ultra-tightly coupled integration [28]
LXXXVIII
3. HARDWARE IMPLEMENTATION 3.1
Inertial Measurement Unit
Inertial measurement Unit (IMU) is used for the collection of INS data that gives gyroscopes, accelerometers and magnetometer rates as output. IMU uses 3 sensors i.e. gyroscopes, accelerometers and magnetometer. These outputs are used in INS code to obtain position, velocity and attitude.
Fig. 3.1 Inertial Measurement Unit In our project, we have tired many times to use the data taken from IMU but the results were beyond our expectation. Then we have taken data of single point and implemented it in our codes but still the results are not acceptable. This is because of the damaged sensors of IMU. The IMU sensors are cheap so they gave inaccurate results and data obtained are not reliable for use. So, for INS data collection we have used a software named Trajectory generator.
LXXXIX
3.2 Trajectory Generator The trajectory software is made by Chinese. Its user interference is in Chinese language. It gives Gyroscope and accelerometers data only by taking initial latitude, longitude, attitude and velocity.
Fig. 3.30 Trajectory Generator Software Interface It has four phases: 1. Initial Position 2. Sensors errors parameters 3. Generate Trajectory
XC
4. Help 3.2.1
Initial Position
In initial position, the interface takes all the initial value of trajectory to be generated, path where the generated files will be stored and frequency of INS and GPS data. Nature of our trajectory i.e 3rd order smooth or uniformly accelerated data is also selected here. To generate trajectory, we have obtained information of latitude, longitude and height from emlid reach receivers, that was quite accurate, whereas, the guess about attitude and was made from output of IMU, although its data was corrupted, but it gave reasonable guess. Moreover, for velocity we have used another mobile app named GPS test. 3.2.2
Sensors errors parameters
It takes account the following sensors errors
Gyro Rand Drift
Gyro const Drift o/h
Accel Rand
Bias µg Accel Cont Bias µg
Error between the ideal and generated trajectory is given in the form of position and velocity error.
XCI
Fig. 3.31 Sensors error Interface 3.2.3
Generate Trajectory
Flight Time Sec, Final Head, Final Pitch and Final Velocity is in this phase. Various segments according to the trajectory to be generated is added here. In the end, it gives the following trajectory files:
GPS data
Navigation data
parameters data
Real GPS data
Track Data
XCII
Navigation data contains gyroscopes and accelerometers data wile track gives the ideal trajectory.
Fig. 3.32 Generate trajectory Interface
3.3 RTK Emlid Reach Recievers The RTK Emlid reach is a GNSS receiver. It enables centimeter level accuracy in positioning by tracking GPS, BeiDou, GLONASS, QZSS, Galileo and SBAS satellites. It has a build in web app that works with a browsers on any device and does not need any internet. Moreover, it gives access to download RINEX logs for post processing, view signal strength and status of satellites and also gives solution file with accurate longitude and latitude information[ CITATION Qui \l 1033 ].
XCIII
3.3.1
RTK Technique
RTK a real time kinematics is a positioning technique that provides range with centimeter-level accuracy. It consists of a single base station at accurately known position with several mobile stations (Rovers). The range is computed by multiplying carrier wavelength with number of cycles between satellite and rover[ CITATION NOV \l 1033 ]. RTK uses the satellite carrier wave signals. The base station rebroadcast the carrier phase signals it observe and the rover compare the phase signals with their own phase measurements to get the position corrections. The rover then use algorithms that incorporate corrections received from the base station and ambiguity resolution for calculating accurate position[ CITATION NOV \l 1033 ]. 3.3.2
Hardware Integration
RTK Reach has two receivers, one of which is selected as rover and other as base. The base is placed at fixed position of known latitude, longitude and altitude. The rover is placed on a platform along with IMU. The platform is moved along a fixed trajectory, and hence the rover and IMU. Rover can be connected to base through, Bluetooth, radio and Wi-Fi. We have connected them through Wi-Fi.
XCIV
Fig. 3.33 Hardware integration of RTK reach receivers 3.3.3
Updating
After connecting the receivers with PC through Wi-Fi, it is checked whether both rover and base are up to date. In our case one of the receiver, which was selected as a base was not up to date, so we updated it, by connecting the receiver to the local internet and using software of intel Edison.
XCV
Fig. 3.34 Updating of RTK Reach receivers
Fig. 3.35 Updated 3.3.4
Settings
When receivers are connected to the PC, they create their own Wi-Fi, using which Emlid reach page is opened that provide a user interface. Through that interface, some settings are made regarding the selection of GNSS satellites, positioning mode, elevation mask angles, maximum acceleration and velocity.
XCVI
Fig. 3.36 Settings of RTK reach receiver After that, selections were made regarding base and rover. The receiver, which was selected as base was connected, and using ‘base mode’ tab, selections were made and its coordinates are calculated.
Fig. 3.37 Setting of base XCVII
Fig. 3.38 Coordinates of base Then, the rover was connected and using ‘correction input tab’, selections were made. During these settings, base is made as server and rover is made a client.
Fig. 3.39 Setting of Rover 3.3.5
Output
RTK reach provide output in real time as well as it gives files in RTCM3, RINEX and other formats that can be post-processed later on. XCVIII
The real time output gives information about position of base, position of rover, velocity of rover, distance between base and rover i.e. base line, age of differential and signal to noise ratio. Moreover, it also depicts the strength of signals from each satellite and solution status.
Fig. 3.40 Real time solution of RTK reach receivers 3.3.6
Post-Processing
The Files that are obtained from RTK reach can be post-processed using: 1. RTKLIB 2. MATLAB 3. Online software we have done it through MATLAB and RTKLIB.
XCIX
3.3.7
Results
3.3.7.1 LLH Solution file The latitude, longitude and altitude are plotted with respect to time. The variation in position seems to be zero, because the data is for very small range
Fig. 3.41 Results of RTK Reach If we plot latitude, longitude and altitude separately, then variation can be observed:
C
Fig. 3.42 Variation of latitude with time
Fig. 3.43 Variation of longitude with time
CI
Fig. 3.44 Variation of altitude with time The data for above graphs is obtained from. LLH file. To generate trajectory, we have obtained information of latitude, longitude and height from emlid reach receivers, that was quite accurate, whereas, the guess about attitude and was made from output of IMU, although its data was corrupted, but it gave reasonable guess. Moreover, for velocity we have used another mobile app named GPS test. 3.3.7.2 Post Processing through RTKLIB The RTK Emlid Reach log files can be process using RTKLIB app. It uses base and rover logs and gives the following outputs:
CII
Fig. 3.45 Trajectory through Post processing
Fig. 3.46 Position variation
CIII
Fig. 3.47 Velocity Variation
Fig. 3.48 Acceleration Variation
CIV
Fig. 3.49 Showing number of satellite 3.3.7.3 Post Processing of RINEX Files through MATLAB The RTK Emlid Reach also gives rinex files as an output. Processing these files gives the final output of velocity and position which were later used in Kalman Filtering.
CV
Fig. 3.50 Variation of Velocity with time
Fig. 3.51 Variation of Position with time
CVI
4. GNSS ALGORITHM 4.1 RINEX: Astronomical Institute of the University of Berne has developed "Receiver Independent Exchange Format" RINEX with the purpose of easy exchange of data collected during EUREF 89 a European GPS campaign. EUREF 89 campaign involved 60 GPS receivers of 4 various manufacturers. Now, the other constellations also use RINEX formats as it is easy to read and process[ CITATION Gur \l 1033 ]. 4.1.1
Why we are using RINEX:
Nowadays, the RINEX format is the universally used data format that gives the user fully authority to process the data and is independent of the type of receiver and antenna used for data collection. The format also contains extra data such as satellite health status, troposphere and ionosphere models errors. RINEX file have different formats (RINEX 2.01, RINEX 3.0, RINEX 3.0 2 etc.) that incorporates the new technology and satellites upgrading. We are using 2.01 RINEX version of navigation and observation file[ CITATION Gur \l 1033 ]. In navigation satellite system, satellites broadcast signals imbedded with different set of information. RINEX Generates two files containing that information: 1. RINEX Navigation File 2. RINEX Observation File 4.1.2
RINEX Observation File:
Observation file contains two principal sections: CVII
1. Header section 2. Data section Header section is usually the top portion of the file and contains global information for the complete file. The header section has been designed for the minimum space requirements. Header section must maintain some order in the record data[ CITATION Gur \l 1033 ].
Fig. 4.52 Header Section of observation file Data section of observation file contains data obtain from one session and one site, to change the receiver’s location completely or reset the location new data set will be CVIII
generated. While updated RINEX versions include data from more than one site. Moreover, the data section of observation file contains fundamental quantities known to be GNSS observables[ CITATION Gur \l 1033 ].
Fig. 4.53 Data Section of observation file The observables includes following fundamental quantities: 1. 2. 3. 4. 5.
Time Pseudo-Range Phase Doppler Satellite Number
4.1.2.1 Time: Time is the measure of the receiver time of that received signals. At a particular epoch, the time measurement is similar for the range and the phase measurement as well as for all the satellites. Each satellite system gives time measurement in their respective time system i.e. UTC for GLONASS.
CIX
4.1.2.2 Pseudo-Range: Pseudo-range of the signal is the distance signals travels between the satellite antenna and the receiver antenna. It includes clock offsets of the satellite and the receiver. So that it reflects true behavior of the satellite clock and the receiver clock[ CITATION Gur \l 1033 ]. PR=Distance+ speed of light (c )∗(−satellite clock offset +receiver clock offset+ other biases) 4.1.2.3 Phase: The phase is basically a signal phase or carrier phase. 4.1.2.4 Doppler: The sign information of Doppler shift is an additional observable as for approaching satellites it is positive. 4.1.2.5 Satellite Number: First letter followed by two digits gives complete information the letter is the satellite system identifier whereas the numbers represent the satellite. 4.1.3
RINEX Navigation File:
Like observation file, navigation file also have two primary sections; the header section and the data section. Header section is unique for all the satellite system and contains data for ionosphere correction. The unique header section is basically a system identifier.
CX
Fig. 4.54 Header Section of Navigation file
CXI
Data section of navigation file contains data from different satellites. The data form each satellite starts from the satellite number further contains transmission time of the massage and health information. This transmission time is not the same as the satellite system time.
Fig. 4.55 Data Section of Navigation file 4.1.4
RINEX files Names:
CXII
Receivers generate RINEX files with different extensions for Navigation and observation files. Observation file have extension ‘.O’ for all satellite systems whereas the extension for navigation file is different for different systems such as ‘.N’ for GPS, ‘.G’ for Galileo, ‘.I’ for GLONASS and ‘.C’ for BeiDou[ CITATION Gur \l 1033 ].
4.2 DATA Acquisition Algorithm: Extraction of data from RINEX file practices the following algorithm: 4.2.1 1. 2. 3. 4. 5.
Observation File: Load observation file Declare the functions of Pseudo range, Julian time and Satellite identifier. Extract the header data Extract Julian time (Year, Month, Day, Hour, Minutes and Seconds) Extract Pseudo-range (Using Satellite number and name) and store them in a predefined variables for later use
4.2.2 1. 2. 3. 4. 5.
Navigation File: Load navigation file Declare the function for ionosphere data and ephemeris Extract the header data (information of ionosphere data) Extract line by line Satellite orbital parameters and clock data Extract clock drift, clock bias, time of ephemeris and time of clock and store them in predefined variables for later use.
CXIII
4.3 Position Estimation 4.3.1
Coordinate Systems
Coordinate system definition is very important for solving the navigation equations. The receiver position and the satellite position must be represented in a single coordinate system. The GPS uses WGS-84 i.e. World Geodetic System. 4.3.1.1 World Geodetic System (WGS-84) World Geodetic System is an Earth-fixed, Earth-centered and geodetic datum. WSG-84 consists of model parameters that defines the Earth’s shape, size, geomagnetic fields and gravity. The WSG-84 frame is defined as follow[ CITATION Wor \l 1033 ]: 1. Origin is at Earth’s center of mass 2. X-axis is along the intersection of plane normal to Z-axis; pass through the origin and the IERS Reference Pole (IRP). 3. Z-axis is in the direction of IRP. 4. Y-axis completes the right hand rule. Table. 4.6 Parameters of World Geodetic System[ CITATION Wor \l 1033 ] Parameter Semi-Major Axis Flattening Factor of the Earth Mean Earth Angular Velocity Gravitational Constant
4.3.2
Symbol α 1/f ωE µ
Value 6378137.0 meters 298.257223563 7292115 ×10−11 radians/second 2 3.986004418 ×1014 meter 3 / second 2
Satellite Orbit Computation
For computing GPS satellite coordinates via calculating orbital parameters following algorithm is used[ CITATION How \l 1033 ]:
CXIV
1. Compute time t s from reference epoch as: t s=t−t oe
Where: t oe is the reference epoch time. 2. Compute the mean anomaly using formula:
( √√ )
µ +∆ n ts a3
M s=M o +
3. Calculate eccentric anomaly using the equation: M s=E s−e sin Es 4. Calculate true anomaly using the following equation:
ν s=arctan
(
√1−e 2 sin E s cos Es−e
)
5. Compute argument of latitude using true anomaly and the argument of perigee as: us =ω+ ν s +Cuc cos 2 ( ω+ ν s ) +Cus sin2 ( ω+ ν s ) 6. Compute the radial distance as: r s =a ( 1−e cos Es ) +Crc cos 2 ( ω+ ν s ) +Crs sin 2 ( ω+ ν s ) 7. Using reference epoch inclination compute the orbital plane inclination by using the following equation: i s=i o +it s +Cic cos 2 ( ω+ ν s )+ Cis sin 2 ( ω +ν s)
CXV
8. Compute longitude of ascending node
λ s using the following equation:
λ s=Ωo + ( Ω−ω E ) t s−ω E t oe 9. Now compute the satellite coordinates as
[]
Xs Y s =R3 (−λ s) R1 (−i s) R3 (−us ) Zs
Where:
[
1 0 R1= 0 cos ϑ 0 −sin ϑ
[
cos ϑ R3= −sin ϑ 0 4.3.3
0 sin ϑ cos ϑ
sin ϑ cos ϑ 0
0 0 1
]
]
Least Square Method for position Estimation
Positioning involves three main tasks; acquisition, tracking and positioning. Acquisition is finding the satellites and their position. It gives rough signal parameters. Tracking tracks these signal parameters as they change with time. Extracted navigation data after tracking than use for computing pseudo-range. As final task the receiver computes the user position. The receivers measure the time delay
τ s=t−t s , where t is the receiver time and
s to the receiver, so that
satellite time. The time delay is then multiple with
√
2
τ s of the signal from each satellite
2
2
ρs = ( X s−X ) + ( Y s−Y ) + ( Z s−Z )
CXVI
ts
is the each
Where speed of light c= 3 ×108
to get the rough estimate of range called as ‘Pseudo-
range’. The receiver gets this measurements simultaneously from four satellites and process them for getting position and time corrections. Observation equation: The most common algorithm for computing receiver position from pseudo-ranges is based upon the least square method. This method uses pseudo-ranges of four or more satellites for calculating receiver position[ CITATION YHe \l 1033 ]. The basic equation for pseudo-range
P
s
calculation is;
Ps= ρs +c ( dt−dt s )+ T s+l s+ e s ρs
is the geometric range computed by applying distance formula between receiver
and each satellite:
( X s , Y s , Z s)
( X ,Y , Z ) is coordinates of user
is coordinates of satellite position and
dt s is satellite clock offset,
position. Moreover, dt is the receiver clock offset, the tropospheric error,
ls
is the ionospheric error and
es
Ts
is
is the observation error.
Tropospheric and ionospheric errors are computed using the models that use broadcast ephemerids data. Substituting geometric range equation back into pseudo-range equation gives the following equation[ CITATION YHe \l 1033 ]; P s=
√( X −X ) +( Y −Y ) +( Z −Z ) + c (dt−dt )+T +l +e s
2
s
2
s
2
s
CXVII
s
s
s
The final pseudo-range equation is non-linear and involves four variables X, Y, Z and dt. The error terms in the equation are minimized by using Least Square (LS) method. As the above equation is non-linear with respect to user position, so before applying LS method the equation must be linearize by Taylor expansion. The non-linear part of the equation is; f ( X ,Y , Z )=
√( X −X ) +( Y −Y ) +( Z −Z ) 2
s
2
s
2
s
The position estimation starts from initial guess and then improve iteratively. Now Taylor expansion of f ( X i +1 , Y i+1 , Z i+1 ) implies[ CITATION YHe \l 1033 ];
f ( X i +1 , Y i+1 , Z i+1 )=f ( X i ,Y i , Z i ) +
∂ f ( X i ,Y i , Z i ) ∂ f ( X i , Y i , Zi ) ∂ f ( X i , Y i , Z i ) + + ∂ Xi ∂Yi ∂ Zi
In the Taylor expansion the higher order terms are ignored partial derivatives are computed for simplicity as[ CITATION YHe \l 1033 ]; ∂ f ( X i ,Y i , Z i ) ∂ = ∂ Xi
√( X −X ) +(Y −Y ) +( Z −Z ) 2
s
2
s
i
s
i
i
∂ Xi
−2 ( X s−X i)
¿ 2
¿−
√( X −X ) +(Y −Y ) +( Z −Z ) 2
s
2
s
i
i
s
2
i
( X s−X i) s
ρi
∂ f ( X i ,Y i , Z i ) −( X −X i ) = s ∂ Xi ρi s
CXVIII
2
Similarly; ∂ f ( X i ,Y i , Z i ) −( Y s−Y i) = ∂Y i ρis ∂ f ( X i ,Y i , Z i ) −( Z −Zi ) = s ∂Y i ρi s
The first order linear observation equation becomes;
Psi = ρsi −
( X s−X i ) ∆ X − (Y s−Y i ) ∆ Y −( Z s−Zi ) ∆ Z +c ρ
i
s i
ρ
i
s i
ρ
s i
i
( d ti−dt s) +T is+ lsi +e si
Applying Least-Squares (LS) method First rearrange the first order linearize equation as;
[
−( X s−X i)
−( Y s−Y i ) −( Z s−Z i)
ρsi
ρsi
ρsi
[]
∆ Xi 1 ∆ Y i =P si −ρis−T si −l is−e si +c dt s ∆ Zi c d ti
]
To solve this equation with four unknowns minimum of four equations are required. Let bis=Psi −ρsi −T si −l si −eis+ c dt s bis=[ b1i , b2i … ,b ni ]
and
T
[]
∆ Xi x i= ∆ Y i ∆ Zi c dti
CXIX
[ ]
A i=
−( X 1−X i )
−(Y 1−Y i)
−( Z1−Z i )
ρ1i
ρ1i
ρ1i
−( X 2−X i )
−(Y 2−Y i)
−( Z2−Z i )
ρ2i
ρ2i
ρ2i
−( X 3−X i )
−(Y 3−Y i)
−( Z3−Z i )
ρ3i
ρ3i
−( X n−X i )
ρ3i . . . n −(Y −Y i)
−( Z n−Z i )
ρni
ρni
ρni
1 1 1
1
Then the LS problem is given by[ CITATION YHe \l 1033 ]: min x ‖ Ai x i−bi‖2 i
The solution has to be added to approximate value. Thus, the receiver position update with increments ∆ X i , ∆ Y i and ∆ Zi
as;
X i +1=X i +∆ X i Y i+1=Y i + ∆ Y i
Z i +1=Z i +∆ Z i The iteration continue until the solution values are in meters. x i Which minimize the error vector e i=bi−A i x i −1
x i=( ATi Ai ) A Ti b i
CXX
is given by
4.3.4
Flow Chart of GNSS Algorithm
Fig. 4.56 Flow Chart of GNSS 4.3.5
Results:
The header section of RINEX observation file shows different observables as show in the figure:
CXXI
Fig. 4.57 Section of RINEX File We have picked some of these observables for showing the variation in the relative measures by using the above described algorithm: 4.3.5.1 Observable P1:
Fig. 4.58 Variation in Coordinates using P1 observable
CXXII
Fig. 4.59 Plot of longitude verses latitude data from P1 4.3.5.2 Observable P2:
Fig. 4.60 Variation in Coordinates using P2 observable
CXXIII
Fig. 4.61 Plot of longitude verses latitude data from P2 The above graphs shows the variation of position in earth centered earth fixed coordinate, with respect to epoch. Epoch is an interval of time which indicate the beginning of an event. The navigation message obtained from satellite give data. It is being observed that as we change the obseevables the relative coordinates changes. But if we look at the plot of longitude verse latitude it remains same for all the cases.
CXXIV
5. INS ALGORITHM 5.1 Inertial Navigation System Algorithm: INS algorithm works on the data obtained from gyroscope and accelerometer is in the form of angular rates and acceleration which are then converted by navigation equations to get the desired output. The output gives position, velocity and attitude in the form of latitude and longitude.
5.2 Algorithm Inertial Navigation system requires a specific set of coordinate system in which all the computations are performed. In our research, we have used strapdown INS, in which the sensors are mounted in vehicle coordinate system such that the x axis pointed towards east, the y axis points north and the z axis points upward. Earth is assumed to be ellipsoidal in shape. The typical flow chart of implementation of strapdown INS is given below:
Fig. 5.62 Scheme of Algorithm of INS [1]
CXXV
It involves the following process: 5.2.1
Transformation from Earth Centered Inertial Frame (ECIF) to North East Up (ENU) Frame: First, the transformation from Earth Centered Inertial Frame (ECIF) to North East Up (ENU) frame is performed using the following angular rotations starting from ECI frame.
...
*Zi , Ze Polaris
Earth Reference Ellipsoid
Earth Rotation
Inertial Ref Meridian
Zg,
Vehicle Position
Yg Xg
Greenwich Meridian h
Local Meridian
c
ωieΔt
λ
Xi
Equatorial Plane
Ye (λ=900)
Xe λ=00 =00
Yi
Fig. 5.63 Transformation in ENU Scheme[ CITATION BcI \l 1033 ] The transformation matric obtained from about Sequence of rotation are following:
[
−sinλ cosλ 0 C = −sinϕcosλ −sinϕsinλ cosϕ −cosϕcosλ cosϕsinλ sinϕ g e
] CXXVI
g
Where
Ce
is called position matrix in the geodetic frame mechanization. Position
navigation i.e. latitude
∅
and longitude
λ
are estimated from the updated solution
of C ge as follows: −1
g
∅ =∅ m=sin (Ce 3,3)
λm =tan −1 (
C ge 3,2 ) C ge 3,1
{
λ m if if Ceg3,1 >0 λ= λ m +180° if C ge 3,1 0 This transformation matrix is used to update the position matrix. 5.2.2 Position Matrix Update It is updated by solving the given differential equation using Euler’s method. ´ ge =Ωgeg C ge C Where
[
0 −ω gegz ω gegy Ωgeg = ω gegz 0 −ω gegx g −ωegy ω gegx 0
]
Other calculation involve in algorithm of INS is as follow: 5.2.3 Earth Spin Rate in ENU Frame Earth Spin rate in earth fixed frame is transformed into ENU frame is given as
CXXVII
[ ]
0 ω = Ω ie cos ∅ Ω ie cos ∅ g ⅈⅇ
Where Ωie =¿ Earth’s rate= 7.29115 x 10-5 rad/s 5.2.4 Transport Rate The transport rate is obtained from g
g
g
−V y V x Vx Tanϕ ¿T R M +h R N +h R M +h ω gⅇg =¿
5.2.5 Position Update from Updated Velocity Components The rates of latitude, longitude and altitude are as follows g
Vy ´ ϕ= R M+ h
R (¿¿ N +h)Cosϕ g ´λ= V x ¿ g ´ h=V z
Where
g g g V y ,V x ∧V z are north, east and up velocity components in body frame w.r.t to
earth respectively. RN is called the constant latitude radius of curvature of earth given as follows:
ϕ RN =ℜ [ 1+ ⅇ sin2 ¿ ¿ CXXVIII
R M is the constant longitude radius of curvature of earth given by: ϕ 2 R M =ℜ[ 1−2 ⅇ+3 ⅇsin ¿ ¿
And
ℜ is known the equatorial radius of curvature of earth.
Latitude, longitude and altitude can be updated by using the rates with following equations t +∆ t
ϕ ( t +∆ t )=ϕ ( t ) + ∫ ∅´ dt t
t +∆ t
λ ( t+ ∆t )=λ ( t ) + ∫ ´λ dt t
t+ ∆ t
h (t +∆ t )=h (t )+ ∫ h´ dt t
5.2.6 Gravity Vector Update Gravity is updated by using following equation
g=g0
Where
(1+0.001932851∗sin2 ϕ)(1−2 h / ℜ) √1−0.006694379∗sin 2 ϕ
g0=9.78 03267714
5.2.7 Velocity Computation Acceleration vector can be computed as V´ ge =f g−( ωgeg +2 ω gie )∗V ge −g g
CXXIX
Where f g=f gib =C gb f bib g f ib is the output given from Accelerometer triad in body frame.
[ ][ ][
g
g
g
g
][ ] g
0 2 ωiez +ω egz −(2 ω iey +ω egy ) V ex V´ gⅇx f gx +0 g g g g g g g 0 2ω iex + ωegx V ey V´ ⅇy = f y + 0 − −(2 ωiez +ω egz ) g g g g g g g 2ω iey + ωegy −(2 ωiex +ω egx ) 0 V ez V´ ⅇz f z +g
Velocity vector in reference frame is obtained by integrating this equation, tn +1 g e
g e
V ( t n+1 )=V ( t n) +
∫ V´ ge dt tn
5.2.8 Transformation of Navigation frame (geographic) to Strapdown Frame: Following angular rotations are performed
-y q g X gYg Z g ¾¾¾¾¾¾¾ ® X g¢ Yg¢Z g¢ ¾¾¾¾¾¾¾ ® X g¢¢Yg¢¢Z g¢¢ ¾¾¾¾¾¾¾ ® X bYb Z b about Zg axis about X g¢ axis about Yg¢¢ axis The transformation matrix obtained from about Sequence of rotation is following:
[
cosγcosψ+ sinγsinθsinψ −cosγsinψ +sinγsinθcosψ −sinγcosθ C = cosθsinψ cosθcosψ sinθ cosγcosψ−cosγsinθsinψ −sinγsinψ −cosγsinθcosψ cosγcosθ b g
]
5.2.9 Updating Attitude Direction Cosine Matrix (DCM) Cbg Direction Cosine Matrix is updated by using a transformation between time interval t to ( t+ ∆ t ) During this small span of time, the following sequence of rotations take place. Dq y Dq x ® ¾¾¾¾¾ Dq z ® X Y Z (t + Dt ) X bYb Z b (t ) ¾¾¾¾¾ ® ¾¾¾¾¾ b b b about Xb about Yb' about Z''b
CXXX
The corresponding transformation matrix at time ( t+ ∆ t ) is given as follows
[
]
0 ωbgbz −ωbgby b ´ bg ( t )= −ωb C 0 ω bnbx C g (t ) gbz ωbgby −ω bgbx 0 ´ bg ( t )=−Ωbgb C bg (t ) C Where Ω bgb
is the skew symmetric matrix corresponding to ω bgb .
5.2.10 Computation of ω bgb . The ω bgb . is calculated as ωibb =ωbig + ωbgb
ω bgb=ω bib−ωbig
ω bgb=ω bib +C bg ωigg
b
ωib
is the rates obtained from gyroscopes and ωigg g
g
g
ωig =ωie +ω eg
[ ] −V gy R M +h
g
ωig =
V gx R N +h V gx ωie sinϕ+ Tanϕ R M +h ωie cosϕ+
CXXXI
is obtained as follow:
5.2.11 Updating Attitude DCM Cbg Using Quaternion Initial quaternion is calculated from initial Cbg as follows
q 0=cos
( ψ2 ) cos( θ2 ) cos( 2γ )+ sin( ψ2 )sin ( θ2) sin ( γ2 )
q1 =cos
( ψ2 )sin ( θ2 )cos( γ2)+sin ( ψ2 ) cos( θ2 ) sin ( γ2 )
q 2=cos
() () () () ()
q 4=cos
( ψ2 )sin ( θ2 ) cos( 2γ )+sin ( ψ2 )cos( θ2 )sin ( 2γ )
ψ θ γ ψ θ γ cos cos −sin sin sin ( ) 2 2 2 2 2 2
ψ , θ
and γ
are also obtained from initial Cbg .
The dynamic equation of Quaternion is as follow ´ 1 M (Q)ω bgb Q= 2 T
ω gb=[ 0 ω gbx ωgby ω gbz ] b
Q=[ q 0 q 1 q2 q3 ]
[
b
b
T
q 0 −q1 −q 2 −q3 q q 0 −q 3 q 2 M ( Q )= 1 q 2 q3 q 0 −q1 q 3 −q2 q1 q0
] CXXXII
b
Quaternion will be updated by solving Dynamic equation. From the updated quaternion, Strapdown DCM ( Cbg .) is calculated from the updated quaternion using following relation.
[
C 11 C 12 C13 b C g= C 21 C 22 C23 C 31 C 33 C33
]
Where 2
2
2
2
C11=q0 +q 1−q 2−q3
C12=2( q1 q2 +q 0 q3)
,
C13=2( q1 q3 +q 0 q3 ) , 2
2
2
2
C21=2(q1 q2−q0 q3 )
, C23=2(q2 q3 + q0 q1 )
C22=q0−q1 +q 2−q3
q C31=2(q1 q3 +q 0 q2) , C32=2(q2 q¿ 0 q 1) 2
2
2
2
C33=q0−q1−q2 +q 3
5.2.12 Updating Quaternion Quaternion will be updated by following process
[]
∆ θx ∆ θ= ∆ θ y ∆ θz
N ( ∆θ )= √ (∆ θ x )2 +( ∆ θ y )2 +(∆ θ z )2
CXXXIII
[
0 −∆ θ x −∆θ y −∆ θz ∆ θx 0 ∆ θ z −∆ θ y M ( Q )= ∆ θ y −∆ θ z 0 ∆θ x ∆ θ z ∆ θ y −∆ θ x 0
[ ]{ [ q 0 (t+ ∆ t) N (∆ θ) q 1 (t+ ∆ t) = cos 2 q 2 (t+ ∆ t) q 3 (t+ ∆ t)
(
[
Cbg=
)
1 0 0 0
0 1 0 0
] ]{
(
N (∆ θ) 0 sin 2 0 + 0 N ( ∆θ ) 1
0 0 1 0
)
} }[ M
q 0 (t) q 1(t) q 2(t) q 3(t)
]
cosγcosψ+ sinγsinθsinψ −cosγsinψ +sinγsinθcosψ −sinγcosθ cosθsinψ cosθcosψ sinθ cosγcosψ−cosγsinθsinψ −sinγsinψ −cosγsinθcosψ cosγcosθ
5.2.13 Attitude Computation Attitude is computed form the updated
−1
ψ g=ta n (
γ g=ta n−1 (
. . .
, as follows
C21 ) C22
C13 ) C33
θ g=sin(C 23) Attitude angle value are adjusted to practical (Actual) values as follows 5.2.13.1 Heading If C22 >0 and ψ g >0
then ψ
= ψg
If C22 >0 and ψ g 1.0e-35, ALF(7) = A*B^3/2; ALF(8) = B^4/9; end; DELTA_RADIUS = RADIUS_TOP; DELTA_RADIUS = DELTA_RADIUS+ALF*RN; TRO = TRO+DELTA_RADIUS*REFERENCE*1000; if DD == 'TRUE ', ddr = TRO; break; end; DD = 'TRUE '; REF_SEA = (371900.0e-6/TK_SEA-12.92e-6)/TK_SEA; H_TOP = 1.1385e-5*(1255/TK_SEA+0.05)/REF_SEA; REFERENCE = REF_SEA*E0_SEA*((H_TOP-HS)/H_TOP)^4; end;
function [Azimuth, Elezation, DA] = topocent(X,DELTA_X) DELTA_RADIUS = pi/180; [FI,LAMDA,Height] = togeod(6378137,298.257223563,X(1),X(2),X(3)); cl = cos(LAMDA*DELTA_RADIUS); sl = sin(LAMDA*DELTA_RADIUS); cb = cos(FI*DELTA_RADIUS); sb = sin(FI*DELTA_RADIUS); F = [-sl -sb*cl cb*cl; cl -sb*sl cb*sl; 0 cb sb]; L_VCTR = F'*DELTA_X; EASTERN = L_VCTR(1); NORTHERN = L_VCTR(2);
CLXX
UPWARD = L_VCTR(3); HORIZONTAL_DISTANCE = sqrt(EASTERN^2+NORTHERN^2); if HORIZONTAL_DISTANCE < 1.e-20 Azimuth = 0; Elezation = 90; else Azimuth = atan2(EASTERN,NORTHERN)/DELTA_RADIUS; Elezation = atan2(UPWARD,HORIZONTAL_DISTANCE)/DELTA_RADIUS; end if Azimuth < 0 Azimuth = Azimuth+360; end DA = sqrt(DELTA_X(1)^2+DELTA_X(2)^2+DELTA_X(3)^2);
function [DEL_PHI,DEL_LAMDA,HIG] = togeod(QA,FIN_VE,X_DIS,Y_DIS,Z_DIS) HIG = 0; TOL_SQUA = 1.e-10; MATIX = 10; RADIAN_DEGREE = 180/pi; if FIN_VE < 1.e-20 ASF = 0; else ASF = (2-1/FIN_VE)/FIN_VE; end ONE_SY = 1-ASF; CB = sqrt(X_DIS^2+Y_DIS^2); if CB > 1.e-20 DEL_LAMDA = atan2(Y_DIS,X_DIS)*RADIAN_DEGREE; else DEL_LAMDA = 0; end; if (DEL_LAMDA < 0) DEL_LAMDA = DEL_LAMDA + 360; end RADIUS = sqrt(CB^2+Z_DIS^2); if RADIUS > 1.e-20 SIN_FI = Z_DIS/RADIUS; else SIN_FI = 0; end DEL_PHI = asin(SIN_FI); if RADIUS < 1.e-20 HIG = 0; return; end; HIG = RADIUS-QA*(1-SIN_FI*SIN_FI/FIN_VE); for i = 1:MATIX SIN_FI = sin(DEL_PHI); cosphi = cos(DEL_PHI); N_phi = QA/sqrt(1-ASF*SIN_FI*SIN_FI); dP = CB - (N_phi + HIG) * cosphi; dZ = Z_DIS - (N_phi*ONE_SY + HIG) * SIN_FI; HIG = HIG+(SIN_FI*dZ+cosphi*dP); DEL_PHI = DEL_PHI+(cosphi*dZ-SIN_FI*dP)/(N_phi + HIG);
CLXXI
if (dP*dP + dZ*dZ < TOL_SQUA) break; end if i == MATIX fprintf(['SOME ERROR'],i) end; end; DEL_PHI = DEL_PHI*RADIAN_DEGREE;
function SAT_POS = satpos(t,Ephmeris); E_U_G = 3.986005e14; OMEGA_DOT = 7.2921151467e-5; Satelite_PRN = Ephmeris(1); AF_2 = Ephmeris(2); MU = Ephmeris(3); ROOTA = Ephmeris(4); DEL_TAN = Ephmeris(5); ECCENTRICITY = Ephmeris(6); OMEGA = Ephmeris(7); C_YOU_C = Ephmeris(8); C_YOU_S = Ephmeris(9); C_ARE_C = Ephmeris(10); C_ARE_S = Ephmeris(11); I_O = Ephmeris(12); I_DOT = Ephmeris(13); C_AI_C = Ephmeris(14); C_AI_S = Ephmeris(15); OMEG_O = Ephmeris(16); OMEGDOT= Ephmeris(17); T_03 = Ephmeris(18); AF_ZE = Ephmeris(19); A_ON = Ephmeris(20); T_C = Ephmeris(21); G = ROOTA*ROOTA; time_k = check_t(t-TOE); NUMB = sqrt(E_U_G/G^3); n = NUMB+DEL_TAN; MU = MU+n*time_k; MU = rem(MU+2*pi,2*pi); ECC = MU; for I = 1:10 ECC_OLD = ECC; ECC = MU+ECCENTRICITY*sin(ECC); d_E = rem(ECC-ECC_OLD,2*pi); if abs(d_E) < 1.e-12 break; end end ECC = rem(ECC+2*pi,2*pi); VEL = atan2(sqrt(1-ECCENTRICITY^2)*sin(ECC), cos(ECC)-ECCENTRICITY); fi = VEL+OMEGA;
CLXXII
fi = rem(fi,2*pi); U = fi + C_YOU_C*cos(2*fi)+CUS*sin(2*fi); R = G*(1-ECCENTRICITY*cos(ECC)) + C_ARE_C*cos(2*fi)+CRS*sin(2*fi); I = I_O+I_DOT*time_k + C_AI_C*cos(2*fi)+C_AI_S*sin(2*fi); OM = OMEG_O+(OMEGDOT-OMEGA_DOT)*time_k-OMEGA_DOT*TOE; OM = rem(OM+2*pi,2*pi); X_ONE = cos(U)*R; Y_ONE = sin(U)*R; SAT_POS(1,1) = X_ONE*cos(OM)-Y_ONE*cos(I)*sin(OM); SAT_POS(2,1) = X_ONE*sin(OM)+Y_ONE*cos(I)*cos(OM); SAT_POS(3,1) = Y_ONE*sin(I);
function rin(ephemerisfile, outputfile) fide = fopen(ephemerisfile); TOP_LINE = 0; while 1 TOP_LINE = TOP_LINE+1; ROW = fgetl(fide); OUTPUT = findstr(ROW,'END OF HEADER'); if ~isempty(OUTPUT), break; end; end; TOP_LINE NUMB_eph = -1; while 1 NUMB_eph = NUMB_eph+1; ROW = fgetl(fide); if ROW == -1, break; end end; NUMB_eph = NUMB_eph/8 frewind(fide); for i = 1:TOP_LINE, ROW = fgetl(fide); end; SATELITE_RN = zeros(1,NUMB_eph); WEEK_NUMBER = zeros(1,NUMB_eph); TIME_OF_CORRE = zeros(1,NUMB_eph); TIME_GD = zeros(1,NUMB_eph); OADC = zeros(1,NUMB_eph); TIME_OE = zeros(1,NUMB_eph); AF_T = zeros(1,NUMB_eph); AF_ON = zeros(1,NUMB_eph); AF_ZE = zeros(1,NUMB_eph); A0_DE = zeros(1,NUMB_eph); DEL_TAN = zeros(1,NUMB_eph); M00 = zeros(1,NUMB_eph); ECCEN = zeros(1,NUMB_eph); ROOT = zeros(1,NUMB_eph); TIME_OE = zeros(1,NUMB_eph); C_AI_C = zeros(1,NUMB_eph); C_ARE_C = zeros(1,NUMB_eph); C_AI_S = zeros(1,NUMB_eph); C_ARE_S = zeros(1,NUMB_eph); C_YOU_C = zeros(1,NUMB_eph); C_YOU_S = zeros(1,NUMB_eph); OMEG_O = zeros(1,NUMB_eph);
CLXXIII
OMEG = zeros(1,NUMB_eph); AI_O = zeros(1,NUMB_eph); OMEG_DOT = zeros(1,NUMB_eph); AI_DOT = zeros(1,NUMB_eph); ACC = zeros(1,NUMB_eph); HAEL = zeros(1,NUMB_eph); FI = zeros(1,NUMB_eph); for i = 1:NUMB_eph ROW = fgetl(fide); SATELITE_RN(i) = str2num(ROW(1:2)); year = ROW(3:6); month = ROW(7:9); day = ROW(10:12); hour = ROW(13:15); minute = ROW(16:18); second = ROW(19:22); AF_ZE(i) = str2num(ROW(23:41)); AF_ON(i) = str2num(ROW(42:60)); AF_T(i) = str2num(ROW(61:79)); ROW = fgetl(fide); % IODE = ROW(4:22); C_ARE_S(i) = str2num(ROW(23:41)); DEL_TAN(i) = str2num(ROW(42:60)); M00(i) = str2num(ROW(61:79)); ROW = fgetl(fide); % C_YOU_C(i) = str2num(ROW(4:22)); ECCEN(i) = str2num(ROW(23:41)); C_YOU_S(i) = str2num(ROW(42:60)); ROOT(i) = str2num(ROW(61:79)); ROW=fgetl(fide); TIME_OE(i) = str2num(ROW(4:22)); C_AI_C(i) = str2num(ROW(23:41)); OMEG_O(i) = str2num(ROW(42:60)); C_AI_S(i) = str2num(ROW(61:79)); ROW = fgetl(fide); % i0(i) = str2num(ROW(4:22)); C_ARE_C(i) = str2num(ROW(23:41)); OMEG(i) = str2num(ROW(42:60)); OMEG_DOT(i) = str2num(ROW(61:79)); ROW = fgetl(fide); % AI_DOT(i) = str2num(ROW(4:22)); codes = str2num(ROW(23:41)); WEEK_NUMBER = str2num(ROW(42:60)); L2flag = str2num(ROW(61:79)); ROW = fgetl(fide); % SAL_ACC = str2num(ROW(4:22)); SAL_HEAL = str2num(ROW(23:41)); TIME_GD(i) = str2num(ROW(42:60)); AI_O_DC = ROW(61:79); ROW = fgetl(fide); % tom(i) = str2num(ROW(4:22)); end status = fclose(fide)
CLXXIV
EPHMERIS(1,:) EPHMERIS(2,:) EPHMERIS(3,:) EPHMERIS(4,:) EPHMERIS(5,:) EPHMERIS(6,:) EPHMERIS(7,:) EPHMERIS(8,:) EPHMERIS(9,:) EPHMERIS(10,:) EPHMERIS(11,:) EPHMERIS(12,:) EPHMERIS(13,:) EPHMERIS(14,:) EPHMERIS(15,:) EPHMERIS(16,:) EPHMERIS(17,:) EPHMERIS(18,:) EPHMERIS(19,:) EPHMERIS(20,:) EPHMERIS(21,:)
= = = = = = = = = = = = = = = = = = = = =
SATELITE_RN; AF_T; M00; ROOT; DEL_TAN; ECCEN; OMEG; C_YOU_C; C_YOU_S; C_ARE_C; C_ARE_S; i0; AI_DOT; C_AI_C; C_AI_S; OMEG_O; OMEG_DOT; TIME_OE; AF_ZE; AF_ON; TIME_OE;
OFILE = fopen(outputfile,'w'); COM_DOWN = fwrite(OFILE,[EPHMERIS],'double'); fclose all
10.1.2 INS code Body Frame X(right) Y(forward) Z(Up) % Navigation Frame X(East) Y(North) Z(Up) clc; clear all; close all; tic % Start Timer clc; clear all; close all; tic % Start Timer fpIMU =fopen('D:\Data final3\navdata.dat','r'); %output of IMU fpGPS =fopen('D:\Data final3\realgps.dat','r'); %output of GPS reciever track =fopen('D:\Data final3\track.dat','r'); %output of track generator software WIEE = 0.000072921151467; %rate of rotation of Earth Re = 6378135.072; % radius of earth g0 = 9.7803267714; %gravitational acceleration at SL e = 1/298.25; % CLXXV
jj=0; Qgyro = 0.5*pi/180/3600; Qaccel = 200E-6*g0; %System Noise. Random Drifts in Gyro and Random Bias in Accelerometer % Qgyro = 0; Qaccel = 0; % initial T=fscanf(fpIMU,'%g',1); Lat=fscanf(fpIMU,'%g',1); Lon=fscanf(fpIMU,'%g',1); Hei=fscanf(fpIMU,'%g',1); Vx=fscanf(fpIMU,'%g',1); Vy=fscanf(fpIMU,'%g',1); Vz=fscanf(fpIMU,'%g',1); fai=fscanf(fpIMU,'%g',1); theta=fscanf(fpIMU,'%g',1); gama=fscanf(fpIMU,'%g',1); % Add Initial Error in Attitude fai = fai+5/3600/180*pi; theta = theta+5/3600/180*pi; gama = gama+5/3600/180*pi; %Initial Attitude Transformation Matrix from enu frame to boday frame (x(rihgt), % y(fwd), z(up)) Cnb = [ cos(gama)*cos(fai)+sin(gama)*sin(theta)*sin(fai), -cos(gama)*sin(fai) +sin(gama)*sin(theta)*cos(fai), -sin(gama)*cos(theta); cos(theta)*sin(fai), cos(theta)*cos(fai), sin(theta); sin(gama)*cos(fai)-cos(gama)*sin(theta)*sin(fai), -sin(gama)*sin(fai)cos(gama)*sin(theta)*cos(fai), cos(gama) * cos(theta)]; Cbn=Cnb'; q(2) = sqrt(abs(1 + Cnb(1,1) - Cnb(2,2) - Cnb(3,3))) / 2; q(3) = sqrt(abs(1 - Cnb(1,1) + Cnb(2,2) - Cnb(3,3))) / 2; q(4) = sqrt(abs(1 - Cnb(1,1) - Cnb(2,2) + Cnb(3,3))) / 2; q(1) = sqrt(abs(1 - q(2) * q(2) - q(3) * q(3) - q(4) * q(4))); if (Cnb(2,3) < Cnb(3,2)) q(2) = - q(2); end if (Cnb(3,1) < Cnb(1,3)) q(3) = - q(3); end if (Cnb(1,2) < Cnb(2,1)) q(4) = - q(4); end q=q'; %Read The output of the gyroscope and the accelerometer without any noise IMU = fscanf(fpIMU, '%f', [6, inf]); CLXXVI
gx = IMU(1,:)'/T; gy = IMU(2,:)'/T; gz = IMU(3,:)'/T; ax = IMU(4,:)'/T; ay = IMU(5,:)'/T; az = IMU(6,:)'/T; num_sins=length(gx); % the output of the gyroscope and the accelerrometer with the noise gx=gx+Qgyro+0*randn(num_sins,1); gy=gy+Qgyro+0*randn(num_sins,1); gz=gz+Qgyro+0*randn(num_sins,1); ax=ax+Qaccel+0*randn(num_sins,1); ay=ay+Qaccel+0*randn(num_sins,1); az=az+Qaccel+0*randn(num_sins,1); gyro=[gx';gy';gz']; acc=[ax';ay';az']; IMUlength = num_sins; Att_s=zeros(3,IMUlength*T-T); VV_s=zeros(3,IMUlength*T-T); Pos_s=zeros(3,IMUlength*T-T); Time=0 for i=1:IMUlength Time=Time+T; if rem(i,10000)==0; Time end Wibb=gyro(:,i); fb=acc(:,i); %%%%%%%%%%%%SINS%%%%%%%%%%%%%%%% Rmh = Re * (1 - 2 * e + 3 * e * sin(Lat) * sin(Lat)) + Hei; Rnh = Re * (1 + e * sin(Lat) * sin(Lat)) + Hei; g_s = g0 * (1 + 0.00193185138639 * sin(Lat) * sin(Lat)) / sqrt(1 - 0.00669437999013 * sin(Lat) * sin(Lat)) * (1 - 2 * Hei / Re); Wien = [ 0; WIEE * cos(Lat); WIEE * sin(Lat)]; Wenn = [ -Vy / Rmh; Vx / Rnh; Vx * tan(Lat) / Rnh]; Winn = Wien + Wenn; Winb = Cnb * Winn; % wibb=size(Wibb) % winb=size(Winb) CLXXVII
angle = (Wibb - Winb) * T; fn = Cnb' * fb; difVx = fn(1) + (2 * WIEE * sin(Lat) + Vx * tan(Lat) / Rnh) * Vy - (2 * WIEE * cos(Lat) + Vx / Rnh) * Vz; difVy = fn(2) - (2 * WIEE * sin(Lat) + Vx * tan(Lat) / Rnh) * Vx - Vy * Vz / Rmh; difVz = fn(3) + (2 * WIEE * cos(Lat) + Vx / Rnh) * Vx + Vy * Vy /Rmh - g_s; Vx = difVx * T + Vx; Vy = difVy * T + Vy; Vz = difVz * T + Vz; Lat = Lat + Vy * T / Rmh; Lon = Lon + Vx * T / Rnh / cos(Lat); Hei = Hei + Vz * T; M = [0, -angle(1), -angle(2), -angle(3); angle(1), 0, angle(3), -angle(2); angle(2), -angle(3), 0, angle(1); angle(3), angle(2), -angle(1), 0]; q = (cos(norm(angle) / 2) * eye(4) + sin(norm(angle) / 2) / norm(angle) * M) * q; q = q / norm(q); Cnb = [ q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4), 2*(q(2)*q(3)+q(1)*q(4)), 2*(q(2)*q(4)-q(1)*q(3)); 2*(q(2)*q(3)-q(1)*q(4)), q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4), 2*(q(3)*q(4)+q(1)*q(2)); 2*(q(2)*q(4)+q(1)*q(3)), 2*(q(3)*q(4)-q(1)*q(2)), q(1)*q(1)-q(2)*q(2)q(3)*q(3)+q(4)*q(4)]; fai = atan(Cnb(2,1) / Cnb(2,2)); theta = asin(Cnb(2,3)); gama = atan(-Cnb(1,3) / Cnb(3,3)); if (Cnb(2,2) < 0) if Cnb(2,1)>0 fai = fai + pi; else fai=fai-pi; end end if (Cnb(3,3) < 0) CLXXVIII
if(gama < 0) gama = gama +pi; else gama = gama - pi; end end % Solution of Pure SINS if (mod(i,1/T) < 5) jj=jj+1; Att_s(:,jj)=[fai;theta;gama]; VV_s(:,jj)=[Vx;Vy;Vz]; Pos_s(:,jj)=[Lat*180/pi;Lon*180/pi;Hei]; end e Att_s=Att_s*180/pi*3600; a=size(Att_s); Att_s(:,a(1,2))=[]; VV_s(:,a(1,2))=[]; Pos_s(:,a(1,2))=[]; Er_Vec=[Att_s'-Att_m' VV_s'-VV_m' Pos_s'-Pos_m']; % Er_Vec(1871,1)=Er_Vec(1870,1); % Er_Vec(1437:1439,1)=Er_Vec(1434:1436,1); % Er_Vec(3425:3430,1)=Er_Vec(3420:3425,1); % Er_Vec(1469:4071,1)=Er_Vec(1466:4068,1); % figure(1); % subplot(311), % % plot(Att_m(1,:)/3600,'r','LineWidth',2.5); hold on; % plot(Att_s(1,:)/3600,'b','LineWidth',2.5); title('Attitude [\circ]','fontsize',14,'fontweight','b'); grid on % ylabel('\psi \rightarrow','fontsize',14,'fontweight','b'); % LEG=legend('Ideal value', 'Estimated Value','location','NorthEast'); % set(LEG,'fontsize',14); % subplot(312), % % plot(Att_m(2,:)/3600,'r','LineWidth',2.5);hold on; % plot(Att_s(2,:)/3600,'b','LineWidth',2.5); grid on % ylabel('\theta \rightarrow','fontsize',14,'fontweight','b'); % subplot(313), % % plot(Att_m(3,:)/3600,'r','LineWidth',2.5);hold on; CLXXIX
% plot(Att_s(3,:)/3600,'b','LineWidth',2.5); grid on % ylabel('\gamma \rightarrow','fontsize',14,'fontweight','b'); % xlabel('Time [sec] \rightarrow','fontsize',14,'fontweight','b'); % figure(2); % subplot(311), % % plot(VV_m(1,:),'r','LineWidth',2.5); hold on; % plot(VV_s(1,:),'b','LineWidth',2.5); title('Velociy (m/S)','fontsize',14,'fontweight','b'); grid on % ylabel('V_E \rightarrow','fontsize',14,'fontweight','b'); % % LEG=legend('Ideal value', 'Estimated Value','location','NorthEast'); % % set(LEG,'fontsize',14); % subplot(312), % % plot(VV_m(2,:),'r','LineWidth',2.5);hold on; % plot(VV_s(2,:),'b','LineWidth',2.5); grid on % ylabel('V_N \rightarrow','fontsize',14,'fontweight','b'); % subplot(313), % % plot(VV_m(3,:),'r','LineWidth',2.5);hold on; % plot(VV_s(3,:),'b','LineWidth',2.5); grid on % ylabel('V_U \rightarrow','fontsize',14,'fontweight','b'); % xlabel('Time [sec] \rightarrow','fontsize',14,'fontweight','b'); % figure(3); % subplot(311), % % plot(Pos_m(1,:),'r','LineWidth',2.5);hold on; % plot(Pos_s(1,:),'b','LineWidth',2.5); title('Position','fontsize',14,'fontweight','b'); grid on % ylabel('Latitude (\circ) \rightarrow','fontsize',14,'fontweight','b'); % % LEG=legend('Ideal value', 'Estimated Value','location','NorthEast'); % % set(LEG,'fontsize',14); % subplot(312), % % plot(Pos_m(2,:),'r','LineWidth',2.5);hold on; % plot(Pos_s(2,:),'b','LineWidth',2.5); grid on % ylabel('Longitude (\circ) \rightarrow','fontsize',14,'fontweight','b'); % subplot(313), % % plot(Pos_m(3,:),'r','LineWidth',2.5);hold on; % plot(Pos_s(3,:),'b','LineWidth',2.5); grid on % ylabel('h (m) \rightarrow','fontsize',14,'fontweight','b'); % xlabel('Time [sec] \rightarrow','fontsize',14,'fontweight','b'); % % % % % Plotting of Only Errors % % figure(4); CLXXX
% % subplot(311),plot(Er_Vec(:,1),'b','LineWidth',2.5); title('Attitude Error Arc Sec','fontsize',14,'fontweight','b'); grid on % % ylabel('\phi_z \bf[\prime\prime] \rightarrow','fontsize',14,'fontweight','b'); % % subplot(312),plot(Er_Vec(:,2),'b','LineWidth',2.5'); grid on % % ylabel('\phi_x \bf[\prime\prime] \rightarrow','fontsize',14,'fontweight','b'); % % subplot(313),plot(Er_Vec(:,3),'b','LineWidth',2.5); grid on % % ylabel('\phi_y \bf[\prime\prime] \rightarrow','fontsize',14,'fontweight','b'); % % xlabel('Time [sec] \rightarrow','fontsize',14,'fontweight','b'); %% % % figure(5); % % subplot(311),plot(Er_Vec(:,4),'b','LineWidth',2.5); title('Velocity Error [m/s] ','fontsize',14,'fontweight','b'); grid on % % ylabel('\deltaV_E \rightarrow','fontsize',14,'fontweight','b'); % % subplot(312),plot(Er_Vec(:,5),'b','LineWidth',2.5); grid on % % ylabel('\deltaV_N \rightarrow','fontsize',14,'fontweight','b'); % % subplot(313),plot(Er_Vec(:,6),'b','LineWidth',2.5); grid on % % ylabel('\deltaV_U \rightarrow','fontsize',14,'fontweight','b'); % % xlabel('Time [sec] \rightarrow','fontsize',14,'fontweight','b'); %% % % figure(6); % % subplot(311),plot(Er_Vec(:,7)*pi/180*Re,'b','LineWidth',2.5); title('Position Error (m)','fontsize',14,'fontweight','b'); grid on % % ylabel('\delta(Lat)\rightarrow','fontsize',14,'fontweight','b'); % % subplot(312),plot(Er_Vec(:,8)*pi/180*Re,'b','LineWidth',2.5);grid on % % ylabel('\delta(Lon)\rightarrow','fontsize',14,'fontweight','b'); % % subplot(313),plot(Er_Vec(:,9),'b','LineWidth',2.5); grid on % % ylabel('\delta(h) \rightarrow','fontsize',14,'fontweight','b'); % % xlabel('Time [sec] \rightarrow','fontsize',14,'fontweight','b'); %% % % figure(7); plot3(Pos_m(1,:),Pos_m(2,:),Pos_m(3,:),'b','LineWidth',2.5);hold on; plot3(Pos_s(1,:),Pos_s(2,:),Pos_s(3,:),'r','LineWidth',2.5); grid on title('Standalone INS Trajectory','fontsize',18,'fontweight','b') xlabel('Latitude (\circ) ','fontsize',14,'fontweight','b'); ylabel('Longitude (\circ)','fontsize',14,'fontweight','b'); zlabel('Altitude (m)','fontsize',14,'fontweight','b'); LEG=legend('Ideal Trajectory', 'Estimated Trajectory','location','NorthEast'); set(LEG,'fontsize',14); CLXXXI
fid2 = fopen('Pos.txt','a'); % creating file format2='%12.7f %8.7f %8.7f \r\n'; fprintf(fid2 ,format2,Pos_s); fclose(fid2); fid3 = fopen('Att.txt','a'); % creating file format3='%12.7f %8.7f %8.7f \r\n'; fprintf(fid3 ,format3,Att_s); fclose(fid3); fid4 = fopen('vel.txt','a'); % creating file format4='%12.7f %8.7f %8.7f \r\n'; fprintf(fid4 ,format4,VV_s); fclose(fid4); toc; 10.1.3
KALMAN FILTER CODE
%******************INS/GNSS Inegration by Kalmn filter********************* % Body Frame X(right) Y(forward) Z(Up) % Navigation Frame X(East) Y(North) Z(Up) clc; clear all; close all; tic % Start Timer %******************OPENING FILES TO GET INPUT****************************** fpIMU =fopen('F:\IST LECTURES\FYP\navdata.dat','r'); %output of IMU fpGPS =fopen('F:\IST LECTURES\FYP\realgps.dat','r'); %output of GPS reciever %***************************INITIALIZATION**************************** ***** %--------------------------CONSTANTS--------------------------------------WIEE = 0.000072921151467; %rate of rotation of Earth Re = 6378135.072; % radius of earth g0 = 9.7803267714; %gravitational acceleration at SL e = 1/298.25; %ellipsoid eccentricity jj=0; % iterations TGPS = 1/5; %Time of GPS %-----------------READING INITIAL NAVIGATION SOLUTION---------------------CLXXXII
%Time T=fscanf(fpIMU,'%g',1);
%time
%position Lat=fscanf(fpIMU,'%g',1); Lon=fscanf(fpIMU,'%g',1); Hei=fscanf(fpIMU,'%g',1);
%latutude %longitude %altutude
%velocity Vx=fscanf(fpIMU,'%g',1); Vy=fscanf(fpIMU,'%g',1); Vz=fscanf(fpIMU,'%g',1);
%Vx %Vy %Vz
%Attitude fai=fscanf(fpIMU,'%g',1); %roll theta=fscanf(fpIMU,'%g',1); %pitch gama=fscanf(fpIMU,'%g',1); %yaw %-----------------------INITIAL ERROR STATE VECTOR ----------------------%state vector will comprises of 15 elements % 3 attitude errors (roll, pitch, yaw) % 3 velocity errors (Vx, Vy, Vz) % 3 position errors (lat, long, height) % 3 errors due to gyro drift along 3 axis % 3 errors due to accelerometer bias along 3 axis x_f = zeros(15,1); %Attitude errors P0fai = 1/180*pi; %roll P0theta = 1/180*pi; %pitch P0gama = 1/180*pi; %yaw %INS Velocity errors P0v = 0.1; %INS Position errors P0LatLon = 50/Re; %error in lat and long P0h = 50; %error in height %Gyros errors P0gyro = (0.3*(pi/180))/3600; %constant drift Qgyro = (0.5*(pi/180))/3600; %random drift % Accelerometer errors CLXXXIII
P0accel = 200E-6*g0; %constant bias Qaccel = 150E-6*g0; %random bias %GPS Velocity errors Rv = 0.05; %GPS Position errors RLatLon = 25/Re; %error in lat and long Rh = 25; %error in height %----------------INITIAL ERROR STATE COVARIANCE MATRIX--------------------P_f = diag([P0theta^2,P0gama^2,P0fai^2,P0v^2,P0v^2,P0v^2,P0LatLon^2,P0LatLon^2,P0h^2, ... P0gyro^2,P0gyro^2,P0gyro^2,P0accel^2,P0accel^2,P0accel^2]); %-------------------SYSTEM NOISE COVARIANCE MATRIX-----------------------Q = diag([Qgyro^2,Qgyro^2,Qgyro^2,Qaccel^2,Qaccel^2,Qaccel^2]); %----------------MEASUREMENT NOISE COVARIANCE MATRIX----------------------R = diag([Rv^2,Rv^2,Rv^2,(RLatLon*Re)^2,(RLatLon*Re)^2,Rh^2]); R=R/TGPS; %-----------------------ERRANEOUS ATTITUDE--------------------------------fai = fai+60/3600/180*pi; theta = theta+20/3600/180*pi; gama = gama+20/3600/180*pi; %-----------------------QUATERNION INITIALIZATION-------------------------Cnb = [ cos(gama)*cos(fai)+sin(gama)*sin(theta)*sin(fai), -cos(gama)*sin(fai) +sin(gama)*sin(theta)*cos(fai), -sin(gama)*cos(theta); cos(theta)*sin(fai), cos(theta)*cos(fai), sin(theta); sin(gama)*cos(fai)-cos(gama)*sin(theta)*sin(fai), -sin(gama)*sin(fai)cos(gama)*sin(theta)*cos(fai), cos(gama) * cos(theta)]; Cbn=Cnb'; q(2) = sqrt(abs(1 + Cnb(1,1) - Cnb(2,2) - Cnb(3,3))) / 2; q(3) = sqrt(abs(1 - Cnb(1,1) + Cnb(2,2) - Cnb(3,3))) / 2; q(4) = sqrt(abs(1 - Cnb(1,1) - Cnb(2,2) + Cnb(3,3))) / 2; q(1) = sqrt(abs(1 - q(2) * q(2) - q(3) * q(3) - q(4) * q(4))); if (Cnb(2,3) < Cnb(3,2)) q(2) = - q(2); end if (Cnb(3,1) < Cnb(1,3)) q(3) = - q(3); end if (Cnb(1,2) < Cnb(2,1)) q(4) = - q(4); CLXXXIV
end q=q'; %-----------------------READING OUTPUT OF IMU-----------------------------%Noise Free output IMU = fscanf(fpIMU, '%f', [6, inf]); gx gy gz ax ay az
= IMU(1,:)'/T; = IMU(2,:)'/T; = IMU(3,:)'/T; = IMU(4,:)'/T; = IMU(5,:)'/T; = IMU(6,:)'/T;
num_sins=length(gx); %Noisy output gx=gx+Qgyro+Qgyro/2*randn(num_sins,1); gy=gy+Qgyro+Qgyro/2*randn(num_sins,1); gz=gz+Qgyro+Qgyro/2*randn(num_sins,1); ax=ax+Qaccel+Qaccel/2*randn(num_sins,1); ay=ay+Qaccel+Qaccel/2*randn(num_sins,1); az=az+Qaccel+Qaccel/2*randn(num_sins,1); gyro=[gx';gy';gz']; acc=[ax';ay';az']; %-----------------------READING OUTPUT OF GPS-----------------------------%Noise Free output GPS=fscanf(fpGPS ,'%f',[7, inf]); Vx_GPS=GPS(2,:)'; Vy_GPS=GPS(3,:)'; Vz_GPS=GPS(4,:)'; Lat_GPS=GPS(5,:)'; Lon_GPS=GPS(6,:)'; Hei_GPS=GPS(7,:)'; num_gps=length(Vx_GPS); %Noisy output Vx_GPS = Vx_GPS + Rv*randn(num_gps,1); Vy_GPS = Vy_GPS + Rv*randn(num_gps,1); Vz_GPS = Vz_GPS + Rv*randn(num_gps,1); Lat_GPS = Lat_GPS + RLatLon*randn(num_gps,1); Lon_GPS = Lon_GPS + RLatLon*randn(num_gps,1); CLXXXV
Hei_GPS = Hei_GPS +Rh*randn(num_gps,1); IMUlength = size(gx,1); T_kf =0; %***************************INS ALGORITHAM********************************* %*******************************INPUT********************************* ***** %output of accelerometer=specific force %output of gyro=roatation rate of body in body frame %*******************************OUTPUT******************************* ****** %Velocity matrix %Position matrix %Attitude matrix %*******************************PROCESS****************************** ****** %1.Radius and gravity corrections are made. %2.conversion of 'gyro output to angle of body' and 'accelro output from body to nav frame'. %3.specific force update is done %4.Velocity update is done %5.Position update is done %6.Attitude update is done %7.Difference b/w velocity and attitude of INS and GPS is calculated %8.This difference is fed to kalman filter %9.Kalman filter gives corrected navigation solution %10. This corrected solution is used to update nav solution in INS algo %11. Thus cycle contines %-------------------------------------------------------------------------for i=1:IMUlength %reading acceleros and gyros output Wibb=gyro(:,i); %turn rate of body w.r.t inertial frame in body frame = [gx gy gz]' fb=acc(:,i); %specific force in body frame=[fx fy fz]' %Crrections Rmh = Re * (1 - 2 * e + 3 * e * sin(Lat) * sin(Lat)) + Hei; radius from north to south CLXXXVI
%meridian radius or
Rnh = Re * (1 + e * sin(Lat) * sin(Lat)) + Hei; %east-west radius of curvature of earth g_s = g0 * (1 + 0.00193185138639 * sin(Lat) * sin(Lat)) / sqrt(1 - 0.00669437999013 * sin(Lat) * sin(Lat)) * (1 - 2 * Hei / Re); %gravity correction %conversion of 'turn rate of body w.r.t inertial frame' to 'angle of body w.r.t navigation frame' Wien = [ 0; WIEE * cos(Lat); WIEE * sin(Lat)]; %Turn rate of EARTH w.r.t inertial frame in nvigation frame Wenn = [ -Vy / Rmh; Vx / Rnh; Vx * tan(Lat) / Rnh]; %Turn rate of NAVIGATION FRAME w.r.r to earth in navigation frame= transport rate Winn = Wien + Wenn; %Turn rate of NAVIGATION FRAME w.r.r to inertial frame in navigation frame Winb = Cnb * Winn; %Turn rate of NAVIGATION FRAME w.r.r to inertial frame in body frame Wnbb=(Wibb - Winb); %Turn rate of BODY FRAME w.r.r to navigation frame in body frame angle = Wnbb * T; %angle of body in body frame %conversion of 'specific in body frame' to 'specific force in navigation frame' fn = Cnb' * fb; %specific force update difVx = fn(1) + (2 * WIEE * sin(Lat) + Vx * tan(Lat) / Rnh) * Vy - (2 * WIEE * cos(Lat) + Vx / Rnh) * Vz; difVy = fn(2) - (2 * WIEE * sin(Lat) + Vx * tan(Lat) / Rnh) * Vx - Vy * Vz / Rmh; difVz = fn(3) + (2 * WIEE * cos(Lat) + Vx / Rnh) * Vx + Vy * Vy /Rmh - g_s; %velocity update Vx = difVx * T + Vx; Vy = difVy * T + Vy; Vz = difVz * T + Vz; %position update Lat = Lat + Vy * T / Rmh; Lon = Lon + Vx * T / Rnh / cos(Lat); Hei = Hei + Vz * T; %quaternion update for attitude computation M = [0, -angle(1), -angle(2), -angle(3); angle(1), 0, angle(3), -angle(2); angle(2), -angle(3), 0, angle(1); angle(3), angle(2), -angle(1), 0]; q = (cos(norm(angle) / 2) * eye(4) + sin(norm(angle) / 2) / norm(angle) * M) * q; q = q / norm(q); CLXXXVII
Cnb = [ q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4), 2*(q(2)*q(3)+q(1)*q(4)), 2*(q(2)*q(4)-q(1)*q(3)); 2*(q(2)*q(3)-q(1)*q(4)), q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4), 2*(q(3)*q(4)+q(1)*q(2)); 2*(q(2)*q(4)+q(1)*q(3)), 2*(q(3)*q(4)-q(1)*q(2)), q(1)*q(1)-q(2)*q(2)q(3)*q(3)+q(4)*q(4)]; fai = atan(Cnb(2,1) / Cnb(2,2)); theta = asin(Cnb(2,3)); gama = atan(-Cnb(1,3) / Cnb(3,3)); if (Cnb(2,2) < 0) if Cnb(2,1)>0 fai = fai + pi; else fai=fai-pi; end end if (Cnb(3,3) < 0) if(gama < 0) gama = gama +pi; else gama = gama - pi; end end %********************KALMAN FILTER ALGORITHAM****************************** %============================================================= %-------------------------------------------------------------------------%****************************PREDICTION****************************** ****** % Xp(k)=F*X(k-1) + G*W(k) PREDICTED STATE VECTOR % Pp(k)=F*P(k-1)*F' +Qk PREDICTED PROCESS COVARIANCE MATRIX %******************************UPDATE******************************** ****** % Z(k)=H*X(k+1)+V(k) MEASUREMENT OF STATE % K(k)=Pp(k)*H'*inv[H*Pp(k)*H'+R] KALMAN GAIN % X(k)=Xp(k)-K(k)*[Z(k)-H*Xp(k)] UPDATED STATE VECTOR % P(k)=[I-K(k)*H]*Pp(k) UPDATED PROCESS COVARIANCE MATRIX
CLXXXVIII
%********************DEFINITION OF VECTORS********************************* % F=Transition matrix of 15x15 % G=Transition matrix of 15x15 % W=Control variable matrix % Q=Process Nosise covariance matrix % V=Measurement noise covariance matrix % R=Sensor noise covariance matrix % H=Transition matrix of 6x15 % F and G matrices will be defined and descritized into PHI and Q respectively % Q and R matrices are already defined % W is not considered % Z is directly defined, so V not required % H will be defined %-------------------------------------------------------------------------if (mod(i,TGPS/T) == 0) T_kf = T_kf + 1; %--------------------INITIALIZATION OF 'H' MATRIX-------------------------Hv = [zeros(3,3),eye(3,3),zeros(3,9)]; Hp = [zeros(3,6),diag([Rmh,Rnh*cos(Lat),1]),zeros(3,6)]; H = [Hv;Hp]; %--------------------INITIALIZATION OF 'G' MATRIX-------------------------G = [Cnb',zeros(3,3);zeros(3,3),Cnb';zeros(9,3),zeros(9,3)]; %--------------------INITIALIZATION OF 'F' MATRIX-------------------------%FN FN=zeros(9,9); F11=[0,WIEE * sin(Lat) + Vx / Rnh * tan(Lat),-WIEE * cos(Lat) - Vx / Rnh; -(WIEE * sin(Lat) + Vx / Rnh * tan(Lat)),0,- Vy/ Rmh; WIEE * cos(Lat)+ Vx / Rnh,Vy/ Rmh,0]; F12=[0,-1/Rmh,0;1/ Rnh,0,0; tan(Lat)/Rnh,0,0]; F13=[0,0,Vy/(Rmh^2);-WIEE * sin(Lat),0,-Vx / (Rnh^2);(WIEE * cos(Lat) +Vx/Rnh/cos(Lat)/cos(Lat)),0,-Vx / (Rnh^2) * tan(Lat)]; F21=[0,-fn(3),fn(2);fn(3),0,-fn(1);-fn(2),fn(1),0]; F22=[1.0 / Rnh * (Vy * tan(Lat) - Vz),2.0 * WIEE * sin(Lat) + Vx / Rnh * tan(Lat),-2.0 * WIEE * cos(Lat) - Vx / Rnh; -2.0 * WIEE * sin(Lat) - 2.0 * Vx / Rnh * tan(Lat),-Vz / Rmh,-Vy / Rmh; CLXXXIX
2.0 * WIEE * cos(Lat) + 2.0 * Vx / Rnh,2.0 * Vy / Rmh,0]; F23=[2.0 * WIEE * cos(Lat) * Vy + 2.0 * WIEE * sin(Lat) * Vz + Vx * Vy / Rnh / cos(Lat) / cos(Lat),0,1* (Vx * Vz - Vx * Vy * tan(Lat)) / Rnh / Rnh; -Vx * (2.0 * WIEE * cos(Lat) + Vx / Rnh / cos(Lat) / cos(Lat)),0,(Vy * Vz + Vx * Vx * tan(Lat)) / Rnh / Rnh; -2.0 * WIEE * sin(Lat) * Vx,0, -(Vx * Vx + Vy * Vy) / Rnh / Rnh]; F31=zeros(3,3); F32=[0,1.0 / Rmh,0;1.0 / Rnh / cos(Lat),0,0;0,0,1]; F33=[0,0,-Vy / Rmh / Rmh;tan(Lat) / Rnh / cos(Lat) * Vx,0,-Vx / Rnh / Rnh / cos(Lat);0,0,0]; FN=[F11 F12 F13 F21 F22 F23 F31 F32 F33]; %FS FS = [Cnb' zeros(3,3) zeros(3,3) Cnb' zeros(3,3) zeros(3,3) ]; %F F = [ FN FS zeros(6,9) zeros(6,6) % Discretization F = F * TGPS; temp1 = eye(15); PHI_k = eye(15); for j = 1:10 temp1 = F * temp1 / j; PHI_k = PHI_k + temp1; end GQG = G * Q * G'; temp1 = GQG * TGPS; Q_k = temp1; for j = 2:11 temp2 = F * temp1; temp1 = (temp2 + temp2')/j; Q_k = Q_k + temp1; end % Measurement Vector %---------------------------MEASUREMENT VECTOR----------------------------CXC
Z = [Vx-Vx_GPS(T_kf);Vy-Vy_GPS(T_kf);Vz-Vz_GPS(T_kf);(LatLat_GPS(T_kf))*Rmh;(Lon-Lon_GPS(T_kf))*Rnh*cos(Lat);Hei-Hei_GPS(T_kf)]; %--------------------------KALMAN FILTER CYCLE-------------------------%PREDICTION x_p = PHI_k*x_f; % Predicted Error State Vector P_p = PHI_k*P_f*PHI_k'+Q_k; % Predicted Value of Error State Cov Matrix P_p=(P_p+P_p')/2; % This process is performed to make the matrix symmetric K = P_p*H'*(inv( H*P_p*H' + R )); % Computation of Kalman Gain %UPDATE x_f = x_p + K*( Z - H*x_p ); % Measurement Update of Error State Vector P_f = ( eye(15) - K*H )*P_p*( eye(15) - K*H )' + K*R*K'; % Measurement Update of Error Covariance Matrix P_f =(P_f +P_f')/2; % This process is performed to make the matrix symmetric %-----------------------CORRECTED NAVIGATION SOLUTION---------------------%Velocity Vx = Vx - x_f(4); %Corrected East Velocity Vy = Vy - x_f(5); %Corrected North Velocity Vz = Vz - x_f(6); %Corrected Vertical (up) Velocity %Psition Lat = Lat - x_f(7); % Corrected Latitude Lon = Lon - x_f(8); % Corrected Longitude Hei = Hei - x_f(9); % Corrected Height %Attitude Atheta = x_f(1); Agama = x_f(2); Afai = x_f(3);
%Pitch Error Estimate %Roll Error estimate %Yaw Error estimate
%Attitude Correction Matrix (Small angle approximation) Ctn=[1 Afai -Agama -Afai 1 Atheta Agama -Atheta 1]; Cnb= Cnb*Ctn; %Computation of Corrected Attitude fai = atan(Cnb(2,1) / Cnb(2,2)); %Corrected Roll theta = asin(Cnb(2,3)); %Corrected Pitch gama = atan(-Cnb(1,3) / Cnb(3,3)); %Corrected Yaw CXCI
%To avoid sigularity if (Cnb(2,2) < 0) if Cnb(2,1)>0 fai = fai + pi; else fai=fai-pi; end end if (Cnb(3,3) < 0) if(gama < 0) gama = gama +pi; else gama = gama - pi; end end % Computation of Correctd Quaternion q(2) = sqrt(abs(1 + Cnb(1,1) - Cnb(2,2) - Cnb(3,3))) / 2; q(3) = sqrt(abs(1 - Cnb(1,1) + Cnb(2,2) - Cnb(3,3))) / 2; q(4) = sqrt(abs(1 - Cnb(1,1) - Cnb(2,2) + Cnb(3,3))) / 2; q(1) = sqrt(abs(1 - q(2) * q(2) - q(3) * q(3) - q(4) * q(4))); if (Cnb(2,3) < Cnb(3,2)) q(2) = - q(2); end if (Cnb(3,1) < Cnb(1,3)) q(3) = - q(3); end if (Cnb(1,2) < Cnb(2,1)) q(4) = - q(4); end x_f(1:9) = 0; %velocity and attitude errors are set to be zero.Why? %----------------------STORING NAVIGATION SOLUTION------------------------VV_s(:,T_kf)=[Vx;Vy;Vz]; Att_s(:,T_kf)=[fai;theta;gama]; Pos_s(:,T_kf)=[Lat;Lon;Hei]; end % This is the end of Kalman Filtering end % This is the end of main navigation cycle % Values converted from radian to Arc Seconds
CXCII
Att_s=Att_s*180/pi*3600; Pos_s=[(Pos_s(1,:))*180/pi; (Pos_s(2,:))*180/pi; (Pos_s(3,:))]; %*****************Getting stanalone INS and GNSS Solution****************** V_GPS=[Vx_GPS Vy_GPS Vz_GPS]; Pos_GPS=[(Lat_GPS*180/pi) (Lon_GPS*180/pi) Hei_GPS]; load Pos.txt lat_ins=Pos(:,1); lon_ins=Pos(:,2); h_ins=Pos(:,3); load Att.txt yaw_ins=Att(:,1); pitch_ins=Att(:,2); roll_ins=Att(:,3); load vel.txt Ve_ins=vel(:,1); Vn_ins=vel(:,2); Vu_ins=vel(:,3); %**************************PLOTTING********************************** ***** %------------------------PLOTTING ATTITUDE--------------------------------figure(1) %yaw subplot(311), plot(Att_s(1,:)/3600,'r','LineWidth',2.5); hold on; plot(yaw_ins/3600,'b','LineWidth',2.5); title('Attitude (\circ)','fontsize',14,'fontweight','b'); grid on ylabel('\psi \rightarrow','fontsize',14,'fontweight','b'); LEG=legend('Integrated', 'standalone INS','location','NorthEast'); set(LEG,'fontsize',14); %pitch subplot(312), plot(Att_s(2,:)/3600,'r','LineWidth',2.5);hold on; plot(pitch_ins/3600,'b','LineWidth',2.5); grid on ylabel('\theta \rightarrow','fontsize',14,'fontweight','b'); %roll subplot(313),plot(Att_s(3,:)/3600,'r','LineWidth',2.5);hold on; plot(roll_ins/3600,'b','LineWidth',2.5); grid on ylabel('\gamma \rightarrow','fontsize',14,'fontweight','b'); xlabel('Time (sec) \rightarrow','fontsize',14,'fontweight','b'); %------------------------PLOTTING VELOCITY--------------------------------figure(2) %V_E subplot(311), CXCIII
plot(VV_s(1,:),'r','LineWidth',2.5); hold on; plot(V_GPS(:,1),'y','LineWidth',2.5);hold on; plot(Ve_ins,'b','LineWidth',2.5); grid on title('Velociy (m/S)','fontsize',14,'fontweight','b'); grid on ylabel('V_E \rightarrow','fontsize',14,'fontweight','b'); LEG=legend('integrated', 'standalone GNSS','standalone INS','location','NorthEast'); set(LEG,'fontsize',14); %V_N subplot(312), plot(VV_s(2,:),'r','LineWidth',2.5); hold on; plot(V_GPS(:,2),'y','LineWidth',2.5);hold on; plot(Vn_ins,'b','LineWidth',2.5);grid on ylabel('V_N \rightarrow','fontsize',14,'fontweight','b'); %V_u subplot(313), plot(VV_s(3,:),'r','LineWidth',2.5); hold on; plot(V_GPS(:,3),'y','LineWidth',2.5);hold on; plot(Vu_ins,'b','LineWidth',2.5);grid on ylabel('V_U \rightarrow','fontsize',14,'fontweight','b'); xlabel('Time (sec) \rightarrow','fontsize',14,'fontweight','b'); %------------------------PLOTTING POSITION--------------------------------figure(3) %Latitude subplot(311), plot(Pos_s(1,:),'r','LineWidth',2.5); hold on; plot(Pos_GPS(:,1),'y','LineWidth',2.5);hold on; plot(lat_ins,'b','LineWidth',2.5) title('Position','fontsize',14,'fontweight','b'); grid on ylabel('Latitude (\circ)','fontsize',14,'fontweight','b'); LEG=legend('Integrated', 'Standalone GNSS','standalone INS','location','NorthEast'); set(LEG,'fontsize',14); %longitude subplot(312), plot(Pos_s(2,:),'r','LineWidth',2.5); hold on; plot(Pos_GPS(:,2),'y','LineWidth',2.5);hold on; plot(lon_ins,'b','LineWidth',2.5);grid on ylabel('Longitude (\circ)','fontsize',14,'fontweight','b'); %height subplot(313), plot(Pos_s(3,:),'r','LineWidth',2.5); hold on; plot(Pos_GPS(:,3),'y','LineWidth',2.5);hold on; plot(h_ins,'b','LineWidth',2.5);grid on ylabel('h (m)','fontsize',14,'fontweight','b'); xlabel('Time (sec) \rightarrow','fontsize',14,'fontweight','b');
CXCIV
%------------------------PLOTTING TRAJECTORY------------------------------figure(4) plot3(Pos_s(1,:),Pos_s(2,:),Pos_s(3,:),'b','LineWidth',2.5);hold on; plot3(Pos_GPS(:,1),Pos_GPS(:,2),Pos_GPS(:,3),'y','LineWidth',2.5);hold on plot3(lat_ins, lon_ins, h_ins,'r','LineWidth',2.5);grid on title('Trajectory','fontsize',18,'fontweight','b') xlabel('Latitude (\circ) ','fontsize',14,'fontweight','b'); ylabel('Longitude (\circ)','fontsize',14,'fontweight','b'); zlabel('Altitude (m)','fontsize',14,'fontweight','b'); LEG=legend('Integrated', 'standalone GNSS','standalone INS','location','NorthEast'); %---------------------PLOTTING INS POSITION ERRORS------------------------figure(5) subplot(311), plot(Pos_s(1,:)'-lat_ins,'r','LineWidth',2.5); hold on; title('INS Position Error','fontsize',14,'fontweight','b'); grid on ylabel('Latitude error','fontsize',14,'fontweight','b'); subplot(312), plot(Pos_s(2,:)'-lon_ins,'r','LineWidth',2.5); grid on; ylabel('Longitude error','fontsize',14,'fontweight','b'); subplot(313), plot(Pos_s(3,:)'-h_ins,'r','LineWidth',2.5); grid on; ylabel('height error','fontsize',14,'fontweight','b'); xlabel('Time (sec) \rightarrow','fontsize',14,'fontweight','b'); %---------------------PLOTTING INS VELOCITY ERRORS------------------------figure(6) subplot(311), plot(VV_s(1,:)'-Ve_ins,'r','LineWidth',2.5); grid on; title('INS Velociy error','fontsize',14,'fontweight','b'); grid on ylabel('V_E error','fontsize',14,'fontweight','b'); subplot(312), plot(VV_s(2,:)'-Vn_ins,'r','LineWidth',2.5); grid on; ylabel('V_N error','fontsize',14,'fontweight','b'); subplot(313), plot(VV_s(3,:)'-Vu_ins,'r','LineWidth',2.5); grid on; ylabel('V_U error','fontsize',14,'fontweight','b'); xlabel('Time (sec) \rightarrow','fontsize',14,'fontweight','b'); %---------------------PLOTTING INS ATTITUDE ERRORS------------------------figure(7) subplot(311), plot(Att_s(1,:)'/3600-yaw_ins/3600,'r','LineWidth',2.5); hold on; title('INS Attitude Error','fontsize',14,'fontweight','b'); grid on ylabel('\psi error','fontsize',14,'fontweight','b'); subplot(312), CXCV
plot(Att_s(2,:)'/3600-pitch_ins/3600,'r','LineWidth',2.5);grid on; ylabel('\theta error','fontsize',14,'fontweight','b'); subplot(313),plot(Att_s(3,:)'/3600-roll_ins/3600,'r','LineWidth',2.5);grid on; ylabel('\gamma error','fontsize',14,'fontweight','b'); xlabel('Time (sec) \rightarrow','fontsize',14,'fontweight','b'); %---------------------PLOTTING GNSSS POSITION ERRORS----------------------figure(8) subplot(311), plot(Pos_s(1,:)'-Pos_GPS(:,1),'r','LineWidth',2.5); hold on; title('GNSS Position Error','fontsize',14,'fontweight','b'); grid on ylabel('Latitude error','fontsize',14,'fontweight','b'); subplot(312), plot(Pos_s(2,:)'-Pos_GPS(:,2),'r','LineWidth',2.5); grid on; ylabel('Longitude error ','fontsize',14,'fontweight','b'); subplot(313), plot(Pos_s(3,:)'-Pos_GPS(:,3),'r','LineWidth',2.5); grid on; ylabel('height error','fontsize',14,'fontweight','b'); xlabel('Time (sec) \rightarrow','fontsize',14,'fontweight','b'); %---------------------PLOTTING GNSSS ATTITUDE ERRORS----------------------figure(9) subplot(311), plot(VV_s(1,:)'-V_GPS(:,1),'r','LineWidth',2.5); grid on; title('GNSS Velociy error','fontsize',14,'fontweight','b'); grid on ylabel('V_E error','fontsize',14,'fontweight','b'); subplot(312), plot(VV_s(2,:)'-V_GPS(:,2),'r','LineWidth',2.5); grid on; ylabel('V_N error','fontsize',14,'fontweight','b'); subplot(313), plot(VV_s(3,:)'-V_GPS(:,3),'r','LineWidth',2.5); grid on; ylabel('V_U error','fontsize',14,'fontweight','b'); xlabel('Time (sec) \rightarrow','fontsize',14,'fontweight','b'); toc; % Stop Timer %============================================================== ============ %********************************END********************************** ***** %============================================================== ============
CXCVI
10.1.4 MAGNETOMETER CALIBRATION clear all close all %********************************************************************** **** %============================INPUT DATA==================================== %********************************************************************** **** load imudatared.txt pitch=imudatared(:,13); % in degrees roll=imudatared(:,14); % in degrees yaw_external=imudatared(:,12); % in degrees hx_raw=imudatared(:,15)*10^-5; hy_raw=imudatared(:,16)*10^-5; hz_raw=imudatared(:,17)*10^-5; hh=30.8457e-6; Dec_angle=2.42; N=size(pitch) ;
% Measured in 10^-6 tesla
%from IGRF mode
%********************************************************************** **** %=============DETERMINATION OF HEADING FROM MAGNETOMETER=================== %********************************************************************** **** for i=1:1:N hx = hx_raw(i)*cosd(roll(i)) + hy_raw(i)*sind(pitch(i))*sind(roll(i)) hz_raw(i)*cosd(pitch(i))*sind(roll(i)); % levelling of magnetometer readings hy= hy_raw(i)*cosd(pitch(i)) + hz_raw(i)*sind(pitch(i)); if hy>0 %heading dtermination heading_uncalib(i)=90-(atand(hx/hy)-Dec_angle); elseif hy0 heading_calib(i)=90-(atand(Hx/Hy)-Dec_angle); elseif Hy
