Autore:
Simone Sabatelli
Firma__________
Relatori:
Prof. Luca Fanucci Firma__________
Prof. Roberto Saletti Firma__________
ING-INF/01 ELETTRONICA
Advanced algorithms and architectures for
MEMS inertial sensor platforms in
orientation tracking and in fall detection
applications
Anno 2013
UNIVERSITÀ DI PISA
Scuola di Dottorato in Ingegneria “Leonardo da Vinci”
Corso di Dottorato di Ricerca in
Tecnologie, dispositivi e sistemi micro e nanoelettronici
Tesi di Dottorato di Ricerca
iii
To my parents that sustained me during all these years of study
v
SOMMARIO
In questo lavoro di tesi ci siamo occupati delle applicazioni dei sensori inerziali a sistemi complessi. La prima parte del lavoro ha riguardato lo sviluppo di un sistema per la stima della posizione angolare. L’obiettivo era ottenere un sistema integrato con la logica di gestione di un sensore inerziale, nel caso specifico un sensore 6 DoF (Degrees of Freedom) giroscopio più accelerometro. Sono stati scelti i quaternioni per rappresentare la posizione angolare grazie alla loro versatilità e facile manipolazione algebrica. È stato ricavato un algoritmo derivato dal filtro di Kalman, con diverse semplificazioni per ridurre la complessità computazionale. In particolare, per migliorare la precisione e utilizzare il sistema anche con sensori 9 DoF con l’aggiunta di una bussola magnetica, è stato inventato un filtro di Kalman a doppio stadio, in cui si è separato in due stadi l’utilizzo dei dati dell’accelerometro dai dati della bussola magnetica nelle equazioni di correzione della posizione angolare. Il sistema è stato modellato con Matlab Simulink, che ci ha permesso di validare i nuovi algoritmi e le semplificazioni effettuate sia con dati simulati che con dati reali acquisiti. Successivamente siamo passati alla creazione di un modello Simulink dell’algoritmo in fixed point, valutando in 20 il numero minimo di bit necessario per ottenere la precisione sperata nella ricostruzione della posizione angolare. Si è deciso quindi di progettare un ASIP (Application Specific Instruction Set Processor) in grado di processare l’algoritmo e garantire la minima occupazione di area e il minimo consumo di potenza con una ALU a 20 bit. È stato realizzato un prototipo del sistema su FPGA utilizzando i sensori SensorDynamics SD746 per giroscopio e accelerometro e Honeywell HMC5843 per la bussola magnetica. È stata realizzata una demo 3D real time. Infine, il sistema è stato testato con delle apparecchiature di laboratorio in grado di eseguire rotazioni ripetute con precisione, per le quali è stata realizzata un’interfaccia di comando LabView. La precisione risultante è stata di 1 grado per rollio e beccheggio e 3 gradi per l’imbardata.
Il secondo tema ha riguardato lo sviluppo di un sistema di fall detection per persone anziane. Era già in sviluppo presso i laboratori dell’università un prototipo basato su un algoritmo innovativo in grado di distinguere le ADL (Activities of Daily Living) dalle cadute, riducendo il numero di falsi positivi. Quando una caduta viene rilevata, viene lanciato un allarme con un modulo GSM. In questo lavoro ci siamo occupati della progettazione di una board indossabile compatta, avente dimensioni di 5x8 cm2 e con durata stimata della batteria superiore a una settimana.
vii
ABSTRACT
In this PhD thesis we carried out a research on the applications of inertial sensors to complex systems. The first part of the work deals with the development of an angular position estimation system. The goal was to obtain an integrated system within the sensor logic, in particular for a 6 DoF (Degrees of Freedom) gyro plus accelerometer sensor. Quaternions were chosen to represent the angular position for their flexibility and easy algebraic manipulation. A sensor fusion algorithm derived from the Kalman filter was developed. Several simplifications were done to reduce its computational complexity. In particular, a double stage Kalman filter was invented to improve the precision with the use of 9 DoF sensors data, with the addition of a magnetic compass. The first filter stage used the accelerometer data, while the second filter stage used the magnetic compass data to correct the angular position. The system was modeled with Matlab Simulink and the algorithms were validated with both synthetic and real acquired data. A Simulink bit true model of the system was developed, using fixed point arithmetic. The experiments shown that 20 bits are necessary to reach the target precision. It was chosen to design an ASIP (Application Specific Instruction Set Processor) to process the algorithm using a 20 bit ALU with minimal area occupation and power consumption. An FPGA prototype was realized using the SensorDynamics SD746 for the gyro plus accelerometer and the Honeywell HMC5843 for the magnetic compass. A real time 3D demo was realized. Moreover, the system was tested in the laboratory with repeatable rotations. A LabView program was designed to command the test machines. The resulting precision of the prototype was 1 deg for roll and pitch and 3 degs for the yaw.
The second part of the work is about a fall detection system for elderly people. A first prototype of this system was under development in the University laboratories. It was based on an innovative algorithm that can recognize the ADL (Activities of Daily Living) from a fall to reduce the false alarms. When a fall is detected, the system launches an alarm with a GSM module. In this work, a wearable compact board with 5x8 cm2 dimensions was designed. The expected battery life is over
ix
INDEX
INTRODUCTION ... 1
1.
CHAPTER 1 AHRS DEFINITION AND CURRENT WORKS ... 9
1.1 Definition ... 9
1.2 State of the art ... 9
1.3 Commercial products ... 11
1.4 Specifications for the current work ... 16
2.
CHAPTER 2. SYSTEM DEFINITION ... 19
2.1 Algorithm definition ... 19
2.2 System state equation ... 20
2.3 Two stage Kalman filter ... 21
2.3.1 Algorithm description for the two stage Kalman filter ... 22
2.4 Time discrete transformation ... 24
2.5 Algorithm summary ... 24
2.6 Simulink model ... 25
3.
CHAPTER 3. BIT TRUE MODEL FOR ASIP IMPLEMENTATION . 31
3.1 Algorithm simplifications ... 31
3.2 Filter tuning ... 33
3.3 Fixed point Simulink model ... 34
4.
CHAPTER 4. ASIP DESIGN ... 43
4.1 Architecture definition ... 43
4.2 ASIP architecture ... 44
4.3 Programmed algorithm ... 47
4.4 Synthesis and verification... 49
5.
CHAPTER 5. FPGA PROTOTYPE AND VALIDATION TESTS ... 51
5.1 Sensor characteristics ... 52
5.2 Test equipment ... 53
5.3 System tests ... 57
6.
CHAPTER 6. FALL DETECTION SYSTEM FOR ELDERLY
PEOPLE ... 67
6.1 Fall detection systems for elderly people ... 67
6.1.1 Commercial devices ... 68
6.2.1 Algorithm description ... 72
6.2.2 Hardware implementation ... 73
6.2.3 Board layout... 77
6.3 Experimentation and test results ... 79
CONCLUSIONS ... 81
APPENDIX A: QUATERNIONS AND ROTATIONS ... 83
A.1 Rotations in a two-dimensional space ... 83
A.2 Rotations in the tridimensional space ... 83
A.3 Quaternions ... 84
A.4 Quaternions and their application to rotations ... 85
APPENDIX B: THE KALMAN FILTER ... 89
B.1 Theoretical description of the Kalman filter ... 89
B.2 The discrete Kalman filter algorithm ... 91
B.3 The extended Kalman filter ... 93
REFERENCES ... 97
1
INTRODUCTION
Nowadays the MEMS (Micro Electro-Mechanical Systems) are widely diffused in everyday technology. A MEMS is a device built with components with a dimension between 1 to 100 µm. They include both mechanical and electronic components and they are one of the most promising technologies of the XXI century, because they can be used in a large variety of applications.
MEMS have a big success because they are manufactured with silicon, using the same production processes that are used for the production of the electronic circuits. The microstructures are built in the silicon with subsequent depositions of material layers, patterning by photolithography and etching to produce the required shapes. The analog or mixed analog/digital control logic of the MEMS sensor or actuator is usually on a separate chip.
Figure 0.1 – A MEMS gyro microstructure
The MEMS applications can be divided into two big fields: the sensing applications and the actuating applications (see Figure 0.2). In the first case, a MEMS is used as sensor to measure some quantity, while in the second case a MEMS is used as actuator.
One of the first applications of a MEMS to a large scale commercial product is as an actuator in the ink-jet head for printers. The printer head is a MEMS that can eject ink drops of only one picoliter in today’s printers. The other big application of MEMS is in sensing: MEMS sensors like accelerometers and gyroscopes are nowadays widely used. One of the first applications of this kind was in automotive safety with the accelerometer for air-bag applications. Actually gyros and accelerometers are used for the Electronic Stability Control (ESC), for rollover detection and also for active suspension systems. In consumer applications,
MEMS are experimenting a massive adoption in many products. The first application in this case as well was the accelerometer for a low cost, reduced dimensions and low power consumption use with respect to other sensors, like the gyro. The accelerometer was first used for display orientation in smartphones and for gaming applications. Actual smartphones commonly use also a gyro or combo sensor, often in combination with a magnetic compass. They are used for advanced applications, like navigation, gaming and gesture recognition. Another common application is for image stabilization in digital cameras.
As we can see from Fig. 0.3 and 0.4, the MEMS market is quickly growing from 10.2 Billion dollars in 2011 to the forecast of 21 Billion dollars in 2017, with a Compounded Annual Growth Rate (CAGR) of 13%. This is a very big growth rate also in the field of electronics market. We can see that some older technology, like the printer heads, are quite stable. Conversely, other mature technologies, like accelerometers, gyroscopes and magnetic compasses, are experiencing a real explosive growth, with an increase of up to 788% in the number of units sold within three years, from 2009 to 2012, as we can see in Fig. 0.5. The commercial value of the compasses has only doubled, however, so that a big price drop was also experimented in the same period. The other types of MEMS that are experimenting a big growth are the microphones and the oscillators, also thanks to their adoption in smartphones and tablets.
Other MEMS are emerging technologies that surely will experience a large diffusion in the next years. They are gas sensors, microbolometers (infrared sensors for thermal cameras), autofocus applications for miniaturized lenses, microdisplays, MEMS speakers and energy harvesting devices.
Mobile applications, like smartphones and tablets, are the driver for future growth: today’s smartphones have as many as 12 MEMS chips and they will have as many as 20 in the near future. Other predicted killer applications will be thermal cameras for low-light security imaging, gas sensors for everything from breath analysis to air pollution control, and micro-speakers. Energy harvesting also is a very interesting field for the realization of diffuse sensor systems that must be powered with small batteries.
3
Figure 0.2 – Types of MEMS and their applications
Figure 0.4 – MEMS market value forecast
Figure 0.5 – MEMS types and their sales increase in number of units from 2009 to 2012
Among the MEMS manufacturers, last year STMicroelectronics was the biggest player, especially for the consumer MEMS. Bosch is the second one, but it is mainly focused on the automotive market, even if in the last years it is growing also within the consumer sector. After them there’s Texas Instruments, which is famous
5
for the digital light processors and picoproiectors, and Hewlett Packard for ink-jet cartridges.
Figure 0.6 – Top 30 MEMS players with their sales in 2012
The reduction of MEMS dimensions and of their power consumption, the design of combo sensors and a contemporary price drop will push the adoption of MEMS for all these applications, with the integration of additional functions to the devices due to the availability of more sensors at a lower cost.
In this PhD thesis we concentrated on MEMS sensors, in particular on the applications of accelerometers, gyros and magnetic compasses. In fact, these sensors are some of the most successful sensors and they can be used for motion sensing applications. In particular, the increasing diffusion of mobile devices like smartphones and tablets is driving the request of sensors so as to have context aware devices. A great effort is spent to have more user-friendly devices, with more intuitive human interfaces, and to design more accurate personal navigation systems that can work also indoor, without GPS signals. It is also important to have devices that can understand what the user is doing, for example if the user is walking, or if he enters in a car and probably is driving, or if he is doing physical activity. All these informations are collected from the sensors of the device and can be used to adapt the functionality of the device itself, for example to not disturb the driver with an incoming call or to automatically activate the speakerphone, for personal navigation purposes or for personal training programs related to the physical activity.
Inertial sensors also feature many complex applications in consumer electronics. We can mention the use of these sensors for image stabilization in cameras or video cameras or even the use of sensors for high-end toys, for example radio-controlled helicopters or quadricopters.
The cost of gyros has rapidly decreased in the last years to a few dollars and even less than one dollar for an accelerometer. We can also find combo devices that combine more sensors in a single package, for example 6 DoF (Degree of Freedom) devices that combine a triaxial gyro and a triaxial accelerometer, or 9 DoF devices that combine a gyro and an accelerometer with a magnetic compass. A key differentiator factor in current applications is the processing of the informations that the sensors provide. This is necessary because many different sensors can be found in a device, but the device, to be context aware, must process the raw data from the different sensors to obtain derived information. This data fusion and data processing is the key to a variety of new applications and to content aware and user friendly devices.
For this reason, this PhD thesis focuses on applications derived from the processing of the signals of inertial sensors. The research activity is mainly divided into two main activities:
(i) the development of an integrated Attitude and Heading Reference System (AHRS) from a 9 DoF sensor;
(ii) the development of a compact fall detection system for elderly people. The first activity is focused on the development of a system integrated within the sensor logic of a 6 DoF sensor to process the raw sensors data in order to estimate the angular position of the device. This work includes the selection and invention of a sensor fusion algorithm and the design of an Application Specific Integrated Processor (ASIP) to process the algorithm itself. An algorithm derived from the Kalman filter was selected and many simplifications invented, while quaternions are used to represent the angular position. A prototype of this system was built on FPGA and successfully tested. It is interesting to see that this is a hot topic in current scientific research. For example, in Table 0.1 it is possible to see how many publications are present in the IEEE library with the keyword “Kalman AND quaternion”.
As it is apparent in Table 0.1 and in Fig. 0.7, it is very noticeable that, apart from some isolated publications before 2006, the development of sensor fusion algorithms based on Kalman filters and quaternions did not widely started in scientific research before 2006, whilst the first boom is in the year 2010, just when this PhD activity was planned.
7 Search criteria Kalman AND quaternion Database IEEEXplore Year Number of pubs 1985 1 1986 0 1987 0 1988 0 1989 1 1990 2 1991 1 1992 1 1993 1 1994 1 1995 2 1996 4 1997 3 1998 3 1999 3 2000 1 2001 3 2002 2 2003 6 2004 6 2005 4 2006 16 2007 14 2008 14 2009 15 2010 29 2011 41 2012 45
Table. 0.1 - Number of publications for each year in the IEEE database with the keywords “Kalman AND quaternion”
Figure 0.7 - Number of publications for each year in the IEEE database with the keywords “Kalman AND quaternion”
Regarding the second research activity performed in the PhD, it was focused on the development of a fall detection system for elderly people. This is also an area of great interest, because of the increase of the life expectance. The main problem is that elderly people may live alone, and a quick alarm is decisive to avoid further injuries in the event of fall, and also for the psychological serenity of the elderly person. For this reason thedesign was started of a compact board that includes a microcontroller, the inertial sensors and a GSM module so as to have as the final product a compact, wearable and low power fall detection module that can launch an alarm in the event of a fall.
This PhD thesis is organized as follows: the first chapter analyzes the state of the art for AHRS systems and explains the approach used in the products manufactured by some companies; the specifications for our product are derived from these products. The second chapter explains the algorithm that was chosen for our AHRS system and the motivations for the choice of this algorithm. It is also described the algorithm development and the building of the Matlab Simulink model to validate the algorithm itself. In the third chapter, the simplifications to obtain the final algorithm are presented with the tests done to validate the model; the definition of the fixed point Simulink model is also presented. The fourth chapter illustrates the ASIP design with the architecture definition. In the sixth chapter the FPGA prototype with a real time demo is illustrated. All the laboratory tests and measurements to validate the system are also presented. In the seventh chapter the fall detection system for elderly people is presented: a state of the art presentation is done, analyzing some commercial products and the development of some boards on the market. Then, our prototype is presented with its innovative approach and the new board design is described.
0 5 10 15 20 25 30 35 40 45 50 Nu m b e r o f p u b lic a ti o n s Year
9
1. CHAPTER 1 AHRS DEFINITION AND CURRENT WORKS
In this chapter an Attitude and Heading Reference System (AHRS) is defined and the current state of the art is analyzed. The specifications for our system are derived.
1.1 Definition
An AHRS must provide the information of the angular position of the device or of the sensor hub. In particular, the roll, pitch and yaw angles must be provided from the AHRS. The first thing to define in this case is a representation method for roll, pitch and yaw angles. It must be flexible and usable by the final user, but it must allow a robust representation for the internal engine of the algorithm. Easy transformations to other representation methods must also be possible without ambiguities.
Figure 1.1 – Roll, pitch and yaw angles
1.2 State of the art
Great amounts of research were done in the last years in the field of sensor fusion algorithms. The research for dead reckoning systems and for the pedestrian navigation recently pushed this sector. All these systems are based on Inertial Measurement Units (IMU), often with the addition of a magnetic compass. All these researches are focused on the improvement of the precision of a single sensor with the combination of the data from multiple sensor units, like 6 DoF or 9 DoF inertial sensors. This work is focused on the angular position estimation with a gyro, an
accelerometer and a magnetic compass. This is a fundamental step for the development of more complex systems, like dead reckoning units for indoor pedestrian navigation. In fact, in a dead reckoning system it is first necessary to estimate the angular position from the inertial sensors. Then, after the angular position estimation, it is possible to calculate the expected gravity vector, which must be subtracted to the measured acceleration to obtain the dynamic acceleration, which must be integrated to obtain the velocity and the movement of the sensors. In [1] the errors that occur in an existing dead reckoning unit are analyzed and it is explained that the main error source in such systems is not the accelerometer noise but the incorrect gravity vector estimation, due to the incorrect angle estimation. This is one of the reasons why we are concentrated in an optimal angular estimation system. The other strong reason is that we want to obtain a compact system to be integrated within the sensor logic. This will be part of a more complex system, like a dead reckoning system, a smartphone or so on.
Analyzing the literature, it was found that almost all the researches in angular estimation systems use quaternions to represent the angular position, because they are more flexible than Euler angles. Moreover, there is no problem with angular singularities and the system can be better linearized. Furthermore, quaternion can be easily converted in other rotation representation methods, like a rotational matrix or a sequence of Euler angles. All studies use some sensor fusion algorithm between the data of the different sensors to obtain the angular position. Again from [1], it is possible to see that the main problem during angular estimation is the sum along with the time of the errors of the gyro, due to the integration of angular rate to calculate the angular position. It can be seen from the theory that gyro angle random walk and the Gaussian white noise introduce an error in the angular estimation that grows with the square root of time, while the bias instability and constant biases on gyro due to calibration errors or to thermal or soldering effects give an error that grows linearly with time.
Now the different approaches to sensor fusion are considered. In [2] an Extended Kalman Filter (EKF) is used with three rate gyros and three accelerometers. The state equation is composed by the quaternion and also by the angular velocity of the gyro drift, so that a correction of the bias of the gyro is possible. The proposed system has a good estimation of roll and pitch angles, but only a low correction on yaw angle.
In [3], [4], [5] and [6] a complete MARG (Magnetic, Angular Rate and Gravity) sensor is used for angle estimation. The integration of the gyro data allows to obtain the new quaternion representing the angular position, while the accelerometer corrects the roll and pitch angles and the magnetic compass corrects the yaw angle. Different algorithmic approaches are used from the same author in these papers: in [3] and [4] the Gauss-Newton iterative algorithm is used to find the best quaternion that relates the measured accelerations and earth magnetic field in the body coordinate frame to the values in the earth coordinate frame. The best quaternion is used as part of the measurements for the Kalman filter. As a result of this approach, the measurement equations of the Kalman filter become linear and the computational requirements are significantly reduced, making it possible to estimate orientation in real time. In [5] and [6], instead of the Gauss-Newton method, an innovative QUaternion ESTimate (QUEST) algorithm is used to estimate the quaternion from accelerometer and magnetic compass data and this quaternion is used in a linear Kalman filter in combination with gyro data.
11
In [7] an Extended Kalman filter with adaptive gain is proposed. The filter weighs in a different way the gyro and the accelerometer data in the correction equation, depending on the dynamic state of the system. If the system is in a high dynamic state, the accelerometer data are less influential for the correction of the angular position and the gyro data have more weight; conversely, in a quiet state the accelerometer is more influential for the angular position correction.
In [8] an Extended Kalman filter is used with a 9 DoF sensor unit. The state of the system is composed by the quaternion and by the bias of the gyro. Correct gyro bias and angular position estimation are obtained.
In [9] an Unscented Kalman filter (UKF) is used instead of the Extended Kalman filter to cope with the system nonlinearities. This Unscented filter is deemed more accurate and cheaper to implement compared to the Extended Kalman filter. However, in [10] a comparison between the Extended Kalman filter and the Unscented Kalman filter for the use in quaternion motion is carried out. The conclusion is that the Unscented Kalman filtering performs equivalently with Extended Kalman filtering, but the Unscented filter resulted more computing intensive. For this reason and because of the quasi-linear nature of the quaternion dynamics, the Extended Kalman filter is a better choice for estimating quaternion motion in virtual reality applications.
In [11] a Scaled Unscented Kalman filter (SUKF) with quaternions state is designed for the determination of the attitude, velocity and position parameters in an inertial navigation system. The weighed mean computation for quaternions is derived in a rotational space as a barycentric mean with renormalization and a multiplicative quaternion-error is used for the predicted covariance computation of the quaternion.
In [12] the problem of the stabilization of the inverse pendulum is faced up. This is only a one-dimensional problem, with 1 DoF gyro and 1 DoF accelerometer. A complementary filter is used to obtain the vertical position of the inverse pendulum, in particular a high-pass filtering is used on the gyro data and a low-pass filtering is used on the accelerometer data.
1.3 Commercial products
Some of the most significative commercial products available are described in this paragraph.
1.3.1 XSense
At the beginning of the work on this PhD thesis, a state of the art commercial device was the XSense MTx [13]. This is a compact device with the form factor of a small box (58x58x22 mm) with a weight of 50 g and a power consumption of 350 mW. In this device three separate sensors are present to form a 9 DoF sensor unit: a three-axial gyro, a three-axial accelerometer and a three-axial magnetic compass, together with a microcontroller. The onboard microcontroller processes some proprietary sensor fusion algorithm and estimates the angular position of the device. It has to be noticed that the precision of the single sensors is not very high, because they are consumer grade devices: the declared gyro bias is 1 deg/s and the full scale range is only 300 deg/s. The sensor data update rate is 512 Hz, but the internal filter update rate is only 120 Hz. The declared static accuracy for roll
and pitch is 0.5 deg, 1 deg for the heading, while the dynamic accuracy is 2 deg RMS.
13
XSense advertises this product for the use in navigation and stabilization of robots and ROVs (Remote Operated Vehicles), orientation and stabilization of ships and marine sensors, camera and antenna stabilization. A version with the GPS, the MTi-G, was also commercialized. In this case, the output includes also the position, with a declared precision of 2.5 m.
A particular application of the MTx sensor commercialized by XSense is a wearable suit with 17 MTi sensors to reconstruct the human body movements. It is advertised for motion capture applications, for example for the special effect of the movies.
The latest product now available is the MTi-300 AHRS, featuring improved sensor performance and improved algorithm, thus allowing 0.25 deg max error on roll, pitch and yaw angles and 0.3 deg RMS (1 deg max) in dynamic performance for roll, pitch and yaw. The output max update rate is 2 kHz and the power consumption is between 675 and 950 mW.
1.3.2 STMicroelectronics
STMicroelectronics is one of the biggest supplier of inertial sensors for consumer devices. The core business of STMicroelectronics is in standalone sensors, though now combo devices are also available. For this reason, STMicroelectronics developed a sensor fusion software library, the iNEMO library [14]. This software library is freely distributed to the developers that use STMicroelectronics sensors. The software library is based on a Kalman filter and allows to obtain the angular position of the sensors with additional information, like the estimated gravity and the dynamic acceleration. This library can be integrated on Android or Windows 8 devices. Auto-calibration routines for the sensors and magnetic interference robustness are supported from the libraries.
The libraries can be used with only STMicroelectronics sensors and a demo board is released to the developers. At the beginning of this thesis, the available iNEMO board was the STEVAL-MKI062V2; it integrates two gyros (one for roll and pitch, the other for yaw), a combo module with a triaxial accelerometer and a triaxial magnetic compass, a pressure sensor, a temperature sensor and an STM32 ARM microcontroller with 32 bit architecture. The board form factor is 40x40 mm2 and it
has USB connectivity.
The new iNEMO board is more compact, only 13x13 mm2, it integrates a triaxial
gyro, a combo module with accelerometer and magnetic compass and a microcontroller with ARM M3 architecture. No pressure or temperature sensors are present.
Figure 1.4 – STMicroelectronics iNEMO board v2
Figure 1.5 – STMicroelectronics iNEMO M1 board
1.3.3 InvenSense
InvenSense was following a different approach to the sensor fusion. While XSense offers discrete modules that process sensor fusion algorithms and STMicroelectronics offers only software libraries, InvenSense has launched integrated sensors with a motion processor integrated inside [15]. This is the same approach that was used in this thesis work. It has to be noticed that this work started with a MS thesis in February 2010 and was continued with this PhD starting
15
from January 2011. The first announcement from InvenSense of this product was in November 2010.
Figure 1.6 InvenSense MPU-6000 combo sensor
The MPU-6000 and MPU-6050 are consumer grade sensors that integrate a monolithic 6 DoF sensor (gyro plus accelerometer) and a motion processor unit in a 4x4x0.9 mm3 package. The current consumption is only 3.9 mA. InvenSense
proposes a compact combo device with a very small form factor and a very low power consumption, capable to be interfaced via a master I2C with an external magnetic compass and to process itself the sensor fusion algorithm to calculate the angular position with 9 DoF sensor fusion. The integration of the motion processor within the sensor allows to discharge the application processor from the computational load due to the sensor fusion and to reduce the global power consumption.
Figure 1.7 – Block diagram of a system with the InvenSense MPU-6000 interfaced with an external magnetic compass
In 2012 it was announced the MPU-9150, a combo device with also the magnetic compass AK8975 within a 4x4x1 mm3 package, integrated with the motion
processor. In 2013 the MPU-9250 was announced, it is smaller (3x3x1 mm3) and
Figure 1.8 – InvenSense MPU-9150 system block diagram
Figure 1.9 InvenSense sensor fusion environment block diagram
The integration of more sensors in a single package, the integration of the motion processor within the sensor, the small package and the reduced power consumption made the InvenSense products winners in the present consumer electronics market: a lot of widespread products nowadays use InvenSense combo MEMS sensors, like some Samsung smartphones, Google tablets and so on. 1.3.4 Other commercial systems
Bosch has presented FusionLib, which is a software sensor fusion library with functions similar to the one from ST. It is available for microcontroller, Android and Windows 8. Advanced soft iron calibration algorithm and different power models to switch on and off the sensors are available.
Basic sensor fusion algorithms based on Kalman filters are natively implemented in Android and Windows 8 operating systems.
Aeronautical grade inertial modules for navigation are available at the cost of hundreds or thousands of dollars. An example is Analog Device ADIS16448, that in a 24.1x37.7x10.6 mm3 case houses a 10 DoF sensor unit (gyro + accelerometer +
magnetic compass + pressure sensors) with and embedded processor that runs an Extended Kalman filter algorithm.
1.4 Specifications for the current work
The purpose of this work is the development of an integrated system. In our case, we wanted to develop a sensor fusion algorithm that can work with both 6 DoF and 9 DoF sensors. The sensor fusion system must be integrated within the sensor logic of a combo sensor gyro plus accelerometer. For this reason, the use of an integrated processor must be evaluated, with the possibility to design an ASIP. The maximum area occupation of the integrated processor with TSMC 0.18 µm
17
technology must be less than 0.5 mm2 with a maximum of 500 µA of current
consumption. The system must work alone or in combination with an external magnetic compass that will help to improve its performances. The external magnetic compass must be interfaced with an I2C master interface.
The required precision should be 1 degree for the static accuracy and 0.5 deg for the RMS error for all the three angles. The filter update rate should be greater than 100 Hz and the filter should be able to track the human movements with a bandwidth of 40 Hz.
19
2. CHAPTER 2. SYSTEM DEFINITION
In this chapter the purpose of this work and the approach followed to build this sensor fusion system are illustrated. The algorithm that we developed is explained and the system modeling is described.
The first version of this angular position system was designed to work with a 6 DoF (gyro + accelerometer) inertial sensor. This combo sensor was in the prototyping stage at SensorDynamics and it was planned to integrate also the sensor fusion algorithm within the sensor in an updated version of this sensor.
In a previous work in SensorDynamics [16], the design center where this PhD thesis was started, a software demonstrator of an angular position system was made using SensorDynamics Cube demo, an array of three different sensors, each one with a gyro and an accelerometer, positioned to form a 6 DoF sensor unit. This demo was interfaced with a PC and rotation matrices were used to run an algorithm on the PC and to calculate the angular position.
A second version of the algorithm was subsequently developed. This algorithm was studied to work with 9 DoF sensors, by adding the use of a magnetic compass to improve the precision in the heading estimation. A two stage Kalman filter algorithm was invented to process all the data with reduced computational requirements. In our plans, the processing of the sensor fusion algorithm should have been integrated within the new 6 DoF IMU sensor logic and an external magnetic compass might be optionally used to have a better estimation of the yaw angle. For this reason, an I2C master capability was necessary on the IMU sensor to read autonomously the magnetic compass data.
2.1 Algorithm definition
The first issue to define is the method for the angular representation. Then, but the two things are also related, it is necessary to define a sensor fusion algorithm. It is necessary to keep in mind that the purpose of this work is to design an integrated sensor with sensor fusion capabilities, so the algorithm should require as less computing power as possible to be executed on a custom application processor (ASIP) to be integrated within the sensor logic.
2.1.1 Angular position representation method
Different representation methods are available for the angular position: a sequence of Euler angles, a rotational matrix and the quaternions.
An Euler angles sequence is composed by three angles and represents the sequence of three rotations that the object must complete to have the desired angular position. It is a human understandable representation of the angular position. The main problems of this kind of representation method are the singularities that may occurr with certain rotations, for example those with 90 deg of rotation. This is also called the gimbal lock problem. With these particular rotations there is also an highly discontinued representation of the angular position, because with an imperceptible rotation some angular values may jump from 0 to 90 deg or from 180 to -180 degrees. The other problem is that a sequence of rotations must be chosen to define this representation method. The most common representation is the aeronautic sequence, that is the rotations with respect the Z, Y and X axis, but this is not the unique Euler angles sequence.
The rotational matrix is a 3 x 3 matrix that allows rotating a 3 x 1 vector in another 3 x 1 vector. The calculations for this rotation are very easy, but the main problem is that for every new rotation the entire rotation matrix of 9 elements must be recalculated. Practically, we must instantiate a system featuring 9 states.
Quaternions are a rotation representation method that is more compact: in fact, only four elements are necessary. A quaternion is a hypercomplex number composed by a real part and by a vector part made of three elements. A rotation is uniquely represented by a quaternion, no singularities are present and the representation is quite compact and can be easily transformed in the other representation methods, like the rotational matrix or a sequence of Euler angles. Quaternions are also used in computer graphic to represent the position of 3D objects, for example in the DirectX or OpenGL libraries. In addition, it was seen from the literature that the use of quaternions with a Kalman filter allows the obtainment of a nonlinear filter, but that can be easily linearized, because its nature is not strongly nonlinear. For all these reasons, quaternions were chosen in this work for the internal representation of the angular position.
2.1.2 Sensor fusion algorithm
The majority of the scientific literature uses some kind of Kalman filter as sensor fusion algorithm. Commercial products also, starting from STMicroelectronics iNEMO libraries to the aeronautic high grade IMUs, use Kalman filters. Different type of Kalman filters are used: generally, because of the nonlinearity of the system state with the quaternions, it is necessary to use an Extended Kalman filter. However, in some works a linearization of the system is done with some tricks or by means of some auxiliary algorithm, like the Gauss-Newton method or the QUEST algorithm. An alternative is the use of the Unscented Kalman filter, but as seen in [10] it does not seem convenient for the complexity of the system.
In this work, the traditional Extended Kalman filter was initially chosen. It will assure the required precision with an acceptable complexity for sensor fusion with a 6 DoF sensor. For the use of a 9 DoF sensor, some simplifications and some tricks will be used instead of the traditional Kalman filter in order to assure that the algorithm will be executable from the ASIP. The use of some linearization algorithms like the Gauss-Newton method was initially excluded because of the linearization algorithm complexity. The inclusion of the gyro biases in the system state was also excluded because the performance improvements are negligible with respect to the increase of the system complexity.
2.2 System state equation
Before starting it is necessary to define two reference systems. The first is the inertial reference system, called the ground frame and denoted with the subscript
n; we can imagine it solidly anchored to the Earth. The other reference system is
the body frame, denoted with the subscript b, which will be solidly anchored with our sensors.
The system state x will be a four element vector only, that is the quaternion representing the angular position.
𝑥 = [𝑞𝑛𝑏]𝑇 = [𝑞0, 𝑞1, 𝑞2, 𝑞3]𝑇 (2.1)
The system state is fully defined from the quaternion 𝑞𝑛𝑏 that identifies the angular
21
It is necessary to describe the evolution of the system with an equation. We define with 𝜔𝑛𝑏𝑏 the vector with the angular velocities of the body frame with respect to the
ground frame:
𝜔𝑛𝑏𝑏 = [𝜔𝑥, 𝜔𝑦, 𝜔𝑧]𝑇 (2.2)
From quaternion algebra it is possible to write the differential equation that describes the system evolution:
𝑞̇𝑛𝑏=12Ω𝑛𝑏𝑛 𝑞𝑛𝑏 (2.3)
In the previous formula Ω𝑛𝑏𝑛 is the matrix with the angular velocities of the system that is built starting from vector (2.2).
Ω𝑛𝑏𝑛 = [ 0 −𝜔𝑥 −𝜔𝑦 −𝜔𝑧 𝜔𝑥 0 𝜔𝑧 −𝜔𝑦 𝜔𝑦 −𝜔𝑧 0 𝜔𝑥 𝜔𝑧 𝜔𝑦 −𝜔𝑥 0 ] (2.4)
With the previous equations the system evolution, corresponding to the so called a
priori update equation of the Kalman filter equation, is fully characterized using the
data of the triaxial gyro only.
Now it is necessary to write the read equation to have the a posteriori correction of the system state with the Kalman filter. This will be done using the accelerometer and magnetic compass data.
2.3 Two stage Kalman filter
We faced up many computational problems during the implementation of the AHRS with the 9 DoF sensors. The first problem that was evidenced is the large amount of operations that is necessary to perform with the implementation of a classical Kalman filter. In fact, in this case the measurement equation is composed by a 6 element vector, with both the acceleration and the magnetic field.
𝑧 = [𝑓𝐼𝑀𝑈, 𝑚𝑐𝑜𝑚𝑝 ]𝑇 (2.5)
For this reason, with a standard Kalman filter we would obtain a 4 x 6 Kalman gain matrix K instead of a 4 x 3 matrix should we use the accelerometer data only to correct the angular position. This implies that also all the operations done to calculate the Kalman gain matrix, matrix multiplications and matrix inversions are done on bigger matrices, with a complexity that increases with the cube of the matrix dimensions. A full Kalman filter will not be executable on a small ASIP that is integrated within the sensor logic. For this reason, a different algorithm was invented. This algorithm is still based on the Kalman filter, but the correction equation is split into two parts, with two different correction stages and two separate Kalman gains. The first correction stage is for the correction of the angular position of roll and pitch angles only and uses the measured gravity as a reference. The second stage corrects the yaw angle only and uses the measured magnetic field as a reference. Each filter stage works with a single vector of three elements of raw sensor data that is used to correct the angular position, so that each filter stage can work with K matrices of just 4 x 3 dimension.
Which is the practical difference between this kind of implementation and a full Kalman filter? From the Kalman filter theory we would have obtained the more
probable state estimation, under the conditions of white random noise of the input data. In this case, on the other hand, we have a very simplified algorithm, with an implementation cost that is two times less than the operations of a Kalman filter that uses the accelerometer only in the read equation, instead of a cubic increase that is not acceptable for us. As a drawback, the algorithm that we propose is a sub-optimal algorithm, so we must validate its performances.
There is another big advantage deriving from this two stage Kalman filter implementation: we need a system that works either with or without the magnetic compass, which may be optionally added to the 6 DoF IMU. With this implementation we can use the same algorithm for both situations, with the only difference that the second stage of the Kalman filter will be disabled if the magnetic compass is not present. This is another advantage with a custom ASIP implementation, because we can reuse the same microcode for the processor, with a reduction of the necessary ROM to store it.
Gyro Accelerometer Stage 1: correction Magnetic compass Stage 2: correction Better angular estimation Angular position estimation
Gravity estimation Magnetic field estimation
Two stage simplified Kalman filter
ANGULAR POSITION
Figure 2.1 Working principle of the two stage Kalman filter
2.3.1 Algorithm description for the two stage Kalman filter
The system state is the quaternion representing the angular position. The first a
priori angular position estimation is done with the gyro data only.
Then we have to estimate the expected gravity ℎ1 from the a priori angular position
and use it to correct roll and pitch in the first filter stage. ℎ1(𝑞𝑘) = ĝ = Rbn[ 0 0 |g|] = |g| [ 2q1q3− 2q0q2 2q0q1+ 2q2q3 q20− q12− q22+ q32 ] (2.6)
The gravity vector 𝑔 in ground frame coordinates is reported to the body frame using the rotational matrix 𝑅𝑛𝑏, that performs a rotation from the ground frame to the
23 𝑅𝑛𝑏= [ 𝑞02+ 𝑞12− 𝑞22− 𝑞32 2𝑞1𝑞2+ 2𝑞0𝑞3 2𝑞1𝑞3− 2𝑞0𝑞2 2𝑞1𝑞2− 2𝑞0𝑞3 𝑞02− 𝑞12+ 𝑞22− 𝑞32 2𝑞2𝑞3+ 2𝑞0𝑞1 2𝑞1𝑞3+ 2𝑞0𝑞2 2𝑞2𝑞3− 2𝑞0𝑞1 𝑞02− 𝑞12− 𝑞22+ 𝑞32 ] (2.7)
We have thus the H𝑘1 matrix that can be used to calculate the 4 x 3 Kalman gain
matrix of the first filter correction stage with the usual Kalman filter equations. H𝑘1 = 𝜕ℎ1[𝑖] 𝜕𝑞[𝑗] = [ −2q2 2q3 −2q0 2q1 2q1 2q0 2q3 2q2 2q0 −2q1 −2q2 2q3 ] (2.8)
The first stage calculates the residual qϵ1 that must be summed to the a priori state
estimation 𝑥̂𝑘− to obtain the first intermediate a posteriori state estimation 𝑥̂𝑘1. The
qϵ1,3 component is put equal to zero in order to ensure that the first stage corrects
the roll and pitch angles only.
qϵ1= 𝐾𝑘1(𝑧𝑘1− ℎ1(𝑥̂𝑘−, 0)) = qϵ1,0+ qϵ1,1+ qϵ1,2+ 0 ∙ qϵ1,3 (2.9)
𝑥̂𝑘1= 𝑥̂𝑘−+ qϵ1 (2.10)
For the second stage it is necessary to calculate the expected magnetic field ℎ2.
ℎ2(𝑞𝑘) = m̂ = Rbn[ 0 1 0 ] = [ 2q1q2+ 2q0q3 q02− q12− q22− q23 2q2q3− 2q0q1 ] (2.11)
The Jacobean matrix 𝐻𝑘2 that is used in the second stage to calculate the second
Kalman gain is the following: H𝑘2 = 𝜕ℎ2[𝑖] 𝜕𝑞[𝑗] = [ 2q3 2q2 2q1 2q0 2q0 −2q1 −2q2 −2q3 −2q1 −2q0 2q3 2q2 ] (2.12)
We obtain the second residual qϵ2 that has to be summed to the first a posteriori
estimation 𝑥̂𝑘1 (the output of the first filter stage) to obtain the final a posteriori state
estimation 𝑥̂𝑘. In this case, the components qϵ2,1 and qϵ2,2 of the residual are put
equal to zero to ensure that the second filter stage corrects the yaw angle only.
qϵ2= qϵ2,0+ 0 ∙ qϵ2,1+ 0 ∙ qϵ2,2+ qϵ2,3 (2.13)
𝑥̂𝑘= 𝑥̂𝑘1+ qϵ2 (2.14)
For this two stage Kalman filter implementation, the following optimal values were chosen after some tests with both real and synthetically generated data.
Q = 10−6∗ [ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ] (2.15)
R1 and R2 are the noise covariance matrices of the measurements of the
acceleration and of the magnetic field respectively. R1= [ 2 0 0 0 2 0 0 0 2] (2.16) R2= [ 1 0 0 0 1 0 0 0 1] (2.17)
2.4 Time discrete transformation
We have now the equations that fully describe the time continuous evolution of the system. To implement a Kalman filter, these equations must be transformed into a time discrete system equations.
We used the standard formula to discretize a time continuous system in a time discrete one. The system is described by an equation of this type, where 𝐴𝑇𝐶 and
𝐵𝑇𝐶 are the matrices that describe the time continuous system and 𝑥(𝑡) and 𝑢(𝑡)
the time dependent variables.
𝑥̇ = 𝐴𝑇𝐶𝑥(𝑡) + 𝐵𝑇𝐶𝑢(𝑡) (2.18)
The derivative can be made explicit in this way: 𝑥̇ = lim
𝑥→0
𝑥(𝑡+𝑇)−𝑥(𝑡)
𝑇 = 𝐴𝑇𝐶𝑥(𝑡) + 𝐵𝑇𝐶𝑢(𝑡) (2.19)
The 𝑇 variable is the delta-time from each filter iteration in a digital system, so it is possible to remove the limit operation:
𝑥(𝑡 + 𝑇) = 𝑥(𝑡) + 𝐴𝑇𝐶𝑥(𝑡)𝑇 + 𝐵𝑇𝐶𝑢(𝑡)𝑇 (2.20)
𝑥(𝑡 + 𝑇) = (𝐼 + 𝐴𝑇𝐶𝑇)𝑥(𝑡) + 𝐵𝑇𝐶𝑇𝑢(𝑡) (2.21)
In conclusion, the time continuous system is transformed in the following time discrete system, where 𝑥𝑘 is equivalent to the variable 𝑥(𝑡) sampled at the time 𝑡 +
𝑇. 𝑥𝑘= 𝐴𝑥𝑘−1+ 𝐵𝑢𝑘 (2.22) 𝐴 = 𝐼 + 𝐴𝑇𝐶𝑇 (2.23) 𝐵 = 𝐵𝑇𝐶𝑇 (2.24) The matrices A and B that describe the time discrete system are calculated starting from (2.1), (2.2), (2.3), (2.4), (2.5) e (2.6). In this particular case, the matrix 𝐵𝑘 is
zero because the system has no inputs, the matrix 𝐴𝑘 relates the evolution of the
time discrete system, while the expression ℎ relates the system state to the inputs.
𝑥𝑘= 𝐴𝑘𝑥𝑘−1+ 𝑤𝑘−1 (2.25)
𝑧𝑘= ℎ(𝑥𝑘, 𝑣𝑘) (2.26)
It is possible to calculate directly the 𝐴𝑘 matrix, because the first equation is a
linear equation. It is not possible to find a linear equation between the system state and its outputs, as we can see from (2.6), that is a quadratic relationship. For this reason, it is necessary to use the Extended Kalman filter for the measure equation, but this will not be a too big complication for the algorithm computation.
2.5 Algorithm summary
This is a short summary of the algorithm that is executed in the Simulink model:
Start of a priori system estimation
Reading of the gyro sensor, obtaining the angular velocities ωx, ωy, ωz Calculation of the discrete time state transition matrix Ak= I +12Ωnbn T Calculation of the “a priori” system state estimation q̂−k = Akq̂k−1
25 Calculation of the “a priori” noise covariance matrix Pk−= AkPk−1ATk+ Qk
Start of the correction stage #1
Calculation of the Jacobean matrix H𝑘1= [
−2q2 2q3 −2q0 2q1
2q1 2q0 2q3 2q2
2q0 −2q1 −2q2 2q3
]
Calculation of the Kalman gain Kk1= Pk−Hk1T (Hk1Pk−Hk1T + V𝑘1Rk1Vk1T) −1 Reading of the accelerometer data 𝑧𝑘1 = 𝑎𝑥 𝑎𝑦, 𝑎z
Calculation of ℎ1(q̂−k) = [
2q1q3− 2q0q2
2q0q1+ 2q2q3
q20− q12− q22+ q32
]
Calculation of the correction factor qϵ1= Kk1(zk1− ℎ1(q̂k−)), qϵ1,3 is put
equal to 0
Calculation of the “a posteriori” state estimation q̂k1= q̂k−+ qϵ1
Calculation of the “a posteriori” error covariance matrix Pk1= (I −
Kk1Hk1)Pk−
Start of the correction stage #2
Calculation of the Jacobean matrix H𝑘2= [
2q3 2q2 2q1 2q0
2q0 −2q1 −2q2 −2q3
−2q1 −2q0 2q3 2q2
]
Calculation of the Kalman gain Kk2= Pk−Hk2T (Hk2Pk−Hk2T + V𝑘2R𝑘2Vk2T) −1 Reading of the magnetic compass data 𝑧𝑘2 = 𝑚𝑥 𝑚𝑦, 𝑚z
Calculation of ℎ2(q̂k−) = [
2q1q2+ 2q0q3
q02− q12− q22− q32
2q2q3− 2q0q1
]
Calculation of the correction factor qϵ2= Kk2(zk2− ℎ2(q̂k−)), qϵ2,1 and qϵ2,2
are put equal to 0
Calculation of the “a posteriori” state estimation q̂k= q̂k1+ qϵ2
Calculation of the “a posteriori” error covariance matrix Pk= (I −
Kk2Hk2)Pk1
2.6 Simulink model
A Matlab Simulink model of the system described from the previous equations was built. Both simulated and real acquired data were used to test the behavior of the Simulink model.
A set of the following tests were done on the Simulink model with simulated data: model still with gyro bias; constant rotation and sine rotation at different frequencies to evaluate the bandwidth. Then, with real acquired data, these rotations were tested: a set of rotations by 90 degrees; a set of rotations in free hand. To acquire the data, the SensorDynamics Cube demo sensor was initially used. Then, in all the following tests and prototypes developed in this thesis, the SensorDynamics SD746 6 DoF gyro plus accelerometer combo sensor was used.
Figure 2.2 – SensorDynamics Cube Demo sensor with three orthogonal SD755 combo sensors
Figure 2.3 – SensorDynamics SD746 6 DoF combo sensor
The system model has an update rate of 100 Hz. This frequency is comparable to the one used from XSense in his first commercial product, and is mandatory for us because of the timing of the protocol used to acquire the data from the Cube demo. Considering a gyro sensor bandwidth of 40 Hz, no problems are expected with this acquiring frequency.
The input of this Simulink model are:
3D gyro data
3D accelerometer data The output of the system are:
Angular position quaternion
In the Simulink model the ZYX sequence Euler angles are also available, even with a graphic 3D visualizer.
27
Figure 2.4 – Simulink model of the Kalman filter system
The Simulink model is divided into different blocks: in the first part, there is the reading of the gyro data and the calculation of the updated quaternion; this is the a
priori system state estimation. Then, starting from this quaternion, in the first filter
stage an estimated gravity vector is calculated, which is compared with the measured gravity from the accelerometer data. The difference between the estimated gravity and this measure is called the measurement innovation or the
residual in the Kalman filter theory. This residual must be multiplied with the matrix
K, which generates a correction quaternion to be summed to the a priori estimate to obtain the a posteriori estimate of the system state. The K matrix is calculated from the covariance statistics of the different sensors that are an input to the filter, following the Kalman filter theory (see appendix B). The K matrix is the one that allows to obtain the best probabilistic estimation of the system state. This a
posteriori quaternion will be used in our algorithm for the second filter stage, that
estimates the expected magnetic field, compares it with the measured one to calculate the second residual, and with a second K gain calculates the correction necessary to obtain the final a posteriori estimation. This final estimation is used in the following filter iteration to calculate the new a priori estimate.
The Simulink model has the sub-blocks that are described in the following paragraphs.
2.6.1 Read_data
In this block, the data from the gyro sensor are read and converted in rad/s. A first filtering is added to this data: after some tests it was chosen to add a dead zone between -0.01 and +0.01 rad/s to reduce the effects due to the gyro bias. The A matrix is built from this data. In this block the accelerometer and magnetic sensors are also read and the vectors z1 and z2 are built; an additional low pass filtering is optionally added to the accelerometer data to reduce the effects of the dynamic accelerations of the system.
2.6.2 State_equation
In this block the a posteriori quaternion from the previous filter iteration is updated by multiplying it with the A matrix to obtain the a priori state estimation. At the startup the system is initialized with the [1,0,0,0] quaternion, which represents a horizontal position. After the quaternion multiplication with the A matrix, a normalization of the quaternion itself is necessary to ensure that it has an unit norm.
2.6.3 Error covariance P-1 and P-2
These blocks calculate the a priori state covariance matrix 𝑃𝑘−. It represents the
error estimation of the angular position calculated with the gyro data only, which is given by this formula:
𝑃𝑘−= 𝐴𝑘𝑃𝑘−1𝐴𝑇𝑘+ 𝑄𝑘−1 (2.27)
In our case we have two different 𝑃𝑘− matrices, because they are calculated with
different 𝑃𝑘−1 matrices that are the output of the two filter stages, that is the a
posteriori error covariance estimation. The Q matrix is the noise covariance of the
system process. We assumed this noise constant and independent. The optimal value that was empirically determined is the following:
Q = 10−6∗ [ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ] (2.28)
At lower values of the Q matrix a system that depends more on the gyros corresponds, because they are considered more reliable. Conversely, at higher values of Q the system uses the accelerometers more to correct the angular position.
An initial value for P must be provided. If the initial value of P is a zero matrix, this means that the initial angular position of the system is fully known. This is not true, because the system may be in any position, so a big value for the initial P0 matrix was chosen. P0 = [ 0.5 0 0 0 0 0.5 0 0 0 0 0.5 0 0 0 0 0.5 ] (2.29)
As expected, it was verified that, with this value, even if the system starts upside-down, it can quickly estimate the correct angular position. Moreover, the P matrix correctly converges versus smaller values, indicating that the angular position of the system is calculated in the right way.
2.6.4 H matrix 1 and 2
These blocks calculate the Jacobean matrix of the partial derivatives of the function
h that relates the output with the system state, which is a nonlinear relationship.
The derivative must be calculated with respect to the state variables, which are the four quaternion elements. The result is shown in eqns. (2.8) and (2.12)
2.6.5 Kalman gain K1 and K2
This block calculates the Kalman matrix K, which is the filter gain. Two blocks are present, one for each filter stage. This formula is applied to calculate K:
𝐾𝑘 = 𝑃𝑘−𝐻𝑘𝑇(𝐻𝑘𝑃𝑘−𝐻𝑘𝑇+ 𝑉𝑘𝑅𝑘𝑉𝑘𝑇)−1 (2.30)
It is necessary to have the matrix of the partial derivatives V. 𝑉[𝑖,𝑗]=
𝜕ℎ[𝑖]
𝜕𝑣[𝑗](𝑥̃𝑘, 0) = 𝐼 (2.31)
In this work, the V matrix is considered equal to the identity matrix: this means that the noises of the accelerometer and of the magnetic compass are independent from the sensor position.
29
The R matrix is the noise covariance matrix of the accelerometer or of the magnetic compass.
To deactivate the filter and use only gyro data, a zero matrix K0 can be used. 2.6.6 x_optimum 1 and 2
These blocks calculate the a posteriori state estimation 𝑥̂𝑘. As an input they have:
the a priori state estimation, the accelerometer or magnetic compass measurements and the Kalman gain matrix K.
The following equation is applied:
𝑥̂𝑘= 𝑥̂𝑘−+ 𝐾𝑘(𝑧𝑘− ℎ(𝑥̂𝑘−, 0)) (2.32)
In this equation 𝑥̂𝑘− is the a priori state estimation from the previous blocks, while
ℎ(𝑥̂𝑘−, 0) is the estimated gravity or magnetic field that is calculated internally within
this block using eqns. (2.6) and (2.11). 2.6.7 Error covariance P1 and P2
The last blocks calculate the a posteriori error covariance 𝑃𝑘 using the following
formula.
𝑃𝑘= (𝐼 − 𝐾𝑘𝐻𝑘)𝑃𝑘− (2.33)
This value represents the error covariance estimation in the a posteriori system state estimation and is necessary for the subsequent filter iterations.
2.5.8 Conversion in Euler angles
For the sake of completeness, the formulas to obtain the Euler angles rotation sequence Z, Y and X are reported.
ɸ = atan2(2(q2q3+ q0q1), (q02− q12− q22+ q32)) (2.34)
θ = asin (2(q0q2− q1q3)/(q02+ q12+ q22+ q32)) (2.35)
31
3. CHAPTER 3. BIT TRUE MODEL FOR ASIP IMPLEMENTATION
In this chapter the simplifications done to obtain a reduced complexity algorithm to be processed with the ASIP and the transformation of the algorithm from the floating point Simulink model to a fixed point model are illustrated.
The algorithm simplifications are important because they allow a reduction of up to 60% in the number of operations to execute, with negligible performance degradation.
The transformation of the floating point model to a fixed point model is quite critical, because the precision of the system directly depends from the number of bits used to represent the different kind of data in the internal filter calculations. A full analysis was done with Simulink model simulations.
3.1 Algorithm simplifications
Many different algorithm simplifications were explored to reduce the number of matrix operations and the overall algorithm complexity. A large amount of multiplications and sums are involved in the calculus of the Kalman gain K matrix. The first obvious thing that comes in mind is to have a fixed Kalman gain. This would mean to waive all the Kalman theory, with the finding of the optimal solution to the system state estimation and being satisfied with a non-optimal state estimation. Conversely, a drastically algorithm complexity reduction would be achieved. Unfortunately, it is impossible to use a fixed Kalman gain, because the values and also the sign that the different elements of the K matrix have depend from the system orientation. This means that, if a fixed gain would be used for the K matrix, the system would work correctly in a certain angular position, but the corrections would be completely wrong in another angular position, even worsening the angular position error or leading the system to its instability.
Figure 3.1 – Evolution of the Kalman K matrix
3.1.1 Use of a fixed P matrix
An algorithm simplification that was successfully tested is the use of a fixed P matrix. In fact, in the full Kalman algorithm it is necessary to calculate the a priori and the a posteriori system covariance matrices 𝑃𝑘− and 𝑃𝑘 at each filter iteration.
The meaning of these matrices is to evaluate the error covariance in the a priori and in the a posteriori system state estimation. We observed that the 𝑃𝑘− matrix
value, after the initialization with a high value, quickly converges to small values in the order of 10-3 to 10-4. These values remain quite constant during the system
evolution. For this reason, in this work we propose to use a fixed P matrix, with the same value for both the a priori and the a posteriori error covariance matrix. This is acceptable, because the correction actuated on the system state from the accelerometer and from the magnetic compass, after the first system startup, are both very small. Therefore, the use of the same value for both the a priori and the a
posteriori error covariance matrices means that, with small corrections, the angular
position before and after the correction is quite similar and the error with respect to the real angular position is almost the same. With these simplifications, four matrix multiplications are directly eliminated.
Figure 3.2 – Evolution of the P matrix in the full Kalman algorithm
The only shrewdness to use is to have a higher value for the P matrix at the system startup, so as to simulate a full Kalman filter behavior, thus allowing a fast angular position correction at the system startup.
The following values were chosen for the constant P matrix: P0 = [ 0.125 0.0003 0.0003 0.0003 0.0003 0.125 0.0003 0.0003 0.0003 0.0003 0.125 0.0003 0.0003 0.0003 0.0003 0.125 ] (3.13) P1 = [ 0.001 0.0002 0.0002 0.0002 0.0002 0.001 0.0002 0.0002 0.0002 0.0002 0.001 0.0002 0.0002 0.0002 0.0002 0.001 ] (3.14)
P0 value is used for the first 128 filter iterations, which is adequate for a full angle estimation even if the system starts upside down. Then, the P matrix error covariance is decreased to P1 for normal filter operation.
