IMU/GNSS integration for precise positioning: algorithm implementation and simulation
Abstract
The Centre Tecnològic de Telecomunicacions de Catalunya (CTTC), is developing a part of a project from the European program H2020 which consists in increase the safety, sustainability, flexibility and efficiency of road transport in a city (its acronym is TIMON). The CTTC contribution is a new positioning module that uses inertial measurement units (IMUs), which bases their performance in the gyroscopes and accelerometers measurement fusion, with a global navigation satellite system (GNSS) assistance to provide a reliable navigation solution to the vehicles drivers. The goal of this project is to improve the performance of the positioning module by adding a three-axis magnetometer to the measurement set used by the positioning algorithm. This device measures the magnetic field components with a given position and using the measurements is possible to estimate their orientation with respect to the magnetic north, which is very useful for the vehicle's heading estimation. The gyroscope measures the angular rate, thus, it gives information about the change of orientation, but does not provide an absolute orientation and it is affected by bias and scale errors among other imperfections. To improve the heading estimation, in this work, the gyroscope and magnetometer data is fused by Bayesian filtering using Kalman filter. This project also includes the exploration of magnetometer calibration's techniques and algorithms to compensate the distortions in the measurements due to the ferromagnetic and magnetic fields near the device, or by the device fabrication imperfections. The main goals of this project are: create a realistic simulator of the magnetometer measurements to implement a calibration algorithm and check its performance. In second place, implement a data fusion algorithm using a Kalman filter to integrate the gyroscope and magnetometer data to estimate the vehicle's heading, and to improve the performance achieved by the original algorithm, obtaining a more accurate navigation solution. The results of this project had been partially published in the 4.3 deliverable of TIMON's project. This project is divided in six chapters. The first chapter provides an introduction to the TIMON project, the organizations involved on it, the impact that will have, the work developed in the CTTC and the goals of this project. Next chapter introduces the inertial navigation system (INS), the inertial sensors characteristics of the simulator, the different coordinate systems used along the project, the transformations and rotations between frames. The third chapter introduces the global navigation satellite system (GNSS) and the advantages of integrating INS and GNSS. The fourth introduces the Earth's magnetic field and the magnetometer, explaining how is calculated the magnetic heading and the distortions and noise that must be added in order to make realistic the simulator. This chapter gives details about the construction of the magnetic simulator and its calibration. Chapter five provides information about the Kalman filter, beginning with a brief introduction and deepening on the equations that form the algorithm. Besides, it details the algorithm's application to obtain the magnetic heading and to fuse the magnetometer and gyroscope data. Finally, in chapter six are the conclusions extracted by the obtained results. The programming language used in the project to implement the simulator and the algorithms is Matlab. In the Annex the code is attached, and also the simulations and graphics that show the results of the heading estimation with the Kalman filter and the heading estimation when the magnetometer and gyroscope data are fused.
Full text
TREBALL FINAL DE GRAU TÍTOL DEL TFG: Magnetometer integration into an IMU/GNSS positioning system: algorithm implementation and simulation TITULACIÓ: Grau en Enginyeria de Sistemes de Telecomunicació AUTOR: Ester Serrano Jiménez DIRECTOR: Javier Arribas Lázaro DATA: 20 de juliol del 2016
RESUMEN En el Centre Tecnològic de Telecomunicacions de Catalunya (CTTC), se desarrolla una parte de un proyecto del programa Europeo H2020 que consiste en incrementar la seguridad, sostenibilidad, flexibilidad y eficiencia del transporte en carretera en una ciudad (su acrónimo en inglés es TIMON). Para ello, se ha creado un nuevo módulo de posicionamiento que utiliza unidades de medidas inerciales (IMUs), las cuales basan su funcionamiento en la integración de las mediciones de giróscopos y acelerómetros, con asistencia de un sistema de navegación por satélite (GNSS) para proporcionar una buena solución de navegación a los conductores de los vehículos. El objetivo de este proyecto consiste en ofrecer una aportación al módulo de posicionamiento realizado en el CTTC, de forma que ayude a mejorar su rendimiento. La contribución para lograrlo es añadir un magnetómetro de tres ejes al conjunto de medidas que utiliza el algoritmo de posicionamiento. Estos dispositivos miden las componentes del campo magnético dada una posición, a partir de las cuales se puede determinar su orientación respecto al norte magnético, lo cual es muy útil para estimar el rumbo del vehículo. Los giróscopos miden la velocidad angular y dan información sobre el cambio en la orientación, pero no proporcionan una orientación absoluta y además sufren de deriva en la integración. Para una estimación de la orientación más exacta, se fusionan los datos del magnetómetro y la información del giróscopo mediante técnicas de filtrado Bayesiano, en forma de filtro de Kalman. Además, debido a que los objetos ferromagnéticos o campos magnéticos cercanos al dispositivo, y las imperfecciones de fabricación del propio magnetómetro, añaden distorsiones y errores a las medidas, este proyecto también incluye la exploración de técnicas y algoritmos de calibración del magnetómetro. Los objetivos principales de este proyecto son: En primer lugar, crear un simulador realista de las medidas de un magnetómetro, para implementar un algoritmo de calibración del mismo y comprobar su rendimiento. En segundo lugar, implementar un algoritmo de fusión de datos usando un filtro de Kalman para integrar de forma adecuada los datos del magnetómetro y el giróscopo para estimar el rumbo del vehículo, mejorando la exactitud y precisión que la integración del giróscopo ofrecía en el algoritmo original, obteniendo finalmente una solución de navegación más precisa. Los resultados de este trabajo han sido publicados parcialmente en el entregable 4.1 del proyecto TIMON. El trabajo está dividido en seis capítulos. El primero ofrece una introducción poniendo en contexto el proyecto europeo, las organizaciones involucradas en él, el impacto que tendrá, el trabajo desarrollado en el CTTC y los objetivos de este proyecto. El segundo informa sobre el sistema de navegación inercial (INS), las características de los sensores inerciales del simulador, los diferentes
sistemas de coordenadas usados a lo largo del proyecto, las transformaciones y rotaciones entre coordenadas. El tercero habla sobre el sistema de navegación global por satélite y las ventajas que ofrece una integración INS/GNSS. El cuarto introduce el campo magnético terrestre y el magnetómetro, explicando cómo se calcula el rumbo (heading) magnético y las distorsiones y ruidos que hay que añadir para que el simulador se comporte de forma realista. En este capítulo se dan detalles sobre la construcción del simulador del magnetómetro y la posterior calibración de éste. El capítulo cinco contiene información sobre el Filtro de Kalman, empezando con una breve introducción y profundizando en las ecuaciones que forman el algoritmo. Además detalla la aplicación de este algoritmo para obtener la estimación del rumbo magnético y también para fusionar los datos del magnetómetro y el giróscopo. Por último, el capítulo seis consta de las conclusiones extraídas como consecuencia de los resultados obtenidos. El lenguaje de programación utilizado en este proyecto para implementar el simulador y los algoritmos es Matlab. En el Anexo está adjuntado el código creado, también las simulaciones y gráficos que muestran los resultados de la estimación del rumbo con el filtro de Kalman y la estimación del rumbo cuando los datos del magnetómetro y el giróscopo son fusionados.
RESUM En el Centre Tecnològic de Telecomunicacions de Catalunya (CTTC), es desenvolupa una part d’un projecte del programa Europeu H2020 que consisteix en incrementar la seguretat, sostenibilitat, flexibilitat i eficiència del transport en carretera en una ciutat (el seu acrònim en anglès és TIMON). Per això, s’ha creat un nou mòdul de posicionament que utilitza unitats de mesures inercials (IMUs), les quals basen el seu funcionament en la integració de les mesures de giroscopis i acceleròmetres, amb assistència d’un sistema de navegació per satèl·lit (GNSS) per proporcionar una bona solució de navegació als conductors dels vehicles. L’objectiu d’aquest projecte consisteix en oferir una aportació al mòdul de posicionament realitzat en el CTTC, de manera que ajudi a millorar el seu rendiment. La contribució per aconseguir-ho és afegir un magnetòmetre de tres eixos al conjunt de mesures que utilitza l’algoritme de posicionament. Aquests dispositius mesuren les components del camp magnètic en una posició donada i la seva orientació respecte al nord magnètic és determinada, el qual és molt útil per estimar el rumb del vehicle. Els giroscopis mesuren la velocitat angular i donen informació sobre el canvi en l’orientació, però no proporcionen una orientació absoluta i a més pateixen de deriva en la integració. Per una estimació de l’orientació més exacte, es fusionen les dades del magnetòmetre i la informació del giroscopi mitjançant tècniques de filtrat Bayesià, en forma de filtre de Kalman. A més, degut a que els objectes ferromagnètics o camps magnètics propers al dispositiu, i les imperfeccions de fabricacions del mateix magnetòmetre, afegeixen distorsions i errors a les mesures, aquest projecte també inclou l’exploració de tècniques i algoritmes de calibratge del magnetòmetre. Els objectius principals d’aquest projecte són: En primer lloc, crear un simulador realista de les mesures d’un magnetòmetre, per implementar un algoritme de calibratge del mateix i comprovar el seu rendiment. En segon lloc, implementar un algoritme de fusió de dades usant un filtre de Kalman per integrar de forma adequada les dades del magnetòmetre i del giroscopi per estimar el rumb del vehicle, millorant l’exactitud i precisió que la integració del giroscopi oferia en l’algoritme original, obtenint finalment una solució de navegació més precisa. Els resultats d’aquest projecte han sigut publicats parcialment al lliurable 4.1 del projecte TIMON. El treball està dividit en sis capítols. El primer ofereix una introducció posant en context el projecte europeu, les organitzacions involucrades en ell, el impacte que tindrà, el treball desenvolupat en el CTTC i els objectius d’aquest projecte. El segon informa sobre el sistema de navegació inercial (INS), les característiques dels sensors inercials del simulador, els diferents sistemes de coordenades utilitzats al llarg del projecte, les transformacions i rotacions entre coordenades. El tercer parla sobre el sistema de navegació global per satèl·lit i
les avantatges que ofereix una integració INS/GNSS. El quart introdueix el camp magnètic terrestre i el magnetòmetre, explicant com es calcula el rumb (heading) magnètic i les distorsions i sorolls que s’han d’afegir perquè el simulador es comporti de manera realista. En aquest capítol es donen detalls sobre la construcció del simulador del magnetòmetre i el posterior calibratge d’aquest. El capítol cinc conté informació sobre el filtre de Kalman, començant amb una breu introducció i profunditzant en les equacions que formen l’algoritme. A més, detalla l’aplicació d’aquest algoritme per obtenir l’estimació del rumb magnètic i també per fusionar les dades del magnetòmetre i les del giroscopi. Per últim, el capítol sis consta de les conclusions extretes com a conseqüència dels resultats obtinguts. El llenguatge de programació utilitzat al projecte per implementar el simulador i els algoritmes es Matlab. A l’Annex està adjuntat el codi creat, també les simulacions i les gràfiques que mostren els resultats de l’estimació del rumb amb el filtre de Kalman i l’estimació del rumb quan les dades del magnetòmetre i el giroscopi són fusionades.
ABSTRACT The Centre Tecnològic de Telecomunicacions de Catalunya (CTTC), is developing a part of a project from the European program H2020 which consists in increase the safety, sustainability, flexibility and efficiency of road transport in a city (its acronym is TIMON). The CTTC contribution is a new positioning module that uses inertial measurement units (IMUs), which bases their performance in the gyroscopes and accelerometers measurement fusion, with a global navigation satellite system (GNSS) assistance to provide a reliable navigation solution to the vehicles drivers. The goal of this project is to improve the performance of the positioning module by adding a three-axis magnetometer to the measurement set used by the positioning algorithm. This device measures the magnetic field components with a given position and using the measurements is possible to estimate their orientation with respect to the magnetic north, which is very useful for the vehicle’s heading estimation. The gyroscope measures the angular rate, thus, it gives information about the change of orientation, but does not provide an absolute orientation and it is affected by bias and scale errors among other imperfections. To improve the heading estimation, in this work, the gyroscope and magnetometer data is fused by Bayesian filtering using Kalman filter. This project also includes the exploration of magnetometer calibration's techniques and algorithms to compensate the distortions in the measurements due to the ferromagnetic and magnetic fields near the device, or by the device fabrication imperfections. The main goals of this project are: create a realistic simulator of the magnetometer measurements to implement a calibration algorithm and check its performance. In second place, implement a data fusion algorithm using a Kalman filter to integrate the gyroscope and magnetometer data to estimate the vehicle’s heading, and to improve the performance achieved by the original algorithm, obtaining a more accurate navigation solution. The results of this project had been partially published in the 4.3 deliverable of TIMON’s project. This project is divided in six chapters. The first chapter provides an introduction to the TIMON project, the organizations involved on it, the impact that will have, the work developed in the CTTC and the goals of this project. Next chapter introduces the inertial navigation system (INS), the inertial sensors characteristics of the simulator, the different coordinate systems used along the project, the transformations and rotations between frames. The third chapter introduces the global navigation satellite system (GNSS) and the advantages of integrating INS and GNSS. The fourth introduces the Earth’s magnetic field and the magnetometer, explaining how is calculated the magnetic heading and the distortions and noise that must be added in order to make realistic the
simulator. This chapter gives details about the construction of the magnetic simulator and its calibration. Chapter five provides information about the Kalman filter, beginning with a brief introduction and deepening on the equations that form the algorithm. Besides, it details the algorithm's application to obtain the magnetic heading and to fuse the magnetometer and gyroscope data. Finally, in chapter six are the conclusions extracted by the obtained results. The programming language used in the project to implement the simulator and the algorithms is Matlab. In the Annex the code is attached, and also the simulations and graphics that show the results of the heading estimation with the Kalman filter and the heading estimation when the magnetometer and gyroscope data are fused.
AGRADECIMIENTOS Quiero agradecer sinceramente la ayuda y la guía proporcionada por mi tutor, Javier Arribas Lázaro, que me acogió desde el momento en que llegué a la empresa. Gracias por dejar que robara tu tiempo y que este proyecto haya sido posible. También agradecer a mi familia, a mis padres, Rafa y Ana, a mi hermana, Noelia y a mi tía Ángela. Son un apoyo incondicional y los que están ahí, aguantando mis momentos de estrés. Y a mis amigos, que siempre están listos para escuchar y animarme, y hacen que todo sea más divertido y llevadero.
Index CHAPTER 1. INTRODUCTION ...................................................................... 1 1.1. TIMON’s Project context ....................................................................... 1 1.1.1. European framework H2020 ........................................................... 1 1.1.2. TIMON’s project definition and objectives ....................................... 2 1.1.3. TIMON’s partners and contribution of the CTTC............................. 4 1.2. Objectives of this project ....................................................................... 5 CHAPTER 2. INERTIAL NAVIGATION .......................................................... 7 2.1. Dead reckoning ..................................................................................... 7 2.2. Inertial sensors ...................................................................................... 7 2.3. Inertial measurement unit (IMU) .......................................................... 10 2.4. Inertial navigation system (INS) .......................................................... 10 2.5. Coordinate frames ............................................................................... 12 2.5.1. Earth-Centered Inertial Frame ...................................................... 12 2.5.2. Earth-Centered Earth-Fixed Frame .............................................. 13 2.5.3. Local Navigation Frame ................................................................ 13 2.5.4. Body Frame .................................................................................. 14 2.6. Transformations and rotations ............................................................. 14 2.6.1. Euler angles .................................................................................. 14 2.6.2. Coordinate transformation matrix .................................................. 16 2.6.3. Earth and Local Navigation Frames .............................................. 16 CHAPTER 3. GLOBAL NAVIGATION SATELLITE SYSTEM ...................... 17 3.1. Definition of GNSS .............................................................................. 17 3.2. GNSS .................................................................................................. 17 3.2.1. How is calculated the distance to a satellite ................................. 17 3.2.2. Main sources of error in GNSS ..................................................... 18 3.3. Advantages of integrating INS and GNSS ........................................... 19 CHAPTER 4. MAGNETOMETER ................................................................ 21 4.1. Earth’s magnetic field .......................................................................... 21 4.2. Magnetometer characteristics ............................................................. 22 4.3. Magnetic heading ................................................................................ 23 4.4. Distortions and noises ......................................................................... 24 4.4.1. Scale factor errors ........................................................................ 25 4.4.2. Bias errors .................................................................................... 25 4.4.3. Misalignment errors ...................................................................... 25 4.4.4. White noise ................................................................................... 26
2 Magnetometer integration into an IMU/GNSS positioning system Excellent science It is aimed to strengthen the EU’s scientific position worldwide. It includes four areas of operation: - Support the best researchers in Europe by providing them with grants to carry out frontier research through the European Research Council (ERC). - Fund collaborative research to open up new and promising fields of research and innovation through support for Future and Emerging Technologies (FET). - Provide mobility grants and fellowships to researchers to boost their careers through Marie Sklodowska-Curie Actions. - Ensure Europe has a world-class research infrastructures accessible to all researchers in Europe and beyond. Industrial leadership Its purpose is to ease the development of technologies and its applications to improve the European competitiveness. It counts with investments in key technologies for industry as: Technologies of the Information and Communication (TIC), nanotechnologies, advanced materials and manufacturing, biotechnologies and spatial technologies. It also helps the innovative SMEs to become leading companies, as well as their participation in collaborative Social and Technologies Projects Challenges. Societal challenges Its aim is to solve concrete problems of citizens and provide answers to the politics priorities and challenges identified in the Europe 2020 strategy. To provide a better life, it is focused in the following areas: Health; Food and agriculture, including marine science; Energy; Transport; Weather environment and efficient use of resources; Inclusive and reflective societies; Security. 1.1.2. TIMON’s project definition and objectives A large majority of European Citizens are living in urban environments. They live their daily lives in the same space, and for their mobility share the same infrastructure. Vehicle circulation in urban environments is responsible for 40% of CO2 emissions and for 70% of other emissions. Of the three factors involved in traffic accidents, driver, vehicle and road, the human factor is predominant. Tiredness, lack of perception, distractions or lack of visibility affect the response time of the driver increasing the chances of having an accident. When it comes to percentages, in 76% of the accidents the
Introduction 3 human factor is the only cause of the accident. Thus, the need to assist the driver in an intelligent way has become essential. In addition, transport users suffer from congestion which increases travel times, makes travel times unpredictable, increases operating costs, wastes fuel increasing air pollution. Blocked traffic may interfere with the passage of emergency vehicles or increase the chance of collisions due to tight spacing and constant stopping-and-starting. Europe would be close to solving problems related to congestion, traffic safety and environmental challenges if people, vehicles, infrastructure and business were connected into one cooperative ecosystem combining integrated traffic and transport management with new elements of ubiquitous data collection and system self-management. TIMON is an H2020 project which offers “Enhanced real time services for optimized multimodal mobility relying on cooperative networks and open data”. The main objective of this project is increasing the safety, sustainability, flexibility and efficiency of road transport systems by taking advantage of cooperative communication and by processing open data related to mobility through a cooperative open web based platform and mobile application, developed with the purpose of delivering information and services to drivers, businesses and Vulnerable Road Users (VRU) in real time. The specific objectives concerning the project for the scenario of a middle size city of approximately 300.000 habitants are: - To increase road safety by reducing accidents caused by human factor by 15-20% via the development of driver assistance system based on vehicular communication V2V (Vehicle to Vehicle) and V2I (Vehicle to infrastructure) minimizing the distraction for drivers thanks to the use of graphical and sound components over textual input. - To achieve a more flexible transport through the progress in Data processing technologies based on the advanced application of fuzzy evolutionary techniques in Artificial Intelligence, Data mining and GNSS (Global Navigation Satellite System) positioning that will allow multimodal route planning in accordance with users’ needs in real time; To take advantage of the different data available related to transport and mobility, from very different sources: infrastructure, open data, vehicles and VRU. Within TIMON project data coming from vehicles is of high relevance, as will act as sensors on the status of traffic, overcoming the boundaries of getting traffic related data only from infrastructure. These will multiple the information collected allowing getting more accurate services, such as higher precision for optimized multimodal route planning, congestion prediction etc. - To reduce pollution emission by 6-10% by means of a more efficient route planning that will contribute to the European Council commitment
4 Magnetometer integration into an IMU/GNSS positioning system to reduce overall Greenhouse emissions from its Member States by 20% in 2020. - To relieve traffic congestion by 12-20% for a more efficient transport thanks to the data gathered from a wide range of data sources in road transport and mobility, basing on highly reliable and accurate prediction artificial intelligence techniques, such as fuzzy evolutionary algorithms: lower time delay and cost reduction. 1.1.3. TIMON’s partners and contribution of the CTTC The project TIMON counts with eleven organizations distributed in Spain, Germany, Italy, United Kingdom, Slovenia, Belgium and Netherlands. The following table summarizes all of the partners involved: Table 1.1 Partners of project TIMON. This project is a contribution for the TIMON positioning work package developed in the Centre Tecnològic de Telecomunicacions de Catalunya (CTTC). The CTTC team involved in TIMON belongs to the Statistical Inference (SI) for Communications and Positioning Department which main objective is to design advanced receiver techniques and architectures for the new generation of Communication and Positioning systems. The main role of CTTC in TIMON include the analysis of positioning solutions based on GNSS technology in the framework of the vehicular scenarios considered in the project and the identification of enhancements where standalone GNSS might not provide the desired performance. CTTC will contribute to the research activities towards enabling positioning in challenging scenarios where GNSS is partially or totally obstructed. For such purpose it will
Introduction 5 investigate cooperative positioning solutions exploiting communications links among the devices of the vehicular scenario. More specifically, CTTC will study data fusion algorithms with inertial sensors, Bayesian techniques and factor graphs algorithms under the framework of cooperative and distributed methods. CTTC team has an strong background and expertise relevant to TIMON that includes: Global Navigation Satellite Systems (GNSS) receivers, signal processing for navigation systems, localization/positioning and tracking algorithms, cooperative and distributed algorithms, data fusion algorithms, advanced signal processing algorithms, Bayesian estimation, digital communications, network coding, iterative information processing and managerial capabilities. 1.2. Objectives of this project Fig. 1.2 Schemes of the original integration performed in CTTC and the integration performed in this project with the magnetometer addition. The goal of this project is to provide a contribution to TIMON’s project realized in CTTC: a positioning module that uses inertial measurement units (IMUs) with global navigation satellite system (GNSS) assistance to provide a precise and reliable navigation solution to the vehicles drivers. The GNSS’s Kalman filter corrects the inertial solution and estimates its skews. In this project a three-axis magnetometer is added and integrated to the set of measures that uses the positioning algorithm in order to improve its performance. The main objectives of this project are: - Create a realistic simulator of magnetometer measurements.
6 Magnetometer integration into an IMU/GNSS positioning system - Implement a calibration algorithm and check its performance. - Implement a data fusion algorithm using a Kalman filter to integrate both gyroscope and magnetometer data to estimate the vehicle’s heading, and to improve the performance achieved by the original algorithm, obtaining a more accurate navigation solution. To achieve it, a previous investigation about navigation systems, coordinate frames and rotations, Earth’s magnetic field, magnetometers, calibration and Kalman filter has been done to acquire the necessary knowledge. Finally, Matlab scripts have been used as the programming language to implement the simulator and algorithms.
Inertial navigation 7 CHAPTER 2. INERTIAL NAVIGATION 2.1. Dead reckoning Dead reckoning, is the process of calculating the current position by measuring either the change in position or the velocity and integrating. In this process, the position solution is the sum of a series of relative position measurements, so dead reckoning is subject to cumulative errors that will grow with time. Dead reckoning requires a known starting position, but after that will provide an uninterrupted navigation solution. Fig. 2.1 Dead reckoning method (from [1]). 2.2. Inertial sensors Inertial sensors comprise accelerometers and gyroscopes. An accelerometer measures specific force and a gyroscope measures angular rate, both without an external reference. Devices that measure the velocity, acceleration, or angular rate of a body with respect to features in the environment are not inertial sensors. Current low cost inertial sensor development is focused on Micro-ElectroMechanical Systems, or MEMS technology. This enables quartz and silicon sensors to be mass produced at low cost using etching techniques with several sensors on a single silicon wafer. There are four main advantages of using MEMS rather than ordinary large scale machinery: 1. The ease of production. 2. They can be mass-produced and, thus, are inexpensive to make.
8 Magnetometer integration into an IMU/GNSS positioning system 3. Easy to make part alterations. 4. Higher reliability compared to large-scale machines. However, MEMS products have their limitations and disadvantages. Due to their size, it is physically impossible for MEMS to transfer any significant power. In addition, because MEMS are made from Poly-Si (Polycrystalline silicon, a brittle material) they cannot be loaded with large forces. This is because brittle materials can be fractured easily under high stress. Also they currently offer relatively poor performance. A triple-axis MEMS gyroscope and a triple-axis MEMS accelerometer are used in this project. Particularly, the IvenSense MPU9250, which axes are oriented as in Fig. 2.2. The devices have the features described in Table 2.1 and 2.2. Fig. 2.2 Orientation of axes of sensitivity and polarity of rotation for accelerometer and gyroscope (from [10]).
Inertial navigation 9 Table 2.1 MPU-9250 Gyroscope specifications. Table 2.2 MPU-9250 Accelerometer specifications.
10 Magnetometer integration into an IMU/GNSS positioning system 2.3. Inertial measurement unit (IMU) An Inertial Measurement Unit (IMU) is an integrated sensor package that combines multiple accelerometers and gyros to produce a three dimensional measurement of both specific force and angular rate. It is used to detect attitude, location, and motion. Typically, the term IMU is used to refer to a box, containing 3 accelerometers and 3 gyroscopes. The accelerometers and gyros are placed such that their measuring axes are orthogonal to each other. The accelerometers measure specific force while the gyros measure the angular rate, and then the IMU integrate the measurements to find the changes from the initial position, which is indispensable to know. The error in each measurement is accumulative, they can be reduced if the sensors used are high quality because they provide a better measurement, but at a higher cost. The IMU by itself does not provide any kind of navigation solution (position, velocity, attitude), it only actuates as a sensor. To overtake IMU limitations, it can be added another measurements to the system. By integrating a GNSS receiver, for example, can be used to correct the long term drift in position. On the other hand, integrating a magnetometer can provide a heading correction. Both systems are integrated in this project. As exposed before, in the beginning there is a navigation system with inertial measurement units with GPS assistance, then a magnetometer it’s added to provide more accuracy in attitude solution. 2.4. Inertial navigation system (INS) An inertial navigation system (INS) is a complete three-dimensional deadreckoning navigation system. INS integrates the measurements of its internal IMU to provide a navigation solution.
Inertial navigation 11 Fig. 2.3 Schematic of an inertial navigation processor (from [1]). The IMU outputs are integrated to produce an updated position, velocity, and attitude solution in four steps: 1. The attitude update; 2. The transformation of the specific-force resolving axes from the IMU body frame to the coordinate frame used to resolve the position and velocity solutions (see Sections 2.5 and 2.6); 3. The velocity update, including transformation of specific force into acceleration using a gravity or gravitation model; 4. The position update. So it gets the angular rate and acceleration from an IMU, calculates position, velocity and attitude information with respect to a reference frame by utilizing six degree of freedom kinematics equations. The principal advantages of inertial navigation are continuous operation, a highbandwidth (at least 50 Hz) navigation solution, low short-term noise, and the provision of attitude (roll, yaw, pitch), angular rate, and acceleration measurements, as well as position and velocity. The main drawbacks are the degradation in navigation accuracy with time and the high accuracy sensor cost. In this project the INS simulator is modeled with the specifications of the following table, which models a consumer grade MEMS IMU:
18 Magnetometer integration into an IMU/GNSS positioning system (3.1) The velocity is the one that has the signal transmitted by the satellite and it travels at light’s speed, so it’s known. The time is the one which takes the signal to travel from the satellite to the GNSS receiver. Then, how is calculated this time? The receiver estimates with a certain accuracy and precision the time difference between the time of transmission by the satellite and the reception in the receiver. These times are extremely small because the signal travels at the speed of light. So this measure requires synchronization in time above 10-7 seconds, which is only available in expensive atomic clocks. To avoid it, there is another method to know the travel time of the signal which doesn’t require having an atomic clock in every receiver. The satellite signal is modulated with a Pseudo Random Code (sequences of 0s and 1s) and, at the same time, the receiver generates internally the same code sequence too. As the signal from the satellite it’s the same as the one created by the GNSS receiver, when the satellite signal arrives to the receiver, we’ll have the same sequence but with a delay. This delay is the travel time of the signal from the satellite to the receiver. But is needed a good synchronization between the satellite and the receiver. The satellite clocks are one of the most critical components of a GNSS system. In order to assure the stability of such clocks, GNSS satellites are equipped with atomic oscillators that accumulate some offsets along time. The satellite clock offsets are continuously estimated by the Ground Segment (which is the responsible for the proper operation of the GPS system and has a Master Control Station (MCS) that estimate clock errors among other things) and transmitted to the users to correct the measurements. The receivers, on the other hand, are equipped with quartz-based clocks, much more economical but with a poorer stability. This inconvenience is overcome by estimating its clock offset together with the receiver coordinates, which is why is needed a fourth equation as exposed in 3.2. 3.2.2. Main sources of error in GNSS GNSS is not exempt of errors. There are different sources of errors that degrade the position of the GNSS and make the measurement less accurate with some meters of deviation. Atmospheric delays The signal from the satellite goes through the ionosphere and this provokes its velocity to decrease. These atmospheric delays can introduce an error in the
Global navigation satellite system 19 calculation of the distance because the signal’s velocity it’s affected. There are different factors that have an influence in this delay: - Satellite elevation: The signal from satellites that are in a low elevation angle will be more affected than the signals coming from a highest elevation angle, because the distance to travel is larger. - The ionosphere density is affected by the Sun: At night, the influence of the ionosphere it’s minimum. But in the day, the ionosphere’s effect increases and velocity is slower. - Water vapor: It is contained in the atmosphere and can also affect the GNSS signals. This effect can result in position degradation, but can be reduced using atmospheric models. Errors in the clocks or orbits Despite the high clock accuracy, sometimes they have a small variation in the speed and produce small errors affecting to the position’s accuracy. Satellites drift slightly from their predicted orbits. These shifts of the orbits can be caused by: - The variation in gravitational field. - The variation in the solar radiation pressure. - The friction between the satellite and free molecules. Multipath effect Multipath effect can appear when the receiver is located near a reflective surface, like a lake or a building. It happens because the signal doesn’t travel directly to the antenna, but first to the nearest object and then is reflected to the antenna provoking a fake measure. This kind of error can be reduced using special antennas which filter signals coming from a low elevation angle. Dilution of the Precision (DOP) It depends on the geometry of the satellites in the moment of calculation the position. If satellites are more spaces it means less error range, but if the satellites are closer it provoke a major error range (and less accuracy). 3.3. Advantages of integrating INS and GNSS Speaking of INS/GNSS integration means the use of GPS satellite to correct and calibrate the solution provided by the INS. As mentioned in 2.4, Inertial Navigation can provide an accurate solution only for a short period of time.
20 Magnetometer integration into an IMU/GNSS positioning system On the other hand, GNSS offer long-term position accuracy with errors limited to a few meters (stand-alone) and the user equipment it is much cheaper (approximately 100$/€ versus inertial sensors of 100.000$/€). However, compared to INS, the output rate is low (typically around 1-10 Hz versus at least 50 Hz with INS), the short-term noise solution is high, and standard GNSS user equipment does not measure attitude. GNSS signals are also subject to obstruction and interference, if there are not at least four satellites available GNSS cannot provide a navigation solution. So it cannot be relied upon to provide a continuous navigation solution. The advantages and drawbacks of INS and GNSS are complementary, so an integrated INS/GNSS system combines the benefits of both technologies and provides accurate and uninterrupted navigation results. The GNSS gives an absolute drift-free position value that can be used to reset the INS solution or can be blended with it by use of a mathematical algorithm, such as a Kalman filter (that will be explained in upcoming chapters). While the INS smoothes the GNSS solution and bridges signal outages.
Magnetometer 21 CHAPTER 4. MAGNETOMETER 4.1. Earth’s magnetic field The Earth behaves like a dipole, currently tilted at an angle of 10 degrees with respect to Earth's rotational axis, as if in the center of our planet there were a bar magnet with this angle. The Earth’s magnetic (or geomagnetic) field points from the magnetic pole to the magnetic South Pole through the Earth, taking the opposite path through the upper atmosphere. Magnetic north is understood as the Earth's region where the lines of the magnetic field are perpendicular to the Earth surface while the field is horizontal near the equator as is shown in Fig.4.1. But the magnetic north is not aligned with the geographic north. The angle which determines this difference is called declination. Fig. 4.1 The Earth’s magnetic field lines (from [15]). Earth’s magnetic field is not static, it changes over time because the motion of molten iron alloys in its outer core. The magnetic poles slowly move, but sufficiently slowly for ordinary compasses to remain useful for navigation. It is needed a thousand years to the Earth's field reverse and north and south magnetic pole switch places. Nowadays, the north geomagnetic pole is situated
22 Magnetometer integration into an IMU/GNSS positioning system near the geographic south and the south geomagnetic pole is the north pole of the magnetic field. A magnetic field is described by the magnetic flux density vector and the international system (SI) unit of magnetic flux density is the Tesla (T). This field value in absolute average ranges between 25,000 and 65,000 nT (100,000 nT = 1 gauss). In this project, the components of the Earth’s magnetic field are calculated using the International Geomagnetic Reference Field (IGRF) model. The IGRF model is the empirical representation of the Earth's magnetic field. It represents the main (core) field without external sources and gives the model in terms of spherical harmonic series. The coefficients of the model are updated every five years. The outputs of the model are magnetic field values in NED components. The IGRF model coefficients are based on all available data sources including geomagnetic measurements from observatories, ships, aircrafts and satellites. For more information of the IGRF, look at reference [11] in the Bibliography. In this project is used the 195-coefficient IGRF2015 model to perform the magnetometer simulator. 4.2. Magnetometer characteristics A magnetometer is a device that measures the components of the magnetic field in body frame. Then, using the IGRF model, and knowing the position of the vehicle, its orientation with respect to the magnetic north can be determined. Therefore, the assumption that the measured variation in the magnetic field is due to an orientation change in the vehicle is made. Notice that variations may occur due to other sources of error. One of the main drawbacks of the magnetometers is that they have a low sampling rate, which makes them unsuitable for high dynamic vehicles. Besides, they present a high vulnerability to external interferences, like atmospherics or signal screenings. However, magnetometers present the major benefit of having a small error and they are a fundamental device to obtain a more accurate estimation of the orientation. A triple-axis MEMS magnetometer is used in this project. Particularly, the MPU9250, which axes are oriented as in Fig. 4.2. The device has the features described in Table 4.1.
Magnetometer 23 Table 4.1 MPU-9250 Magnetometer specifications. PARAMETER TYP UNITS MAGNETOMETER SENSITIVITY Full-Scale Range 4800 µT ADC Word Length 14 Bits Sensitivity Scale Factor µT/LSB ZERO-FIELD OUTPUT 0.6 Initial Calibration Tolerance 500 LSB Fig. 4.2 Orientation of axes of sensitivity for magnetometer (from [10]). 4.3. Magnetic heading A magnetometer is a device that measures the total magnetic flux density, denoted by the subscript m, resolved in the body frame, b: ( ) (4.1) where , , and are, respectively, the magnitude, declination, and dip of the total magnetic flux density, and , the transformation matrix from NED frame to body frame. Applying the transformation matrix (see section 1.5) is obtained the expression: ( )( ) (4.2) where is roll, is pitch and the magnetic heading or yaw, , is given by the difference between the true heading and the magnetic declination:
24 Magnetometer integration into an IMU/GNSS positioning system (4.3) Obtaining the magnetic heading using the magnetometer measurements can be achieved through two approaches. The first approach consists in considering that the roll and pitch are zero, in which case an estimate of yaw is: ( ) (4.4) where and are the (noisy) components of the magnetic field obtained by the magnetometer in xand yaxes. A second approach considers that roll and pitch are known or estimated (and possibly non-zero), in which case the heading estimate turns to be: ( ) (4.5) We consider the second approach. Even the project does not deal with planes or aircrafts (having high dynamics), any inclination in the vehicle can contribute to the roll and pitch components. If these contributions are not taken into account, errors will appear and affect yaw estimates. 4.4. Distortions and noises A magnetometer simulator was developed, which computes the magnetic field when is fed with a certain trajectory. Given a set of times, positions, velocities and attitudes, the system provides the signal that a magnetometer sensor would measure in that situation. In reality, the magnetometer measurements would be affected by errors. These errors are due to fabrication limitations or environmental errors. For instance, errors appear because magnetometers are sensitive to ferromagnetic materials and surrounding magnetic fields. The simulator computes first the errorless magnetometer measurements and then corrupts them with realistic error sources. In this section, the distortions and noises that perturb the signal are defined. It is worth mentioning that temperature dependence was not modeled.
Magnetometer 25 4.4.1. Scale factor errors This error corresponds to constants of proportionality relating the input to the output. ( ) (4.6) where is the 3-dimensional errorless data in body frame, is the output signal in body frame and is the scale error vector which is applied in each one of the three axis of the magnetometer and is different between them. Soft-iron errors are related with scaling offset errors. The soft iron distortions refer to the presence of close ferromagnetic materials, which skew the density of the Earth's magnetic field locally. 4.4.2. Bias errors Bias refers to the offset in the measurement provided by a sensor. It can be modeled as: (4.7) where is the offset error added in every axis. Besides the inner error of the sensor, the bias value is increased due to hardiron distortions. This distortion is created by objects that produce magnetic fields around the sensor, so this field is added to Earth’s magnetic field, generating an offset to the output of the magnetometer axes. If the piece of magnetic material is attached to the same reference frame as the sensor, this hard-iron distortion will cause a permanent bias in the sensor output unless it is corrected. 4.4.3. Misalignment errors Misalignments refer to the errors due to the non-orthogonality of the assembly mechanism. To simulate misalignments the simulator implements the following equation: ( ) ( ) ( ) (4.8)
26 Magnetometer integration into an IMU/GNSS positioning system where , , , , and are the misalignment between the axes. 4.4.4. White noise To model other non-systematic errors that are not accounted for in the model, it is useful to include a random term in the form of an Additive White Gaussian Noise (AWGN). The simulator generates this random noise and outputs the magnetometer noisy measurement: (4.9) where is the AWGN noise vector generated with desired given standard deviation. 4.5. Magnetometer simulator Fig. 4.3 Magnetometer simulator block diagram. Considering all the information exposed before, we have the tools to create the magnetometer simulator. This simulator is programmed in Matlab and all the details of the code are attached in the Annex II – Code in Section A, at the end of this project. In this section is explained how is implemented.
Magnetometer 27 Fig. 4.3 shows all the blocks that compose the magnetometer simulator. The goal is to obtain the components of the magnetic field (in body frame) in the three axes and also the magnetic declination for a specific location. Those are the outputs of the simulator. The inputs are the date, the true attitude (roll, pitch and yaw) and the coordinates of the trajectory in ECEF. To obtain the components of the magnetic field it is required geographic coordinates. The function Cart2geo is responsible to transform the ECEF coordinates in geographic coordinates, that is, longitude, latitude and height in the WGS84 datum. The transformation made by Cart2geo is necessary because the IGRF function needs specific input parameters. It requires the latitude, longitude and height of the position where the geomagnetic field values are generated. These are supplied in degrees (latitude and longitude) and in meters (height). The date must also be given so it can be selected the most recent IGRF model. With all these parameters as an input, the IGRF function returns the components of the 3D magnetic field [nT] in NED frame (Bned = [Bx, By, Bz] in Fig. 4.1 where B is the component of the magnetic field). A magnetometer gives the components of the magnetic field in body frame so a transformation matrix (equation (2.6), section 2.6.2) is applied here to change from NED to body frame (Bbody = [Bx, By, Bz] in Fig. 4.1). This transformation matrix can be built using the vehicle attitude. Finally, the magnetometer impairments are added to the true magnetic field values. As exposed in section 4.4, in this project are considered scale factor error (4.6), bias error (4.7), misalignment error (4.8) and white noise (4.9). So all of these are added to the magnetic field obtained (Bbody). To match the units of the magnetic field and the units of the errors all values are converted to [uT]. The true magnetic declination is given by: ( ) (4.10) where and are the components of the body frame magnetic field in xand yaxes, respectively. 4.6. Magnetometer calibration As discussed in section 4.4, the measurements of the magnetic field sensors are corrupted by several errors including sensor fabrication issues and
34 Magnetometer integration into an IMU/GNSS positioning system Fig. 4.11 Comparison about the heading estimation between the true, the calibrated and non-calibrated data. In Fig. 4.11 is shown represented the outputs of the magnetic heading estimation. In green is drawn the true heading, in which are compared the other two outputs. The signal in blue shows the magnetic heading estimation when the calibration has not been implemented. Interestingly, the estimation slightly follows the true heading, but is has a large error due to the uncalibrated sensor impairments. On the other hand, the signal in orange represents the magnetic heading after calibrating the magnetometer data. Now, the magnetic heading is more accurate. The difference with the non-calibrated data is remarkable. Despite the presence of errors, the orange signal follows the true heading with a bounded error. So it is demonstrated that the calibration improves the performance of the magnetic heading estimation. Next chapter introduces the Kalman filter and its applications to this problem to further improve the vehicle heading estimation performance.
The Kalman filter 35 CHAPTER 5. THE KALMAN FILTER 5.1. Introduction to the Kalman filter The basic technique of the Kalman filter was invented by Rudolf E. Kalman in 1960 and has been developed further by numerous authors since. The Kalman filter is a multiple-input, multiple-output digital filter that can optimally estimate, in real time, the states of a system based on its noisy outputs. These states are all variable needed to completely describe the system behavior as a function of time (such as position, velocity, voltage levels…). In fact, one can think of multiple noisy outputs as a multidimensional signal plus noise, with the system states being the desired unknown signals. The Kalman filter then filters the noisy measurements to estimate the desired signals. The estimates are statistically optimal in the sense that they minimize the meansquare estimation error. The main purpose of the Kalman filter is the estimation of those variables which cannot be measured directly. It consists of two steps per each time sample: prediction and correction (or update). Fig. 5.1 Conceptual scheme of Kalman Filter (from [9]). In the prediction step, the Kalman filter produces estimates of the current state variables, along with their uncertainties. Once the outcome of the next measurement (necessarily corrupted with some amount of error, including random noise) is observed, these estimates are updated using a weighted average, with more weight being given to estimates with higher certainty. Because of the algorithm's recursive nature, it can run in real time using only the present input measurements and the previously calculated state; no additional past information is required.
36 Magnetometer integration into an IMU/GNSS positioning system From a theoretical point, the main assumption of the Kalman filter is that the underlying system is a linear dynamical system and that all error terms and measurements have a Gaussian distribution. Extensions and generalizations to the method have also been developed, such as the extended Kalman filter which work on nonlinear systems. 5.2. Kalman filter equations Time update (“Predict”) To explain the equations that form the algorithm of the Kalman filter, it is used an example to improve its readability. It is going to be supposed that the position vector and velocity vector (in ECEF frame) of a vehicle in motion are wanted to be estimated. The estimation (or predicted) state is called , where k is the time of iteration and the subscript p denotes predicted. So it is needed the previous state (at time k-1) to predict the next state at time k. But also it has to be considered the error in the estimate, so initially, a part of having the previous state, , is needed the error covariance matrix, : ( ) (5.1) where and are the vehicle ECEF position and velocity vectors, respectively. The error covariance matrix expression is ( ) (5.2) where and its the diagonal elements are its variances and the other elements are its covariances. The state can be further extended to acceleration, but here in (5.1) and (5.2), only position and velocity are considered. To represent the prediction step is used the state transition matrix . It moves the previous state estimated into the next predicted state where the system would move if that original estimate was the right one. A different transition matrix is derived for every Kalman filter application as a function of that system. The transition matrix is always a function of the time interval between Kalman filter iterations but can be also a function of other parameters.
The Kalman filter 37 (5.3) Following the example, to know the position and velocity at the next moment in the future is used the well-known conservation of movement kinematic formula that follows (considering in the following example, the position and the velocity as scalars): (5.4) where is the initial position, the velocity and the variation of time. So, the extension to the ECEF state vector position and velocity form, from (5.3) is modeled as: ( ) (5.5) where stands for the identity matrix. Then to update the covariance matrix, , the propagation of the covariance matrix has the following expression: (5.6) Note that the first matrix propagates the rows of the error covariance matrix, while the second, , propagates the columns. Following this step, each state uncertainty should be either larger or unchanged. But there are changes that are not only for the state itself, but for how the outside world could affect the system. For example, let’s say that is known the expected acceleration due to the throttle setting or control commands. This information is introduced in the model as follows: (5.7) where is called the control matrix which relates the control vector to the state. For simple systems the control parameters are omitted.
38 Magnetometer integration into an IMU/GNSS positioning system Until now, it has been considered when the state evolves based on its own properties and when the state evolves based on external forces (the way the world affect the system), but it’s needed something more: the system model is inaccurate, and this variation compared to reality is modeled using a covariance matrix . There are things that may happen that cannot be tracked but are definitely going to affect the prediction. These uncertainties are modeled adding a new uncertainty after every prediction step. The untracked influences are treated as noise with covariance matrix . So is gotten the expanded covariance by simply adding , giving the complete expression for the prediction step: Table 5.1 Final Kalman filter equations for the prediction step. Time update (prediction) In conclusion, the new best estimate ( ) is a prediction ( ) made from previous best estimate ( ), plus a correction ( ) for known external influences ( ). And the new uncertainty ( ) is predicted ( ) from the old uncertainty ( ), with some additional uncertainty from the own system model ( ). Measurement update (“Correct”) The remaining steps in the Kalman filter algorithm comprise the measurement update or correction phase. Now that the measurements (that will be denoted ) obtained by the sensor are being taken into account, it has to be considered that the units and scale of the measurements might not be the same as the units and scale of the state that is being tracked of. The measurement matrix, , defines how the measurement vector varies with the state vector. So the measurement prediction is . There is also the measurement noise covariance matrix, , which is the noise or the uncertainty provided by the sensor, and may be assumed constant or modeled as a function of kinematics or signal-to-noise measurements.
The Kalman filter 39 The goal is to find an equation that computes a posteriori state estimate, , as a linear combination of an a priori estimate, , and a weighted difference between an actual measurement, , and a measurement prediction, , as shown below in equation: ( ) (5.8) The difference, ( ), is known as the measurement residual. The residual reflects the discrepancy between the predicted measurement, , and the actual measurement, . A residual of zero means that the two are in complete agreement. The matrix is the Kalman gain, and is the key point of the Kalman filter algorithm. This matrix minimizes the a posteriori error covariance, . The Kalman gain equation is: ( ) (5.9) If the measurement error covariance approaches to zero, the gain increases and thus, the filter trust more on the measurements. On the other hand, as the predicted estimate error covariance approaches to zero, the gain decreases and thus, the filter trust more on the system model (the predicted state). In other words, if the measurement covariance error approaches to zero, the actual measurement is “trusted” more, while the predicted measurement is trusted less. On the other hand, as the predicted estimate error covariance matrix approaches to zero the actual measurement is trusted less, while the predicted measurement is trusted more. Finally, to update the error covariance, the equation given is: ( ) (5.10) where is the identity matrix. It is important to update the error covariance because it will be the k-1 in the next itineration.
40 Magnetometer integration into an IMU/GNSS positioning system (5.8), (5.9) and (5.10) are the equations of the Kalman filter for the correction step. Summarizing equations To finalize the section, the next block diagram shows the equations exposed before for every step in the Kalman filter algorithm and represents the cycle for every iteration: 5.3. A Kalman filter for the magnetic heading estimation The goal of this project is the integration of a magnetometer and a gyro to estimate the heading of a vehicle. To accomplish it there are several ways, one of them is using a Kalman filter, and that is the option implemented in this project. But before getting into the magnetometer and gyro fusion, a simpler Kalman filter has been applied to get used with the equations and check the functionality of the algorithm. Fig. 5.2 Kalman filter algorithm block diagram.
The Kalman filter 41 Fig. 5.3 Scheme of the Kalman filter construction for heading and rate estimation. The figure above shows schematically how the Kalman filter is constructed. The states wanted to estimate are the heading and the heading rate, and the available measures are the calibrated magnetometer data. To move the previous state estimated into the next predicted state, the matrix is modeled as (5.5) where has a value of 0.1 [s] in the simulations, equivalent to a magnetometer sampling rate of 10 Hz, which is a realistic value. Process noise covariance matrix and measurement noise covariance matrix are constants too, so both of them are defined as filter configuration parameters: ( ) (5.11) ( ) (5.12) where and . Notice that both of the matrices only have values in the diagonal. That is because is assumed that the variables are independent of each other, and there is no correlation between them. is modeled in [rad2]
42 Magnetometer integration into an IMU/GNSS positioning system and the diagonal in represents the covariance (or variance) provided by the sensor in each axis. This covariance is calculated with: (5.13) where is the standard deviation and is the scale factor in xaxis. With the other axes the procedure would be the same. When the first itineration starts are initialized the previous state and the error covariance matrix associated with the error in the prediction: ( ) (5.14) ( ) (5.15) where is the measured magnetic heading, and . Notice that it is necessary the attitude estimation. The is initialized with a value of zero. Now that the initial state is defined, the predicted state can be obtained with the equations of Table 5.1. Then, it has to be updated with the measurements and the Kalman gain. The measurements input, , is given by the output of the magnetometer: ( ) (5.16) where , and are the components of the magnetic field in the x-, yand zaxis for the iteration k. To compute the Kalman gain is necessary to obtain matrix. This matrix defines how the measurement vector varies with the state vector, thus has to relate the state with the measurements . In other words, it has to transform in the corresponding measurement so the difference ( ) can be made.
The Kalman filter 43 To model , is necessary to take into account the expression that collects the magnetometer measure in equation (4.2). This equation, , represents the measurement , but the expression in (4.2) is not linear so the Jacobian matrix of has to be found. First the parameters in (4.2) are multiplied to obtain a single matrix: ( ) (5.17) ( ) Then the Jacobian matrix is found with: ( ) (5.18) ( ) Where is a matrix with partial derivatives of with respect to (in the first column) and (in the second column). is the magnitude of the magnetic flux density and is modeled with a value of 1. Now the Kalman gain can be computed and the predicted state and the error covariance matrix updated with (5.12), (5.11) and (5.13) respectively. Then the new estimated state is saved in the output vector that retains the and for every iteration k, and and becomes the previous state and the previous error covariance matrix for the next one. For more information about the implementation of the algorithm in Matlab, see the Annex II – Code Section C. Notice that the nomenclature used in Matlab is slightly different. The next table summarizes the variables denoted different:
50 Magnetometer integration into an IMU/GNSS positioning system ANNEX I – SIMULATIONS A. Kalman filter for magnetic heading estimation In section 4.6 and 4.7 was explained the importance of the magnetometer data calibration to obtain an improved magnetic heading estimation. Fig. 4.10 shows how calibrated data provided a more accurate solution that the non-calibrated data. Now is presented the impact of the Kalman filter when the algorithm is applied for estimate the magnetic heading. Fig. A.1 Comparison between the true heading, the calibrated heading and the Kalman heading estimation with small noise.
Simulations 51 Fig. A.1 shows in orange the heading estimation when the Kalman filter is applied. It can be observed that is pretty accurate because its follows the true heading graphic (drawn in green) and also that slightly reduce the noise more than the estimated heading after the calibration. For this simulation, the magnetometer standard deviation was set to 3.34 uT in the simulator. In Fig. A.2 the magnetometer noise standard deviation was increased to 10.34 uT. The Kalman filter provides a more accurate solution. The Kalman filter tuning for the simulations realized in this section are described in the next table: Table A.1 Values of , , , standard deviation and scale factor. Initialization for the covariance matrix [rad2] Process noise covariance matrix Q [rad2] Measurement noise covariance matrix R [uT2] Magnetometer standard deviation (SD) [uT] Magnetometer simulation scale factor [uT] Diagonal with 1.0247 Diagonal with 1.0247 Diagonal calculated with equation (5.15) [uT2] 3.34 (in Fig. 5.5) 10.34 (in Fig. 5.6) = 0.6 = 0.01 = 0.2 Now, establishing a standard deviation of 3.34 uT for the magnetometer simulation, and focusing only in the heading provided by the Kalman filter, is Fig. A.2 Comparison between the true heading, the calibrated heading and the Kalman heading estimation with elevated noise.
52 Magnetometer integration into an IMU/GNSS positioning system observed the performance of the magnetic heading estimation for different measurement noise covariance matrix values. Table A.2 Different measurement covariance matrix values applied in Fig. A.3. Small R [uT2] Medium R [uT2] Big R [uT2] ( ) ( ) ( ) The sensor noise covariance matrix determines the error in the measurements and it makes the Kalman gain to ‘decide’ if is more trustful the prediction or the measurements for the new state estimation. In case that the measurements covariance is big in matrix , it means that the measurements are not good, they are not trustful, so the prediction is more reliable. In Fig. A.3 the blue line shows that case. The output is more accurate because it have less noise, so the line drawn has less oscillations, but is less exact, the heading estimation changes slowly when is compared with the true heading trajectory. In case that the measurements covariance is small in matrix , it means that the error in the measurements is small and the Kalman gain will weight in favor to them. In Fig. A.3 the orange line shows that case. It converges more quickly to the true heading value but with more noise, so it can be observed more oscillations. Fig. A.3 Kalman filter heading estimation for different measurement covariance matrix values.
Simulations 53 The best option is to find an intermediate case where equilibrium between trusting the measures and the prediction is found. In Fig. A.3 the pink line represents it. The magnetic heading estimation has less noise and is more responsive than the two previous cases, giving a good estimation and more closer to the true heading. In the following graphics, the difference between the true heading and the trajectories of the magnetic heading estimation with different values of is represented in absolute value: It can be observed that it has less error (which means that there is less difference between the true heading and the Kalman heading estimation) when the covariances of have a medium value (pink line). There are peaks higher when covariances of have medium values than when has small values (orange line) but in general, the error is closer to zero. B. Kalman filter for the gyro and magnetometer fusion In this section is presented the behavior of the Kalman filter when the data of the gyroscope and the magnetometer are mixed to obtain an improved heading estimation. First, in Fig. A.5 is shown the heading estimation when only the gyroscope is working. Fig. A.4 Error between the true heading and the Kalman heading with different measurement covariance matrix values.
54 Magnetometer integration into an IMU/GNSS positioning system Fig. A.5 Heading estimation with only the gyroscope. With a good initial value, it starts providing an accurate estimation of the heading. But inertial sensors suffer of drift due to every iteration adds a new error to the last one and they accumulate, thus, the trajectory moves away from the true heading despite it follows the true trajectory curvature. The magnetometer measurements are added to the gyroscope measurements and mixed with the Kalman filter in order to fight the gyro drawbacks and provide a better heading estimation. The Kalman filter matrices are initially modeled with the next specifications: Table A.3 Values of , , , standard deviation and scale factor. Initialization for the covariance matrix [rad2] Process noise covariance matrix Q [rad2] Measurement noise covariance matrix R Magnetometer simulation standard deviation (SD) [uT] Magnetometer simulation scale factor [uT] Diagonal with 1.0247 Diagonal with 1.0247 Magnetometer Diagonal calculated with equation (5.15) [uT2] Gyroscope Diagonal with 7.6154·10-7 (rad/s2)2 3.34 = 0.6 = 0.01 = 0.2
Simulations 55 Notice that the error in the measurement established for the gyro in the table above is provided by the gyroscope simulator. With these values, is plotted the heading estimation: Fig. A.6 Heading estimation with the Kalman filter for the magnetometer and gyro fusion. It can be observed that is pretty accurate, but to have a better and more accurate solution is needed to adjust the Kalman filter parameters and see how they affect to the heading estimation. Fig. A.7 Heading estimation evolution by setting more covariance in the magnetometer measurements than in the gyroscope measurements.
56 Magnetometer integration into an IMU/GNSS positioning system Table A.4 Measurement covariance matrix values applied in Fig. A.7. Measurement noise covariance matrix Magnetometer Diagonal with 1.872 [uT2] Gyroscope Diagonal with 7.6154·10-7 (rad/s2)2 In Fig. A.7 is shown the behavior of the heading when the gyroscope data is more trusted. To obtain it, is modified. Its covariances are bigger in the part of the magnetometer (Table A.4), which means that there is more error in the measurement of the magnetometer than in the gyro. Fig. A.7 represents in purple this output, and it can be observed that is a very accurate solution because it follows closely the true heading trajectory. Fig. A.8 Heading estimation evolution by setting more covariance in the gyroscope measurements than in the magnetometer measurements. Table A.5 Measurement covariance matrix values applied in Fig. A.8. Measurement noise covariance matrix Magnetometer Diagonal with 0.1525 in component (1,1), 0.2028 in (2,2), and 0.2305 in (3,3) [uT2] Gyroscope Diagonal with 1.0966 (rad/s2)2 To obtain Fig. A.8, is modified again, now giving more error in the measurement of the gyro instead of the magnetometer (Table A.5). The purple
Simulations 57 line follows the true heading curvature but now is less accurate, it has more oscillation. With these results seems that trusting the gyro provides a better solution, but here is being supposed that the initialization is pretty accurate. Though it always has an error, because the heading is calculated in the first iteration (when there is no previous state yet) with equation (4.5), the error in the initialization is small, so the inertial sensors (with a little help of the magnetometer data) can estimate a good heading along the trajectory (Fig. A.7). In the next plot is shown the heading estimation when the initialization has an error of 45 degrees and the magnetometer covariances are bigger than the gyro covariances: Fig. A.9 Heading estimation evolution by setting more covariance in the magnetometer measurements than in the gyroscope measurements and with an error in the initialization. In Fig. A.9 is observed that the Kalman filter with no good initial conditions does not work as well as in the case before. It conserves the error, represented as an offset, along the entire trajectory and only converges slowly to the true value. This happens, because even though the measurement covariance matrix is smaller in the gyro, the magnetometer data is still mixed in the Kalman filter and the magnetometer measures have an absolute reference that is able to correct the heading bias.
58 Magnetometer integration into an IMU/GNSS positioning system Fig. A.10 Heading estimation evolution by setting more covariance in the gyroscope measurements than in the magnetometer measurements and with an error in the initialization. Now in Fig. A.10, there is the same error in the initialization too, but the magnetometer gets to converge to the true heading trajectory more quickly. Thus, the magnetometer helps the gyro to reduce the error between the estimated heading and the true trajectory. The error covariance in the prediction of the heading, , is represented for both cases (when it has an error of 45 degrees in the initialization): Fig. A.11 of the heading estimation evolution by setting more covariance in the magnetometer measurements than in the gyroscope measurements and with an error in the initialization.
Simulations 59 The covariance variation in Fig. A.11 starts in the initialization value and changes slowly to a single value to remain more or less constant. The covariance variation grows because the system has a higher value than the established in the initialization. The transition to a single constant value is slow because the magnetomoter covariances are big in Fig. A.9. Fig. A.12 of the heading estimation evolution by setting more covariance in the gyroscope measurements than in the magnetometer measurements and with an error in the initialization. The covariance in Fig. A.12 starts with a small initialization value and quickly finds a stable value in which remains constant. The covariance variation grows because, like before, the system has a higher value than the established in the initialization. Now the transition to a stable value is faster, that is because the covariances of the magnetometer are smaller so it is weighted in its favor. Notice that the constant value of is established near 1.5 while in Fig. A.11 is established in approximately 100. The error covariance in the prediction of the heading is bigger when the magnetometer covariances are bigger, which means that the magnetometer reduce the noise in the prediction and provides a more accurate heading estimation. Finally, it is represented the behavior of the heading estimation for different process noise covariance matrix values. While is the initial error in the prediction, represents the uncertainty that has the system model in the prediction. The following table summarizes the chosen values:
66 Magnetometer integration into an IMU/GNSS positioning system for k=1:1:length(data_calibrated_mag) %pitch Att_theta=obs_attitude.pitch(k); %roll Att_phi=obs_attitude.roll(k); mag_x=data_calibrated_mag(1,k); mag_y=data_calibrated_mag(2,k); mag_z=data_calibrated_mag(3,k); if k==1 %Magnetic heading measurement (10.6). Initializing yaw Att_psi_mag = atan2(mag_y * (-cos(Att_phi)) + mag_z * sin(Att_phi),... mag_x * cos(Att_theta) + mag_y * sin(Att_phi) * sin(Att_theta) + mag_z * cos(Att_phi) * sin(Att_theta)); %Initializing rate psi_rate = 0; %Magnetic heading estimation x_previous=[Att_psi_mag; psi_rate]; %state covariance matrix (error in the estimate) p_previous = zeros(2); %[1 0; 0 1]; p_previous(1,1)=degtorad(58)^2; %58 p_previous(2,2)=degtorad(58)^2; else %Measurement Y = [mag_x; mag_y; mag_z]; %Time update ("predict") x_pred = A*x_previous; p_pred = A*p_previous*A' + Q; H = [(-cos(Att_theta))*cos(0)*Bm*sin(x_previous(1,1)) 0;... ((-sin(Att_phi))*sin(Att_theta)*cos(0)*Bm*sin(x_previous(1,1))- cos(Att_phi)*cos(0)*Bm*cos(x_previous(1,1))) 0;... ((- cos(Att_phi))*sin(Att_theta)*cos(0)*Bm*sin(x_previous(1,1))+sin(Att_phi)*cos(0)*Bm*cos(x _previous(1,1))) 0]; h = [(-cos(Att_theta))*cos(0)*Bm*sin(x_pred(1,1)) 0;... ((-sin(Att_phi))*sin(Att_theta)*cos(0)*Bm*sin(x_pred(1,1))- cos(Att_phi)*cos(0)*Bm*cos(x_pred(1,1))) 0;... ((- cos(Att_phi))*sin(Att_theta)*cos(0)*Bm*sin(x_pred(1,1))+sin(Att_phi)*cos(0)*Bm*cos(x_pre d(1,1))) 0]; %Kalman gain K = p_pred*H'*inv(H*p_pred*H' + R); %Update estimate with measurement x_updated = x_pred + K*(Y-h*(x_pred-x_previous)); %Update the error covariance p_updated = (I-K*H)*p_pred; x_previous = x_updated; p_previous = p_updated; end if (x_previous(1)>pi) x_previous(1)=x_previous(1)-2*pi; end if (x_previous(1)<-pi) x_previous(1)=x_previous(1)+2*pi; end heading_and_rate_estim(:,k) = [x_previous(1); x_previous(2)]; end
Code 67 D. Code for the Kalman filter for the gyro and magnetometer fusion function [attitude_estim, error_prediction] = kalman_inertial_mag_fusion(imu_obs_BODY, data_calibrated_mag, gnss_obs_ECEF, decli, scale) for k=1:1:length(imu_obs_BODY.f) %process noise covariance matrix Q = eye(6)*degtorad(58)^2;%10 %noise parameters noise_SD_uT=3.34; %SD divided by SC (obtained in the calibration) cov_x = (noise_SD_uT*noise_SD_uT)/scale(1); cov_y = (noise_SD_uT*noise_SD_uT)/scale(2); cov_z = (noise_SD_uT*noise_SD_uT)/scale(3); %sensor noise covariance matrix (error in the measurement) R = zeros(6); R(1,1) = cov_x; R(2,2) = cov_y; R(3,3) = cov_z; % R(1,1) = 1.87^2; %magnetic field normalized ^2 % R(2,2) = 1.87^2; %1.87^2 % R(3,3) = 1.87^2; R(4,4) = (degtorad(3)/ 60)^2; R(5,5) = (degtorad(3)/ 60)^2; R(6,6) = (degtorad(3)/ 60)^2; % R(4,4) = degtorad(60)^2; %(rad/s^2)^2 % R(5,5) = degtorad(60)^2; %degtorad(60)^2 % R(6,6) = degtorad(60)^2; %Identity matrix I = eye(6); %Time variation AT = 0.1; %Magnitude of the total magnetic flux density Bm = 1; Earth_rotation = 7.2921159*10e-5; %rad/s A = [1 0 0 AT 0 0;... 0 1 0 0 AT 0;... 0 0 1 0 0 AT;... 0 0 0 1 0 0;... 0 0 0 0 1 0;... 0 0 0 0 0 1]; if (k==1) %pitch Att_theta=atan(- imu_obs_BODY.f(1,1)/sqrt(imu_obs_BODY.f(2,1)^2+imu_obs_BODY.f(3,1)^2)); %roll Att_phi=atan2(-imu_obs_BODY.f(2,1),-imu_obs_BODY.f(3,1)); mag_x=data_calibrated_mag(1,k); mag_y=data_calibrated_mag(2,k); mag_z=data_calibrated_mag(3,k); %yaw. Magnetic heading measurement (10.6) Att_psi_m = atan2(mag_y * (-cos(Att_phi)) + mag_z * sin(Att_phi),... mag_x * cos(Att_theta) + mag_y * sin(Att_phi) * sin(Att_theta) + mag_z * cos(Att_phi) * sin(Att_theta)); Att_psi = Att_psi_m + decli(1,k);
68 Magnetometer integration into an IMU/GNSS positioning system %Initializating rates phi_rate = 0; %roll rate theta_rate = 0; %pitch rate psi_rate = 0; %yaw rate x_previous = [Att_phi; Att_theta; Att_psi; phi_rate; theta_rate; psi_rate]; %state covariance matrix (error in the estimate) p_previous = eye(6)*degtorad(58)^2;%10 else %Eq.2.15 % Euler angles to Attitude matrix is equivalent to rotate the body % in the three axes: Ax= [1 0 0 ; 0 cos(x_previous(1)) sin(x_previous(1)); 0 -sin(x_previous(1)) cos(x_previous(1))]; Ay= [cos(x_previous(2)) 0 -sin(x_previous(2)); 0 1 0; sin(x_previous(2)) 0 cos(x_previous(2))]; Az= [cos(x_previous(3)) sin(x_previous(3)) 0; -sin(x_previous(3)) cos(x_previous(3)) 0 ; 0 0 1]; C_n_b=Ax*Ay*Az; % Attitude expressed in the LOCAL FRAME (NED) r_estim_eb_e=gnss_obs_ECEF.r(:,k); [Lat_deg, Long_deg, Heigh] = cart2geo(r_estim_eb_e(1), r_estim_eb_e(2), r_estim_eb_e(3), 5); %ECEF -> WGS84 geographical Lat_rad=pi*Lat_deg/180; Long_rad=pi*Long_deg/180; % Calculate ECEF to NED coordinate transformation matrix using (2.150) cos_lat = cos(Lat_rad); sin_lat = sin(Lat_rad); cos_long = cos(Long_rad); sin_long = sin(Long_rad); C_e_n = [-sin_lat * cos_long, -sin_lat * sin_long, cos_lat;... -sin_long, cos_long, 0;... -cos_lat * cos_long, -cos_lat * sin_long, -sin_lat]; %ECEF to NED C_n_e=C_e_n'; %NED to ECEF %C_b_e=C_n_e*C_b_n; C_e_b=C_n_e'*C_n_b; %Earth rotation in ECEF WE_e = [0; 0; Earth_rotation*AT]; %Earth rotation in body frame WE_b = C_e_b*WE_e; %Measurement mag_x=data_calibrated_mag(1,k); mag_y=data_calibrated_mag(2,k); mag_z=data_calibrated_mag(3,k); Y = [mag_x; mag_y; mag_z; imu_obs_BODY.w(:,k)]; %Time update ("predict") x_pred = A*x_previous; p_pred = A*p_previous*A' + Q; %Matrix H from the mag Kalman H_mag = [(-cos(x_previous(2)))*cos(0)*Bm*sin(x_previous(3)-decli(1,k));... ((-sin(x_previous(1)))*sin(x_previous(2))*cos(0)*Bm*sin(x_previous(3)- decli(1,k))-cos(x_previous(1))*cos(0)*Bm*cos(x_previous(3)-decli(1,k)));... ((-cos(x_previous(1)))*sin(x_previous(2))*cos(0)*Bm*sin(x_previous(3)- decli(1,k))+sin(x_previous(1))*cos(0)*Bm*cos(x_previous(3)-decli(1,k)))]; %Matrix H H = zeros(6); H(1:3,3) = H_mag; H(4:6,4:6) = C_n_b; %C_b_n' %Kalman gain K = p_pred*H'*inv(H*p_pred*H' + R);
Code 69 delta_att_n=(x_pred(1:3)-x_previous(1:3)); x_p = [delta_att_n; x_pred(4:6)]; W_rate =[0; 0; 0; WE_b]; %Update estimate with measurement x_updated = x_pred + K*(Y-(H*x_p+W_rate)); %Update the error covariance p_updated = (I-K*H)*p_pred; x_previous = x_updated; p_previous = p_updated; end if (x_previous(1)>pi) x_previous(1)=x_previous(1)-2*pi; end if (x_previous(1)<-pi) x_previous(1)=x_previous(1)+2*pi; end if (x_previous(2)>pi) x_previous(2)=x_previous(2)-2*pi; end if (x_previous(2)<-pi) x_previous(2)=x_previous(2)+2*pi; end if (x_previous(3)>pi) x_previous(3)=x_previous(3)-2*pi; end if (x_previous(3)<-pi) x_previous(3)=x_previous(3)+2*pi; end attitude_estim(:,k) = x_previous; error_prediction(k) = p_previous(3,3); end end
70 Magnetometer integration into an IMU/GNSS positioning system BIBLIOGRAPHY [1] Groves, P. D., Principles of GNSS, Inertial, and Multisensor integrated Navigation Systems, Artech House Publishers, London (2008). [2] Pares Calaf, M. E., “On de development of a magnetometer simulator”, CTTC, pg. 1-3 (2016). [3] Ozyagcilar, T., “Calibrating and eCompass in the Presence of Hardand Soft-Iron Interference”, Freescale Semiconductor, (2013). [4] Ozyagcilar, T., “Implementing a Tilt-Compensated eCompass using Accelerometer and Magnetometer Sensors”, Freescale Semiconductor, (2013). [5] Renaudin, V., Afzal, M. H. and Lachapelle, G., “Complete Triaxis Magnetometer Calibration in the Magnetic Domain”, Hindawi Publishing Corporation, (2010). [6] Won, D., Ahn, J., Sung, S., Heo, M., Im, S. H. and Lee, Y. L., “Performance Improvement of Inertial Navigation System by Using Magnetometer with Vehicle Dynamic Constraints”, Hindawi Publishing Corporation, (2015). [7] Zhou, J., Traugott, J., Scheringer, B., Miranda, C. and Kipka, A., “A New Integration Method for MEMS Based GNSS/INS Multi-sensor System”, Trimble TerraSat GmbH and Applanix Corporation, (2015). [8] Welch, G. and Bishop, G., “An Introduction to the Kalman Filter”, University of North Carolina at Chapel Hill, (2006). [9] Levy, L. J., “The Kalman Filter: Navigation’s Integration Workhorse”, The Johns Hopkins University, (2002). [10] InvenSense, San Jose (California), MPU-9250 Datasheet, January 2014. [11] “International Geomagnetic Reference Field”, Accessed March 16, 2016, http://www.ngdc.noaa.gov/IAGA/vmod/igrf.html [12] “Ellipsoid fit – File Exchange”, Accessed April 2, 2016, https://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoidfit [13] “European Space Agency – Navipedia”, Accessed May 30, 2016, http://www.navipedia.net/index.php/User_Guides
Bibliography 71 [14] “Euresearch – Horizon 2020”, Accessed April 27, 2016, https://www.euresearch.ch/de/european-programmes/horizon-2020/ [15] “Earth’s magnetic field”. Accessed March 3, 2016, http://phys106spring10.pbworks.com