domain: spi phys math info

text:
To start o with, I would like to thank the jury, and especially the two rapporteurs, for their highly relevant critique and for raising many questions that shall open doors to further expanding the embryo of research presented in this thesis.

Un grand merci à vous deux, Philippe et Augustin, de m'avoir accompagné pendant ces trois ans de thèse. Je suis bien conscient que mes pensés et idées sont souvent plus d'une quantité que d'une qualité, mais vous m'avez toujours aidé à les dépouiller pour (re)trouver le chemin. Philippe, je suis tout à fait impressionné par tes connaissances qui semblent percer partout dans les domaines d'automatique, de traitement de signal et au delà, et surtout ta curiosité sans limites sans laquelle cette thèse n'aurait pas vu le jour. Augustin, le couteau suisse de l'équipe, tu as toujours pris le temps pour me donner des astuces pour avancer, même quand t'étais noyé de travail et que je venais te voir pour la énième fois, grosso merdo.

Merci également au reste de l'équipe qui à été (in)volontairement impliqué dans mes recherche.

Alexandre et Guillaume pour votre aide avec les expériences, Nicolas pour les astuces de programmation, Yoko pour les discussion de diverses contenus, Pierre, et bien sûr Christelle. Merci à Corentin et François à l'ISAE pour toute l'aide avec les expériences dans l'arène de vol.

Thanks also to all my day up-lightening oce sharers and break prolongers at ONERA, Matthieu, Guillaume (for all the pastries!), Adèle, Pauline, Sovannna. Grazie Mario per tutte le pause caè inopportune

It doesn't matter who we are, what matters is our plan.

Bane, The Dark Knight Rises (2012) iii Résumé L'utilisation croissante des drones et leur intégration dans le trac aérien nécessite de fournir un certain nombre de garanties de sûreté et de preuves de fonctionnement. La sécurité du vol est directement tributaire de la précision et de la abilité de la localisation qui est généralement obtenue par une fusion multi-capteurs, réalisée à l'aide d'un ltre estimateur. Ce travail de thèse s'intéresse au problème de la navigation tolérante aux défauts et aux pannes capteurs dans le cas de capteurs non redondés. L'objectif principal est de proposer des méthodes et des architectures d'estimations de l'attitude et de la position qui permettent de préserver la justesse de l'estimation, mais aussi d'améliorer sa consistance et son intégrité, même en cas de perturbations prolongées des capteurs. Un premier axe de travail concerne l'estimation et le rejet de biais multiples et fréquents sur un capteur de position, comme peut y être soumis un récepteur GNSS (multi-trajets), ou un capteur visuel (erreur de poursuite). Une architecture de détection et de correction de l'estimation de position a été développée pour cela et vient compléter les méthodes existantes basées sur le GLR. Un second axe de travail a été de proposer une architecture d'estimation de l'attitude qui soit robuste aux perturbations magnétiques et aux accélérations spéciques. Elle comporte principalement trois briques: (1) Des modèles de performance permettent d'estimer les sorties capteurs nettoyées au mieux des perturbations; (2) Une étape de consolidation de mesures utilise des tests statistiques pour sélectionner les signaux à fusionner entre les mesures brutes ou nettoyées, ou simplement rejeter les signaux dans les cas où la consolidation échoue; (3) Un estimateur d'attitude basé sur un ltre de Kalman fusionne les mesures consolidées, avec des propriétés de découplage vis-à-vis des perturbations résiduelles, ainsi qu'un modèle de biais saturé. Les algorithmes d'estimation de position et attitude ont été validés en simulation et séparément lors de diverses campagnes d'essais expérimentales.

Abstract

The ever-increasing use of drones and their integration within the existing air trac demand a certain number of guarantees of safety and functional proofs. The ight safety is directly impacted by the precision and reliability of the localisation which is achieved most of the time by a multi-sensor fusion, itself provided by a state estimating lter. The work of this thesis focuses on the problem of fault tolerant navigation and sensor fault in the nonredundant sensor case. The main objective is to propose methods and architectures for attitude and position estimation providing a correct estimation, but also improving its consistency and integrity, even in the case of long lasting sensor perturbations. A rst line of work concerns the estimation and rejection of multiple and frequent biases on a position sensor, which is often the case for a GNSS receiver (multi-path errors), or a visual sensor (tracking errors). An architecture for bias detection and correction of the estimated position has been developed to this end, to complement standard methods based on the GLR. A second line of work has been to propose an attitude estimation architecture robust to magnetic disturbances and specic accelerations. It is divided into three principal blocks: (1) Sensor performance models allowing the estimation of sensor outputs cleaned as well as possible from perturbations; (2) A measurement consolidation stage utilises statistical tests to select the signals to fuse between the raw measurements and the cleaned ones, or simply reject the signals in case the consolidation fails; (3) An attitude estimator based on a Kalman lter merges the consolidated measurements, with decoupling properties to mitigate the eect of residual perturbations, and a saturated bias model. The algorithms for position and attitude estimation have been validated in simulation and separately during various experimental test campaigns.

Introduction

Summary

T his chapter introduces the problem of supervision for drone ight safety, starting from the fundamental concept of drone navigation, including the concepts of sensors and data fusion.

It then outlines the characteristics of dierent faults that aect the capacity of a drone to navigate safely, and introduces the concept of fault tolerant navigation for drones, and especially the denition of fault tolerance referred to in this work.

The main contributions of this work are then listed, along with a brief explanation of the practical methods employed in the scope of the thesis. Finally, an outline of the remainder of the thesis is presented, and a list of publications produced in the scope of this thesis is given.

1.1. What is the matter with these drones, anyway? 3 Elrond: 'Nine companions. So be it. You shall be the fellowship of the ring.' Pippin: 'Right! Where are we going?'

The Lord of the Rings: The Fellowship of the Ring; Peter Jackson (2001) 1.1 What is the matter with these drones, anyway?

Unmanned Aerial Vehicles (UAV), Unmanned Aerial Systems (UAS), remotely (or autonomously) piloted aircraft, are commonly grouped under the popular category "Drones". The common denominator is that they are all airborne vehicles, capable of self-sustained ight, without a physical pilot on-board.

The shapes and sizes of drones are vast, from micro-drones in the shape of multi-rotor helicopters weighing a couple of grams, to high altitude strategic surveillance drones of the size of a regular ghter aircraft.

A classication of drones

Drones can be subdivided into numerous categories. Here we focus on the division by mission type, since it is also related to the complexity and size of the vehicle:

Mini-drones, or hobby drones, typically used for leisure, aerial photography, or academic research, in the form of lightweight multi-rotor or xed wing drones. A common factor among these drones is the use of low-cost sensors and actuators, making them prone to failures. In terms of ight safety, these drones are easily completely lost if subject to external perturbations, due to their lack of redundancy and poor sensor quality. However, due to their small size, they pose little threat to anything but the unfortunate unprotected human or animal.

Medium sized, professional drones, typically used for longer autonomy professional photography, surveying, or local surveillance. These drones can reach weights of tens of kilos, often in the shape of large multi-rotor drones or xed wing variants. The sensors used in this category are usually of better quality than for the mini-drones, but space and weight constraints often hinder the addition of redundant sensors. From a safety point of view, this category is critical since their hardware does not allow for a high level of redundancy and safety, and their size makes them hazardous to their environment in case of a crash.

Large professional drones, typically used in military surveillance or advanced research programs. In this category we nd smaller autonomous helicopters and large xed wing drones.

These drones are typically equipped with higher quality sensors and might oer some sensor redundancy. However the environments in which they are employed might still be hazardous to their sensors (urban, low-level ight, etc.). If drones of this category crash, severe damage to their environment can be expected.

Tactical/strategic drones, typically xed wing medium/high altitude surveillance and/or armed oensive drones. In this category, the sensor and actuator quality is on par with large manned aircraft and the sensor redundancy oers a high level of safety. The ight safety of these drones is on par with regular aviation and crashes are very rare, apart when due to malevolent interference.

The number of drones, of all categories, in the airspace has exploded in recent years, and the trend is expected to continue (see Figure 1.1).

Current diculties for drone ight safety

Drone ight today is subject to stringent regulation, eectively keeping them away from urban or hazardous environments. In addition, ight operations are almost always performed with a remote pilot in the loop and autonomous drone ight is almost exclusively done in controlled laboratory environments.

The reason for this is at the core technical:

• Low quality sensors are easily degraded by external factors. This can rapidly degrades the drone ight safety in cases of sensor faults or disturbances.

• Lack of sensor redundancy means that vital information such as orientation (attitude ) or position only has a single source, such as an IMU, a compass, or a GNSS receiver.

• Lack of actuator redundancy and/or neutral stability risks making any actuator loss fatal for the vehicle.

Very few studies have assessed the integrity of drone navigation.

The integrity for drone navigation is far from applicable in the classical aeronautical requirements.

The components used in the vast majority of drones carry no guarantee of quality and in the majority of cases no certication or guarantee of functionality exists for actuators and sensors.

This list is by no means exhaustive but aims to briey present the current diculties facing generalised acceptance for drone ight. It is self-evident that overcoming these points is crucial if autonomous drones are to leave the leisure and laboratory environment and for real sweep into and share the space of general aviation. The research that treats the above points is called fault tolerant navigation. Before getting into the details of fault tolerant navigation, let us rst introduce what navigation is about for us.

1.2. Drone navigation

Drone navigation

Navigation is, according to the Oxford dictionary, The process or activity of accurately ascertaining one's position and planning and following a route. The keywords here are process, accurately ascertaining, position, and following a route. For drones, this means employing an algorithm (process) that makes use of dierent navigation sensors in order to establish the position, orientation and course as accurately as possible. The process of combining information from dierent sensors is known as sensor fusion. The orientation is also known as the attitude, the two words are used interchangeably in this thesis.

Sensors for drone navigation

A multitude of navigation sensors are available to drones. We can divide these into primary and secondary sensors. The primary sensors give an information about the absolute state of the drone with respect to a global reference (for example the earth), e.g.

Inertial measurement units (IMU) containing accelerometers and gyroscopes measure the orientation of the drone with respect to gravity, and the orientation rates (if the non gravitationnal acceleration is negligeable).

Magnetometers (or compasses) measure the drones heading with respect to the magnetic north.

Barometers measure the static pressure (and temperature) in order to determine the altitude of the drone.

Global Navigation Satellite System (GNSS) receivers measure the position and velocity of the drone with respect to the center of the earth using a constellation of satellites with known positions and velocities.

The secondary sensors provide an information of the relative state of the drone with respect to a local reference (for example a nearby object), e.g.

Vision based sensors can provide a relative position and orientation with respect to the objects in the eld of view. We can nd, for example, sensors using features of individual images and past knowledges (feature positions) to determine instantaneous relative position and orientation; related to SLAM algorithms (Simultaneous Localization and Mapping). Or continuous optical ows to determine relative velocity and orientation rates; related to Odometry algorithms.

Ranging sensors, based on light (Infrared, laser), sound (ultrasound), or electromagnetic echo (radar), provide a relative distance measurement to the reected objects. A typical use is for determining the drone's height above ground.

Air data sensors measure the angle and speed of the drone with respect to the air around the drone. These sensors are mainly used in xed wing drones due to the steady airow around the drone needed for precision. For absolute, global, navigation, i.e. from point A to point B in a global reference frame, the navigation must in general make use of the primary sensors (IMU / GNSS / magnetometers). As a matter of fact, the combination of an IMU, a magnetometer, and a GNSS receiver is the minimal sensor suite needed to determine the orientation and position of the drone at all times. This kind of navigation suite, commonly referred to as GNSS/INS, is the classical conguration when referring to drone navigation sensors.

For relative, local, navigation, the use of secondary navigation sensors may suce. Indeed if the initial position relative to the surroundings is known, it is enough to use local measurements to navigate, under the assumption of at least quasi stationary surroundings, i.e. the environment is not changing or moving. This kind of navigation is commonly found in GNSS-denied environments such as indoor navigation. It also appears in vision based navigation when navigating for example relative to a runway (e.g. during xed-wing nal approach) or an inspected structure.

In many cases for drones, primary and secondary navigation sensors are used simultaneously in the sensor fusion to provide a certain level of information redundancy. A common example of this is the combination of IMU, GNSS, and barometer to determine accurately the altitude of the drone.

Whichever conguration of sensors is considered, the problem of determining the position and orientation accurately at all times becomes dicult if the sensors involved are potentially erroneous.

1.3. Orientation and position estimation

Orientation and position estimation Sensor fusion principle

Since the sensors at hand are never perfect and available at all times, necessary navigation information (position, velocity, orientation) cannot always be obtained directly from the sensors. A common solution to this problem is to use sensor fusion based on kinematic or dynamic models to determine the state of the drone at all times. In this case, the state of the drone refers to the position, velocity, and orientation of the drone.

An easy way to estimate an unknown quantity is to integrate its derivative, given that the initial value of the quantity in question is known. For example, the integration of the accelerations of a drone should give its velocity if integrated from a known initial velocity (for example at standstill). This is an example of estimation through direct integration using a kinematic model (derivative of velocity = acceleration ). The obvious problems with this method is that the initial velocity might not always be known, and the accelerations of the drone might not be perfectly captured by the accelerometer, causing a drift in the estimated velocity.

However, an accelerometer will likely provide an integrated velocity at a much higher rate than an actual velocity sensor such as GNSS receiver provides a measured velocity. The two sensors can therefore be used in a complementary fashion, where the accelerometer provides a high output rate (bandwidth) velocity estimate and the velocity sensor corrects for the drift in the estimate and provides long term stability.

This is in fact the basis, although simplied, of sensor fusion for drone navigation: high bandwidth rate sensors integrated through kinematic models, corrected by supposedly unbiased low rate position and angular sensors. Note that the "supposedly unbiased" is important since, as we shall see later, when a single source of information is available (e.g. a non-redundant position or attitude sensor), it is not possible to discriminate between a healthy measurement or a faulty (or biased ) one, i.e. the bias is unobservable.

Combined full state estimation

A common sensor fusion conguration in the case where the full kinematic state of the drone is estimated (i.e. orientation, rotational speed, velocity, and position) is to make a single lter for estimating all states simultaneously (c.f. Figure 1. [START_REF] Hamel | Attitude estimation on SO [] based on direct inertial measurements[END_REF]. The advantage of estimating the entire state in this fashion is that an estimator such as a Kalman lter can take advantage of the coupling between the states. For example the velocity measurement can be used directly to assist the attitude estimation to avoid drifts due to high accelerations.

The obvious drawback of a combined estimation is that faults in all sensors involved will impact the entire state. Isolation of the source of a fault therefore becomes more dicult. Also the full state kinematics are a mix of linear and nonlinear kinematics meaning that linear techniques cannot easily be applied to only a part of the estimation.

It can therefore be interesting to use a combined estimation of the full state if the sensors can be considered reliable and fault-free. However from a fault tolerance point of view this architecture is of little interest since it enforces couplings between the states and makes fault isolation and correction harder.

Splitting position and attitude estimations

The easiest part of drone state estimation, at least mathematically, is the position estimation. The position and velocity can classically be obtained from GNSS or vision sensors, and the acceleration is found using accelerometers (c.f. Figure 1. 4). Indeed, the kinematics of the translation problem are linear, making the design of an estimator an easy job. The core point here is to project all measurements onto the same inertial reference frame. Attitude estimation is a trickier part of the drone state estimation. First of all, one has to choose how to represent the attitude, a multitude of options exist, each with its advantages and disadvantages.

Secondly, the attitude estimation problem is nonlinear by nature. Meaning that more elaborate methods than for position estimation must be employed. Classically, the attitude is estimated using rate sensors such as gyroscopes, and attitude sensors such as accelerometers and magnetometers (c.f. Figure 1.5).

Figure 1.5: Schematic overview of an attitude estimator.

Obviously, attitude errors induce translation errors and thus attitude estimation can be aided by translation measurements. Drone navigation takes advantage of an acceleration being often close to (the opposite of ) gravity. And the position estimation requires sensed acceleration to be projected onto an inertial frame via a rotation equal to the estimated attitude. So, even if the problems are apparently dealt with in separate ways, there are important links between the two estimates.

Sensor faults and disturbances

In this thesis we will only consider an estimation architecture which decouples the position and attitude estimations. This allows for higher exibility and modularity of the state estimators and opens up the eld of sensor specic fault isolation. Into this we read detection and isolation of faults that nominally impact only a part of the drone, e.g. the position, the heading, the inclination, etc.

Estimation techniques for sensor fusion

Numerous techniques exist for fusing the sensor data in the position and attitude estimation. The main objective of the sensor fusion is to provide an estimation of the position and attitude, and also of the uncertainty related to the estimated state, i.e. a covariance. This covariance is essential for online testing the integrity of the navigation.

A very common method to use is the Kalman lter, a linear optimal state estimator in the sense that it minimizes the covariance estimation, given the prescribed covariances of the sensors used in the sensor fusion. The Kalman lter is only optimal for linear problems, typically the position estimation in our case. For nonlinear problems, a linearised extension called the Extended Kalman lter (EKF) exists. This extension, and other related nonlinear lters are often used in attitude estimation, where the kinematics are nonlinear.

The feasibility of position and attitude estimation boils down to the problem of observability, simply put whether the position or attitude can be analytically deduced from the position or attitude sensors involved. This is of course the case under nominal conditions when the sensors behave as they are supposed to, but becomes a problem if external factors prevent the sensors from functioning properly.

Sensor faults and disturbances

Only in the ideal case (open sky, high above ground, steady ight), the primary navigation sensors provide reliable position and orientation information. When including secondary sensors into the mix, the ideal case might be set aside, however other constraints come into play (good light conditions, static environment, calm air). Whichever the types of sensors used, when they are used on a drone in an environment which is not ideal, perturbations external or internal to the drone can cause the sensors to provide erroneous measurements.

A classication of sensor faults

Apart from measurement noise, the eects that cause the sensors to provide an erroneous information can be classied as sensor faults, or sensor disturbances. In this work, a sensor disturbance designates the disturbance that provokes an erroneous measurement. A sensor fault on the other hand designates the fault as seen on the sensor output, regardless of the actual source (external or internal). The trivial case, although not from an algorithmic point of view, of a complete sensor failure, implying that the sensor itself is not functioning properly at all, is disregarded in this thesis.

The most common sensor disturbance, which is not to be classied as a sensor fault, is the zeromean noise that will impact any navigation sensor. The causes for sensor noise vary from high frequency electrical noise to vibration induced noise, noise induced by image processing algorithms, sensor quantication. From a navigation point of view, sensor noise is important to mention since it degrades the position and orientation accuracy, but hardly poses a threat to the integrity of the navigation like other sensor faults due to its zero-mean.

Types of sensor faults considered

The most common sensor fault aecting drone navigation sensors are osets, or sensor bias. These biases can grossly be categorised into four categories: transient bias ; step-wise bias ; constant bias ; and drifting bias. These dierent types of measurement bias commonly aect dierent types of navigation sensors:

Transient bias are commonly seen in accelerometers due to non-gravitational acceleration and in magnetometers due to environmental magnetic disturbances. Barometers subject to transient changes in ambient pressure can also experience this kind of bias.

Step-wise bias are often seen on GNSS receivers in urban canyons or near the ground (lowlevel ight) due to various reasons such as multi-path reception and signal blocking. This is a major reason for loss of navigation accuracy and integrity for drones (we shall come back to these concepts later).

Step-wise biases may also appear on vision sensor measurements when processing algorithms cling on to incorrect information.

Constant bias, or quasi-constant bias are often seen on rate sensors such as gyroscopes and accelerometers due to vibrations and axis misalignment, among other causes.

Drift bias are typically seen on barometers and air-data sensors due to changing atmospheric conditions. They can also be seen in vision based sensors due to a non-static environment.

1.5. Fault tolerant navigation for drones

In order to achieve accurate navigation, these potential sensor biases must be handled properly.

Fault tolerant navigation for drones Fault tolerance

In this work, we refer to fault tolerance as robustness to any process causing erroneous sensor measurements.

The basis of fault tolerant navigation is to make sure that the measurements used will not degrade the estimated position, velocity, or orientation. If several sensors that measure the same information are available (redundancy) and these sensors don't have the same fault modes, i.e. are not perturbed by the same physical phenomena, we can use simple fault detection algorithms to exclude the faulty sensor. On the other hand if a sensor is the unique source of a certain information, excluding it would mean losing completely the necessary information, and by consequence the observability. Thus the only option is to either estimate the perturbation or use analytical redundancy to reconstruct the sensor information using other sensors.

More specically, in the UAV case we have some redundant information and some non-redundant information. The redundant ones are for example the GNSS altitude channel, baro-inertial altimeters, and eventual range nders (ultrasound, LiDAR (Light Detection and Ranging)) which all give an information about the altitude or height. It can also be the magnetometer and GNSS heading combination in the case of non-hover ight (and low wind conditions). Non-redundant information will be for example the GNSS position measurement, the accelerometer gravity measurement, or magnetometer heading in GNSS-denied environments (although relative heading can be assured by vision in some cases).

The main objective of a fault tolerant navigation is to provide a state estimation of the drone which is consistent and integrate. A consistent estimation means that the actual error made is coherent with the estimated one. An integrate estimation practically means that the probability of the actual estimation error remains in a given safe set is upper bounded by some preset integrity risk.

Regarding the non-redundant sensor problem, which is the main focus of this thesis, two methods are developed, corresponding to two types of sensing errors. The rst type corresponds to faults for which a parametric model is known (which will be processed by parametric estimation techniques). The second type refers to non parametric disturbances (for which decoupling strategies will be proposed).

Both methods involve statistical testing to manage the non stationarity induced by the sensor error occurrences.

Parametric faults

The rst case to be dealt with is when the perturbation has some known dynamics (shape), and where the only ambiguities are the apparition time and the amplitude. A typical example of this perturbation is the GNSS position subject to jumps (i.e. step-wise bias) due to multipath errors or other interferences (c.f. gure 1.7).

In this case a classical GNSS/INS fusion will systematically fail due to loss of observability when the measurement is biased. However, we shall see that it is possible to estimate the bias if the time of apparition can be determined. Since the kinematics are linear, the bias estimation and the position estimation can be seen as two distinguishable parts of the same estimation. In this case we can apply bank of lters where each lter uses the hypothesis of a bias starting at a certain time and then select the most likely start time followed by a maximum likelihood estimation of the amplitude given the estimated start time. An algorithm performing this task is the GLR algorithm [START_REF] Willsky | A generalized likelihood ratio approach to state estimation in linear systems subjects to abrupt changes[END_REF].

A problem that arises with the GLR algorithm is that it assumes a single bias over an innite time horizon. In practical applications, this is not the case since for example GNSS multipath errors tend to arrive in batches of multiple frequent bias apparitions. A modication of the GLR algorithm is possible in this case to account for the appearance and disappearance of multiple frequent measurement bias.

We shall present in this thesis the MGLR algorithm we have developed for that purpose.

Non parametric disturbances

The second case is when the dynamics of the perturbation is unknown, meaning that a generally valid model cannot be found. In this case the perturbation cannot be easily estimated and the erroneous sensor information should be excluded from the estimation or replaced with an auxiliary information.

The main problem in this case is to detect the perturbation quickly and to correctly isolate the erroneous sensor components. A typical example in this case is inertial accelerations impacting an IMU or magnetic interference to a magnetometer (c.f. Figure 1.8).

Another complicating factor that comes into play for IMU and magnetometer perturbations is that the kinematics are nonlinear, or at least linear time varying at best. It is therefore not straightforward to separate the eects of the measurement bias from the nominal state estimation.

The standard solution in the literature is to detect and reject erroneous measurements. This method unfailingly leads to a loss of observability, meaning that the attitude estimation risks drifting, exhibiting a temporary or permanent bias, and becoming highly unreliable.

Consistency and integrity

In classical use cases, consistency and integrity are disregarded and not a mandatory part. For example, from a control point of view, an attitude bias or transient disturbance is not a problem since the attitude drift is handled by position and velocity integrators (designed to reject physical disturbances of the attitude).

However, other applications denitely need to handle consistency and integrity, for future certication, safety guarantees or just by functional needs. For example, if the estimated attitude and its estimated uncertainty are used directly, for example in conjunction with vision or mapping systems, this drift and uncertainty increase poses a real problem. Another application for which consistence is important can be a path-planning with obstacle avoidance; if the position uncertainty is inconsistent, this can lead to collisions if uncertainties are underestimated.

We shall see in this thesis that it is possible to set up an analytical pseudo redundancy for the attitude and attitude rate sensors. This signicantly reduces the attitude drift and improves the consistency and integrity of the attitude estimation during periods of attitude measurement disturbances.

Main contributions

The contributions of this thesis are divided into contributions to fault tolerant position estimation and fault tolerant attitude estimation.

Fault tolerant position estimation

In fault tolerant position estimation the following contributions are claimed:

Extension of the GLR algorithm to handle multiple frequent position measurement bias (MGLR).

This is achieved through a separation of the detection and estimation steps, and the development of a global bias re-identication step from previously detected bias jump times.

Development of a method for estimated bias elimination by matching the amplitudes of detected and estimated sensor biases and eliminate zero-sum bias combinations.

The applications of these techniques in simulation and experimental tests show that the consistency and integrity of the position estimation is signicantly improved in the case of frequently appearing position measurement biases, such as in the case of GNSS multipath errors in an urban environment.

Fault tolerant attitude estimation

In fault tolerant attitude estimation, the following contributions are claimed:

Development of a measurement consolidation stage based on sensor output estimation using sensor performance models, and a fault detection stage using the outputs of the sensor output estimation to detect faults in the sensor measurements and the sensor performance models. An

How did I actually do all this?

The structure and contents of this thesis is focused on the algorithmic development and the results of the experimental evaluations in the position and attitude estimation cases respectively. Very little attention is paid to how the calculations were actually made and practically how the results were obtained. The reason for this is to avoid an endless tutorial with little scientic interest. This concise section is therefore devoted to explaining the actual work process throughout the thesis.

1.7. How did I actually do all this?

Position estimation and GLR

In this part of the thesis, which was also the rst one chronologically, a simulation environment was developed in Matlab/Simulink. As a rst step, the classical GLR was modelled as a baseplate for bias estimation. As the theoretical development was pursued, it was extended to handle multiple consecutive biases.

For what concerns the experimental evaluation, a simulator implementing three GLR (or MGLR) algorithms in parallel was developed in order to handle biases on the three position axes independently.

Each GLR implementation accepted any number of inputs and measurements and the measurement sampling was done automatically (by comparing two subsequent data) in order to handle experimental data with potential time varying sampling times.

Regarding the MGLR developments, a good amount of time was spent trying to get the compensation step right. In lack of a better word, the nal algorithm is an abomination of feedback loops and transient corrections that all need to click (transfer of state to measurement errors, covariance corrections, etc.). It is highly likely that whatever application result came out of the algorithms until a few months before the edition of this manuscript are all tainted more or less by various bugs in the MGLR code.

Attitude estimation

The work on attitude estimation started by trying out dierent existing attitude estimation techniques in an existing Matlab/Simulink multi-rotor simulator developed for the X4-MaG quadrotor [START_REF] Manecy | X4-MaG: a lowcost open-source micro-quadrotor and its linux-based controller[END_REF]. So, in addition to the attitude estimation architecture presented in this thesis, numerous other attitude lters were developed and tested, notably:

-A standard quaternion EKF with pseudo measurement decoupling.

-A standard quaternion EKF with Martin-style decoupling.

-An EKF with acceleration priming through actuator performance modelling.

-An IEKF incorporating a performance model for the magnetic and gravity references.

-A full position/orientation sequential EKF for velocity aided attitude estimation.

For various reasons (poor disturbance rejection, dependency on external sensors, stability), these developments were halted but each of them gave important insight for the development of the present architecture.

In the wake of some of the EKF developments mentioned here-above, where all Jacobians where derived by hand, the power of the Symbolic maths toolbox was discovered by the author. This greatly facilitated further developments in the area.

After the initial assessment of the attitude estimation problem, another simulation environment was developed purely in Matlab. This new environment also allowed for the use of recorded sensor data for experimental evaluation of the developed estimation techniques. Notably a function for evaluating data from the Crazyie [Crazyie 2.0 ] quadrotor and the PX4 Autopilot [Open Source for Drones -PX4 Autopilot ] was added.

The trickiest part of the development concerning the attitude estimation was to establish how the attitude depended on the actual disturbance and to which degree the errors were actually due to attitude drift as a consequence of measurement rejection. Indeed the last, but nonetheless highly signicant, development added to the architecture was the switched gyroscope bias estimator.

It should also be mentioned that the experimental evaluation of the attitude estimation was slightly underestimated in terms of workload, and numerous error sources such as the optical tracking system, the gyroscope coning and accelerometer sculling errors, and the heterogeneous magnetic eld in the ight arena were not properly addressed.

Despite the shortcomings listed in this section, it is hoped that the reader nds the contents of this thesis useful and worthwhile.

Outline

The

Publications

Conference papers [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF], G. Öman Lundin, P.

Summary

T his chapter investigates the state of the art of fault tolerant UAV navigation. The rst part is consecrated to the linear problem of position estimation where attention is primarily given to dierent methods of fault detection and bias estimation of non-redundant position sensors. We briey explore the domain of innovation based change detection and thereafter some application specic methods to establish which criteria necessitate further development.

The second part of the state of the art inquires the nonlinear problem of fault tolerant attitude estimation. An overview of attitude estimation in the fault-free case with focus on complementary lters and Kalman lters lays the rst bricks. This is then built upon with attitude estimators integrating fault tolerant capacities. The fault tolerant aspect is divided into rejection of attitude measurement bias, decoupling of disturbances, gyroscope bias estimation, and consistency and integrity.

In the end, a summary of the identied critical points to be treated is given.

Sam: 'This looks strangely familiar.'

Frodo: 'Because we've been here before. We're going in circles.'

The Lord of the Rings: The Two Towers; Peter Jackson (2002) 2.1 The position estimation problem 2.1.1 Position estimation without position sensor bias

The position estimation problem consists of, as the name suggests, to determine a position using position, velocity, and acceleration sensors. The translation kinematic equations are written:

ṗ = v v = a (2.1)
where p is the position vector (dimension 3 × 1), v is the velocity vector, and a is the acceleration vector.

The position is often given by a GNSS receiver or a vision sensor, the velocity can be retrieved by GNSS receivers or optical ow sensors, the acceleration is found with accelerometers.

In the simplest case, only a position measurement and an acceleration measurement is available. In this case we assume that the acceleration is given in the same coordinates as the position (this normally requires to transform the accelerometer measurement from the body-xed frame to the inertial frame).

The measurements are assumed to be disturbed by zero-mean Gaussian noise. We can therefore model the sensors as:

a m = a + v a p m = p + w p with v a ∼ N (0, V a ) w p ∼ N (0, W a ) (2.2)
Using only the position and acceleration measurements together with the kinematics gives us the classical position estimation model:

˙ p v = 0 I 0 0 p v + 0 I a with a m = a + v a p m = p + w p (2.3)
By denoting the state x = (p, v) T , the input u = a m and the measured output y = p m , we can write the model on a classical stochastic state-space form with the matrices A, B, C, G u , G y as:

ẋ = 0 I 0 0 A x + 0 I B u + 0 -I Gu v a y = I 0 C x + I Gy w p (2.4)
It is easy to see that this model is observable by constructing the observability matrix

O =       C C A . . . C A 5      
and verifying that its rank is indeed the same as the size of x. Since the model 2.4 is a linear stochastic model with Gaussian noises, an optimal position estimation is easily found with a Kalman lter.

Impact of position sensor bias

Now consider that the position sensor is subject to a bias b. This bias can, for example, be due to a multi-path error in GNSS sensor or a visual tracking error. The position measurement is in this case

p m = p + b + w p .
In order to estimate this bias, we can augment the state and assign b some dynamic, for example ḃ = v b , with v b ∼ N (0, V b ), if we can consider the bias to be quasi constant. The augmented model becomes:

ẋa = A 0 0 0 Aa x a + B 0 Ba u + G u 0 0 I Gu,a v a v b y = C I Ca x a + G y Gy,a w p (2.5)
Constructing the observability matrix for this system reveals that it is no longer observable. It is therefore not possible to estimate the bias and the position at the same time using a single position sensor and the model (2.5). This obviously results from the fact that there is no measurement redundancy.

Just as the bias-free model is not a reasonable model to use in drone position estimation, the model where the bias is ever present is neither. Sensor faults like multi-path errors and visual tracking errors are better modelled as biases appearing at a certain time on a previously bias-free measurement.

Instead of augmenting the model as in (2.5), one can see the bias as an unknown amplitude disturbance appearing at an unknown time k. In this case, the biased model takes the form

ẋ = A x + B u + G u v a y = C x + F Γ k b + G y w p (2.6)
where F is the bias distribution matrix (i.e. F = I if all measurements are biased independently), and Γ k = Y (t -k) is Heaviside's step function at time k, marking the appearance of the bias.

The reason to model the position bias as a step function becomes quite obvious at a closer look.

Imagine for a moment that a Kalman lter is implemented on the model (2.6) under the hypothesis that b = 0. If a position measurement bias appears at t = k, the prediction error (innovation) of the Kalman lter will no longer be zero-mean for t > k (c.f. Figure 2.1). Since the bias is unobservable, the innovation will tend to zero as the estimated position converges to the measured one.

Clearly, the information of the bias appearance in the innovation is only available for a short time (precisely T obs in Figure 2.1), before the eect of it is drowned in noise. On the other hand, since the system is linear, the innovation is just the sum of a zero-mean noise and the fault signature. The fault signature is the deterministic response of the Kalman lter to the appearance of bias in the measurement (or the input). It can be calculated analytically if the appearance time k is known. And then the bias can be estimated. Consider the augmented model (2.5), but slightly modied to show that the bias is not permanent but appears at instant k. For this purpose, a time varying measurement matrix is used

C a = I F Γ k .
Then it can be shown that this model is weakly observable. Indeed its observ- ability grammian is non singular as soon as the bias appears on the observation range. However this grammian does not satisfy the complete uniform observability condition which is required for a Kalman lter to converge. We can therefore say that the bias is weakly observable and can only be estimated

when t ∈ [k, k + T obs ].
Estimating the bias is therefore not impossible, even if a conventional estimator cannot do it. But the solution to this problem requires a combination of techniques from the domains of change detection and state estimation. The estimation of the bias is in short composed of two steps:

Estimation of the bias appearance time.

Estimation of the bias given the estimated appearance time.

The Generalized Likelihood Ratio (GLR) algorithm

A standard solution to the joint detection and estimation problem of biases in linear systems, developed in [START_REF] Willsky | A generalized likelihood ratio approach to state estimation in linear systems subjects to abrupt changes[END_REF], is the Generalized Likelihood Ratio approach (GLR, see Appendix A). It is normally given as an implementation in discrete time (with step time ∆t), which is also the notation we shall use in this thesis, however the analysis behind the algorithm can, of course, be done in continuous time as well.

The principle of the GLR algorithm is the following:

A linear Kalman lter is implemented under the zero hypothesis "No bias present on the measurement at time t".

A bank of L linear regression lters is set up. Each lter calculates innovation and state fault signatures under the hypothesis "A bias of unit amplitude has appeared at time t -(M + k i )∆t in the past", where M ≥ 0 is a xed lag, and k i ∈ [0, . . . , L -1] is the supposed relative bias appearance time.

A statistical maximisation over all regression lters determines the most likely appearance time and a bias is detected if a predened threshold is crossed. The estimated bias amplitude is taken as the amplitude found by the maximising linear regression lter.

The correction of the estimated state and the innovation are calculated from the fault signature of the maximising linear regression lter.

The GLR algorithm was developed for detecting and estimating sparse jumps in inputs or measurements, i.e. only a single bias is assumed to appear on the observed window [t -(M + L)∆t]. In order to detect and estimate subsequent bias appearing after the rst one, the detected and estimated bias must be permanently removed from the measurement. Of course this implies that the fault signature is cancelled and the bias estimation can no longer be rened.

Position estimation integrity with non-redundant measurements

An important aspect of fault tolerant position estimation is the concept of integrity. Simply put, an integrate position estimate is an estimate whose estimation error is consistent with its estimated error (through its covariance), and whose protection level (P L), calculated from the covariance, remains bounded within a predened bound, precisely the alert limit AL.

The details on how to calculate the protection level are given in Appendix B. In short, P L depends on the corrected state covariance and the fault signatures. The higher the covariance, the higher the protection level. The longer the detection window, the lower the protection level, although this is limited by the actual length of the Kalman lter innovation.

State of the art -Fault-tolerant position estimation

The eld of fault-tolerant position estimation, and the larger scope of fault-tolerant navigation is incredibly vast. The state of the art as presented in this section only handles the eld of position bias estimation, a narrow but central part of fault-tolerant navigation. The major part of works in this domain concern INS/GNSS integration schemes and the detection and estimation or rejection of faulty measurements. From an application point of view, this is quite obvious since it makes up the de facto foundation of modern navigation.

A common assumption in the literature, especially regarding GNSS position bias caused by multipath events, is that the biases are rare and sparse events. A recent review on GNSS integrity literature [START_REF] Zhu | GNSS position integrity in urban environments: A review of literature[END_REF] states that this assumption rarely holds in urban environments. This might be a trivial statement after all but it highlights the need for bias estimation algorithms that handle multiple frequent biases.

Another factor hidden in the INS/GNSS literature is the question of non-redundancy of measurements.

Indeed each satellite only provides one position measurement (the radius of a sphere), and four are required to solve for the 3D-position, the problem only reduces to a non-redundancy problem if less than four satellites are available. As a consequence of this, the rich body of INS/GNSS multipath algorithms cannot be applied directly to the case of non-redundant position measurement bias problem, however some overlapping exists.

Finally, the question of position integrity must be posed. Indeed if the number of detected and estimated biases accumulates, so does the total uncertainty related to the corrected position since the biases are unobservable. The integrity problem has received massive attention by the GNSS community for obvious reasons, resulting especially in numerous works on measurement fault detection and exclusion [Giremus et al., 2007a;[START_REF] Nikiforov | Advanced RAIM algorithms for safe navigation based on the constrained GLR test[END_REF]. However, the simple case where a position is estimated with a single sensor of unspecied technology has been left in the shadow.

Bias estimation using the Generalized Likelihood Ratio

Willsky and Jones developed in [START_REF] Willsky | A generalized likelihood ratio approach to state estimation in linear systems subjects to abrupt changes[END_REF] the Generalized Likelihood Ratio algorithm for detecting and estimating jumps of unknown amplitudes and jump times in linear systems. The GLR test is a so called multiple hypothesis test, in which the concurrent hypotheses are the potential jump times. In practice, the GLR test is implemented as a sliding window method, where the jump time is assumed to exist on the sliding window and the jump amplitude can be estimated by a Least Squares (LS) estimator using the analytical response (signature) of a linear(ised) Kalman lter as the regressor.

An obvious drawback of the original GLR algorithm is that only one jump per window is considered, meaning that a compromise has to be made regarding the detection delay and the bias estimation precision. This limits the applicability of the algorithm to cases where faults appear sparsely in time.

For high-grade navigation applications, this is not a problem a priori as described above. A large number of applications of the GLR algorithm have been reported, with GNSS integrity and target tracking as two large domains. However, the method has received a fair amount of critique for not being robust or being optimal under only very limited circumstances (linear model, Gaussian white noise), which would limit its usefulness in practice [START_REF] Kerr | Drawbacks of Residual-Based Event Detectors like GLR or IMM Filters in Practical Situations[END_REF]. In the case of position bias estimation, both the dynamics and the measurement equations are linear, meaning that only the question of Gaussian noise risks polluting the results.

In the late 20th century and the early 21st century, his algorithm has been used extensively in GNSS health monitoring for detecting range failures. In his licentiate thesis, [START_REF] Palmqvist | Integrity monitoring of integrated satellite/inertial navigation systems using the likelihood ratio[END_REF] proposed to use the GLR algorithm to detect bias and drift errors in a navigation lter. Only the detection part of the GLR was used however, and only single biases considered.

In an eort to detect and exclude step and ramp errors in a GPS/Galileo/IRS context, [Giremus et al., 2007a] applied the GLR algorithm to civil aviation navigation with an integrity perspective.

Again only the detection part of the algorithm was tested, but it was shown to be an eective means to detect and isolate faulty measurements.

Later [START_REF] Spangenberg | Detection of variance changes and mean value jumps in measurement noise for multipath mitigation in urban navigation[END_REF] used GLR tests to mitigate multipath eects in urban environments, although again only sparsely appearing faults were considered. The GLR test, in this case, was used to classify the multipath eects either as an increase in variance or as a mean value jump.

However, the detection was performed using a simple χ 2 test.

A take on modifying the GLR algorithm to accommodate multiple frequent failures was proposed by [START_REF] Jamouli | A mixed active and passive GLR test for a fault tolerant control system[END_REF]. The authors propose a two-stage Kalman lter where the bias is included in the state and the fault signature is included in the state dynamics. In the rst stage, the prediction of the state and the bias is done as per the standard Kalman procedure, a GLR test is then performed on the innovation. If a bias is detected, a correction of the innovation is calculated to account for the bias, and the bias part of the state is reinitialised with the value of the detected bias. The authors eectively solved the problem of conditional observability associated with the non-redundant actuator/sensor problem. However, their solutions does not account for the accumulating uncertainty when continuously detecting conditionally observable biases. Also the solution treats the bias estimation problem in a completely serial way, meaning that it is only the last detected bias that is continuously estimated. Their solution can therefore be seen as a complete solution only for the single bias case.

A problem of directly integrating the GLR algorithm in navigation solutions is that it does not solve the non-redundancy problem. That is if only one position sensor (GNSS / Vision / Radar / etc.) is available and it is biased, the position is no longer observable and can only be conditionally estimated if the bias apparition time is known.

In addition to this, the bias estimation is truncated due to the sliding window. This means that when a bias estimation leaves the sliding window, the position estimation will have a static bias corresponding to the truncated bias estimation. The accumulation of bias detections and estimations leads to an ever increasing covariance of the corrected state. In the end this means that despite the corrected state estimation being close to the real state, the uncertainty of the corrected state only increases with time.

This problem is of course inherently related to the non-redundancy and must be handled by any sensible bias estimation algorithm.

Bias estimation using nonlinear ltering

In the classical INS/GNSS position estimation problem the measurements used to correct the position are the satellite pseudo ranges. These depend in a nonlinear fashion on the position of the receiver which means that a linear(ised) lter such as the GLR will be suboptimal. Although this has, as we have seen, not stopped people from using it in general aviation contexts where the nonlinearities remain modest.

In fact, a nonlinear version of the GLR algorithm, based on an UKF was developed in [START_REF] Liu | A nonlinear version of the generalized likelihood ratio test[END_REF]. Some brief simulation results indicate that the estimation of time-varying additive faults in a nonlinear system can eectively be estimated using a nominal fault free model and a batch of nonlinear fault model each assuming a fault appearance at a certain time instant. The developed method couples the fault tracking performance, the detection delay, and the estimation accuracy with the length of the sliding window. The tracking performance is assumed to deteriorate with a longer window since the fault is fundamentally supposed constant in the window. In the linear GLR, only the estimation accuracy is determined by the window length since only a single bias is assumed. The detection delay depends only on the xed-lag of the window.

As what regards fully nonlinear methods, [Giremus et al., 2007b] attacks the problem of multipath estimation with a particle lter. It is shown to improve signicantly the precision of the bias estimation as compared to the GLR, although with a computational cost many times higher. The multipath biases are also assumed to be sparse, and no conclusions are drawn regarding the implication of frequently appearing biases.

Conclusion

Estimating position bias is a research area which has so far mostly concerned high grade INS/GNSS applications in aviation. As a consequence, the basic assumption found in the literature is that bias events are suciently rare to be regarded as isolated events. This has led to the development of various techniques to detect and isolate single biases in position measurement, notably multipath biases in GNSS pseudoranges.

The GLR algorithm has shown to be an ecient method for detecting faults linear and nonlinear system, however, the usefulness of the estimation part of the algorithm is limited to linear systems and measurements, such as the general positioning problem. Furthermore, the GLR algorithm is often constrained to the sparse fault case, with a few exceptions for sequential faults.

Furthermore, the question of integrity for non-redundant measurements has not been properly investigated in the literature. Especially the case where multiple biases are detected and estimated.

Indeed the lion's share of studies on integrity for position estimation concerns GNSS receivers or INS aided GNSS, a large part through the use of detection/exclusion strategies. The question of the bias estimation impact on the position estimation integrity is rarely posed.

To conclude, in order to develop a consistent and integrate position measurement algorithm for general non-redundant position and velocity measurements, a few points require some additional attention:

Bias appearances must be detected and estimated in the case where biases cannot be considered as rare events. Notably, in the case of sliding window approaches, no assumption on the number of biases appearing in the window should be made.

Biases should be continuously estimated as long as they remain observable in order to use the maximum amount of information and decrease the uncertainty of the bias estimation.

The position estimation must remain integrate despite the cumulative detection and estimation of multiple biases.

In Chapter 3 we will develop a tentative solution to the above points based on the GLR algorithm extended to the case of multiple biases or drifts in the same sliding window. Furthermore, the integrity problem is attacked by means of elimination of estimated biases over time.

The attitude estimation problem

Attitude determination is the problem of nding the orientation of a body with respect to a reference (inertial) frame (see Figure 2.2). In UAV applications, the attitude information is crucially important as it is used by the (auto-)pilot to stabilise the vehicle and to properly navigate. The attitude information can also be used by secondary systems for mapping and/or perceiving or manipulating the environment.

The orientation of the body xed frame (B) w.r.t the inertial frame (I ) is represented by the rotational matrix R. This rotation and its inverse R -1 = R T link the components of any vector r expressed in the inertial frame (r I ) and in the body frame (r B ).

r B = R T r I ⇔ r I = R r B .
(2.7)

Figure 2.2: Representation of rotation conventions between a body xed frame (B) and an inertial frame (I) using the rotation matrix R.

The attitude estimation problem consists of nding a rotation matrix R satisfying (2.7) using a set of body frame vector measurements r m of some (known) inertial frame reference vectors r ref .

In the general case, two independent reference vectors (r ref,1 , r ref,2 ) with associated measurements (r m,1 , r m,2 ) dene R. Indeed we have:

r m,1 r m,2 r m,1 × r m,2 = R T r ref,1 r ref,2 r ref,1 × r ref,2
If r ref,1 and r ref,2 are independent, the right hand side matrix is invertible, and R can be computed. The estimated rotation matrix is denoted R.

Attitude representations

The attitude can be expressed in a multitude of ways. Indeed the attitude consists of three independent variables. Each representation uses at least three variable to represent the attitude, the rotation matrix uses nine for example. In the cases where more than three are used, a number of constraints are introduced on the remaining variable to constrain the number of free variables to three.

Except for a few singularities, all representations of attitude are equivalent, and relationships exist to move from one to the other. Depending on the estimators, their context of use, a representation may be preferred. Numerical aspects are important to take into account, especially for non-minimal representations for which constraints between redundant variables must be maintained.

The attitude estimation error can be represented with the same settings as the attitude representation, but this is not mandatory. An important point is the representation of uncertainties by the covariance of the estimation error. It is often preferred to move to a minimum representation to do so.

• Rotation, or Direct Cosine Matrix (DCM): This fundamental representation introduced here-above has nine elements (three orthonormal 3D-column vectors), linked by six nonlinear equations (R T R = I), resulting in three degrees of freedom. When using this representation in estimation problems it may be quite dicult to preserve the orthonormalisation constraint.

The DCM is also not very practical from an estimation point of view when representing uncertainties, e.g. for the estimation error rotation matrix δR = R RT . Since δR is already two dimensional (δR ∈ R 3×3 ), its covariance matrix adds another dimension (P δR ∈ R 3×3×9 ). The computational complexity therefore risks being elevated when using this representation directly.

• Euler angles: The classical representation of attitude in aeronautical contexts. The attitude is represented by a sequence of rotations, namely yaw (ψ), pitch (θ), and roll (ϕ). In this work, we use the (intrinsic) sequence R = R ψ R θ R ϕ . The relation between the Euler angles and the rotation matrix R using the above rotation sequence is: R =    cos ψ cos θ cos ψ sin θ sin ϕ -sin ψ cos ϕ cos ψ sin θ cos ϕ + sin ψ sin ϕ sin ψ cos θ sin ψ sin θ sin ϕ + cos ψ cos ϕ sin ψ sin θ cos ϕ -cos ψ sin ϕ -sin θ cos θ sin ϕ cos θ cos ϕ

  

A major drawback of this representation is its singularity at θ = ±π/2, where the angles ϕ and ψ cannot be resolved.

The estimation error representation in this case is straightforward, we have

   δϕ δθ δψ    =    φ θ ψ   -    ϕ θ ψ   
Clearly the associated error covariance matrix P Euler ∈ R 3×3 is of a minimal dimension, which is an advantage in estimation compared to using the nine-dimensional R. A disadvantage of representing the error in euler angles is that at large yaw angles, the errors in roll and pitch become mixed. To illustrate, we see that about the point (ϕ, θ, ψ) ≈ (0, 0, 0) we have the rotation matrix:

R (0, 0, 0) ≈    1 -ψ θ ψ 1 -ϕ -θ ϕ 1   
This approximation is called a micro-rotation (it's not quite a rotation anymore). But about the point (ϕ, θ, ψ) ≈ (0, 0, π/2) we instead have

R (0, 0, π 2 ) ≈    -ψ -1 ϕ 1 -ψ -θ -θ ϕ 1    =    0 -1 0 1 0 0 0 0 1    Transformation R (0, 0, 0)
The transformation matrix on the right hand side switches the x and y axes (and changes signs).

• Inclination/Heading: This attitude representation is akin to the euler angles. The dierence is that the roll and pitch angles are clumped together as one inclination angle α. The heading (ψ ) is dened as the angle around the inertial vertical axis (e 3 ). The inclination (α) is dened as a rotation about an axis u perpendicular to the heading axis (cf. Figure 2.3). Any rotation of the mobile platform about the vertical axis preserves the inclination angle α. There exists a unique rotation angle ψ such that R = R ψ R α . By undoing the rotation α, the remaining angle ψ is the angle between X I and X B . The advantage of this representation over the Euler angles is that the inclination is represented by a single angle which is invariant to rotation around the vertical axis (e 3 ) since its about a perpendicular axis. This can be interesting in the case of multirotor drones, where the inclination of the platform is more important than the individual angles (roll and pitch), e.g. for stabilisation.

The error representation in this case is a bit trickier than for the Euler angles. Clearly we have reduced the number of angles to only two (ψ , α). However since we have added constraints on the axes about which these rotations are dened, we add additional uncertain variables (u).

Then the total number of uncertain variables in this representation is four, since u 3 ≡ 0, and e 3 is absolutely dened. The error covariance matrix is therefore P ψ ,α ∈ R 4×4 . Note that because of the nonlinear constraint (u T u = 1) there are only three degrees of freedom.

• Quaternion: The attitude can also be represented as a four dimensional vector using a unit vector η and an angle β , i.e.

q q β = (cos (β/2) , η sin (β/2)) T = (s, v) T .

(2.8)

where s is the scalar part of the quaternion and v is the vector part. For a quaternion to represent an attitude it must be unitary, i.e. q T q = 1. Indeed we have q T q = cos 2 (β/2) + η T η sin 2 (β/2).

Clearly the unit norm of q can only be reinforced if η is unitary.

The quaternion has the advantage of providing a non-singular attitude representation for all attitudes. However it suers from an ambiguity since q(η, β) = q(-η, -β). The quaternion rotation is related to the Euler angles as it can be decomposed as a sequence of rotations, i.e. q = q ψ ⊗ q θ ⊗ q ϕ (Euler angles) or q = q ψ ⊗ q α , (Inclination/Heading)

The relation between R and q is called Rodrigues' formula and is written as R(q) I -2 s v × + 2 v 2 × , where v × is the skew-symmetric representation of v. The rotation associated to q β is the rotation of angle β around the axis η.

With quaternions, the attitude estimation error is dened by δq = q q-1 , and the associated covariance matrix is P q ∈ P 4×4 . The unit constraint of the quaternion implies there exists a constraint on the error covariance matrix when quaternion errors are regarded. We note that since the error quaternion retains the unit constraint (δq T δq = 1), the covariance matrix is constrained to always have a trace of 1, representing that there are only three degrees of freedom.

• Modied Rodrigues Parameters (MRP): Apart from using Euler angles, it is also possible to use a three-component projection of the quaternion. One example is the Modied Rodrigues Parameters [START_REF] Wiener | Theoretical analysis of gimballess inertial reference equipment using deltamodulated instruments[END_REF], denominated a p and derived from a quaternion q (s, v) as:

v/(1 + s) = η tan (β/4) a p /4
The factor 1 4 ensure that ||a p || ≈ |β| for small rotations. This three dimension representation avoids the easily attainable singularity at θ = π/2 which plagues the Euler angles. For the MRP, the singularity is shifted to β = 2π, analogous to a 2π rotation in any of the Euler angles.

The attitude error representation with MRPs δa p is obtained directly from the quaternion error representation δq. For small error angles (δβ ≈ 0) we nd the Euler error angles, i.e. δa p = (δϕ, δθ, δψ). Note that the relation between the micro-rotation and the error rotation matrix for small angles is

(δa p ) × = δR .
The interest of using a three parameter representation of the attitude error is that it allows for an unconstrained non-singular error representation. This is particularly interesting for representing uncertainties since it implies no constraints on the covariance matrix. This is in contrast with quaternion and rotation matrix uncertainties, which are coupled and unit constrained, and Euler angle uncertainties, which are periodic.

The three-component attitude error representation is also the smallest dimension non-singular error representation, making it interesting from a computational point of view.

Modeling for attitude estimation

Henceforth we will focus mainly on the estimation of quaternions and rotation matrices since they provide a non-singular representation of the attitude.

Attitude kinematics

The rotation and quaternion kinematics are given by

Ṙ = R ω × (Rotation matrix) and q = 1 2 Ω(ω)q, Ω(ω) = 0 -ω T ω -ω × (Quaternion) (2.9)
where ω (ω x , ω y , ω z ) is the vector of body angular rates in the body frame. The pure integration of the body angular rates theoretically allows to reconstruct the attitude if the initial orientation is known.

Gyroscope model and attitude prediction model

The body angular rates are often measured by gyroscopes. A common drawback of gyroscopes is that they are more often than not subject to a low-frequency bias, in addition to being sensitive to vibrations. The low-frequency bias is also often partially a consequence of vibration eects. A commonly used measurement model for gyroscopes is

ω m = ω + b ω + v ω ḃω = v bω (2.10)
where v ω ∼ N (0, V ω ) and v bω ∼ N (0, V bω ) are modelled as Gaussian white noises.

Together with the kinematics model we can synthesise an attitude quaternion prediction model:

q = 1 2 Ω(ω)q ḃω = v bω with ω = ω m -b ω -v ω (2.11)
As mentioned above, if the initial orientation is perfectly known and the bias of the gyroscope is zero (b ω = 0), the attitude can be found using this model with only the gyroscope as a measurement.

However, since this is practically never the case in reality, additional measurements are needed to obtain a drift-free estimation of the attitude that converges towards the real attitude despite an unknown initial attitude.

Attitude measurements

In order to correct the attitude drift and estimate the gyroscope bias, attitude measurements or vector measurements, are classically used. These measurements are measurements in the body xed frame of some known vector in the inertial frame r * ref .

However when an inertial disturbance (∆r I ) is present the sum of the two is measured. Disturbances may also appear in the body frame (∆r B ). The vector measurement r is properly modelled as r m = R(q) T (r * ref + ∆r I ) + ∆r B + w r where w r ∼ N (0, W r ) is a Gaussian white noise. For terrestrial and atmospheric applications, such as drones, the main sensors used for vector measurements are accelerometers and magnetometers :

-Accelerometers measure local gravity (a * ref ) and specic accelerations (∆a). This means that they can be used as inclinometers as long as the specic acceleration is zero. By construction they are also very sensitive to vibrations, often causing high frequency noise on the sensor output.

These factors make their direct use as inclination sensors limited in UAV platforms, which are more often than not subject to both.

-Magnetometers measure the local magnetic eld (m * ref ) and local disturbances (∆m). Classi- cally their use is twofold. It can be used as a compass, where the magnetic direction, that is the normalised measurement, is used to indicate the magnetic north. It can also be used to measure the full magnetic eld. This measurement can then be compared to an a priori known reference eld to deduce the true north. In this second case, the norm can also be used to detect anomalies in the measurement.

In the UAV context, local magnetic disturbances are not a trivial problem. For one part, magnetometers are often installed in an environment full of electromagnetic activity in terms of electric motors, antennas, etc. . . , and for the other part, the environment in which the UAV evolves is often electromagnetically polluted with power lines and iron structures.

We denominate the measurements from accelerometers by a m and from magnetometers by m m . If the disturbances ∆a and ∆m are modelled in the inertial frame, the corresponding measurement models are given by

a m = R(q) T (a * ref + ∆a) + w a = R(q) T a ref + w a m m = R(q) T (m * ref + ∆m) + w m = R(q) T m ref + w m
(2.12)

A typical example of this is when they result from an external disturbance such as power line interference [START_REF] Chesneau | Improving magneto-inertial attitude and position estimation by means of a magnetic heading observer[END_REF].

But if the disturbances are modelled in the body frame, the measurement models become

a m = R(q) T a * ref + w a = R(q) T a ref + ∆a + w a m m = R(q) T m * ref + w m = R(q) T m ref + ∆m + w m . (2.13)
This model can be useful if a disturbance is expected in the body frame, such as an electromagnetic payload being switched on, or an actuator malfunction creating structural vibrations.

The noise terms in both cases, w a ∼ N (0, W a ) and w m ∼ N (0, W m ), are modelled as Gaussian white noises and represent zero-mean uncertainties such as vibration eects or electrical noise. In some cases these noise terms are modelled in the inertial frame for the sake of deriving convergence proof of attitude estimators.

In this thesis, we mainly consider external perturbation, and will use the model with inertial frame disturbances 2.12 as the measurement model for estimation model synthesis.

Estimation model synthesis

In the nominal case, for estimator synthesis, the disturbance is assumed to be zero, i.e. ∆a = ∆m = 0, implying that the measured reference vector is actually the known one (r ref ≡ r * ref , for both r = a and r = m).

Together with the attitude prediction model (2.11), these measurements are used to provide a synthesised model for attitude quaternion estimation and gyroscope bias estimation for drones:

   q = 1 2 Ω(ω)q ḃω = v bω with ω = ω m -b ω -v ω a m = R(q) T a ref + w a m m = R(q) T m ref + w m (2.14)
The estimation lter can be designed from the continuous time model, and then the lter is discretised at the implementation stage. However it is also possible (an more often done) to discretize the synthesis model, and then design the lter within the discrete time framework.

Estimation model drawbacks

If the model (2.14) is used directly in the design of a model based lter we note some obvious aws:

The gyroscope bias is modelled as a random walk meaning that it can take any value with equal probability. This is not likely in a normally functioning attitude determination system where we know that this bias is physically limited in amplitude. A more reasonable model would take into account the most probable interval of the gyroscope bias.

The estimation of the gyroscope bias depends on the attitude measurements. This means that it will be impacted in the case of a perturbation of the attitude measurements. From a physical point of view this is of course unreasonable since the gyroscope bias is attitude independent and concerns a physically dierent sensor. A better solution would be to estimate the gyroscope bias independently of the attitude measurements during periods of external perturbations.

Both attitude measurements, accelerometer and magnetometer, are measurements using the full attitude. This will unfailingly lead to all estimated attitude angles (roll, pitch, and yaw) being impacted in case of perturbations on one of the measurements. It would be more cautious to have each measurement only correct the part of the attitude where it has the most information.

For example the accelerometer should primarily correct the inclination and associated gyroscope biases, and the magnetometer only the heading. This procedure, called decoupling is more or less explicit depending on the attitude representation used in the estimator.

The attitude measurements are supposed to measure their respective reference vectors (a ref = a * ref ). This will not be the case if there are external perturbations such as accelerations or magnetic disturbances (∆a = 0 or ∆m = 0). Using some mechanism that detects disturbances, or a model that allows deviations from the known reference would be a more cautious approach.

Conclusion

As we have seen in this chapter, there exists a multitude of ways to represent attitude, attitude errors and uncertainties. The models used to estimate the attitude also exist in numbers. Here we have only presented a synthesised model in quaternions, but it could equally well be written using any other attitude representation. The choice of attitude representation decides which problems in terms of singularities are needed to address at the estimation level. It is therefore application specic which representation is chosen. For example a glider aircraft is not likely to encounter pitch angles of π/2 radians.

Furthermore, the presented synthesised estimation models are not self-sucient for fault-tolerant attitude estimation, and barely so under near-nominal conditions. In order to handle measurement disturbances for example, the attitude measurement models could be replaced so that each attitude measurement is not seen as a function of the full attitude. In the upcoming section we will look at the major groups of techniques which exist for attitude estimation under fault-free conditions. We will then move on to the eld of fault-tolerant attitude estimation.

State of the art -Attitude estimation algorithms

A large body of literature has whipped the attitude estimation problem for several decades. Before the turn of the millennium, applications mostly concerned high-precision and high-cost cases, meaning that the problems arising with low-cost hardware and hostile sensor environments were not on the agenda.

The survey [START_REF] Crassidis | Survey of nonlinear attitude estimation methods[END_REF] provides a good historical summary of attitude estimation lters, concluding with the remark "if it ain't broke, don't x it", referring to the proven record of the extended Kalman lter (EKF). The authors however point out that initialisation errors are typically small in high performance applications generally treated so far and that this can cause problems for EKF-based lters.

This investigation of the state of the art is divided into two major parts:

• Fault-free attitude estimation, or attitude estimation under nominal conditions, focusing on convergence and precision in the absence of disturbances.

• Fault-tolerant attitude estimation, that is to say attitude estimation under the inuence of measurement disturbances and/or faulty sensors. Here the focus is generally error minimisation during disturbances.

Fault-free attitude estimation

The concept of fault-free attitude estimation or attitude estimation with fault-free sensors designates the basic problem of converging to the real attitude and staying there with sucient precision. The main assumption being that the sensors used provide some fault-free (but noisy) information on the attitude and/or attitude rates. In common speak, this is referred to attitude ltering, and can be done recursively or non-recursively (as for ltering in general).

Let us for a moment reect upon the recent literature regarding attitude ltering. the main families of solutions are either based on constant gain observers (with members such as complementary lters and sensor based observers ), or Kalman lters. The rst category consists of fusing data according to their relevant bandwidth. For example, the valuable gyroscope information lies in it's high frequency content (amplitude of attitude changes), whereas for an accelerometer it lies in its low frequency content (inclination with respect to gravity over time).

A constant gain observer, will thus in general be posed as an observer where the low frequency information from attitude sensors such as accelerometers and magnetometers is used to compensate for the low frequency gyroscope bias. A Kalman lter on the other hand, integrates a prediction model and then corrects the predicted estimate with attitude measurements which are weighted according to their uncertainty.

Constant gain observers

Constant gain observers comprise the conceptually simplest kind of attitude estimators. Despite this fact, their major developments have been done in the last two decades.

• Complementary lters gained popularity as the development of low-cost UAVs took o, the obvious advantage being the low computational complexity. The lters in [START_REF] Mahony | A coupled estimation and control analysis for attitude stabilisation of mini aerial vehicles[END_REF] and [START_REF] Hamel | Attitude estimation on SO [] based on direct inertial measurements[END_REF] were among the rst attitude observers to provide (almost) global stability. The "almost" comes from the attitude ambiguities discussed in 2.3.1, where topological constraints of the rotation group prevent global stability (e.g. the orthonormal constraint on R or the unit constraint on q). The basic lter is posed as an attitude and gyroscope bias observer on the rotation group (for a rotation matrix R or a quaternion q). The lter is given in a direct version, accompanied by the stability proof and a noisier estimation, and a passive version with a weaker stability proof and a smoother estimation.

The passive version, deemed more promising due to the nature of the estimate was further developed in [START_REF] Mahony | Nonlinear complementary lters on the special orthogonal group[END_REF], leading to the explicit complementary lter. The main dierence being that only measured directions are used and not the full measurements, also the algebraic reconstruction of the attitude is avoided. The lter is also shown to have almost global stability.

Strengthened by this result, further research in the same direction resulted among other in the lter by [START_REF] Hua | Nonlinear attitude estimation with measurement decoupling and anti-windup gyro-bias compensation[END_REF]. The proposed lter is an extension of the explicit lter from [START_REF] Mahony | Nonlinear complementary lters on the special orthogonal group[END_REF]. The new lter sports a local measurement decoupling, that is a decoupling of roll and pitch from magnetic disturbances, previously introduced in [START_REF] Martin | Invariant observers for attitude and heading estimation from low-cost inertial and magnetic sensors[END_REF], and a saturated bias model. From a fault-tolerant point of view, this lter can be seen as a rst where measurement disturbances were seriously considered.

• Constant gain pseudo-attitude observers were developed to overcome the "almost" global stability of complementary lters. The authors of [Batista et al., 2012a] proposed an observer which estimates the column of R independently, thereby estimating a "pseudo-attitude" which converges asymptotically to a real attitude. The recomposed rotation matrix is then orthogonalised to provide an approximation of a rotation matrix. This trick allows the lter to ignore the constraints on R in the estimation step and cleverly guarantee global exponential stability.

Along the same lines, the lter presented in [START_REF] Grip | Globally exponentially stable attitude and gyro bias estimation with application to GNSS/INS integration[END_REF] estimates a full rotation matrix, which akin to the lter in [Batista et al., 2012a] has to be orthonormalized to remain a rotation matrix. The proposed lter also proposes a saturated bias model, in practice similar to the one in [START_REF] Hua | Nonlinear attitude estimation with measurement decoupling and anti-windup gyro-bias compensation[END_REF] but based on a parameter projection instead of an anti-windup compensation.

Neither the lter in [Batista et al., 2012a], nor the one in [START_REF] Grip | Globally exponentially stable attitude and gyro bias estimation with application to GNSS/INS integration[END_REF], is mentioned to have decoupling qualities. This limits their usefulness in cases where disturbance rejection is required.

• Sensor based observers continue along the lines of constant gain observers and estimation of unconstrained variables in order to establish global stability. Two examples are the sensor based lter by [START_REF] Martin | A global observer for attitude and gyro biases from vector measurements[END_REF], and its further development in [START_REF] Martin | A global observer for attitude and gyro biases from vector measurements[END_REF]. The speciality of these lters is that they estimate two non-parallel directions and construct a rotation matrix algebraically. Unlike [Batista et al., 2012a] and [START_REF] Grip | Globally exponentially stable attitude and gyro bias estimation with application to GNSS/INS integration[END_REF], only 6 attitude parameters are needed to estimate the full attitude. In addition, the reconstructed attitude has excellent decoupling properties from magnetic disturbances. The drawback is that the gyroscope bias estimation is very sensitive to measurement disturbances, and thus to the rejection of measurements due to external disturbances.

Summa summarum, the major contributions to constant gain observers have managed to assure global stability, at the cost of exact attitude representation. Since the gains are xed and constant, the trade-o between convergence speed and precision requires meticulous tuning. Furthermore, the lack of an uncertainty estimation limits the usefulness of this class of lters in fault-tolerant applications.

Kalman lters

Since their initial development in the 1960's [START_REF] Kalman | A new approach to linear ltering and prediction problems[END_REF], Kalman lters have been a workhorse of attitude ltering. Their main appeal lies in the ease of tuning which is done automatically once the noise and uncertainty characteristics of the sensors and the model have been established.

Starting from one of the earliest attitude applications [START_REF] Farrell | Attitude determination by Kalman ltering[END_REF], where a torque model together with a sun sensor and a magnetometer were used to determine the attitude of an orbital spacecraft. The lter is based on the estimation of Euler angles meaning that the additive correction of the standard EKF does not pose a problem.

Fast-forward to [START_REF] Leerts | Kalman ltering for spacecraft attitude estimation[END_REF] and we see the more familiar "modern" formulation using a kinematics model propagated by biased gyroscope measurements and corrected by noisy attitude measurements. The authors also present Kalman lters based on dierent attitude error representations, namely the error quaternion and the three parameter attitude error.

The Kalman lter, in particular the extended Kalman lter, although practical for attitude estimation, suers from a number of major drawbacks.

1. The estimated attitude does not remain a rotation since the predictions and corrections are additive. The estimated attitude must therefore be forced ad-hoc to remain a true attitude (e.g.

through quaternion normalisation or rotation matrix orthonormalisation).

2. Due to the singularities encountered in various ways in all attitude representations, the estimated covariance is not guaranteed to be bounded. This potentially leads to divergence of the lter.

3. The consistency cannot always be assured, unless the covariance is gravely overestimated. This is essentially a problem of initialisation and convergence after disturbances.

4. Long periods of unobservability lead to divergence of the covariance and possibly drift of the state. This can, in turn, cause lter divergence or instabilities when observability is regained.

5. The convergence of the lter is only guaranteed if the dynamics and measurements are linear, and the noises are Gaussian and uncorrelated with the state.

Several developments over the past twenty years have provided more or less complete solutions to these drawbacks.

• The rst point can be handled by replacing the additive correction by a multiplicative one. This is the case for the aptly named Multiplicative Extended Kalman Filter (MEKF) [Markley, 2003;[START_REF] Markley | Multiplicative vs. additive ltering for spacecraft attitude determination[END_REF]. The trick is to multiply, at each update, the previously estimated (or reference) quaternion with the correction. The standard MEKF also propagates the covariance at each update with the attitude kinematics linearised around zero attitude error. It uses the micro-rotation error representation.

The MEKF itself was further developed in [START_REF] Martin | Generalized multiplicative extended kalman lter for aided attitude and heading reference system[END_REF] as the Invariant Extended Kalman

Filter (IEKF). The authors pointed out that the gains (and thus the covariance variations) of the MEKF are constant only when the rotational speed is zero, whereas it is possible to achieve constant gains on trajectories where the gyroscope bias is constant. A consequence of this is that the state error dynamics only depend on the error itself and the lter invariants, quantities that are invariant to rotation, thus improving the lter stability. Two variants were developed, named left invariant and right invariant. In the former version, the disturbances are modelled in the inertial frame, whereas in the latter they are modelled in the body frame.

• The second point and the third point can be handled by using an unscented transformation and thus bypass the singularities by calculating the covariance from a set of transformed attitudes instead of a single point linearisation.

To this end, the idea behind the MEKF was reused in the formulation of an attitude lter based on the Unscented Kalman Filter (UKF), named UnScented QUaternion Estimator (USQUE) [START_REF] Crassidis | Unscented ltering for spacecraft attitude estimation[END_REF]. The UKF uses the nonlinear prediction and measurement models directly to predict and correct the state and covariance, thus avoiding linearisation errors.

The USQUE claimed to better capture the nonlinearities of the attitude problem and was shown to achieve signicantly higher accuracy than the MEKF. It also arguably improves the estimation of the covariance since the problems of linearisation are avoided. However, the USQUE still retains the small error approximation of the MEKF, implying potential problems during disturbances.

The UKF for attitude ltering also got a facelift in terms of invariant ltering, called the IUKF [START_REF] Condomines | Invariant unscented Kalman lter with application to attitude estimation[END_REF]. It is shown that applying the general invariant observer framework indeed results in an estimator with quasi-constant gains and covariance estimation. This is in addition to the accuracy improvements already shown for the attitude UKF with respect to the MEKF.

• The fourth point is rarely handled in the literature. One of few papers addressing the lack of measurements is [START_REF] Vinther | Inexpensive cubesat attitude estimation using quaternions and unscented Kalman ltering[END_REF] where the behaviour of an attitude UKF is studied using various state models for the application of a nanosatellite during a solar eclipse.

• The fth point is also rarely handled with Kalman lters due to the nonlinearities of the attitude problem. One of few solutions which tackles the stability problem of the nonlinear Kalman lter is the exogenous Kalman lter (MXKF) [START_REF] Stovner | Attitude estimation by multiplicative exogenous Kalman lter[END_REF] combining a globally exponentially stable observer with a MEKF. The principle is quite simple. Instead of linearising around the MEKF estimation, the MEKF is linearised around the state estimated by the nonlinear observer.

The authors show that by doing so, the complete cascade estimator inherits the stability properties of the nonlinear observer and the steady state precision of the MEKF.

The paper [Batista et al., 2012b] proposes a completely dierent approach, compared to the (I)UKF and (I)EKF, akin to the vector based attitude observer in [START_REF] Martin | A global observer for attitude and gyro biases from vector measurements[END_REF]. The authors develop a globally asymptotically stable lter by showing that if the estimation is posed as a vector estimation of the acceleration and magnetic eld, plus the gyroscope bias, a linear parameter varying model can be found if the vector measurements are used in the linearisation.

They then reduce the stability problem to proving uniform observability of the model, under nominal conditions, implying global stability as for a linear Kalman lter. The main drawback of the approach is that the excellent decoupling which was established in [START_REF] Martin | A global observer for attitude and gyro biases from vector measurements[END_REF] is not present in this lter due to the state correlations introduced by the Kalman mechanism.

To conclude on the development of the Kalman lter for attitude estimation, especially the past two decades, we can say that all above stated weaknesses have been treated in the nominal, fault-free case. At the risk of forgetting certain aspects, the holes that are still gaping are:

-The consistency of the estimation in the presence of measurement disturbances.

-The decoupling of the roll and pitch from the yaw estimation and vice versa.

-The integrity of the estimations.

Despite its shortcomings, the Kalman lter has a major advantage in terms of fault tolerance.

The measurement error, or innovation, is Gaussian zero-mean with a known covariance under nominal conditions. This means that fault detection mechanisms are fairly straightforward to implement, as we will see further on.

Fault-tolerant attitude estimation

Now that the solution to the attitude estimation problem itself seems to have been asymptotically converged upon under nominal conditions, the logical question would be: What to do if the measurements are erroneous?

In this section, we will see that there exist a number of partial solutions to this problem. In general, we either nd solutions that are inherent to the attitude lter itself such as decoupling of the disturbance from parts of the estimation, gyroscope bias estimation, and lter consistency . Or solutions that are sensor or lter based such as measurement fault rejection methods.

Rejection of bias on the attitude measurements

Measurement biases and perturbations are a reality for any attitude estimator evolving outside a controlled laboratory environment. In the context of this thesis, when we talk about attitude measurement biases, we consider any disturbance to the measurement which is not modelled as a zero-mean white noise with a priori known covariance. This is to clarify that we do not hypothesise on how or why the measurement does not measure its known reference. The scope of this thesis does not cover the case of measurements with varying covariances.

In some limited cases, sensor biases can be estimated if they are considered constant and the trajectory is modied as to provide persistent excitation of the rotational speed [START_REF] Martin | A global observer for attitude and gyro biases from vector measurements[END_REF]. For spacecraft applications [START_REF] Alonso | Attitude-independent magnetometer-bias determination: A survey[END_REF] surveys methods for identifying constant in-ight magnetometer bias. In most cases however, it is not realistic to model attitude measurement bias as constant, at least not for UAV applications. This is due to accelerometer and magnetometer biases often being the consequence of some unknown external disturbance and will more often than not depend on the speed and trajectory of the drone itself. The only viable option in this case is to detect when the bias appears and disappears and take proper action (single vector estimation or pure gyroscope integration).

When it comes to IMU/Magnetometer-only fault-tolerant attitude estimators, we can distinguish between those focused on compensating acceleration and those focused on magnetic disturbances.

In most solutions, apart from the most rudimentary, the solutions for handling acceleration or magnetic disturbances appear to dier. Disregarding the causes however, there is no dierence between the two since both are manifested as a disturbance that deviates an attitude measurement from measuring its reference. This is why solutions dened to either kind of disturbance (acceleration or magnetic eld) can be easily adapted to the other. Both categories thus fall under the general category of "attitude measurement fault rejection".

Specically regarding the types of solutions, the cheapest one is to blatantly reject measurements that do not satisfy their reference norm [START_REF] Feng | A new quaternion-based Kalman lter for real-time attitude estimation using the two-step geometricallyintuitive correction algorithm[END_REF][START_REF] Wu | Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors[END_REF][START_REF] Sabatini | Quaternion-based extended Kalman lter for determining orientation by inertial and magnetic sensing[END_REF]. The obvious downside is of course that the lter diverges due to a lack of corrections. A more cautious approach is to use an adaptive gain calculated as a function of the measurement norm [START_REF] Valenti | Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs[END_REF][START_REF] Rehbinder | Drift-free attitude estimation for accelerated rigid bodies[END_REF][START_REF] Xing | Accurate Attitude Estimation Using ARS under Conditions of Vehicle Movement Based on Disturbance Acceleration Adaptive Estimation and Correction[END_REF]. The advantage of using an adaptive gain is that the lter is constantly corrected, which can help to repel divergence but during enduring perturbations may lead to attitude drift since some measurement information is retained. Whichever norm based method is used, a signicant problem persists. An erroneous norm necessarily means that the measurement is unhealthy, although inversely a good norm does not imply that the measurement is healthy.

Some solutions therefore reside to model based residual generation and innovation or model based gain adaptation [START_REF] Roetenberg | Compensation of magnetic disturbances improves inertial and magnetic sensing of human body segment orientation[END_REF][START_REF] Costanzi | An attitude estimation algorithm for mobile robots under unknown magnetic disturbances[END_REF][START_REF] Soken | Robust Kalman ltering for small satellite attitude estimation in the presence of measurement faults[END_REF][START_REF] Suh | Orientation estimation using a quaternion-based indirect Kalman lter with adaptive estimation of external acceleration[END_REF]. The adaptation is ether performed directly on the gain (the observer case), or on the measurement or innovation covariance (the Kalman case). These solutions allow to detect erroneous measurement whose norm is still good, however another problem arises in these cases. Innovation based fault detection assumes that the reason for a large innovation/residual is due to an erroneous measurement. This is not necessarily the case, especially during enduring perturbations, since the model itself can be biased, typically due to attitude drift. More exotic solutions involve the estimation of the disturbance itself [START_REF] Martin | A global observer for attitude and gyro biases from vector measurements[END_REF][START_REF] Sabatini | Variable-state-dimension Kalman-based lter for orientation determination using inertial and magnetic sensors[END_REF][START_REF] Suh | Orientation estimation using a quaternion-based indirect Kalman lter with adaptive estimation of external acceleration[END_REF]. These solutions might work well in specic cases but are limited in the sense that the postulate certain characteristics of the disturbances in form of dynamics or duration.

For completeness, we should also mention some estimators that make use of supplementary sensors.

We note for example the use of GPS velocities in [START_REF] Hua | Attitude estimation for accelerated vehicles using GPS/INS measurements[END_REF] to compensate for transverse accelerations.

In [START_REF] Chesneau | Improving magneto-inertial attitude and position estimation by means of a magnetic heading observer[END_REF], a magnetic eld gradient sensor is used in combination with a power grid disturbance model to de-bias the IMU/Magnetometer and provide a velocity estimation.

To briey conclude, an ecient fault rejection architecture should combine coarse norm rejection techniques with more precise model based methods. However to assure the integrity of the fault rejection, eventual models used should also be monitored to ensure that faults are not detected due to model invalidity.

Decoupling of the heading and inclination estimations

Coupling of the estimated attitude angles is inherent to the attitude problem itself. Serious eorts to decouple the estimation is however fairly recent, primarily because it is a problem that aects low altitude and ground bound navigation and to a lesser extent satellites or high ying aircraft, the decoupling of the yaw axis from the roll and pitch is an important aspect of fault-tolerant attitude estimation.

Globally we can divide the topic into two main categories.

The rst one is decoupling by lter construction where for example Euler angles are used and the analytical relations are used to deduce the roll and pitch measurements from the accelerometer and the yaw measurement from the magnetometer. The angles are then estimated independently of each other and the full attitude is recovered as the concatenation of the three Euler angles [START_REF] Huang | Study of the algorithm of backtracking decoupling and adaptive extended Kalman lter based on the quaternion expanded to the state variable for underwater glider navigation[END_REF][START_REF] Li | Eective adaptive Kalman lter for MEMS-IMU/magnetometers integrated attitude and heading reference systems[END_REF][START_REF] Feng | A new quaternion-based Kalman lter for real-time attitude estimation using the two-step geometricallyintuitive correction algorithm[END_REF]. Although simple by construction this decoupling relies on Euler angles with known and easily attained singularities.

A variant of this decoupling is to dene quaternions directly from the measurements instead and imposing constraints on the elements [START_REF] Valenti | Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs[END_REF][START_REF] Yun | A simplied quaternion-based algorithm for orientation estimation from earth gravity and magnetic eld measurements[END_REF]. As for the case of the Euler angles, the quaternions have an ambiguity where q(u, β) = q(-u, -β). This fact creates the need for a binary decision in the correction, making the lter prone to chattering.

Another promising addition to this category is the direct construction of a unitary orthogonal matrix from ltered magnetometer and accelerometer measurements [START_REF] Martin | A global observer for attitude and gyro biases from vector measurements[END_REF].

The second category of decoupling is the use of pseudo measurements. A pseudo measurement is a quantity that acts as a normal measurement for all intents and purposes but is in reality a combination of measured and estimated states. The use of pseudo measurements for yaw to roll/pitch decoupling is straightforward. Instead of using the three dimensional heading information given by the magnetometer, we eliminate the inclination information by projecting the magnetometer measurement on the plan perpendicular to gravity [START_REF] Martin | Invariant observers for attitude and heading estimation from low-cost inertial and magnetic sensors[END_REF][START_REF] Hua | Nonlinear attitude estimation with measurement decoupling and anti-windup gyro-bias compensation[END_REF][START_REF] Costanzi | An attitude estimation algorithm for mobile robots under unknown magnetic disturbances[END_REF][START_REF] Allibert | Velocity aided attitude estimation for aerial robotic vehicles using latent rotation scaling[END_REF]. The gravity axis is either taken as the measured acceleration or the inertial vertical axis projected in the sensor frame.

Consistent estimation of gyroscope biases

For any attitude estimator, when attitude measurements are rejected, the attitude estimation must rely fully or partially on pure gyroscope integration. Especially for MEMS gyroscopes this poses a problem since they are as often as always aected by a signicant bias due misalignment and vibrations. It is therefore crucial to employ a bias estimation which is robust to measurement faults to be able to use a pure gyroscope integration over time with limited attitude drift.

Many attitude estimation schemes utilise the observability of the gyroscope bias through attitude measurements to simultaneously estimate the attitude and the gyroscope bias, see for example [START_REF] Mahony | Nonlinear complementary lters on the special orthogonal group[END_REF][START_REF] Markley | Multiplicative vs. additive ltering for spacecraft attitude determination[END_REF]Batista et al., 2012a]. An obvious drawback of this approach is that attitude measurement disturbances directly impact the gyroscope bias estimation, which is physically unreasonable.

Another intuitive approach, used in some popular schemes [START_REF] Madgwick | Estimation of IMU and MARG orientation using a gradient descent algorithm[END_REF][START_REF] Valenti | Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs[END_REF],

is to assume the gyroscope bias to be the low frequency component of the gyroscope measurement when the sensor (or the platform) is at rest. The condition for being at rest can be either trivially tested with a whiteness test of the gyroscope signal, or with some more sophisticated method. The obvious drawback of this approach is that the platform movement is unknown and therefore no guarantee of convergence can be given (a trivial example being non-decoupled spinning spacecraft or guided munitions). However for applications on aerial platforms this has shown to work well in practice.

In order to ensure the accuracy of the gyroscope bias estimation (and the attitude in pure integration/prediction) we can combine the two methods described above. If we can detect precisely the departure from a steady state and also when the attitude measurements are faulty, we should be able to use the best possible estimate of the gyroscope bias for attitude prediction. Another solution to render the bias estimation robust is to use saturated gyroscope bias models [START_REF] Grip | Globally exponentially stable attitude and gyro bias estimation with application to GNSS/INS integration[END_REF][START_REF] Hua | Nonlinear attitude estimation with measurement decoupling and anti-windup gyro-bias compensation[END_REF] that will help safeguarding the attitude accuracy during disturbances by using a more realistic model (given that a maximum bias can be assumed). However the presented solutions are bias observers and not estimators, hence the critical question of covariance estimation and consistency is avoided.

The following open questions can be identied regarding fault-tolerant gyroscope bias estimation:

• Is is possible to improve the gyroscope bias estimation by switching to an open loop low-pass bias estimation during attitude measurement disturbances?

• How do we transcribe the attitude observer bias saturation into a Kalman lter form? How is the covariance propagation of the bias aected by the saturation?

Attitude estimation consistency and integrity

The last important aspect of fault-tolerant attitude estimation is to ensure a consistency between the estimated attitude uncertainty and the actual error. This is handled well in the fault free case by Kalman lters that minimize the estimated covariance. However in the cases discussed above, that is when observability is lost due to measurement disturbance rejection, the covariance estimation will grow indenitely.

Vary few papers treat this issue as it can be perceived as a problem inherent to covariance based estimators. One of them is [START_REF] Vinther | Inexpensive cubesat attitude estimation using quaternions and unscented Kalman ltering[END_REF] which proposes a dynamic model based UKF for nano-satellites. The authors showcase the importance of properly estimating biases in view of enduring phases of unobservability (in the given case a solar eclipse).

Conclusion

Estimating the attitude of a UAV under nominal conditions, with fault-free measurements, is a very mature research area, both regarding convergence and precision aspects.

As soon as we step outside the nominal case, introducing measurements potentially subject to external perturbations, the playing eld becomes very divided. Many solutions exist for fault-tolerant attitude estimation that supposedly handle transverse acceleration and magnetic disturbances alike.

The existing attitude lters show very satisfactory behaviours during nominal conditions. However, the fault rejection mechanisms frequently employed are rudimentary and often specialised for a specic type of application (UAV, satellite, pedestrian navigation, AUV). Only rarely is the question of consistency between covariance and error covariance during perturbations raised. The evaluation is often performed under conditions where the noise levels are far from those encountered in low-cost UAVs under operational conditions. On the other hand, we cannot blame the community since the go-to strategy for UAVs is generally to "lter like a dog". That is to say, lter out the perturbations on the accelerometer and magnetometer, rely heavily on the gyroscope, and hope for the best. Furthermore, the question of gyroscope bias estimation during perturbations is not handled well, even though a proper bias estimation is crucial to avoid attitude drift.

To start o, how does the state of the art handle the estimation model weaknesses identied in Section 2.3.2?

• The gyroscope bias is modelled as a random walk meaning that it can take any value with equal probability. This is not likely in a normally functioning attitude determination system where we know that this bias is physically limited in amplitude. /.../ This point is treated with a saturation in the form of an anti-windup or parameter projection method in the few examples of the literature.

How to relate the saturation of a bias to the consistency of the bias estimation in the case of a Kalman lter based estimator?

In Chapter 6, the bias saturation problem is extended to the Kalman lter case with the development of a stochastic linearisation of the saturation function. This results in an equivalent gain which is easily implemented in an EKF framework to provide a consistent bias estimation.

The estimation of the gyroscope bias depends on the attitude measurements. This means that it will be impacted in the case of a perturbation of the attitude measurements. /.../ Some authors for UAV oriented attitude estimators use the low-pass ltered gyroscope signal, when the UAV is approximately steady in rotation, to calculate the gyroscope bias. Whereas this is an intuitive approach, it does not guarantee convergence of the bias as in the case of an attitude coupled estimation.

Could a consistent bias estimation during perturbation be achieved if a combination between open loop and closed loop bias estimation is used as a function of detected perturbations?

In Chapter 5, a method for switching between open-loop and closed-loop gyroscope bias estimation is developed. The open loop estimation comes from a low-pass lter and the closed loop estimation from the attitude lter. A switching logic based on attitude measurement fault detection is then used to switch between the two.

• Both attitude measurements, accelerometer and magnetometer, are measurements using the full attitude. This will unfailingly lead to all estimated attitude angles (roll, pitch, and yaw) being impacted in case of perturbations on one of the measurements. /.../

Decoupling of the roll/pitch and yaw estimations is handled by a number of complementary lter based estimators, either by using pseudo-measurements or partial state corrections. For Kalman lters it is more delicate due to the implicit correlation introduced by the covariance matrix.

Is it possible to develop a Kalman lter assuring the same or better decoupling than the one found in some complementary lters?

In Chapter 6 two dierent Kalman lter based estimators with measurement decoupling are developed. The rst one is a quaternion based EKF with a multiplicative correction state based on decoupled heading and inclination corrections. The second one is an IEKF using a pseudo measurement to decouple the roll/pitch from magnetic disturbances.

• The attitude measurements are supposed to measure their respective reference vectors (a ref = a * ref ). This will not be the case if there are external perturbations such as accelerations or mag- netic disturbances (∆a = 0 or ∆m = 0). /.../

The disturbance rejection methods described in the literature are based on ad-hoc techniques such as norm testing or innovation testing, and adaptive gain techniques along the same lines.

Results show that these approaches work fairly well when the perturbations are transient and the signal-to-noise level is high.

How to design a disturbance rejection solution that is ecient even during lasting perturbations and low signal-to-noise ratios?

In Chapter 5, a sensor model based architecture for detecting perturbations and consolidating the sensor data is developed. The architecture is based on one EKF per attitude sensor (accelerometer, magnetometer) for sensor output estimation and fault residual generation, coupled with χ 2 /GLR-tests for fault detection. A consolidation logic thereafter allows to choose between the measurement and the sensor output estimation and to qualify or disqualify the data for attitude update.

An aspect which was not discussed in the introduction is that of consistency and integrity. Although a reader with a background in fault detection will protest at the joining of these two aspects, they are intricately related in the scope of this work. Rejecting measurements to keep the consistency high during disturbances implies relying on prediction, which will increase the covariance of the estimation.

The integrity of the system however relies upon the covariance being small. These two objectives are therefore contradictory.

To the best of the authors' knowledge, the problem of consistency and integrity of the attitude estimation during enduring disturbances is a non-confronted problem so far. We therefore conclude with a nal research question:

• How should a fault-tolerant attitude estimator be designed in order to provide a consistent and integrate attitude estimation during periods of lasting perturbations?

In Chapter 7 we develop an attitude estimation architecture joining the mechanisms for measurement consolidation, consistent gyroscope bias estimation, and disturbance decoupled attitude estimation. We show that each individual fault-tolerant mechanism developed contributes to improving the consistency and integrity of the estimated attitude.

Chapter 3

Development of an algorithm for multiple consecutive measurement bias estimation

Summary

T his chapter presents the development of an algorithm (MGLR) for multiple consecutive mea- surement bias estimation. The algorithm is based on the well known GLR algorithm originally designed for single bias estimation. The result is an algorithm that signicantly improves the bias detection and estimation when biases appear frequently, compared to the original GLR algorithm.

-The rst part handles the extension of the algorithm to the multi-bias case, through an online correction of the innovation and re-identication of detected biases.

-The second part treats the problem of long-term estimation integrity and discusses dierent strategies to eliminate estimated biases through amplitude matching.

-The chapter is concluded with a comparison between the original GLR algorithm and the proposed modied algorithm, named MGLR, in the case of multiple consecutive measurement biases.

Introduction

In this chapter our objective is to develop a position estimator capable of detecting and estimating multiple measurement biases appearing frequently. This notably improves the precision, consistency, and integrity of the state estimate compared to legacy algorithms. Dierent strategies to eliminate previously detected biases are also discussed, in order to preserve the integrity and precision of the position estimate over time.

Motivation and principles

As we have seen in the literature, position biases are considered rare events. Existing techniques are therefore focused on detecting and estimating single biases. A single bias in this context means that the transient impact on the innovation can be with the appearance of a single bias on a long time horizon relative to the response time of the lter (see e.g. Figure 2.1).

It has been (trivially?) pointed out in the recent literature that for vehicles navigating in urban environments, the common assumption of rare measurement faults does not hold. This means that any algorithm used to detect and estimate measurement faults must take into account the possibility of multiple frequent faults.

If the case of multiple biases is not taken into account, the detection and estimation of biases, in this case, might be degraded since the fault signature on the innovation can hardly be described by a single bias (compare with Figure 2.1). Figure 3.1 shows the behaviour of a typical Kalman lter innovation at the occurrence of three consecutive measurement biases. In this case, Willsky's GLR in its original form will provide a biased estimation since it nominally only considers a single bias on the sliding window. For a bias sequence such as the one depicted in What happens when using Willsky's nominal GLR in the case of multiple biases appearing in the sliding window is that the bias estimation becomes degraded since the innovation no longer matches the fault signature corresponding to the appearance of a single bias. Since the GLR detector only assumes the appearance of a single bias, the condence set post-fault only reects the increased uncertainty due to a single bias, despite several biases being present. These two points must be treated in the multi-bias case in order to maintain a precise and consistent estimation.

The ad hoc procedure of turning Willsky's nominal GLR into a detector for multiple consecutive bias consists of the following steps:

When a bias is detected, freeze the value of the bias at its estimated value at detection.

Remove the bias permanently from the measurement and correct the Kalman lter state and covariance with the bias fault signature.

Accumulate the covariance corrections for the detected bias to represent the uncertainty of the corrected state in steady state.

Reset the bias estimation of the GLR lter bank to zero.

In the remainder of this thesis, unless otherwise noted, we will refer to Willsky's GLR as the algorithm implementing Willsky's nominal GLR congured for multiple consecutive bias estimation as per the above procedure.

In the case where multiple frequent faults appear on a non-redundant sensor, the detection and estimation of theses faults, and the subsequent correction of the estimated state, leads to an accumulating uncertainty of the actual state. From an integrity point of view, this is worrying since it is eectively a one-way ticket to an integrity alert and a mission abort. Note that this assumes that each bias appearance is an independent event and that the estimation of these events is to be treated in a cumulative fashion, thereby leading to a cumulative increase of the state uncertainty. The same result would not be achieved if all bias appearances are assumed from the same time varying sequence, in which case the same event is intermittently estimated and no uncertainty accumulation takes place.

Which of these approaches is in ne correct has not yet been explored and should be the subject of future work. In order to handle this problem, a system for disqualifying old biases must be used for eliminating old biases that are judged to have disappeared (through a sequence of biases in the opposite direction). Indeed, the uncertainty induced by the occurrence of a bias can be removed after the bias has disappeared.

We must note that the cumulative increase of the state covariance due to multiple bias detection is not written in stone, since the bias could be seen as a single time-varying sequence. Regardless of this, it is important to eliminate old biases that have disappeared from a precision point of view to avoid a static error on the state estimate.

Solution overview

The proposed solution to the above problems builds upon the classical GLR algorithm (Appendix A)

for sparse single faults, modied to accommodate multiple faults. The developed algorithm for handling multiple faults also includes an optional algorithm for accumulated fault rejection in order to reset old biases that are likely to have disappeared in order to reduce the uncertainty of the corrected state. The complete algorithm is composed of the following steps:

1. Estimate the position using a Kalman lter under the zero hypothesis "no bias on the sensors".

The innovation ν of this Kalman lter will be transiently biased when a fault appears on the measurement.

2. Correct the biased innovation of the Kalman lter with the signatures of all previously corrected faults within the GLR observation window (L).

3. Use a GLR detector on the corrected innovation to determine bias appearance times ( ki ) and bias amplitudes ( bi ) at detection. Store the estimated appearance times and amplitudes. If a bias is detected, the GLR detector is reset to nd a subsequent bias.

4. Re-identify all detected bias in an estimation window of length L est (L est ≥ L) using the biased innovation and the known appearance times and amplitudes with a (recursive) least-squares optimisation.

5. Calculate a new short-term innovation and state correction with the re-identied biases. When a bias appearance time leaves the estimation window, the nal value of the corresponding bias estimation is added to the total accumulated bias estimation bacc , along with its covariance.

6. When a new bias is added to the accumulated bias, a bias elimination algorithm can be run:

(a) Partial bias elimination: The newly accumulated bias is compared to a list of previously accumulated bias estimations to nd the combination giving the smallest total bias. If this total bias is undetectable given the total uncertainty of the biases, all biases used in the combination plus the newly accumulated one are removed from the list.

(b) Total bias elimination: The total estimated bias accumulation is compared to zero given the total accumulated uncertainty, under the zero hypothesis that the total bias is zero. If the test passes, the entire accumulated bias estimation is set to zero.

7. The sensor measurement is nally corrected with the total accumulated bias estimation.

A comparison between the architectures of the classical GLR algorithm and our proposed Modied GLR algorithm is seen in Figure 3.4. The MGLR architecture involves two new functions: the bias (re-)identication and the bias elimination. Note also that the compensation is now split into two parts: one for long term compensation and the other for short term.

As we can see in the classical GLR algorithm, the Kalman lter and the measurement are corrected as soon as a bias is detected. This imposes a compromise between detection delay and bias estimation precision since the fault signature of the bias is very quickly nullied (to the accuracy of the bias estimation).

The proposed algorithm continues to estimate the bias after it is detected, as long as the estimated occurrence time remains in the estimation window ki

∈ [t -M -L est , t -M ].
Only the innovation used by the GLR detector is corrected (short term compensation), whereas the innovation used to continuously estimate the bias is only corrected when the instant of apparition leaves the estimation window (long term compensation). The estimation of the state and the innovation can be expressed as the sum of a nominal fault-free part (superscripted n) and a faulty part (f ). In the case of the classical GLR, only one bias is assumed to be present and if a Kalman lter is used we have:

xt = xn t + xf t ν t = ν n t + ν f t with xf t = Φ t,k b ν f t = φ t,k b
The separation into a nominal and a faulty part results from the system linearity. When a fault occurs, the state estimation and the innovation ( xt and ν t ) delivered by the Kalman lter must be corrected in order to accurately recover the fault-free part are: xn t and ν n t .

At the detection time, the fault is estimated. Let us denote by k the estimated bias occurrence time, and bk the estimated bias amplitude at this time. The correction terms are computed as follow:

∆x c t Φ t, k bk ∆ν c t φ t, k bk
It could be considered more natural to correct the measurement y t (by subtracting the estimated bias) instead of the state. However the state correction is mandatory to compensate for the detection delay, i.e. to remove the impact that the bias had on the state estimation before the fault was detected.

At subsequent times after the detection, instead of correcting the state, it is better to correct the measurement. Indeed, this correction is time invariant, which is not the case for the failure signature used to correct the estimated state. The measurement correction is:

∆y c t F bk
It is also important to corrected the measurement when an extended Kalman lter is used in order to keep the lter working around the good operating point. The state correction is therefore applied only once, at the time of detection, and then the measurement correction is applied permanently.

Moreover, because the correction subtracted from the estimated state is random, its covariance must also be corrected. The state covariance correction writes:

∆P c t Φ t, k Λ -1 k Φ T t, k where Λ -1
k is the bias estimation covariance matrix at the detection time. The covariance of the estimation error P t is impacted by the state correction at the detection time because we add the random variable ∆x c t . But P t is not permanently impacted by the measurement correction, since the applied correction term F bk is a constant: it is a random variable, but is not time varying. Thus one should apply the covariance correction only once.

Note that the correction of the state estimation covariance is applied as if the estimated bias was a random variable. This is of course not the case since the bias is constant but unknown. The state covariance correction should therefore be interpreted as an ad-hoc representation of the maximum error induced by a bias occurrence and detection. This in turn has consequences for the estimation integrity (detailed in Appendix B).

A rst remark about the GLR algorithm is that the way of estimating biases is suboptimal since it wastes any potential useful information which is still present in the (uncorrected) innovation after its detection. Indeed, after the correction is applied, the information that remains in the corrected innovation is often masked by the measurement noise. It seems more appropriate to wait until the eect of the bias has been observed for a sucient long period of time to estimate it correctly and compensate for it.

A key idea of our proposed algorithm is to reset the Kalman lter under H 0 only when a detected bias is no longer observable and to use the biased Kalman innovation to re-identify the bias as long as it remains observable. In practice, this is when it leaves the estimation window. This has been already

proposed in [START_REF] Jamouli | A mixed active and passive GLR test for a fault tolerant control system[END_REF] Another remark concerns the state covariance correction. In the presence of faults, the state uncertainty is no longer represented by the covariance matrix. There is a bias on the estimated state, that results from the residual error on the measurement compensation bbk . The state Mean Square Error (MSE) is no longer equal to the state covariance, since it is increased by the square value of the state estimate bias. This permanent residual error must be taken into account because the integrity of the state estimation is one of our main concerns. The MSE cannot be computed since the residual bias is obviously unknown. In order to take it into account we have chosen to permanently apply the state covariance correction.

Multiple consecutive bias estimation

In the multiple bias case, the innovation and state fault signatures cannot be described by a single bias but take the form:

xf t = N b i=1 Φ t,k i b i ν f t = N b i=1 φ t,k i b i
where N b is the number of biases present. In order to calculate a correction of the fault signatures in this case, N b apparition times and equally many biases must be estimated.

We have already said that the classical GLR with immediate state correction does not yield the most accurate estimation of the biases. It will thus cumulate errors in the multi-fault case. If the state correction is delayed until the estimated fault occurrence time leaves the detection window (as we proposed here above), a more accurate estimation of the bias is computed.

Nevertheless the overall behaviour of the estimator is optimally improved only if a single fault is present on the window. Indeed if several faults are present the bias estimation is degraded because the basic hypothesis of the GLR assumes only a single fault on the window. To solve this problem we propose to split the data processing into several parts as depicted on gure 3.4:

-The Kalman lter now operates on measurements from which accumulated bias has been removed.

The accumulated bias sums up all biases that fall out of the identication window, i.e. ki < t -M -L est .

-The GLR detector only outputs the estimation of a new bias occurrence time if any is detected. For that purpose it uses the innovation corrected from all previously detected and estimated biases.

Even if the GLR detector estimates the bias amplitude, this estimation is dropped because it may not be accurate enough.

-The jump times estimated by the GLR detector (on the detection window) are used by an identication process to globally estimate the amplitude of all detected biases. We also call it reidentication since the GLR detector also estimates the bias amplitudes. The identication continuously improves the estimation of bias amplitudes as long as the estimated fault occurrence times remain in the estimation window. For that purpose it is fed with the uncorrected innovation.

-The next step called elimination (or bias cancellation ) used the information of past biases to determine whether some of them cancel each other out. This information will be used to minimize the state covariance increase due to the compensation stage.

-With the estimated characteristics of the faults (occurrence time and amplitude) it is then possible to compute the estimated fault signatures. This is done by the compensation step. "Short-term"

and "long-term" refer to faults occurring on the horizon L est , and to previous faults respectively.

-Finally, the Kalman resetting process resets the lter state and covariance, and updates the accumulated bias once a bias leaves the estimation window.

GLR detector

The GLR detector considers all hypothesis H k "a fault occurred at time k" for all k belonging to the detection window [t -M -L , t -M ] (recall that M is a xed lag). Then it tests the hypothesis H 0 "no fault occurred" against the most likely of all H k hypothesis. The performances of the GLR detector are characterized by: the false alarm rate P F A , the missed detection rate P M D and the detection delay.

In order to optimize the GLR detector's performance, it must be fed with an innovation signal that satises as broadly as possible the underlying hypothesis that "at most one failure is present on the observation window". So, the GLR detector is able to properly deal with multiple faults, only if the signatures of all previous faults have be removed from the innovation signal which is used to compute the GLR statistics i.e. the compensation stage introduced is mandatory.

Obviously the true fault signatures are not known and estimated ones are used. Note that since the estimated values of the faults are now time varying, this compensation runs in a feedback loop: b now depends upon t. For the innovation the overall compensation writes:

∆ν c t = N b i=1 φ t, ki bt, ki
where N b is the number of detected biases. Given this correction, we see that it will be possible to handle multiple successive faults since we can get back to "H 0 " whenever a fault is detected and estimated without any delay or error.

This compensation is similar to the one proposed by the classical GLR algorithm, but nevertheless somewhat dierent. The main dierence is that the GLR detector only outputs here estimations of the fault occurrence times and not the fault amplitudes. Another dierence is that an estimated fault occurrence time ki cannot be output twice by the detector. Indeed ki is excluded from the GLR detector range [t -M -L, t -M ]. Whatever the fault detection delay is, whatever the impact of a fault is compared to other faults, after detection any fault amplitude is continuously estimated. And the last dierence comes from the fact now the value of b is not xed at the detection time but will evolve over time.

Short and long term compensations

With the classical GLR algorithm, the bias amplitude estimations are output by the GLR detector.

However to maximize the accuracy of the bias amplitude estimation, the impact of biases must not be removed at all along the estimation window. The innovation signal used for that purpose must not be corrected of faults that occurred on this window. We only need to remove the eect of faults that are not currently estimated, i.e. that have left the estimation window.

For this reason, the residual compensation must be split into two parts. The long term compensation and the short term one. When a time ki leaves the estimation window L est , the associated amplitude bt, ki will no longer be estimated. We separate this long term compensation ∆ν c,lt t from the short term one ∆ν c,st t that involves fault occurrence times belonging to the current window:

∆ν c t = ki ≤t-M -Lest φ t, ki bt, ki ∆ν c,lt t + t-M -Lest< ki ≤t-M φ t, ki bt, ki ∆ν c,st t
The long term compensation will be dealt with by the Kalman lter resetting function. In Figure 3.5, the residual ν t is corrected from the impact of the oldest detected failures (long term compensation).

So the corrected innovation is computed by removing the most recent fault signatures (short term compensation):

ν t = ν n t + ν f t -∆ν c,lt t ν c t = ν t -∆ν c,st t Figure 3
.5: Innovation, ν, and corrected innovation, ν c , at the appearance and detection of a single bias appearing at k = 6 s. Note that the innovation is re-centred once the bias leaves the estimation window (t > k + L est ). Here the xed lag is M = 0.

Bias re-identication

In the presence of multiple faults the GLR detector is suboptimal. We thus try to increase its accuracy through a re-estimation function. This function takes the estimated times ki of fault occurrence, and re-estimates the amplitudes bt, ki using all the available information. At time t, we have to solve a set of L est linear equations (t -M -L est < k ≤ t -M ) with respect to a given (time-varying) number of unknowns, written as:

ν t = ki >t-M -Lest φ t, ki bt, ki (3.1)
since the long term compensation has been removed from ν t . A least-squares (LS) estimator can be used to solve this system, denoted here MGLR-LS (for Multiple GLR Least Squares). The number of unknowns increases each time the detector detects a new occurrence of failure. It decreases each time a failure occurrence date leaves the estimation window.

In the context of a real-time implementation, and in a similar way to the detector algorithm, we also propose a recursive solution RLS denoted here MGLR-RLS (for Multiple GLR Recursive Least Squares). For any detected kj > t -M -L est at time t its recursion is:

bt

, kj = bt-1, kj + Λ -1 t, kj φ T t, kj S -1 t (ν t - ki >t-M -Lest φ t, ki bt-1, ki ) Λ t, kj = Λ t-1, kj + φ T t, kj S -1 t φ t, kj (3.2)
where S t is the covariance of ν t . Each time a new bias is detected, it is initialized with the estimate and covariance given by the GLR detector.

The rst simplication in (3.2) compared to a conventional RLS concerns the covariance matrix which is here kept block diagonal. Correlations between the re-estimated biases are therefore neglected.

The second is related to the eect of detection delays. Indeed if a fault is detected with a delay, its eect already induced on the previous biases estimates cannot be removed. The simplied recursive solution chosen here seems to be a good compromise between computational complexity and accuracy.

However numerical tests show that the LS version is more accurate when biases are frequent 3.4.2.

Bias isolation and conrmation

The GLR detector often only looks for an inconsistency between the set of all measurements and the system dynamics (A.1), i.e. one takes F = I or F ≈ I if some columns are dropped that correspond to outputs which are known to always be bias free. This results in fault detection without isolation.

To locate the fault, several GLR detectors must operate in parallel (with single column F ), which greatly increases the amount of calculus, and highly complexies the compensation step. The identication step we introduced makes it possible to isolate the biases quite easily, at the expense of not too much calculus. Indeed if various F are tested when re-identifying, this obviously increases the amount of calculus but far less than if it was done in the detector because only some select fault occurrence times are involved.

From the re-identication step described here-above, new Likelihood Ratio (LR) statistics may be computed. Since the fault occurrence times are xed, they are just LR and not GLR. A fault conrmation / rejection test may then be introduced. One can then use a re-identication step with a test of all possible F to determine the most probable location of the bias. The conrmation test becomes another GLR test but now the maximum is searched over the set of measurements. The isolation is then done by nding the bias that maximises the LR test statistic (A.21) among all bt,i :

bt, k(isol) = argmax 1≤i≤m bT t, k(i) Λ t, k(i, i) bt, k(i) (3.3)
All other components of bt, k are set to zero.

Kalman resetting

In the classical GLR algorithm the corrections of the state and the measurement are applied in a feedback loop upon detection. This allows for the detection of a subsequent bias but prevents the estimation of the last detected bias to improve since the fault signature on the innovation is zeroed. In the MGLR we apply the feedback compensation to the state only when a detected and conrmed bias has been re-identied and leaves the sliding window. The introduction of the re-identication step, improves the accuracy of the corrections of the Kalman lter state.

Whenever an estimated bias bt, k leaves the estimation sliding window, the accumulated bias bacc,t is updated. bacc,t is the sum of all previously estimated biases that have left the identication window.

The measurement is permanently corrected with the accumulated bias.

bacc,t+1 = bacc,t + bt,

k z t+1 ←-z t+1 -bacc,t+1 with Λ -1 acc,t+1 = Λ -1 acc,t + Λ -1 t, k (3.4)
The measurement covariance W is kept unchanged, because the correction term is a constant even if random. The state correction is done only once, when the fault leaves the estimation window.

xt+1 ←-xt+1 -Φ t+1, k bt+1, k P t+1 ←-P t+1 + Φ t+1, kΛ -1 t+1, kΦ T t+1, k (3.5)
The eect of the increase in covariance is only temporarily visible since the covariance is continuously updated by the Kalman lter and the measurement covariance is kept unchanged.

It must be pointed out that for integrity analysis the state covariance must reect all uncertainties related to previous faults. If some bias has not been fully corrected, the remaining residual bias may be taken into account by increasing the state covariance. For that purpose we use an accumulated covariance correction term:

∆P acc,t+1 ←-∆P acc,t+1 + Φ t+1, kΛ -1 t+1, kΦ T t+1, k (3.6)
The state covariance used for integrity calculation is: P t+1 + ∆P acc,t+1 . Note that this is not a perfect solution because for a transient period the correction is added twice.

Clearly from an integrity point of view, the more biases are detected and estimated, the more uncertain the corrected state becomes. This is a one-way trip to an integrity failure.

Note however that this is based on the assumption that each bias is to be treated as a separate uncertain variable. Alternatively, one could treat the bias as a single time-varying variable, which is weakly observable. In the scope of this thesis, the implication of the latter assumption on the estimation of the state covariance has not been investigated. It is likely however, that the resulting expression of ∆P acc,t+1 would dier in this case, but this should be a topic of future investigation.

Whichever assumption is used to calculate the state uncertainty, the question of long-term precision remains and it is imperative to detect when the bias disappears completely. When applying this kind of algorithm, it is therefore important to be able to detect when a previously estimated bias disappears in order to remove the eect of the bias from the state estimate and its covariance (through ∆P acc,t+1 )

and prevent an inconsistent state estimation. Some methods for this are proposed in Section 3.3.

Parameter tuning considerations

Recall that the parameters to tune for the standard GLR is the sliding window length L and the detection threshold l GLR,det through a probability of false alarm P F A . In the case of the detection window length, the window size can be set according to the length of the innovation fault signature, i.e. L = T obs . A longer window is useless since the Kalman lter drives the innovation to zero (recall that we deal with non redundant measurements). If an adaptive Kalman lter is used, the convergence time may vary, and L can be chosen for the case giving the maximum T obs .

When setting up the MGLR algorithm, we need to add to this the estimation window length L est .

The tuning of the estimation window length depends on a qualitative guess (not very helpful) of how often biases appear. For infrequent biases one can take L est = L. But in the case of frequent biases a sensible length of the estimation window could be 2L since this covers the case where two biases appear with a dierence of up to L time steps. An advantage of L est = 2L over L est = L is that the individual noise samples will have a smaller impact in the latter case and that the nal bias estimation will be more certain. A disadvantage is that the computational load for solving the LS bias estimation problem increases with increasing L est , and proportionally the memory space needed to store the sliding window innovation and fault signatures.

The detection delay is not directly impacted by the parameters of the MGLR algorithm, however

Willsky proposed in his original GLR algorithm to use a sliding window starting M steps behind, allegedly leading to a lower sensitivity to outliers. Note that in the derivation of the algorithm we have taken into account this lag, however in the simulations and experiments in this thesis we systematically set M = 0 to mimimise the detection delay. Note that the length of the estimation window, just as for the detection window, does not impact the bias detection delay, it only impacts the precision of the bias estimation.

Bias elimination

As mentioned above, an inherent problem of the GLR algorithm is that the total bias estimation is purely cumulative and not recursive (since the bias is only weakly observable). This means that in such applications where the mean bias is zero, for example GNSS position jumps due to multipath errors, the algorithm will not necessarily converge to zero bias despite the measurement bias disappearing. In these cases the accumulated bias b acc should tend to zero over time but it does not systematically.

We propose two methods to reduce this drift of the accumulated bias: a global and a sequential.

Both solutions are detailed here below.

-The principle of the global method is to reset the estimation of the accumulated bias when its value is small with respect to some covariance. A statistical test is used for making this decision.

-The sequential method is uses a dictionary of bias values. Any new detected bias is compared with the elements of the dictionary to look for a possible disappearance of a previous bias.

Global bias elimination

The estimated accumulated bias is the true accumulated bias plus estimation errors (due to missed detections, noise, niteness of the estimation window, lack of observability)

bacc,t = b acc,t + N b i=1 b,i
where b acc,t is the actual out-of-window bias and b,i are the estimation errors. These estimation errors are centred for detected biases, but in case of missed detection they are equal to the actual bias. Assume all biases have been detected, then the estimation error of the accumulated bias is also centred with covariance Λ -1 acc,t . In order to decide whether to reset or not the estimated accumulated bias, we set up the following zero hypothesis:

H 0 : bacc,t ∼ N 0, Λ -1 acc,t A rejection test is then dened as passed if H 0 is satised, hence if bT acc,t Λ acc,t bacc,t < l GLR,det (3.7)
The used threshold is the same detection threshold as for the GLR detector. The idea underlying this tuning is that if a bias is removed it must not be detected as a new jump by the GLR detector. In this case the estimated accumulated bias bacc is reset to zero as well as Λ acc,t .

This reset of the accumulated bias basically implies a transfer of the error from the measurement to the state of the Kalman lter (which is working under the hypothesis "No bias present"). The covariance of the lter should therefore be increased at the moment of resetting. The measurement correction is F bacc,t whose covariance is F Λ -1 acc,t F T . After correction the measurement should be z = C x because all biases have been removed. The change in covariance is thus also equal to C ∆P t C T where ∆P t is the equivalent change of the state covariance to be applied. By equalising the two terms we nd the solution

∆P t = C + F Λ -1 acc,t C + F T (3.8)
where C + is the Moore-Penrose pseudo-inverse of C. If this covariance correction is not performed, the removal of the accumulated bias from the measurement could be seen as a new bias by the GLR detector.

Clearly as the number of detected and estimated biases increases, the chance of resetting the accumulated bias increases. Intuitively, albeit not theoretically, we can argue for this by the increase of the accumulated covariance matrix proportional to the number of biases detected. As mentioned in the beginning of this section, the bias present in some typical applications is on average zero. We therefore have the case of an accumulated bias which in steady state will tend towards zero and at the same time an associated covariance which grows towards innity. The only sensible decision in this case is therefore to reset the accumulated bias as soon as it is likely that it consists only of articial error terms.

An obvious drawback of this method is that the bias estimation will only be reset to zero after zero-sum sequence leaves the sliding window. This means potentially accepting a period of biased estimation even though the real bias has disappeared since the nal nullifying bias has not yet been added

to the accumulated bias.

Yet another drawback is that in the case of a large bias appearing followed by a sequence of smaller biases with a zero mean, the test (3.7) may pass only due to the accumulated covariance growing large.

The rst drawback can be remedied by testing the total bias estimation in addition to the accumulated bias. Indeed if the total bias estimation has a smaller statistic than only the accumulated one, we can consider that not only the accumulated bias, but the total bias estimation is suciently close to zero to be removed. The total bias estimation is given by the sum of the accumulated bias and all biases currently being estimated:

btot, t = bacc,t +

N b i=1 bt, ki
The corresponding covariance matrix, and the total bias estimation test statistic are given by:

Λ -1 tot,t = Λ -1 acc,t + N b i=1 Λ -1 ki ,t l tot = btot, t Λ tot,t bT tot, t .
If the statistic l tot is smaller than l acc and we have tested that l acc < l GLR,det , then we also remove all the estimated biases in the estimation window. In order to compensate the state of the Kalman lter for this additional bias elimination, we increment the covariance correction in (3.8) and apply a correction to the state:

∆x t ←-xt - N b i=1 Φ ki ,t bt, ki ∆P t ←-∆P t + N b i=1 Φ ki ,t Λ -1 ki ,t Φ T ki ,t

Sequential bias elimination

In order to avoid the case where a non-zero accumulated bias gives a false positive in the test (3.7), we can test only the minimum number of out-of-window biases that sum up to an undetectable bias. The corresponding biases are then set to zero, leaving a set of bias estimations whose sum is greater than the minimum detectable bias.

More generally, we can consider a catalogue of N tot biases in which we are looking for if some of them have just disappeared. The elements of this dictionary can be the last detected biases, the total accumulated bias, or partial sums of biases (biases accumulated and stored on certain dates).

Every time a bias exits the estimation window we can compare it to the set of elements of the dictionary. Let us denote by B tot the matrix whose columns are the elements of the dictionary.

B tot = btot,1 , . . . , btot,Ntot

(3.9)
The goal is to nd the combination of biases previously stored in the dictionary which is closest to a bias bt, k produced by the identication function. The biases combination writes B tot x where x is a binary vector (x ∈ {0, 1} Ntot ). The residual and its covariance are:

ε t (x) = bt, k -B tot x Λ -1 t,ε (x) = Λ -1 t, k + Ntot i=1 Λ -1 tot,i x i (3.10)
The criterion we minimised is

x * = min x∈{0, 1} N tot ||ε t (x)|| 2 (3.11)
The minimisation is to be done every time a bias leaves the estimation window. To reduce the elimination delay it can also be run every time a new bias is detected (or even continuously), but the elimination might not be accurate enough.

If a non-zero solution to (3.11) is found, we can calculate the residual bias and its covariance as

ε t (x * ) and Λ -1 t,ε (x * ).
Note that the covariances of the elements of the dictionary must also be stored for that purpose. Thereafter we perform a hypothesis test to decide if the obtained combination of biases is close enough to the bias candidate for elimination.

ε T t (x * ) Λ t,ε (x * ) ε t (x * ) < l GLR,det (3.12)
If the test is passed, the corresponding biases and their information matrices are set to zero. This concerns bt, k and all elements of the dictionary pointed out by the binary vector x * . As for the previous method, when biases are eliminated, the estimated accumulated bias bacc,t must be updated as well as the estimated state and its covariance.

Simulations have shown that not taking into account the covariance in the criterion could lead to a deterioration in the functioning of the elimination procedure. It would seem preferable to use the criterion 3.13:

x * = min x∈{0, 1} N tot ε T t (x) Λ t,ε (x) ε t (x) (3.13)
In that case the solution optimises the likelihood over all combinations. But as it stands, it is the previous criterion which has been (unfortunately!) minimised.

The problem 3.11 is NP-hard and for a large number of biases (> 20) becomes prohibitively dicult to solve in real time. It can be approximatly cast into a quadratic optimisation problem [START_REF] Park | A semidenite programming method for integer convex quadratic minimization[END_REF][START_REF] Luo | Semidenite Relaxation of Quadratic Optimization Problems[END_REF]. Although up to a modest number of biases (∼ 10) it can be solved by brute force enumeration. This is the way we solved it in this thesis.

In our algorithmic implementation, the dictionary consists of three subsets:

-B tot,1 which lists the last N 1 estimated biases that have left the estimation window.

-B tot,2 which lists the N 2 oldest estimated biases that are still present on the estimation window.

-the estimate of the accumulated bias, from which are subtracted the N 1 bias above. The numbers N 1 and N 2 are time varying. The maximum of N 1 is 10, as well as the maximum of N 1 + N 2 . When a bias is removed from B tot,1 it is replaced by the oldest element of B tot,2 .

Conclusion

We have presented two methods for eliminating old biases by matching estimated biases that add up to close to zero in various ways. The rst one can be seen as an extreme simplication of the second one where the dictionary is reduced to only one element ( bacc,t ). The key tuning parameters are the composition of the dictionary, the way it is updated and the choice of the instants at which the elimination is done. As a short summary we can state some advantages and drawbacks for each method:

• Global bias elimination: This method has the advantage of being simple and adding a negligible computational load to the algorithm. Intuitively it should work on most common cases. For example the total bias on GNSS and visual tracking sensors will generally go to zero. However this might take a very long time and the elimination procedure cannot succeed before this happens. Furthermore the accumulated bias can be (wrongly) rejected despite being present only due to the covariance increasing with each detected bias.

• Sequential bias elimination: This method has the advantage of gradually eliminating estimated biases even though the total accumulated bias is not close to zero. This can help improve the integrity over time in extended periods where the bias is non-zero, for example in a traverse of an urban canyon. The disadvantage of this method is that it has a relatively high computational load even for a relatively modest dictionay size. Furthermore, the bias matching does not take into account the uncertainty of each bias, meaning that no distinction is made as to whether the eliminated biases correspond to the greatest reduction in uncertainty. It could be interesting to develop a method solving problem 3.11 while maximising the uncertainty of the eliminated biases.

Applying the elimination procedure just after the detection of a new bias will obviously speed up the elimination. But the elimination is then rather coarse, meaning that the uncertainty of the estimated biases, and therefore of the total closeness to zero, is signicantly higher than for the elimination of accumulated biases.

Simulations have shown that the rst method, and the second one with a priority given to accumulated biases (as we propose here above) are quite ecient. The second approach with only the more recent estimated biases in the dictionary is less ecient especially in the case of frequent biases. We will evaluate these methods in the next section.

Contrary to intuition, applying the second method does not systematically eliminate the accumulated bias bacc,t as eciently as the rst one. Indeed, even if bacc,t is almost equal to a sum of element of the dictionary it is not necessarily the one with the smallest residual error due to estimation noise. It may therefore be interesting to combine both methods.

Example: One axis position estimation with measurement bias

In this section we illustrate the behaviour of the various GLR algorithms in the case of a single bias, and a multi-bias scenario. The interest of using a bias elimination procedure is also studied. The simulation model considered is:

ṗ = v v = a with p m = p + b + w p a = a m -v a (3.14)
with w p ∼ N (0, σ 2 p ) and v a ∼ N (0, σ 2 a ). In the example below, the noise standard deviations are set to σ p = 1/3 m and σ a = 1/3 m/s 2 . In the rst example b is a quite large single step (3 σ p ). In the second example b is a succession of steps of the same amplitude. The detection window length is L = 4 s, and the estimation window length is L est = 2 L. The false alarm rate is P F A = 10 -6 .

Single bias estimation

In this example, a single bias appears on the measurement. Each of the gures 3.6, 3.7, and 3.8 illustrates the behaviour of one of the methods. And each gure shows the bias with its estimation, the statistic with the threshold, and the estimation error with its 3σ condence interval. It is reset every time a bias is detected, and thus the bias estimation is not rened after detection. The MGLR-RLS algorithm (gure 3.7) renes the bias estimation even after detection, leading to a more precise bias (and state) estimation. In the single bias case the behaviour of the MGLR-LS algorithm (gure 3.8) is identical to the MGLR-RLS algorithm since the assumption of the estimation of all detected biases being decorrelated holds by denition.

Multiple bias estimation

In this example, multiple biases appear consecutively on the measurement. The classical GLR algorithm (Figure 3.9) shows the same behaviour as in the single bias case. Since the bias estimation error at each detection is Gaussian zero-mean, the total estimation error has a zero mean expectancy but its covariance diverges. For the simulated sequence of six jumps, the corrected state estimation becomes inconsistent since the algorithm inherently only takes into account the bias uncertainty at the moment of detection. After each detection, the covariance is reduced by the Kalman lter correction.

The MGLR-RLS algorithm (Figure 3.10) continuously estimates the detected bias, under the assumption of them being uncorrelated. Contrary to what was expected we see that the result is less precise than with the classical GLR. The bias estimation is less accurate and the state estimation error is transiently inconsistent. Nevertheless the behaviour of the statistic between the detections seems to be more insensitive to the number of jumps.

In this multi-bias case, we see the interest of using a block LS solution to nd the biases (Figure 3.11). It yields the best results in terms of accuracy and consistency. Since the bias estimation is memoryless and takes into account the correlations between the biases, the total bias estimation is more accurate and consistent than for the RLS solution. Note that by consequence, the test statistic after the consecutive detections is lower than for the RLS case, meaning that the risk for false detections following a chain of detection is lower the in the RLS case.

Multiple biases and bias elimination

In this example we show the interest of using a bias elimination strategy to reduce the uncertainty of the state estimation when the accumulated bias sums up to zero over time. We only show the example of the MGLR-LS with and without the bias cancellation steps (sequential and/or global).

In this scenario, we simulate a random sequence of biases which ends with a return to zero. We are especially interested in the consistency (via the estimated uncertainty σ x) and integrity (via the protection level P L and the alert level AL) for each method. The integrity parameters P L and AL are dened as per Appendix B. In this academic scenario, AL has little realistic value (AL = 5 m),

however it has been chosen to correspond to an arguably characteristic length in urban navigation.

The methods used for bias elimination are the global and sequential methods described in Section 3.3. Each method is rst shown on its own, and then the two methods are combined (i.e. applied sequentially) to illustrate the gain which can be drawn from each method, and also the potential drawbacks of combining them.

Global bias elimination

Figure 3.12 shows the bias estimation, GLR statistic, and state estimation error when using the MGLR-LS with global elimination. This approach tries to eliminate the accumulated bias only. The bias elimination at the end of the simulation is eective since it detects and removes the entire accumulated bias after it returns to zero.

As expected, the eect of the bias elimination on the statistic is negligible. This is likely because a large number of bias are summed up, meaning that the residual is small and its covariance is large,

Sequential bias elimination

Figure 3.13 shows the bias estimation, GLR statistic, and state estimation error when using the MGLR-LS with sequential bias elimination. The sequential elimination method does not manage to reject the accumulated bias estimation. Notably the return to zero is not detected.

The impact on the statistic is overall negligible, however we can see the eect of it at around 38-40 s where two sequential eliminations slightly increase the statistic.

From an integrity point of view, this method gives a better result (lower P L) than the global elimination since it eliminates biases as they are accumulated. At the end of the simulation we see that the estimation error tangents the protection level, indicating a potential integrity problem.

Combined sequential and global bias elimination Figure 3.14 shows the bias estimation, GLR statistic, and state estimation error when using the MGLR-LS with combined sequential and global bias rejection. By combining the two elimination methods, we eectively combine the eects of the two with regards to the bias rejection. In the end the return to zero is properly detected and the remaining bias is rejected. Furthermore, we see that the covariance of the state estimation error is reduced while still retaining a consistent estimate. The GLR statistic in this case is slightly more impacted due to the bias rejection than in the two previous cases. However the scaling of the rejection by the increased state covariance still keeps the residual eect from crossing the detection threshold. This eect is clearly due to the fact that fewer biases are summed up to near zero. The associated covariance is lower than with only the global elimination method. And thus the statistic is transiently larger.

The integrity of the estimate is equivalent to that of the global rejection case since in steady state the estimation error is well within the protection level.

Conclusion

In this chapter we have developed an algorithm for detecting and estimating multiple consecutive measurement biases aecting non-redundant measurements. Furthermore, some methods for eliminating old biases have been proposed to improve the integrity of the (position) estimate.

From the simulation results presented herein we can draw some important conclusions:

In the context of infrequent biases, the bias re-identication allows to improve the accuracy and the consistency of the corrected state estimate over the classical GLR algorithm.

In the context of frequent biases, using a global LS re-identication is more interesting than using an RLS one. This advantage diminishes as the bias-to-noise increases [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF]. For the MGLR-LS, the bias elimination methods signicantly improve the integrity of the state estimate since it no longer takes a one way trip to an integrity failure. The consistency for the corrected state estimate is also signicantly improved thanks to the application of the elimination methods.

The solutions presented here are by no way a holy grail to redeem sensor biases, and some major drawbacks can be identied:

Only biases of step type are handled. Even if it can be easily extend to drift 4.2, when the biases have any other form, they might be sub-optimally detected and estimated.

If the system considered is nonlinear, the algorithm also becomes sub-optimal in the sense that the Kalman lter is replaced by a linearised one. This is typically the case for integrity in GNSS receivers where the measurement equation is nonlinear.

The noise is always considered Gaussian. This might not always be the case, especially with visual tracking sensors, and even GNSS receivers with internal fault treatment. Some of the above points have been attacked in the state of the art, but the connection to the multiple bias case and the explicit question of state estimation integrity has not been studied so far.

In the upcoming chapter, we will apply the techniques to a series of real-world problems to assess the actual validity of the proposed algorithm. The idea is to determine whether the extra steps introduced in the MGLR, as compared to Willsky's GLR, allow to improve the estimation accuracy, consistency and integrity in typical operational scenarios.

Three dierent applications are considered:

-Vision based landing of an airliner.

-Handheld GNSS with abrupt mode changes (RTK / SBAS / Single).

-Vehicle mounted GNSS/INS in urban navigation.

In the rst scenario, a ground truth position was available as a reference. In the other two a reference position was created a posteriori using the velocity measurement and a non-causally ltered measurement.

The best way to show that a stick is crooked is not to argue about it or to spend time denouncing it, but to lay a straight stick alongside it. D.L. Moody

Introduction

The techniques for fault tolerant position estimation, or more precisely measurement bias tolerant position estimation are tested in this chapter in some typical operational scenarios. The idea is not to

showcase the performance of an ideal algorithm but to highlight the interest of the proposed category of fault tolerant methods with respect to estimation accuracy, consistency and integrity.

In each scenario we will see the dierence between Willsky's GLR and the MGLR-LS algorithm previously developed. It is hoped that the dierences between the two algorithms in terms of precision, consistency, and integrity are highlighted by replaying a number of dierent use-cases:

1. Visual pose estimation for vision based landing of an airliner. In this case the biased sensor is a visual tracking algorithm that estimates the pose of an aircraft with respect to a runway threshold. The biases experienced by the sensor are due to tracking errors where the lines of the runway, i.e. the features, are continuously tracked. However since there are many features of an airport that can be interpreted as a runway (e.g. taxiways or parallel runways), the tracking tends to experience jumps akin to GNSS multipath errors.

2. Handheld GNSS receiver with abrupt mode change (RTK to SBAS) and multipath. In this example a handheld GNSS receiver was walked around the runway of a ight club. The receiver was working in Real-Time Kinematics (RTK) mode for precise positioning. At a few point along the runway, the RTK correction appears to have been lost or severely degraded, with large biases and lost precision as a consequence.

Vehicle mounted GNSS/INS fusion in an urban environment

In this scenario, a car was tted with a dual-antenna GNSS receiver and a low-cost IMU and navigated in an urban environment (specically the city centre of Toulouse, France). The GNSS receiver was primarily working in SBAS mode but due to partial signal blocking, it was occasionally degraded to single mode, i.e.

no systematic signal corrections. The objective of the scenario was to test the capability of the algorithms to handle classical GNSS multipath eects and precision degradations.

Due to some long periods of complete GNSS outage where our class of IMU did not manage to avoid position divergence, the scenario is divided into three parts.

Application to a visual runway pose estimation 4.2.1 Introduction Description

In this scenario we consider a aircraft on precision approach for a vision based landing. The position of the aircraft with respect to the runway is given by a feature based visual tracking algorithm which calculates the aircraft pose from a camera mounted in the cockpit. The actual mechanics of the vision algorithm are omitted here. The aircraft is also equipped with a sensor fusion algorithm allowing the calculation of the ground speed (and specic accelerations). The position sensor, i.e. the vision based algorithm, is subject to time varying jumps due to factors inherent to the tracking algorithm (image feature ambiguities, scaling ambiguities, etc.).

In order to correct the jumps in the position solutions, we apply on one hand Willsky's GLR algorithm and on the other hand our proposed MGLR-LS without the bias elimination. The rationale for not using the bias elimination in this case is that the measurement variance varies signicantly, meaning that the risk of faulty elimination is high.

In this evaluation we are only interested in the longitudinal position of the aircraft relative to the runway. The main reason for this is that the accuracy of the visual tracking in the z-and y-axes is prohibitively inaccurate and does not show the same kind of bias jumps as the x-axis.

The rst two scenarios (S1 and S2) are using data from a high delity industrial ight simulator

where the tracking algorithm works on synthetic video. The third scenario (E1) is based on real ight data and a tracking algorithm using images from a video camera placed in the cockpit.

The results for (S1) and (S2) are not presented here. The measurement error is similar to that of scenario (E1). Results show that the MGLR reduces the position drift due to accumulated bias errors.

GLR setup

The GLR algorithms use the ground speed as the input and the potentially biased position sensor the measurement. The input is sampled at 0.04 s, while the measurement is sampled at a time varying rate, either at 0.04 s or 0.08 s.

The covariance of the position sensor is known and time varying and depends on the specicities of the tracking algorithm, of which the details are omitted here. As we shall see, the measurement covariance in this case has little physical meaning and it should be used with caution. We used it for the Kalman ltering but not in the MGLR-LS re-identication, which is performed as an unweighed least squares optimisation.

The standard deviation of the speed input is set to 0.1 m/s as it is considered reliable and the initial position is supposed to be known within a 3 σ of 10 m. The GLR parameters are set to L = L est = 5 s for the detection and estimation windows and P F A = 10 -6 for the probability of false alarm.

Evaluation criteria

In this use-case, we will look at the consistency of the error through the absolute error and the condence interval, and the integrity indicator through the protection level. The protection level (P L) is calculated as per Appendix B, under the section of sensors subject to faults. The alert level (AL) is taken as 40 m, which is the standard horizontal alert level established by the International Civil Aviation Organization (ICAO) for category 1 precision approaches (good visibility).

Scenario 3: Experimental trajectory 1 (E1)

The main dierence in the experimental data in Figure 4.1 as compared to the simulated scenarios is the measurement standard deviation. It increases drastically at three occasions between 5 s and 20 s into the experiment. The cause of this is unknown but it is also reected in the measurement with a temporary increase of the measurement noise.

Precision: In this experimental scenario, the overall precision is roughly equivalent for both algorithms. It is worth noting that the loss of precision seen for the MGLR-LS is due to a late detection of a measurement jump. This is obviously a aw in the re-identication step that we have discussed earlier in Chapter 3 regarding late detections. A specicity here is that the late detection is due to the measurement uncertainty given by the visual tracking algorithm being high. Since the re-identication is done with an unweighted least squares optimisation, the undetected bias jump has a relatively large impact on the previously detected biases.

Consistency: Willsky's GLR (Figure 4.3) shows a rather good consistency apart from a few occasions (around 8 s and 15-18 s). For the MGLR-LS, the consistency is overall better, apart from the error peak at around 4 s into the scenario (commented above regarding the precision). We note that again Willsky's GLR generally has an overall slightly lower uncertainty bound than the MGLR-LS. We further note that the MGLR-LS has a temporary increase in the uncertainty bound due to the biases detected and estimated during the rst half of the scenario where the measurement standard deviation is signicantly higher than in the second half.

Integrity: Willsky's GLR is more integrate in this scenario than the MGLR-LS. First of all, we notice that the protection level is lower than the alert level at all times, which is not the case for the MGLR-LS. Secondly, the estimation error made by Willsky's GLR during the time of consistency loss (around 7 s and 17 s) is lower than the protection level at those moments. This means that the estimation error stays below the maximum guaranteed error at all times.

The MGLR-LS has an integrity breach between 7 s and 20 s into the scenario. This eectively represents the part of the scenario where several bursts of measurement biases appear and are detected and estimated. It can be argued that in this case the real error is very low compared to the estimated protection level. Nonetheless, the protection level as it is calculated in this case represents the maximum guaranteed error when the uncertainty of the fault-free system, as well as the one of the detected bias, and the one of non-detected bias are taken into account (so called double allocation, see Appendix B).

If we look at the measurement error (Figure 4.2) and the measurement standard deviation (Figure 4.1) we notice a combination of phenomena leading to an increased protection level. Firstly, the three intermittent increases in the measurement standard deviation appears at the same time as the apparent measurement variance increases (the parts of the measurement error that more resemble an increase in noise than biases). This leads to a non-detection of the variance change as bias (see the test statistic at 4-6 s, 9-12 s, and 16-19 s in Figure 4.4), nevertheless, an increase in measurement variance means an increase in the innovation variance. This in turn increases the maximum undetectable bias, which increases the maximum error that can be made due to a missed detection, and thus the protection level allocated for missed detections.

Secondly, the detections made between the periods of variance increase are also made at moments where the measurement standard deviation is relatively high as compared to the alert level. This means that the compensation step which compensates the state estimation and its covariance for the detection of biases will have a relatively large impact on the corrected state covariance. This then leads to an increase of the protection level allocated for the fault-free system.

Thirdly, in this scenario we have chosen an estimation window length of 5 s which roughly corresponds to the length of the Kalman lter innovation. This also has an impact on the protection level through the corrected state covariance. Indeed if a shorter estimation window is used, the fault signature gets truncated and the full information of the bias uncertainties are not captured by the corrected state covariance. In Figure 4.5 is shown the same scenario, but with the MGLR-LS using a detection and estimation window length of 2 s (note that test statistic is nearly identical to the nominal result).

We see that while the estimation error is roughly the same as in Figure 4.4, the protection level never crosses the alert limit. This further implies that the integrity test is bad, in this case, as the detected and estimated biases would normally lead to an integrity failure if they were estimated long enough.

In the prolongation of this reasoning, we can also make a qualied guess that the protection level calculated for Willsky's GLR is underestimated and does not represent the maximum guaranteed error in the same way as the MGLR algorithm.

Conclusion

In the applicative example presented in this section we have mostly seen a gain in consistency with the MGLR-LS over Willsky's GLR. Notably for Willsky's GLR the estimated uncertainty is systematically underestimated due to the "detect and forget" strategy used (i.e. biases are only compensated at detection) to correct the state covariance. This leads to the accumulated uncertainty due to multiple bias detection not being reected in the state covariance, and thus consistency problems over time.

The drift of the estimation error seen for Willsky's GLR also seems to be a consequence of the inherent functioning of the algorithm, where the total bias is only updated if a new detection is declared.

The MGLR-LS algorithm eectively compensates for the accumulated uncertainty in the multiple bias case, leading to a better consistency. Of course this temporarily increased covariance also leads to a temporarily decreased integrity margin. The drift of the error is signicantly reduced as compared to Willsky's GLR. This appears to be thanks to the re-identication process which allows for some adaptability in the bias estimation. This comes at a prices in some exotic cases (e.g. as seen in Figure 4.4) where the total bias estimation drifts temporarily due to an undetected bias.

An interesting phenomena in all the showcased scenarios is that the integrity globally improves over time despite detection and estimation of multiple biases. This is of course thanks to the nature of the measurement which becomes less uncertain as the aircraft approaches the runway. On a side note, the relevance of the protection level can be questioned in this example since the variance of the measurement is clearly not representative. This scenario unfortunately lacks a reference position against which the estimated position could be compared. Quite ironically, the GNSS-RTK signal which is the measurement used here is itself often the reference position in outdoor robotic applications. Despite this, a reference position has been calculated a posteriori using the speed measurements to ll in the gaps in the position measurement where the RTK is lost and applying some innovative non-causal ltering. The corrected measurement was then ltered slightly using a non-causal 2nd order lter and passed through a Kalman lter using

GLR setup

The GLR algorithms use the GNSS speed measurement (Doppler/RTK) as an input and the GNSS position measurement as a non-redundant measurement. The standard deviation of the speed input is set to 0.1/3 m/s and the standard deviation of the position measurement is set to 0.2/3 m in the north and east axes, and 0.5/3 m in the down axis. The initial position is supposed to be known within a standard deviation of 0.5 m. The GLR parameters are L = L est = 5 s for the detection window and the estimation window. The probability of false alarm is set to P F A = 10 -6 .

Evaluation criteria

The alert limit is set to AL = 5 m in all axes. The rationale for this alert limit would be that it would be reasonable in an urban scenario when navigating among buildings.

Estimation along the north axis

In Figure 4.6 we see the speed input, the position measurement, and the position measurement error.

We see from the input that the bearer of the handheld GNSS receiver walks in a start-stop fashion (seen as a variation from zero to non-zero speed in a square wave form). The data collection for this experiment was a survey of precise runway coordinates which demanded of the bearer of the receiver to make frequent stops and pose the receiver on the ground (the low-amplitude sections).

Precision: The large position measurement bias experienced along the north axis (around 50-100 s in the "Meas. error" of Figure 4.6) is for the most part eectively detected and estimated by both algorithms (Figures 4.7 and4.8). Overall the MGLR has a slightly smaller error than Willsky's algorithm.

We also see that the bias elimination is eective at removing the residual bias at the end of the scenario.

Consistency: The consistency of the estimation is quite bad for both Willsky's GLR and the MGLR-LS. For Willsky's GLR, for example, we notice that despite the covariance increase over time, the error remains inconsistent. This is not strange since the covariance increase of the GLR (described in Appendix A) is only an ad-hoc correction and does not necessarily compensate for the random walk of the estimation error.

Just as Willsky's GLR, the MGLR-LS shows a bad consistency throughout, apart from a few occasions (in the beginning, in the end, and around 60-100 s). We note that in the end it is the bias elimination that restores the consistency of the estimation error.

Integrity: The classical behaviour is seen in terms of the estimation integrity for both algorithms.

For Willsky's GLR (Figure 4.7), the protection level is well estimated throughout. It also remains high (as compared to the nominal value) at the end since no mechanism for reducing is used.

The MGLR-LS (Figure 4.8)has a protection level which is underestimated at two occasions (around 220 s and around 245 s). In these cases, the actual error is signicantly lower than the alert limit, meaning that the At the end of the scenario, the protection level becomes anew well estimated thanks to the bias elimination.

Estimation along the east axis

In Figure 4.9 we see the speed input, the position measurement, and the position measurement error.

Again we identify the walking pattern in the input as the square wave form due the start-stop nature of the bearers trajectory. The measurement error is signicantly larger along the east axis than the north axis. This is purely coincidental.

Precision: For Willsky's GLR (Figure 4.10) we see the same behaviour as for the north axis (Figure 4.7): The lion part of the bias is properly rejected, however the small bias appearing around 145 s induces a static error since it is badly estimated.

The MGLR-LS (Figure 4.11) also overall replicates its behaviour from the north axis (Figure 4.8)

with the dierence that the residual bias estimation is not eliminated.

Consistency: The consistency of both estimators also follow the same pattern as for the north axis (Figures 4.7 than the alert limit, however it does represent a temporary dangerous operation since the maximum error cannot be guaranteed.

Estimation along the down axis

In Figure 4.6 we see the speed input, the position measurement, and the position measurement error.

Note that the recurring pattern in the input which resembles a noise variance change is in fact due to the receiver being handheld and follows the movement of the bearer. At the beginning of each of these variance changes, the positive peak is when the bearer starts walking and receiver is hastily lift up from the ground and inversely the negative peaks are when the receiver is posed on the ground. The actual noise variance of the receive is thus the variance of the signal during the low-amplitude phases.

It is also worth noting that the measurement error seen in the down axis is signicantly larger than the one seen in the north and east axes (Figures 4.6 and 4.9). This is a common behaviour for GNSS receivers since a precise vertical position measurement requires high elevation satellites, and multipath eects are more easily obtained due to reections against the ground than against building (of which there are none in this scenario).

Precision: During the large perturbation (around 50-150 s) the precision is degraded for both algorithms (Figures 4.13 and 4.14).

The error is transiently slightly larger for the MGLR-LS than for Willsky's GLR (around 70-100 s).

However since the MGLR-LS eectively eliminates the residual bias at 140 s and then again at 250 s, the long term precision is improved as compared to Willsky's GLR. Test statistic [-] l GLR l GLR,det Consistency: Again in the down axis we see the same lack of consistency already identied in the north and east axes above (Figures 4.7,4.10,4.8,and 4.11). It is by now quite obvious that we have either badly dimensioned the measurement standard deviation or that the ad-hoc correction of the state estimation covariance badly represents the increased uncertainty in the multiple consecutive bias estimation case.

The rst theory seems wrong since the consistency is good as long as no bias is present (up to around 50 s). It therefore appears that the state covariance corrections proposed both for Willsky's GLR (Section 3.2.1) and the MGLR (Sections 3.2.1 and 3.2.7) do not properly reect the random walk of the estimation error post-fault.

Integrity: For Willsky's GLR (Figure 4.13) the protection level follows the same pattern as for the north and east axes (Figures 4.7 and 4.10), where the protection level increases over time as more biases are detected. Despite this, the protection level is underestimated at three occasions (between 65 s and 120 s). During these occasions, the actual error still stays below the alert limit however meaning that the integrity test is bad but the safety is not compromised.

For the MGLR, the integrity is lost transiently at one occasion (at 100 s) and at another occasion the error touches the protection level (this is the same occasion also seen for Willsky's GLR around 80 s). Interestingly, whereas the condence set (represented by the 3 σ-bound) does not manage to reect the uncertainty due to multiple bias detection and estimation, the protection level follows the evolution of the error fairly well.

Finally we see that the bias elimination eectively removes the residual bias and restores the protection level to its nominal level.

Time [s]

Test statistic [-] l GLR l GLR,det Test statistic [-] l GLR l GLR,det Test statistic [-] l GLR l GLR,det Test statistic [-] l GLR l GLR,det Test statistic [-] l GLR l GLR,det

Conclusion

This scenario showcases two important aspects of position estimation integrity that we have stressed earlier. The rst one concerns the steady increase of the protection level as a multitude of consecutive biases are detected and estimated. As we can see in the case of Willsky's GLR, the integrity margin steadily decreases since no elimination of old biases is done. Since only the estimation up to the point of detection is taken into account when calculating the state covariance correction (and by consequence the protection level), the information about the uncertainty of the bias estimation is truncated, leading systematically to an underestimation of the protection level.

In the MGLR, the uncertainty of each bias estimation is taken into account continuously, the protection level can transiently cross the alert limit, causing a transient integrity failure. It would be reasonable to believe that the protection level calculated in this manner is more representative of the actual uncertainty than the one obtained through Willsky's GLR.

Also, old biases are eliminated at several occasions when the total bias likely reaches zero, meaning that the integrity margin is restored continuously.

Apart from the integrity, it appears that the bias elimination method is also eective for improving the long term precision of the state estimate. However it is clear from one example (Figure 4.11) that the method is far from perfect from a consistency point of view.

The consistency, on a side note, seems to be little representative post-fault as we have seen on all three axes. Indeed the interpretation of the condence set in the post-fault case rests on loose statistical ground, i.e. corrections are done to the covariance to compensate for constant eects. Furthermore, it is unclear what the interpretation of the Kalman lter covariance post-fault should actually be.

To sum up the results of this scenario, we can say that the MGLR has shown a signicantly more relevant protection level than Willsky's GLR and that the long term error is smaller thanks to the bias elimination. Regarding the estimation precision during periods of severe bias, the results are not conclusive.

Application to a vehicle mounted GNSS/INS fusion in an urban environment 4.4.1 Introduction Description

In this scenario, a car tted with a GNSS receiver and a low-cost IMU was navigated in an urban environment in order to provoke the appearance of multipath eects on the GNSS position measurements.

The scenario is divided into three parts due to occasional long periods of total GNSS signal blocking.

Since the objective of this scenario is not to show Kalman lter resilience to GNSS signal blocking, the evaluation was split into three sub-scenarios. It can nonetheless be interesting to include periods of short signal outage since they are often surrounded by periods of poor satellite availability increasing the risk for unmitigated multipath eects.

In this section we will only present the results from the second part, which is also the part with a majority of urban canyons.

GLR setup

The GLR algorithm uses the GNSS position and velocity (from Doppler measurements) as measurements (both sampled at 0.2 s) and the IMU acceleration (projected in the inertial frame) as input (sampled at 0.02 s). The standard deviation of the GNSS position is set to 5/3 m in the north and east axes, and 5 m in the down axis. The standard deviation of the speed measurement is set to 1/3 m/s on all axes. Finally the standard deviation of the acceleration measurement is set to 5/3 m/s 2 on all axes.

The GLR detection window length is set to L = 5 s and the estimation window to L est = L s. The probability of false alarm is set to P F A = 10 -6 .

Evaluation criteria

In this experiment no ground truth is available, the consistency of the estimation error is therefore impossible to asses. Nonetheless, we will be able to qualitatively judge whether apparent measurement biases are detected and estimated and whether the bias elimination is eective by eliminating the long term bias. The main focus of the evaluation therefore lies on the integrity, which is evaluated through the protection level relative to the alert level.

The Alert level is set to 20 m. Since no regulation exists for drones in urban environments, this number is taken out of the air. It can be argued that urban aerial navigation can be compared to a permanent precision approach where the horizontal (x-and y-axes) alert limit is 40 m and the vertical (z-axis) alert limit is 10-35 m. A general alert level of 20 m therefore seems appropriate.

Estimation along the north axis

The acceleration input, and speed and position measurements are seen in Figure 4.15. The measurement error with respect to the (a posteriori reconstructed) reference is seen in Figure 4.16. We note that while the position measurement experiences some jumps, the speed measurement does not. This is because the receiver velocity is calculated from the Doppler shift and is not sensitive to the range error sources aecting the position measurement.

Precision: Willsky's GLR (Figure 4.17) manages to detect and reject most of the position measurement biases. It does however end up with a small residual bias. The MGLR-LS (Figure 4.18) on the other hand shows a better precision in the second half of the scenario thanks to the systematic bias elimination.

Consistency: Overall for both algorithms, the condence set is badly estimated. This was already commented in the previous example (Section 4.3) and appears to be due to the loosely grounded interpretation of the state error covariance post-fault. The MGLR-LS is slightly better o due to the bias elimination but the overall consistency is not satisfactory.

Integrity: In terms of the protection level, it is generally well estimated for both, apart from one occasion around 150 s. A further analysis not shown here showed that the integrity loss is due to a burst of measurement jumps which provokes a temporary loss of estimation accuracy. For the MGLR-LS, this impacts the re-identication step, and for Willsky's GLR the impact is a sequence of missed and late detections.

We note also that the actual error is still signicantly lower than the alert limit but that such a behaviour can lead to a dangerous operation since the integrity test is bad. Test statistic [-] l GLR l GLR,det

Estimation along the east axis

The input and measurements seen in Figure 4.19 exhibit a similar behaviour seen for the north axis. In the measurement error (Figure 4.20) we identify the same types of bias seen in the north axis (Figure Test statistic [-] l GLR l GLR,det an eective use of the bias elimination which limits the random walk of the estimation error.

Consistency: In this case the consistency is lost completely early on for Willsky's GLR due to the drift of the error. The MGLR-LS on the other hand remains fairly consistent apart from a period around 50 s and a small error peak around 145 s. Otherwise, the same comment already passed on the estimated condence set can be recited: The error covariance post-fault is not a good measurement for the estimation consistency.

Integrity: Here the integrity is lost transiently for Willsky's GLR around 100 s due to the estimation error drift. The MGLR-LS remains integrate throughout the scenario. It can be noted that the bias elimination helps keeping the the protection level near its nominal value in steady state. Note also the varying height of the protection level peaks for the MGLR-LS (Figure 4.22), which depends partially on the number of biases currently being estimated (refer to the density of the statistics plot).

Estimation along the down axis

The vertical axis is naturally the one that exhibits the most biases since in order for it to be precise, a large number of satellites with high elevation are required. This is rarely the case in urban navigation.

In addition, the measurement is pestered with what appears to be multipath eects during almost the entire scenario (see Figure 4.24).

Note that in this scenario due to the nature of the measurement, the reconstructed ground truth is almost solely based on the speed measurement. It is hoped that the calculated ground truth is good enough, however it is possible that some of the estimation error drift is actually due to a bad reference trajectory.

Precision: The precision along the down axis is very bad for both the algorithms. It appears that the drift seen towards the ends of the scenario is lower for Willsky's GLR than for the MGLR-LS.

Consistency: Along the down axis, the consistency is lost completely for both algorithms, apart from the period around 110-170 s.

Integrity: Willsky's GLR shows a better integrity overall and the protection level is only crossed transiently at a few occasions (around 90 s, and 240-260 s). The MGLR-LS on the other hand, remains Test statistic [-] l GLR l GLR,det integrate up until around 210 s where the estimation error drifts past the protection level.

Conclusion

In the presented scenario of urban navigation, mainly the estimation along the north and est axes were the base for conclusions since the down axis measurement was very bad and the ground truth relevance was questionable. Regarding the dierence between Willsky's GLR and the MGLR algorithms one can single out a few points:

The re-identication step of the MGLR seems to conrm the hypothesis that additional exibility in the estimation of the bias can help in reducing the estimation drift over time. Trivially speaking, the MGLR can update the estimation after detection whereas Willsky's GLR needs a subsequent detection (which can be due to a previous under-or overestimation).

The re-identication step, in addition to rening the bias estimation, helps estimating the protection level properly. Indeed since Willsky's GLR truncates the estimation at detection, the impact of the bias uncertainty on the state estimation is not necessarily representative, as we have seen.

The bias elimination is crucial to maintain the integrity over time. It also help reducing the drift and residual bias in the estimation error. We have also noted that Willsky's GLR contains no bias elimination, which leads to the integrity margin decreasing over time and potential error drift or oset in steady state.

The state estimation covariance is hardly representative after biases have appeared and have been detected and estimated. This has been shown for Willsky's GLR as an underestimation of the error drift and for the MGLR-LS as an inability to reect error peaks at detection. Test statistic [-] l GLR l GLR,det Test statistic [-] l GLR l GLR,det

Conclusion

In this chapter we have evaluated the MGLR(-LS) algorithm against the classical GLR algorithm in a number of realistic scenarios and real datasets. The objective of the evaluation has been to asses the estimation accuracy, the error consistency, and the estimation integrity.

Some general conclusions can be made: Test statistic [-] l GLR l GLR,det The MGLR-LS reduces the drift of the estimation error as compared to Willsky's GLR when biases appear in a rapid succession or are hidden in noise.

The re-identication step of the MGLR both serves as a renement of the bias estimation, and as an improvement of the protection level calculation which is otherwise truncated.

The proposed bias elimination works fairly well in most cases and keeps the residual error and protection level low over time in absence of bias.

The major drawbacks identied are:

The MGLR re-identication step assumes Gaussian noise and is rapidly degraded if this condition is not met.

The algorithm is Kalman-based and so it requires the covariances involved to be well known or estimated in order to avoid false alarms and detection delays.

The bias elimination is built on very approximate hypotheses, meaning that it is dicult to give any guarantees on how large the maximum non-rejectable "zero-bias" is.

The estimated error covariance post-fault seems to have no physical relevance in the verication of the continued error consistency in the case of faults. It does however play a role in the calculation of the protection levels, which seem properly estimated in most cases.

The development of the MGLR algorithm for position estimation and the evaluation presented herein have assumed that the attitude estimation is perfect. That this is not always the case and what to do about it will be the work of the upcoming chapters. T his chapter proposes a strategy for measurement consolidation of IMUs. The strategy has three distinct parts.

-The rst part is a sensor output estimator and residual generator sensitive to dierent faults (measurement, attitude, model).

-The second part is a fault detection based on either χ 2 -or GLR testing.

-The third part is a data consolidation logic for determining whether the sensor estimation or the actual sensor measurement should be used in the subsequent attitude estimation and whether the consolidated data qualies for attitude update or not.

Additionally, a simple algorithm for continuous gyroscope bias estimation despite loss of observability during external perturbations is developed.

One accurate measurement is worth a thousand expert opinions Grace Hopper

Motivation and principles

Attitude estimation is classically done with attitude measurements that are considered fault-free. We recall from the introduction that the most common attitude sensors for UAV attitude estimation are accelerometers and magnetometers. These are complemented by gyroscopes for attitude rate measurements.

Measurement disturbances and faults

The disturbances and faults impacting attitude sensors and attitude rate sensors are dierent by nature.

Attitude sensors, are classically considered to be impacted by transient perturbations, caused for example by dynamic manoeuvres or magnetic dipoles in the environment. The solution is thus to lter out these perturbations by selecting lter time constants that are suciently long.

However there are cases where these assumptions do not hold: Long manoeuvres, for example during surveying or patrolling cause long-lasting transverse accelerations impacting accelerometers.

Urban environments often contain ferromagnetic materials and magnetic elds that will corrupt magnetometer readings over time.

In these cases, disturbance rejection methods based on detection and measurement rejection or adaptive ltering must be employed.

Attitude rate sensors, usually in the form of gyroscopes, are by far more tolerant to perturbations although they are systematically corrupted by bias which must be estimated. The classical solution is to estimate the gyroscope biases by using the attitude sensors. The obvious aw is that the bias estimation will be impacted if the attitude sensors are biased. This becomes an even more serious problem if the attitude measurement perturbations are long lasting.

In the case where the transients measurement perturbations turn into longer lasting ones, the attitude will unfailingly drift. In stabilisation and positioning applications, attitude drift is handled by the integrators of the speed and position loops. However in applications where the attitude is used directly, for example in projections in vision based navigation (with cameras or LiDAR) or mapping, attitude drift can cause large errors in the positioning system. the only faults considered here are time-varying bias and outliers.

Measurement consolidation process

In this chapter, we treat attitude measurement perturbations and attitude rate biases with a process called measurement consolidation. The principle is quite simple:

For attitude measurement faults, the process is:

1. Calculate a norm residual of the measurement by using the reference vector norm (gravity or local magnetic eld).

2. Estimate the output of the sensor, using an EKF based on a sensor model and the a priori known reference vector. The measurement norm residual is used in the EKF to activate or deactivate the state correction.

3. The sensor output estimation is then used to calculate residuals which are sensitive to either measurement outliers, or sensor model errors (typ. attitude drift).

4. All calculated residuals: norm; outlier; model error, are tested with an array of statistical tests, producing a set of fault detection booleans, called the fault signature.

5. These fault signatures are then used by a consolidation logic to determine whether the measurement or the estimated output should be used for attitude update, and whether the update should be performed at all.

The fault detection applied to the sensor lter residuals is either based on χ 2 or GLR testing.

For attitude rate measurements, a slightly dierent measurement consolidation process is used:

1. A statistical test of the gyroscope measurement is performed to detect when the body is not rotating.

2. An open loop estimate of the gyroscope bias is then calculated on condition of no rotation.

3. The fault detection booleans from the attitude measurement consolidation are used to decide whether the currently estimated gyroscope bias from the attitude lter should be replaced with the open loop attitude independent gyroscope bias estimation.

By applying the above consolidation strategies for attitude and attitude rate measurements, it is hoped that the attitude estimator working downstream will be less impacted by measurement disturbances and faults. In the ideal case, the faults and disturbances impacting the sensors will be transparent to the attitude estimator.

Attitude measurement consolidation

For the sake of clarity, we have divided the section on attitude measurement consolidation into two parts:

• Sensor output estimation and residual generation (Section 5.2.1) concerns the EKF-based sensor output estimation and the generation of the fault sensitive residuals.

• Fault detection and data consolidation (Section 5.2.3) concerns for one part the statistical testing of the fault sensitive residuals, and for another part the fault signature table resulting from the combination of the detection booleans.

The attitude measurement consolidation is by all means a function, which takes as input an attitude measurement r m and attitude rate measurements ω, and the current estimated attitude q and gyroscope bias bω , and outputs the consolidated sensor data r c (measurement or estimated sensor output) and the associated status ag r_status. The functional principle is illustrated in Figure 5.1.

Section 5.2.1 Section 5.2.3

Figure 5.1: Overview of the attitude measurement consolidation for an attitude measurement r m .

Details on the notation seen in Figure 5.1 shall be claried further on in this section.

An EKF for sensor output estimation

The rst step of the measurement consolidation is the estimation of the sensor output. The purpose of this is twofold. Firstly it provides an analytically redundant information of the expected sensor output, noted r. Secondly it allows to dene fault sensitive residuals (ε rm , and ε r ref ) which can be used to determine whether the actual sensor measurement is good to use for attitude update or not.

An overview of the sensor output estimation and residual generation is seen in Figure 5.2.

Denition of a performance model for sensor output estimation

Sensor models for attitude measurement sensors commonly assume that the measured reference vector is constant [Batista et al., 2012a;[START_REF] Martin | A global observer for attitude and gyro biases from vector measurements[END_REF]. This translates to the rate of change of the measurement being modelled with only the (de-biased) gyroscope. However during external perturbations, i.e. disturbances in the inertial frame, the measured reference vector is no longer constant. This implies that the rate of change of the measurement can no longer be modelled only using the gyroscope. Thus we propose to allow the reference vector to vary around its presumed constant value with a prescribed decay dynamics. This will capture the dynamics of transient disturbances while keeping an attraction towards the actual measurement reference.

First recall that in the general case, we write an attitude measurement in the body frame as

r m = R(q) T r ref + w r = r + w r
where R(q) is the transformation matrix from the sensor frame to the reference frame and w r ∼ N (0, W r ) is a (Gaussian) zero-mean sensor noise. The sensor noise usually results from factors such as vibrations in the sensor frame. We nd the dynamics of the vector measurement in the sensor frame by deriving it in the sensor frame with respect to time:

ṙ = -ω × R(q) T r ref + R(q) T ṙref .
The angular speed ω is dened in the sensor xed frame. The term ṙref describes the evolution of the reference vector in the reference frame. As we previously said, a common assumption is that the reference vectors are xed ( ṙref = 0), or at least known at all times [START_REF] Martin | A global observer for attitude and gyro biases from vector measurements[END_REF]. This is not likely to be the case in several situations that UAVs may encounter, for example large inertial accelerations or persistent magnetic disturbances.

A more cautious model for ṙref would be to assume that r ref is known on average. This translates for example into using the rst order dynamic model, that we call a performance model :

ṙref = 1 τ r r ref -r * ref (5.1)
where r * ref is the undisturbed reference vector and τ r is a time constant related to the expected duration of the disturbance. For example if the expected disturbances on r ref are fast transients, τ r will be small and vice versa if the disturbances are enduring. By inserting this model into Equation (5.1) we get

ṙ = -ω × R(q) T r ref + 1 τ r R(q) T r ref -R(q) T r * ref and thus: ṙ = -ω × r + 1 τ r r -R(q) T r * ref (5.2)

Kalman lter model synthesis

With the above models, we can estimate the sensor output with an estimator such as a Kalman lter using the measurement model (5.1). Clearly if the attitude matrix R is known (or well estimated), the model (5.2) allows to attenuate the eect of a perturbation on r m since the estimated sensor output is drawn to R(q) T r * ref . However when R is not well estimated, or unknown, it is more cautious to use the model (5.1) with ṙref = 0 since the attraction of r to R(q) T r * ref can no longer be assured. This implies that in order to properly determine when the measurement and the sensor model are good, we require a set of residuals that are sensitive to either erroneous measurements, erroneous attitude, or both.

At the sensor output estimation level, we assume we have access to the estimates q and bω of the attitude quaternion and of the gyroscope bias 5.2:

q = q + v q, v q ∼ N (0, V q) b ω = bω + v bω , v bω ∼ N 0, V bω Then, from the gyroscope measurement ω m = ω + b ω + v ω we can write ω = ω m -b ω -v ω = ω m -bω + bω -b ω -v ω = ω m -bω -v bω -v ω Or in a concise form ω = ω + v ω with ω = ω m -bω v ω = -v bω -v ω v ω ∼ N 0, V bω + V ω . (5.3)
Then the continuous time Kalman lter synthesis model is written using (5.2) and (5.3) as:

Sensor output EKF, model synthesis

ṙ = -(ω + v ω) × r + 1 τ r r -R(q + v q) T r * ref r m = r + w r with v ω ∼ N 0, V bω + V ω v bω ∼ N 0, V bω v q ∼ N (0, V q) (5.4)
A discrete version of (5.4) can be acquired by Euler's method with an integration step ∆t:

r t+1 = r t + -(ω t + v ω) × r t + 1 τ r r t -R(q + v q) T r * ref ∆t .
(5.5) Since our goal is to reduce the impact of erroneous measurements, we can try to mitigate the eect of measurement outliers, either by outright rejection or by modifying the weight of seemingly incoherent measurements (smoothing). In our case a combination of the two is to be preferred as we will see.

In addition to measurement errors, the sensor output estimation is sensitive to so called performance model errors, that is errors on any of the input signals used by the prediction model, i.e. q and bω .

Defence against measurement perturbations

A sensor output lter such as the one previously presented will be biased in case of biased or perturbed attitude measurements. In order to keep the output of the sensor estimation lter as clean as possible it is imperative to correct the state of the lter only with measurements that are judged non-faulty and cautiously correct with measurements that seem incoherent with the current state. The rejection of measurements risks leading to a drift of the sensor output estimation. This must be handled by checking the coherence between the sensor output estimation and the corresponding reference vector.

To test the measurement on its own, we propose to use norm testing. For the incoherence test between the state and the measurement, we look at innovation testing and outlier mitigation. Finally, the coherence between the sensor output estimation and the reference vector is what we here call reference testing.

Disturbance rejection through measurement norm testing

A rst defence against measurement perturbations is to discard measurements based solely on the measurement itself [Öman [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF][START_REF] Schwatke | DAHITIan innovative approach for estimating water level time series over inland waters using multi-mission satellite altimetry[END_REF]. This is particularly interesting for vector measurements such as accelerometers and magnetometers since they have an a priori known (5.6)

We assume that this residual behave approximately as a Gaussian. Our zero hypothesis is "H 0 : ε ||rm|| ∼ N 0, W ε ||rm|| ". The associated variance W ε ||rm|| is found by linearising (5.6), i.e.

W ε ||rm|| = ∂||r m || ∂r m W r ∂||r m || ∂r m T = r T m W r r m r T m r m (5.7)
The simplest way of testing the residual is with a scalar χ 2 -test, i.e.

ε 2 ||rm|| W ε ||rm|| Norm NOK ≷ Norm OK l ||rm|| ∼ χ 2 (P F A , 1) .
(5.8)

However to improve the detection rate in low fault-to-noise cases, other tests may be considered such as the GLR test (Appendix A). If a GLR test is used, a model of the innovation and of the disturbance is also required.

If the measurement fails the above norm test, there is a high chance of a problem with the measurement. In this case the measurement should not be used for state correction at all. The other way around is not true, however. That is if the measurement passes the norm tests, it is not certain that no disturbance is present. In this case we need to consider all measurement components to detect faults where the measurement norm remains constant.

Innovation based methods

A second defence against erroneous measurements can be set-up before the correction of the estimated sensor output. As the simplied reasoning goes for vector measurements, a deviating norm always indicates an erroneous measurement but an erroneous measurements does not always exhibit a deviating norm. We shall therefore also test the innovation of the Kalman lter with hopes to mitigate the eect of measurement outliers not impacting the measurement norm. From a fault rejection point of view, we observe two uses of the innovation:

1. The innovation can be used to calculate an adaptive correction gain of the Kalman lter. This is known as outlier mitigation, as the measurement is used in the correction with an adapted (reduced) gain.

2. The innovation can be used as a fault sensitive residual for the measurement. This residual can then be tested outside the Kalman lter and be used for fault isolation.

Both these cases are used in the measurement consolidation, the rst one is presented below as a mitigation mechanism of the Kalman lter itself. The second one is used at a later stage in the fault isolation. Adapted to the notation used here, we write the innovation and its covariance:

ν = r m -r+ S = H P H T + W r
(5.9)

Here P is the covariance of rr and H = I since r m = r + w r .

Several methods for innovation based outlier rejection exists in the literature, often directly integrated in the Kalman lter such as the robust weighted Kalman lter [START_REF] Ting | A Kalman lter for robust outlier detection[END_REF], innovation based rejection [START_REF] Sukkarieh | A high integrity IMU/GPS navigation loop for autonomous land vehicle applications[END_REF][START_REF] Berman | Outliers rejection in Kalman lteringSome new observations[END_REF], adaptive gain approaches [START_REF] Valenti | Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs[END_REF][START_REF] Soken | Robust Kalman ltering for small satellite attitude estimation in the presence of measurement faults[END_REF], or switching approaches [START_REF] Rehbinder | Drift-free attitude estimation for accelerated rigid bodies[END_REF].

As a short conclusion, rejection based methods will likely work better for large errors but will throw away all information even if the error is small. Mitigation methods will likely work better for small errors since the information is partially retained, but depending on the mitigation level, measurements with large persisting errors will still be seen as partially benecial to include. This might in turn lead to faster error growth of the estimate when compared to pure prediction with random walk errors given by the rejection based methods.

Outlier mitigation by covariance bloating Here we choose a mitigation based method where the covariance ellipsoid assigned to the measurement is extended to cover both the measurement and the predicted measurement (c.f. Figure 5.3), in case no intersection is found between the predicted covariance ellipsoid and the measured one. This process, called covariance bloating, is performed before every correction step according to a simple procedure. For each element ν i of the innovation ν, the following procedure is applied:

• Check whether an intersection exists between the predicted output and the measurement, calculated at a chosen uncertainty level n σ . This corresponds to

ν i No intersection ≷ Intersection n σ S i,i
• If an intersection is found, no modication is done to the measurement covariance.

• If no intersection is found, the current element of the measurement covariance is recalculated using the current innovation element.

The chosen uncertainty level n σ is a tuning parameter which has to be tuned according to a level of false alarm or missed detections. For example, if n σ = 3, the non-intersection of the prediction and measurement uncertainties is assured. This procedure is formalised by the following algorithm:

Initialization: W r = W r ; for i = 1:dim(ν) do if ν i > n σ S (i,i) then W r,(i,i) ← (ν i /n σ ) 2 ; end end Output: W r Algorithm 1: Covariance bloating procedure
The bloated innovation covariance given by S = H P H T + W r is then used in the calculation of the Kalman gain used for state correction.

Outlier detection by global innovation testing In addition to the outlier mitigation procedure described above, we can use the innovation and its covariance to globally test for outliers, i.e.

ε rm = ν W εr = S

(5.10)

We can write a test under the zero hypothesis "H 0 : ε rm ∼ N (0, S)" as

ε T rm S -1 ε rm Outlier ≷ No outlier l rm ∼ χ 2 (P F A , dim(r m )) .
(5.11)

Clearly this test will be short-term sensitive to erroneous measurements since the innovation of the Kalman lter assumes a zero mean innovation, which will not be the case if r m is far from r+ . The covariance S, will not have adapted over time through the covariance bloating procedure, meaning that it will be coherent with the zero hypothesis.

On the long term however, the static behaviour of the residual and the test is impacted by two signicant factors:

-Since the measurement disturbances are unobservable, the Kalman lter estimation will converge towards the measurement and the innovation will go to zero, implying ε rm → 0.

-Due to the covariance bloating procedure, an enduring measurement disturbance will lead to an increasing innovation covariance (S).

These phenomena will increase the false negative detection rate for long lasting disturbances.

Sensor model integrity through reference testing

Since the innovation based detection is not reliable on the long term due to the potential drift of the estimated sensor output, another test is required to detect when this estimate is no longer reliable. A drifting sensor output estimation could be related to:

-A perturbation on the measurement, as described above.

-Errors on the EKF inputs, that is q and bω .

In the nominal fault free case, we have that r m = R(q) T r * ref + w r with w r ∼ N (0, W r ). We also have in this case r = R(q) T r * ref + v r with v r ∼ N (0, P ). In order to test if the output estimation r has drifted away from this nominal case, we test the zero hypothesis r ≈ R(q) T r * ref .

To this end, we dene the model warning residual ε r ref and its covariance in the inertial frame as

ε r ref = R(q) r -r * ref W εr ref = R(q) T P R(q) + ∂ε r ref ∂ q T V q ∂ε r ref ∂ q
(5.12)

where we recall that r * ref is the known reference vector in the inertial frame. this residual has a sensitivity to disturbances which is complementary to those of ε ||rm|| and ε rm . Clearly ε r ref is short-term sensitive to both divergences in the sensor output estimation, through r, and the attitude estimation, through q. It can therefore be seen as a complementary residual to the outlier-and model sensitive residual ε rm . In the long-term, since r converges to r m , the residual is also sensitive to measurement errors. This means that in the long term, ε r ref is not exploitable for fault segregation but it will still indicate the health of the attitude estimator.

Illustration of the sensor output estimation during a prolonged measurement disturbance Clearly the sensor model manages to reject most of the disturbances, however some drift can still be seen. This drift is likely due to the outlier mitigation through covariance bloating which only dilutes the disturbance when the norm is deemed good, thus ltering out only a part of the disturbance.

The fault sensitive residuals resulting from the above estimation are shown in Figure 5.5.

The norm-and outlier residuals are clearly aected as soon as the disturbance appears. The model sensitive residual however grows as the estimated sensor output drifts from the real value. This implies that at the beginning of the disturbance, the measurement error can be distinguished from the model error. As the disturbance persists, this becomes increasingly dicult, essentially because the disturbance is not observable.

In the following section it will be shown how the short-and long term eects on the residuals can be used in practice to isolate the fault.

Fault detection and data consolidation

The residuals resulting from the sensor output estimation, ε ||rm|| , ε rm , and ε r ref , are each sensitive to dierent errors in the attitude estimator. Notably the key segregation is to dier between an attitude estimation error and a measurement error, and to determine when the sensor output estimation is no longer reliable. The objective of the fault detection and data consolidation is to detect faults (biases, outliers)

appearing on the measurements r m , then through a process of data consolidation, decide whether the measurement (r m ) or the estimated sensor output (r) should be used as the consolidated measurement rc , and whether it is t for attitude update.

We rst recall the sensitivity of our three residuals. Then we show how to detect faults by statistical testing. Finally, using the detection booleans we present our data consolidation logic.

Residual sensitivity analysis

As previously mentioned, our three residuals are sensitive to faults in dierent signals. Notably, these signals are the attitude measurement r m , the estimated sensor output r, the attitude estimate q, and the gyroscope bias estimate bω . Furthermore, we can talk about short-term and long-term sensitivity of the residuals.

If a residual is short-term sensitive to a fault in a certain signal, this means that the residual will depart from zero when the fault appears. If a signal is long-term sensitive to a fault in a certain signal, this implies that the the residual will remain away from zero in steady state. Now, if a residual is short-term sensitive to a fault in a certain signal but not long-term sensitive, this means that the eect of the fault will only be visible after a certain time, and not directly at the time of appearance of the fault. Inversely, if the residual is long-term sensitive, but not short-term sensitive to a fault in a certain signal, the residual will not depart from zero as the fault appears, but might later due to the faulty signal inducing drifts in the other signals to which the residual is short-term sensitive.

We see from the example in Figure 5.5, where a fault appear on on r m , that the norm residual (ε ||rm|| ) and the outlier residual (ε rm ) both are short-term sensitive to the fault since they react as soon as the fault appears. The model warning residual (ε r ref ) on the other hand is only long-term sensitive to the fault since the eects of the fault (degraded estimation, drift, etc.) are only visible on the residual after a certain time.

An additional example is given in Figure 5.6, where a large attitude error (e.g. due to a violent reinitialisation) is introduced. We clearly see on the model warning residual (ε r ref ) both the short term eect due to the denition of the residual, and the long-term eect, due the sensor output estimation drift, which is in turn caused by a perturbation of the attitude rate bias estimation. We also note the drift of the outlier residual (ε rm ) that this chain of events leads to. On a side note, this highlights the importance of a robust attitude rate bias estimation as we will see later on.

Conclusively, with regards to faults on q, the residual ε rm is not short term sensitive, since the innovation depends only on the estimated attitude through the prediction equation. It is however longterm sensitive since a faulty attitude estimate will make the estimated sensor output drift towards a faulty reference.

Table 5.1 shows the long-and short-term eect of attitude estimation errors ( q), gyroscope bias estimation errors ( bω ), measurement errors (r m ), and sensor output estimation errors (r), on the fault sensitive residuals. The errors considered here are (time-varying) biases and outliers. From a fault detection point of view, short-term eect means that the fault can be isolated if it is detected at the time of its apparition whereas long term eect is the over-time detectability of the fault. Note that no conclusion can be made on the observability of the fault given the long term eect.

The separation of short-and long term eects follows the logic of short-term accuracy versus longterm robustness. Given the above fault sensitivity table, we can draw some conclusions on the usage of the residuals:

1. ε ||rm|| and ε rm are short-term sensitive to measurement errors since they both depend on the measurement r m directly.

2. ε r ref is short-term sensitive to attitude errors since it depends on q directly.

3. ε r ref and ε rm are long-term sensitive to performance model errors, i.e. q and bω . ε r ref since it depends on the attitude directly and the bias estimation through integration of the performance model, and ε rm since it depends on both through integration of the performance model.

4. Only ε ||rm|| can be used to long-term distinguish between attitude estimation errors and measurement errors since both ε r ref and ε rm are both long-term sensitive to attitude estimation errors, and ε r ref but not ε rm are long-term sensitive to measurement errors.

5. Constant norm measurement perturbations are not long-term distinguishable from attitude estimation errors. this follows from the above point that only ε ||rm|| can be used to distinguish on the long-term between attitude estimation errors and measurement errors. This means that if at any given point in time, ε ||rm|| and ε rm are close to zero, and ε r ref is far from zero, it is not possible to deduce whether a measurement disturbance is present or not, or whether it is due to an erroneous attitude estimation.

The short-term sensitive residuals can be used to detect faults on the measurements or the attitude:

-If ε ||rm|| or ε rm grow but not ε r ref , we can deduce that this is due to a fault on the measurement and therefore use the estimated sensor output for downstream attitude correction instead of the measurement itself.

-If on the other hand ε r ref grows but neither ε ||rm|| or ε rm , the fault is likely to be in the attitude or the sensor output estimation and the sensor measurement should be used to recover the attitude observability.

-In the cases where ε r ref and either ε ||rm|| or ε rm grows, it is not possible to isolate the fault and we must disqualify the sensor data (measurement and output estimation) completely until the long-term eect has been established.

The next step is to detect the fault eect on the residuals in order to proceed with the fault isolation and safe-guard the attitude estimation.

Fault detection

The residuals presented in Section 5.2.2 must indeed be translated into binary values through hypothesis testing in order to be of any use. The simplest hypothesis test that can be used is the χ 2 -test, i.e.

l χ 2 = ε T W -1 ε ε H 1 ≷ H 0 l det (5.13)
where the hypotheses H 0 and H 1 are in the general case:

H 0 : No fault present H 1 : Fault present (5.14)
In the high-noise case, which is predominant in UAV applications, the χ 2 -test is quickly disarmed in cases where the perturbation amplitude has a low SNR. Also in cases where the perturbation grows slowly in comparison to the dynamics of the vehicle. Therefore it would be wiser to use a more powerful test for fault detection.

An alternative is to use the GLR-test as described in Appendix A. In this case we will model the residual as white noise and the GLR algorithm is set to search for a bias drift in a random walk process.

We dene the problem as

ε t+1 = ε t ε m, t+1 = ε t+1 + Γ ε, t-k b ε + w ε (5.15)
where we can identify the noise w ε * ∼ N (0, W ε * ), and the measurement fault signature Γ ε * , t-k = (t -k)Y t-k ∆t. In this case we consider all states of the residual to be biased. A drawback of the GLR test is that it requires a model of the residual, and the model driving it, including a linear model of the non-central bias b ε * (here represented by the function Γ ε * , t-k ).

In our case we simply assume that the residual follows a white noise driven process and that the bias is modelled as a linear drift. The reason to model the bias as a drift is to catch the slowly growing faults hidden in noise that are missed by the χ 2 -test. Details on the χ 2 -and GLR-test as well as how to calculate the detection threshold with respect to a probability of false alarm (P F A ) and a probability of false detection (P M D ) are found in Appendix A.

Illustration of fault detection methods during a measurement disturbance Continuing with the illustration of the measurement consolidation steps, we now look at the dierence between the χ 2 test and the GLR test in the case of a time varying magnetic disturbance. The χ 2 -and GLR tests are applied to the norm-and outlier residuals (ε ||rm|| and ε rm ), whereas the model sensitive residual is only χ 2 -tested. The GLR tests are tuned to detect drifts in the residual, i.e. the fault signature function is set to Γ ε, t-k = t -k to represent a drift of unit slope. As can be noted in Figure 5.7, an advantage of using a GLR test over a simpler χ 2 -test is that the noise level has a smaller impact on the test statistic thanks to the LS-estimation. A faster detection is also assured thanks to the a priori information in the form of the fault signature Γ ε . As for the notation of the specic measurements, the measurement specic booleans are a_norm, a_outlier, a_warning, and a_status for the accelerometer, and m_norm, m_outlier, m_warning, and m_status for the magnetometer.

Fault detection booleans and measurement consolidation logic

Data consolidation

The above boolean denitions together with Table 5.1, are used to set up a data consolidation truth table. Note that the logic in this table does not take into account the long-term eects since no dierence can be made at the statistical testing level.

Case # r_outlier r_warn r_norm r c r_status Fault description In short, if a measurement perturbation is detected, the estimated sensor output is used for attitude update. Inversely, if the sensor model is detected compromised, the measurement is used for attitude update. The status boolean, r_status, is set to one in the cases where either the measurement or the sensor output is deemed t for attitude correction (cases 1, 2, 3, 5, and 6). In the cases where it is not possible to isolate a single faulty signal, i.e. both the sensor output estimation and the measurement are deemed unreliable, the status boolean is set to zero.

1 0 0 0 r m 1 Nominal, fault-free 2 1 0 0 r 1 r m NOK 3 0 1 0 r m 1 q or r NOK 4 1 1 0 N/A 0 q, r m , or r NOK 5 0 0 1 r 1 r m NOK 6 1 0 1 r 1 r m NOK 7 0 1 1 N/A 0 q, r m , or r NOK 8 1 1 1 N/A 0 q, r m , or r NOK
The cases 4 and 8, where the status boolean is set to zero correspond to the cases in Table 5.1 where the long-term eects on the residuals ε rm and ε r ref coincide. Case 7, where the r_norm and r_warn are both 1 corresponds to the case where a severe measurement disturbance has long-term impacted the sensor output estimation, making it too unt for attitude update, in addition to the measurement itself.

Illustration of the measurement consolidation during a measurement disturbance The measurement consolidation logic described here-above takes the measurement, the estimated sensor output, and the detection booleans as an input and outputs a consolidated measurement and an associated status boolean. We illustrate the result of such a consolidation using the previously introduced magnetic disturbance (Figure 5.4) and the resulting test statistic (Figure 5.7). The resulting measurement consolidation is seen in Figure 5.9. It is clearly seen that in the beginning of the perturbation (∼22-36s), the estimated output is selected over the measurement. Towards the end however, the estimated output has drifted to such an extent that it is no longer trusted and priority is given to the measurement, although it is still biased. The status boolean m_status allows attitude corrections up until around 50s. Past this point the sensor estimation starts to drift and the consistency of the attitude corrections can no longer be assured. It can also be noted that the status boolean does not switch back to OK as soon as the disturbance disappears sine the output estimation has drifted and takes some time to converge.

Conclusion

The attitude measurement consolidation developed in this section makes use of an EKF based on a sensor performance model to estimate an unbiased sensor output in case of measurement disturbances.

A number of fault sensitive residuals are dened to be sensitive to either measurement disturbances or performance model errors, or both. We recall that measurement disturbances are here assumed to be either time-varying biases and outliers aecting r m , and performance model errors are errors on the signals alimenting the sensor performance models, i.e. the attitude estimate q, bω , and r

These residuals are then tested with χ 2 -or GLR-tests to produce a set of fault detection booleans.

The booleans are in turn used in a fault signature table to determine whether the measurement or the output estimation should be used for attitude update, i.e. the consolidated data. A status ag associated to the consolidated data indicating whether or not the data is good enough for attitude update.

Some key features of the measurement consolidation are:

• Through sensor model based ltering and disturbance rejection, it is possible to estimate an attitude sensor output which acts as a virtual sensor which can replace the real measurement at least during transient disturbances.

• The discrimination between sensor model errors and measurement errors allows to produce a correct attitude estimate in more cases than would be the case if only measurement fault rejection was employed. This should lead to a better consistency of the attitude estimator used downstream.

• The level of fault rejection can be adjusted by choosing dierent fault detection methods for the fault sensitive residuals (e.g. χ 2 , GLR). Sliding window methods (such as GLR) provide more consistent fault detection but suer from a tail of false positives when the fault is over.

• The lack of observability of the attitude measurement bias can cause drift in the sensor output estimation during long lasting perturbations if the gyroscope bias is badly estimated. It is therefore important to complement the attitude measurement consolidation with a robust and consistent gyroscope bias estimation.

Attitude rate measurement consolidation

We have explained how to consolidate attitude measurement, that is magnetometer and accelerometer measurements. In this section we study the consolidation process of attitude rate measurements. The attitude rate measurements, usually from gyroscopes, are considered short-term reliable, meaning that the do not suer from transient disturbances or time varying bias in the same way as accelerometers and magnetometers. The only disturbance considered for attitude rate measurements is a slowly timevarying, almost constant, bias.

Consolidation principle

The consolidation of attitude rate measurements practically implies a consolidation of the gyroscope bias estimation. As we have seen in the introduction, the most common way to estimate gyroscope bias is through attitude estimation using the observability of the bias gained from the attitude measurements.

However if the platform is a UAV, it is reasonable to assume that it will be stationary in rotation for a major part of its mission. In this case a low-pass lter can be used to extract the quasi-constant component of the sensor signal. We thus have two means of estimating the attitude rate sensor bias.

Common attitude rate sensors like gyroscopes measure the body frame angular rates, i.e.

ω m = ω + b ω + w ω (5.16)
where b ω is a slowly time varying bias and w ω ≈ N (0, W ω ).

During perturbations of the attitude measurements, the gyroscope bias estimation, bω , classically provided by most attitude estimation lters thanks to observability will be biased, or at least partially for decoupled ones.

Classical attitude lters rely on the ltering time constant to render the bias estimation robust to measurement perturbations, however this strategy falls on the fact that the bias estimation still depends on the attitude measurement error. This implies that a severely perturbed measurement may severely impact the bias estimation despite a large estimation time constant.

A backup bias estimation independent of the attitude measurements can thus be useful when measurement perturbations are present.

The idea behind the attitude rate measurement consolidation is simple:

• Estimate the gyroscope bias via an attitude lter under the assumption of no attitude measurement disturbances, rendering bω .

• Estimate the low-pass component of the gyroscope under the assumption of stationarity, rendering a redundant bias estimation bω,LP .

• In case of detected attitude measurement disturbances, substitute the attitude lter bias estimation with the low-pass estimated one, rendering the consolidated bω,c .

Gyroscope bias estimation through low-pass ltering

In the popular attitude lters [START_REF] Madgwick | Estimation of IMU and MARG orientation using a gradient descent algorithm[END_REF][START_REF] Valenti | Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs[END_REF], the gyroscope bias is be found as the low-pass component of the gyroscope signal when the gyroscope is at rest. To determine whether this is the case, we can perform a simple χ 2 -test, i.e.

ω T m V -1 ω ω T m Rotation ≷ No rotation l ω
(5.17)

The gyroscope bias is then estimated as a saturated rst order lter response of the gyroscope signal, i.e.

ḃω,LP = -

1 τ LP bω,LP -ω m - 1 τ b bω,LP -sat bω,LP , b max (5.18)
with the unsaturated lter time constant τ LP conditioned by 'Rotation'/'No rotation' as

τ LP = ∞, if 'Rotation' τ LP , if 'No rotation'
.

(5.19)

The time constant τ b is the rst order anti-windup time constant and b max a vector of reasonable maximum bias amplitudes. As an order of magnitude, τ LP is normally around 100s and τ b around 1s, b max will depend on the quality of the sensor but should reasonable be in the order of 1deg/s for low-cost hardware. For discrete implementation, we can integrate (5.18) with Euler's method and an integration step ∆t, yielding:

bω,LP,t+1 = bω,LP,t -

∆t τ LP bω,LP -ω m - ∆t τ b bω,LP -sat bω,LP,t , b max (5.20)
The idea of using a saturated lter comes from the fact that the gyroscope bias in all practical applications is known to be bounded. If in any case the bias estimation is pushed through its saturation limit, the sensor itself is likely at fault. This diagnosis problem falls outside the scope of this thesis.

This bias estimation technique is interesting to use when the attitude measurements are perturbed since the gyroscope bias estimation from an attitude lter will be biased in these cases.

Gyroscope bias estimation consolidation

The idea is to use this bias estimation during perturbations and the bias estimation of the attitude lter under nominal conditions. We will therefore replace (or re-initialise) the bias estimation of the attitude lter continuously during perturbations according to the detection booleans (a/m_norm and a/m_outlier).

The booleans a_norm and a_outlier are related to disturbances on the accelerometer. The accelerometer primarily contains information on the inclination and thus on the body xed xand y-axes.

Perturbations on the accelerometer will therefore mainly impact bias estimations on the body xed

xand y-axes, i.e. the biases bω,x and bω,y . On the other hand, the magnetometer contains mainly heading information and perturbations will therefore impact the bias estimation on the z-axis, i.e. bω,z

Illustration of the bias consolidation

We illustrate the interest of this switched bias estimation with three scenarios, one containing acceleration disturbances, one containing magnetic disturbances, and one containing both disturbances. Each scenario consists of four distinct parts:

1. No rotation or perturbation present (beginning/end/between disturbances).

2. No rotation with measurement disturbances present.

3. Agitated rotation without measurement disturbances.

4. Agitated rotation with measurement disturbances present.

For each scenario we compare the bias estimation with and without the bias consolidation presented in this section.

The attitude lter bias estimation b ω is estimated by a heading/inclination decoupled MEKF. The fault detection scheme detailed in Section 5.2 is used to generate the booleans a_norm, m_norm, a_outlier, and m_outlier as described in Section 5.2.3. For the sake of illustration, the attitude and bias estimator is assumed to use all measurements, that is the status booleans a_status and m_status are forced to zero.

Simulated scenario 1: Bias estimation during an acceleration disturbance.

Comments:

• The bias estimation is greatly improved in the case where the bias consolidation strategy is used.

Also note that the attitude lter eectively mainly impacts the xand y-axis gyroscope bias estimations. The reason for this is in this case that the attitude is close to (hover), which is most often the case for UAVs. This also acknowledges the decision to only replace these biases during acceleration disturbances. The small peak seen at around 150s in Figure 5.12 is the result of the switch back to using the attitude lter bias estimation. The reason is that the disturbance is not completely gone, thus there is a residual impact on the attitude lter estimation. Comments:

• As for the acceleration disturbance, the bias estimation during a magnetic disturbance is also improved in the case where the bias consolidation strategy is used. Notably the attitude lter • The combined disturbance scenario shows the most agrant dierence between the cases with and without bias consolidation. Where the bias estimation of the attitude lter is severely impacted by the measurement disturbances, the low-pass estimation is not. This means that the eect of the disturbance is almost transparent when using the bias consolidation.

The only anomalies seen when using the bias consolidation is that the bias estimation is briey slightly impacted at the end of the disturbance at around 150s in Figure 5.18. This is the same phenomenon seen in Figure 5.12 and related to the fault detection capacity.

Conclusion

We have presented a simple switching algorithm for consolidating the attitude rate measurement bias estimation during periods of external disturbances. A redundant bias estimation method based on a conditional low-pass ltering and a bias estimation from an attitude ler are used. The consolidated attitude rate bias estimation is then a function of the fault detection booleans developed for the attitude measurement consolidation, and replaces the current bias estimation of the attitude estimator.

Simulations show the eectiveness of the consolidation during periods of external disturbances on the attitude sensors, combined with aggressive rotations.

Conclusion

In this chapter we have developed methods for securing attitude-and attitude rate measurements in case of external perturbations. We have seen that by using simple kinematic models and performance models, it is possible to dene a set of virtual sensors that can be used instead of the real sensors when deemed more reliable according to a consolidation logic.

Concerning attitude rate sensors, we have seen the interest of a switching logic between biases estimated in closed loop and open loop. The closed loop ones with an attitude lter and the open-loop ones through low-pass ltering Since we are working in an EKF framework, the tuning of the sensor estimation lters themselves can be easily done by using the prescribed or measured sensor noise levels. In addition to this, we have introduced a number of parameters concerning modelling and fault detection that must be tuned. In summary, the parameters needed to be tuned are the following:

Sensor performance model time constants τ r through the condence of the reference vector r ref .

Detection thresholds for fault sensitive residuals (l ||rm|| ,l rm ,l r) through the probability of false alarm, and window length L in the case of GLR-based detection.

Mitigation threshold in standard deviations, n σ , for the sensor output EKFs.

Low-pass gyroscope bias estimation time constant, τ LP , and rotation detection threshold through the probability of false alarm.

In the next chapter we will look at some fault tolerant developments of the attitude lter itself.

Introduction

Motivation and principles

No attitude estimation can be done without the attitude estimation lter itself. By using the measurement consolidation strategy from Chapter 5, most measurement errors should have been handled when it comes down to the attitude ltering. The attitude lter uses the consolidated measurements a c (acceleration), m c (magnetic eld), and the consolidated gyroscope bias b ω,c . Despite consolidation, the fault rejection is not perfect, and measurement errors may slip through.

On the other hand, if the measurement consolidation rejects measurements instead of using the estimated sensor output, the attitude lter risks being partially without measurements for long periods.

The risk of undetected faults and prolonged unobservability motivates the development of fault tolerant mechanisms in the attitude lter itself. As mentioned in Section 2.4, these mechanisms mainly consider disturbance decoupling and gyroscope bias saturation. The former since it allows to isolate the impact of a disturbance, and the latter since a completely random walk bias model is not physically reasonable.

To this end, we recall the open questions from Section 2.4.3 concerning the attitude ltering:

• Is it possible to develop a Kalman lter that provides the same or better decoupling than the one found in some complementary lters?

To answer this question, we will look into dierent ways of assuring a decoupling while maintaining a consistent covariance estimation. The principal challenge being the Kalman gain which is calculated from the state covariance and thus implies couplings between all states (attitude and bias).

• How to relate the saturation of a bias to the consistency of the bias estimation in the case of a

Kalman lter based estimator?

This point is important for Kalman lter based estimators since enduring periods of unobservability can lead to lter destabilisation. An attempt to answer this question is to look into stochastic linearisation of the saturated bias model. Simply put, if the covariance of the bias is large, it will grow much slower than if it is small. If the covariance is large in comparison to the bias saturation value, the probability of a growing bias is lower since the saturation function will pull the bias back towards the saturation limit.

These two questions that we try to answer in this chapter make up the fault tolerant mechanisms of the attitude lter itself. The rst question is important since the answer to it allows to isolate the impact of a faulty measurement on the state. The second point is important since it safeguards the consistency of the lter in the absence of measurement, in our case most likely due to rejected erroneous measurements.

Choice of baseline attitude lters

For the baseline attitude lter, one is required which provides an estimation of the attitude and the gyroscope bias as well as as estimation of the associated covariance.

In this chapter we shall denote the state and its covariance

x (q, b ω ), P P q,4×4 P q,bω,4×3 P bω,q,3×4 P bω,3×3 (6.1)

Since we require an estimation of the state covariance and that the covariance is as consistent as possible, we choose to focus on Kalman-based estimators. Our rst chosen contender for attitude ltering is the simple discrete-time EKF with quaternion normalisation since it has proven to work very well in practice despite the lack of any formal convergence proof.

The standard EKF for quaternion and gyroscope bias estimation is based on a discretised version of the synthesised model (2.14).

The second contender is the Invariant Extended Kalman lter (IEKF) which is an attitude estimator using multiplicative attitude corrections instead of additive ones. The multiplicative EKF has in general a higher accuracy than the additive EKF in steady state since the linearisation used does not depend on the attitude itself but rather it is always linearised around zero attitude error, implying a small error angles assumption. The IEKF, posed as a continuous time estimator, is based on the continuous synthesised model (2.14).

See Appendix C for details on the mechanics of the EKF, the IEKF will be detailed herein.

Chapter structure

This chapter is divided into three parts:

• The rst part handles the disturbance decoupling problem. Two solutions are presented:

1. Decoupling through partial quaternion corrections. The attitude corrections are separated into an inclination correction depending only on the accelerometer, and a heading correction depending only on the magnetometer. The corrections are then applied to the predicted attitude. Bias corrections are also derived from the error dynamics of the quaternion corrections.

2. Decoupling through pseudo-measurement. The magnetometer measurement is transformed using the estimated gravity to only retain information perpendicular to gravity.

The rst solution is based on a discrete time attitude estimation model. The attitude corrections are calculated as multiplicative quaternions corrections. The second solution is independent of the attitude estimation model since it is only a measurement transformation and not an attitude correction.

• The second part discusses the stochastic linearisation method for a saturated bias model. The propagation of the covariance is shown to take the form of an equivalent linear form whose gain is calculated from the current covariance and the saturation limit.

• The third part presents two dierent contenders for fault tolerant attitude lters. One is based on the EKF formalism and uses the partial quaternion correction to decouple the attitude. Another one is based on the IEKF formalism and employs disturbance decoupling through pseudo measurements. Both lters integrate the saturated bias model using the stochastic linearisation for covariance propagation.

Disturbance decoupling strategies for attitude estimation Kalman lters

The general objective of the disturbance decoupling is to isolate the impact of a disturbance of a certain sensor to a part of the estimated state where it nominally provides the most information. In our practical context, the problem is to limit the impact of acceleration disturbances to estimation of the roll/pitch, or inclination, and the impact of magnetic disturbances to the estimated heading or yaw.

The disturbance decoupling takes place at the attitude correction stage of the attitude Kalman lter. Depending on the type of Kalman lter employed, the correction of the state is done in one of several ways:

• Direct use of the measurement for additive correction. The measurement y = h (x) + w y is used to calculate the innovation ν = y -h(x + ), using the predicted state x+ . For a discretised lter, the state and its covariance are then corrected additively as

x = x+ + Kν P = (I -K H) P + (I -K H) T + K W y K T
Alternatively for a continuous time Kalman lter:

ẋ = ẋ+ + Kν Ṗ = Ṗ + -P H T W -1 y H P
where Ṗ + is the term in Ṗ related to prediction.

To achieve decoupling in this case, the measurement itself must be decoupled in the sense that it should not provide any information on the states it is not supposed to correct.

• Construction of a pseudo measurement. Instead of using the measurement directly in the correction, a pseudo measurement can be used to construct the innovation directly as a function of the measurement y m , the predicted state x + , and some reference parameters {c ref }.

ν = g ν y m , x + , {c ref } (6.2)
By linearising the innovation with respect to the state and the measurement, the measurement matrix H and the approximate innovation covariance S can be calculated:

H = ∂g ν ∂ x+ , S = HP + H T + ∂g ν ∂y m W y ∂g ν ∂y m T W * y (6.3)

Use of a pseudo measurement for additive correction

The Kalman gain is then calculated as usual and the correction is applied using the pseudo measurement directly.

x = x+ + Kν, K = P H S -1

The covariance correction in this case is calculated with Joseph's form:

P = (I -K H) P + (I -K H) T + K W * y K T (6.4)
And for a continuous time lter

ẋ = ẋ+ + Kν, K = P H T W * -1 y Ṗ = Ṗ + -P H T W -1 y H P

Use of a pseudo measurement for multiplicative correction

The correction of the estimated quaternion can be done in a multiplicative way. In that case, for a continuous time lter we have

∆ ẋ = Ω (K q ν) q+ K b ν
and the covariance evolution due to the correction becomes

Ṗ = Ṗ + -P H T W * y -1 H P
Using a pseudo measurement for ν instead of a classical dierence allows to obtain desired decoupling properties, on condition that the resulting function has continuous rst order derivatives.

• Direct state correction. The state correction is calculated as a nonlinear function of the measurement y m , the predicted state x + , and some predened gains G. For a discrete lter we have:

x = g x y m , x+ , G (6.5)
In this case the covariance correction is calculated by linearising the correction function directly with respect to the state and the measurement:

P = ∂g x ∂ x+ P ∂g x ∂ x+ T + ∂g x ∂y m W y ∂g x ∂y m T (6.6)
By identication with the classical Joseph's form (6.4), we can see that the equivalent Kalman gain K and the measurement matrix H can be found by identifying:

I -K H = ∂g x ∂ x+ K = ∂g x ∂y m
Note that in this case the correction gain K is not optimal and the covariance has no guarantee of converging to a minimum.

From a decoupling point of view, this is an attractive way of correcting the state since g x can be a very nonlinear function and the correction gains do not depend on the linearisation of g x .

A Kalman lter using this kind of correction should simply be called a nonlinear Kalman lter (NL-KF) since it deviates quite heavily from the Kalman lter principle.

In this section we investigate two dierent decoupling strategies. The rst one is built on direct state corrections for the accelerometer and magnetometer respectively. This allows to decouple the inclination from magnetic disturbances, but also the heading from acceleration disturbances.

Decoupled inclination correction

The relation 6.7 can be written using rotation matrices as

R (q) = R ψ ,e 3 R α,u ⇔ R (q) T = R T u, αR T e 3 ,ψ
Clearly the vector e 3 is invariant under rotation about itself, we thus have

R (q) T e 3 = R T u, α R T ψ ,e 3 e 3 e 3 = R T u, αe 3
We can nd the the inclination axis as the unitary body xed axis perpendicular to gravity and invariant of rotation around e 3 , i.e. we need to nd a vector û that satises the following conditions:

ûT e 3 = 0 ûT R(q) T e 3 = 0

ûT û = 1 (6.8)
The rst two conditions requires the vector to lie in the inertial horizontal plane and be perpendicular to the estimated gravity. We can therefore get a solution for û as

û Ker e T 3 R(q) e T 3 = (û 1 , û2 , 0) T (6.9)
The problem of calculating the inclination axis in this manner is that (6.9) is badly conditioned for small estimated inclination angles (R( q)e 3 ≈ e 3 ). This implies that at small estimated inclination angles, û is very sensitive.

If the real inclination is zero and the estimated inclination is close to zero, this is not a problem since û the error angle subsequently calculated will be small and at most some chattering can be experienced. In the opposite case, when the real attitude is non-zero and the estimated angle close to zero, an erroneous û can lead to a large angular error along the wrong axis. As a consequence, the estimated inclination might not converge to the real one which can result in a static error on the inclination estimate. A potential solution to this problem could be to use the a T m instead of e T 3 R(q). This represents using a measurement of the inclination axis of the actual attitude instead of assuming that the estimated inclination axis and the true inclination axis are parallel.

Assuming that the inclination axis has been found (i.e. û = u), we go on to calculate the inclination error angle δα. Following Rodrigues' formula R α, û = I +sin α û× +(1 -cos α) û2 × , we nd the estimated inclination by seeing that

R(q) = R ψ,e 3 R α,û ⇒ R(q) T = R T α,û R T ψ,e 3
therefore the inclination angle can be found through the relations cos α = e T 3 R(q) T e 3 sin α = -ûT e 3 × R(q) T e 3 .

(6.10) Now consider the inclination error measurement that we dene by:

y m R α, û a m ||a m || (6.11)
Note that we here use the measurement notation a m for generality instead of the consolidated measurement a c , this changes nothing in the derivation of the decoupling.

We can see that this pseudo measurement indicates the inclination angle error when no acceleration disturbances are present (a m = -g R(q) T e 3 ). Indeed in that case we have:

y m = -R α, û R(q) T α, u R(q) T ψ ,e 3 e 3 = -R α-α,û e 3 , if u = û
Remark: As mentioned earlier regarding the estimated inclination axis û, the determination of this axis is very sensitive when the estimated inclination is zero and we could instead use the accelerometer measurement instead. It is worth noting that this subtle dierence turns the pseudo measurement y m into a transformed measurement of the inclination angle error, eectively making it substantially more sensitive to acceleration perturbations and noise.

Let us pose the error angle (or innovation) δα = α m -α. From (6.10), and since y m = -R δα,u e 3 , we can see that: We can now calculate the error angle δα α m -α through cos δα = -e T 3 y m sin δα = ûT (e 3 × y m ) .

(6.12) A correction quaternion can then be calculated using a gain k α ∈ [0, 1], the error angle δα, and the inclination axis û as δq α = cos ∆α û sin ∆α , ∆α = k α δα/2 (6.13)

A corrected quaternion where only the inclination component is corrected is achieved by right-multiplying with the correction quaternion as qc = q ⊗ δq α = q ψ ⊗ q α ⊗ δq α = q ψ ⊗ q α-kαδα

(6.14)
Obviously, this is exact only if û = u. Using the error angle δα and the inclination axis û we can also correct the gyroscope bias estimation as

b ωx, k+1 = b + ωx, k+1 + k bα ûδα k+1 (6.15)
The z-axis bias estimation is not corrected since û3 ≡ 0.

With the gain k α xed, k bα is xed by investigating the dynamics around the equilibrium (φ, θ, ψ) ≈ (0, 0, ψ 0 ). This study is is carried out at continuous time for the sake of simplicity. At this point the kinematics of the euler angles coincide with those of the body angles, hence φ ≈ p, θ ≈ q, and ψ ≈ r.

For the inclination correction, we are interested in the dynamics for φ and θ. Since the total inclination is a combination of roll and pitch, the inclination rate is also a combination of p and q as a function of û. For example, if û = (1, 0, 0), the inclination is purely about the roll axis (in which case δα δφ), and if û = (0, 1, 0), the inclination is purely about the pitch axis (in which case δα δθ).

This means that the gain calculation will be identical for k p û1 k bα and k q û2 k bα . Without loss of generality, assume that û = (1, 0, 0), meaning a pure roll error is present. To calculate k bα we rst set up the kinematics

α = p = p m -b ω,x α = p m -bω,x + k α (α -α) ḃω,x = 0 ḃω,x = k bα (α -α)
.

By dening the errors 1 = α -α and 2 = b ω,x -bω,x we get the error system

˙ 1 2 = -k α -1 -k bα 0 A 1 2
The corresponding characteristic polynomial is of second order

|sI -A| = (s + k α ) s -k bα = s 2 + 2ζω 0 + ω 2 0 ⇒ k bα = -ω 2 0 k α = 2ζω 0 (6.16)
We xed the damping coecient to ζ = 0.7 and the pulsation ω 0 = 3/T rep , where T rep is the chosen response time. For a discrete implementation of the lter, the gains must be multiplied by the time step.

Decoupled heading correction

We have seen how to correct the quaternion inclination without changing the yaw angle. We now come to the inverse, i.e. how to correct the heading without modifying the estimated inclination.

The main idea here is to nd a heading correction that does not impact the inclination estimation, using only the estimated attitude and the magnetometer measurement. Furthermore we require the correction to be valid for all heading errors, i.e. δψ ∈ [-π, π]. We start by nding the planar magnetic error. We dene the two orthogonal pseudo measurements y 1 and y 2 as

y 1 = -m T ref (R(q) m m × e 3 ) y 2 = (m ref × e 3 ) T (R(q) m m × e 3 )
y (y 1 , y 2 ) . (6.17) These pseudo measurements have been dened in order to capture the heading error. To see that, consider an inclination error near zero. In that case, we have:

R (q) m ref = R (q) R (q) m ref = R ψ-ψ m ref Then a simple calculation shows that y 1 = m 2 ref,1 + m 2 ref,2 sin ψ -ψ y 2 = m 2 ref,1 + m 2 ref,2 cos ψ -ψ
Let us pose the heading error δψ = ψ m -ψ, we then have

     sin δψ = y 1 ||y|| cos δψ = y 2 ||y|| .
The heading error can then be computed as δψ = atan2 (y1, y2) .

Using the pseudo measurement, a heading correction quaternion is now formed as δq ψ = cos ∆ψ e 3 sin ∆ψ , ∆ψ = k ψ δψ/2 (6.18)

We then correct the estimated quaternion by left-multiplying it with the correction quaternion in order to correct only the heading component, i.e.

qc = δq ψ ⊗ q = δq ψ ⊗ q ψ ⊗ q θ ⊗ q φ = δq ψ+k ψ δψ ⊗ q θ ⊗ q φ (6.19)
Similar to the gyroscope biases relative to the inclination, the z-axis gyroscope bias b r can be corrected as follows:

bω, z = b+ ω, z + k b ψ δψ (6.20)

The correction gains k ψ and k b ψ are xed to achieve a given response time and damping.

Covariance correction

Since the partial quaternion decoupling procedure described in this section is a direct state correction, the correction of the covariance is done as per 6.6. The direct correction functions g α (a m , q+ , b+ ω , k α , k bα for the inclination, and g ψ (m m , q+ , b+ ω , k ψ , k b ψ ) for the heading are identied as

q bω = q+ ⊗ δq û, kαδα b+ ω + ûk bα δα g α a m , q) + , b+ ω , k α , k bα (6.21) and q bω = q+ ⊗ δq û, k ψ δψ b+ ω + e 3 k b ψ δψ g ψ m m , q) + , b+ ω , k ψ , k b ψ (6.22)
Once again it is worth noting that the corrections are not optimal. The gains are used to x a response time and not to minimize the covariance.

Conclusion

To conclude on the decoupling through partial quaternion corrections, some advantages and disadvantages can be listed:

+ Disturbances on the magnetometer will not impact the estimated inclination and disturbances on the accelerometer will not impact the estimated rotation around the inertial vertical axis.

+ The decoupling of magnetic disturbances will work even if the inclination is badly estimated thanks to the structural decoupling. In fact, the rotation axes used to correct the prediction, i.e.

e 3 and û are orthogonal.

-The actual inclination axis, u, might be very dierent from the estimated one, û, for large inclination errors. As a matter of fact, if û = u, then the pseudo measurement y m is not a measurement of the actual inclination error, and the reduction of the error cannot be assured.

This being said, the zero inclination can always be converged upon thanks to the inclination axis ambiguity. Precisely, u is not well dened, therefore we can always assume that û = u is an inclination axis that will perform a rotation back to zero estimated inclination.

-The state covariance is not corrected with optimal gains so it is not guaranteed to decrease.

-At zero inclination, the inclination axis u is very sensitive, which can lead to random walk of the estimated inclination around zero. A solution to this could be to maintain the previous inclination axis as soon as the norm of R(q) e 3 approaches zero.

Disturbance decoupling through pseudo measurements

The decoupling through partial quaternion corrections represents an elaborated way to decouple the inclination and heading axes. The nature of the correction guarantees that the quaternion remains unitary, however as mentioned above, the associated covariance is not guaranteed to decrease.

In practical applications, the decoupling of acceleration disturbances from impacting the heading is less important than the decoupling of magnetic disturbances from the inclination. A qualied guess is that this is due to the nature of the disturbances. While accelerations are usually controlled or at least caused by the platform itself and are usually transient, magnetic disturbances tend to appear randomly and unpredictably. Due to this, most authors are content with decoupling magnetic disturbances from the inclination estimation, however the decoupling for Kalman lter based estimator remains obscure in the literature.

Denition of the pseudo measurement

The fundamental concept of magnetic disturbance decoupling is that the innovation, i.e. the magnetic error, is in a plane perpendicular to the inclination. If this is not the case, the innovation eectively contains information on the inclination and using it for correcting the estimated attitude implies correcting the estimated inclination.

Decoupling using the accelerometer A rudimentary way to assure that this is not the case is to project the magnetometer measurement onto the plane perpendicular to the accelerometer measurement, since the accelerometer under nominal conditions indicate gravity, i.e:

m m,⊥ = a m × m m
This was proposed in [START_REF] Martin | Invariant observers for attitude and heading estimation from low-cost inertial and magnetic sensors[END_REF] and reused in [START_REF] Hua | Attitude estimation for accelerated vehicles using GPS/INS measurements[END_REF] and [START_REF] Hua | Nonlinear attitude estimation with measurement decoupling and anti-windup gyro-bias compensation[END_REF], in the case of constant gain observers.

Indeed, in the ideal case where no noise or perturbations are present, a m = R(q) T a * ref and m m = R(q) T m * ref , and we have:

m m,⊥ = R(q) T a * ref × R(q) T m * ref = R T a * ref × m * ref .
Under an assumption of small errors, i.e. (δϕ, δθ, δψ) ≈ (0, 0, 0), we have (I -R R T ) ≈ (δϕ, δθ, δψ) × .

Recalling that a ref = (0, 0, -g) T , a simple calculation gives the innovation

ν = -gR(q) T    -m ref,1 δψ -m ref,2 δψ m ref,1 δϕ + m ref,2 δθ   
Clearly we cannot say anything about the innovation's decoupling properties for all estimated attitudes (global decoupling) since it contains R(q) T . However if φ ≈ 0 and θ ≈ 0, we only have an estimated rotation about the inertial vertical and the innovation takes the form:

ν = -g    -m ref,1 cos ψ δψ -m ref,2 sin ψ δψ m ref,1 sin ψ δψ -m ref,2 cos ψ δψ m ref,1 δϕ + m ref,2 δθ   
In this case the innovation does not contain the estimated inclination ( φ), thus if it is used in a Kalman lter, the corresponding gains would be (at least close to) zero.

To quickly summarise, the projection of the magnetometer measurement in this manner for decoupling is insucient in a Kalman lter framework, notably because:

-The decoupling is only locally eective around zero inclination -The innovation is directly sensitive to perturbations on the accelerometer.

-The noises of the accelerometer and magnetometer add up, as do their covariances, leading to a slow convergence of the heading in the nominal fault-free case.

Decoupling using estimated gravity To remedy the problems appearing when simply projecting the magnetometer onto the measured gravity indicated by the accelerometer, we propose to use the following pseudo measurement:

ν m,⊥ = T R RT (e 3 × m ref ) -RT e 3 × m m
This is indeed a pseudo measurement since it combines estimated states and measurements. A closer look shows that we can rewrite it as:

ν m,⊥ = T R RT (e 3 × m ref ) -RT e 3 × (R (m ref + ∆m) + w m ) = T     e 3 ×     I -RR T m ref Magnetic error -R R T ∆m Disturbance -R w m Noise        
Since the magnetic error in the inertial frame is projected onto the horizontal plane (via (e 3 ) × ), this pseudo measurement does not contain any information about the inclination of the body. Furthermore, the magnetic disturbance is also limited to the plane perpendicular to gravity, meaning that the inclination will not be impacted if ∆m = 0. Additionally, since this projection nullies the third vector component for both the magnetic error and the noise term, the matrix T is used to extract only the non-null part of the pseudo measurement to avoid numerical issues, i.e.

T = 1 0 0 0 1 0

To conclude, some pros and cons for using this decoupling method can be listed: + Magnetic disturbances will not impact the inclination estimation thanks to the projection onto the inertial horizontal plane.

+ The pseudo measurement uses the ltered inclination and not the accelerometer measurement directly. This reduces the inuence of transient accelerations and noise on the heading estimation as compared to the transformed measurement (6.2.2).

From a practical point of view, this decoupling does not require the accelerometer measurement to be available and synchronised with the magnetometer measurement, making it simple to implement in any existing attitude lter.

-The linearisation of the pseudo measurement with respect to the state depends on the measurement, meaning that the correction gain is directly sensitive to noise and disturbances. It is unclear what the impacts on the convergence of the lter due to this fact are.

-This decoupling method is only one-way, that is only magnetic disturbances will not impact the inclination estimation. However, acceleration disturbances, if not rejected, can impact the heading estimation.

Conclusion

Two dierent disturbance decoupling strategies have been presented in this section. One based on partial quaternion corrections and one built on the creation of a pseudo measurement using the estimated state and the magnetic measurement.

Both presented methods have their pros and cons, and it it not obvious which one will work better under which conditions.

The rst method, built on partial quaternion corrections employs a disturbance decoupling strategy for both the accelerometer (inclination estimation) and the magnetometer (heading estimation).

The advantages of this method is that the gains for inclination and heading estimation are easily tuned for a desired response time. However they are not necessarily optimal and no guarantee of a decreasing covariance can be given.

Another advantage is that the decoupling is inherent to the structure of the correction, since the axes used for correcting the inclination and the heading are by construction orthogonal. This structural decoupling is also a weakness of the method from a convergence point of view, since the only inclination which will unambiguously be converged upon is the zero inclination.

Luckily enough, from a drone point of view, the zero inclination is a very common inclination, implying that this kind of decoupling might be useful in certain cases.

The second method, theoretically much simpler builds on the premise that in order to limit the impact on the inclination estimation of a magnetic disturbance, the magnetic error must be dened in a plane perpendicular to gravity.

The advantage of this method is that it marries fairly well with optimal ltering methods since it takes the form of an innovation signal which can be easily linearised.

The disadvantage of this decoupling method is that since it is intended to work with a Kalman lter, no formal guarantee can be given regarding the decoupling since the covariance used to calculate the correction gain contains cross terms between the inclination and the heading.

Bias saturation model with stochastic linearisation

The random walk process used for the gyroscope bias in 2.10 allows the bias estimation to assume any value. In practice we know that this is not the case and that there exists some maximum bias which will not be exceeded under normal conditions. A more realistic bias model is therefore:

ḃω = - 1 τ b (b ω -sat (b ω , b max )) + v bω (6.23)
This model ensures that the bias cannot grow far past b max . Note that in this section, a scalar bias is considered for simplicity. This is a decent hypothesis for IMU bias since the biases on the three axes can be considered independent scalars. A linearisation of the model about the estimated bias is also used by the EKF in order to propagate the covariance P bω . But the linearisation is zero as long as | bω | ≤ b max and the covariance will grow indenitely over time periods when the bias is weakly observable. Typically during measurement exclusion due to the rejection of an external perturbation.

Covariance propagation through stochastic linearisation

A way of handling this problem is to use a stochastic linearisation technique. The key idea is to replace the non-linearity by its best linear approximation from a stochastic point of view, i.e. the one that minimizes the RMS error:

N eq arg min

N E [sat(b ω ) -N b ω ] 2 (6.24)
The equivalent stochastic linearisation is written as

ḃω = - 1 τ b (1 -N eq ) b ω + v bω .
For this linearised dynamics, the covariance propagation obeys the following Lyapunov equation:

Ṗbω = - 2 τ b (1 -N eq ) P bω + V bω (6.25)
This equation is used to propagate the covariance P bω , while equation (6.23) is still used to propagate the mean bω .

In order to solve the optimization problem related to the equivalent gain denition (6.24) its necessary to specify the stochastic characteristics of b ω . This optimization is classically solved assuming that b ω ∼ N (0, P bω ). In such a case the equivalent gain is expressed as a function of the standard deviation σ bω (see for example [START_REF] Ching | Quasilinear control theory: An overview[END_REF]):

N eq = erf b max σ bω √ 2 (6.26)
With this expression, the covariance propagation (equation (6.25)) becomes nonlinear since N eq depends on σ bω = P bω . When P bω is small, it is increased by the propagation since N eq ≈ 1 and thus Ṗbω ≈ V bω . On the contrary, when P bω is large, the propagation decreases it since N eq ≈ 0. This behaviour is similar to the one achieved with an unscented transformation.

If this model is used in conjunction with a Kalman lter, another advantage of this bias model is to eliminate lter instabilities in case of unobservability during extended periods. What classically happens in a Kalman lter during periods of unobservability is that the covariance increases due to a lack of corrections. Once the measurement is available anew, the correction gain becomes very large due to the large covariance, potentially destabilising the lter.

By using this saturated bias model, the covariance growth is limited by the saturation, limiting the risk of destabilisation following long periods without measurements. As a matter of fact, if the real bias is indeed within the saturation limits, the estimated errors are bounded, with the maximal bias covariance being equal to τ b V bω /2.

A discrete version of equation ( 6.23) can be found with Euler's method, using an integration step ∆t:

b

ω,t+1 = b ω,k - 1 τ b (b ω,t -sat (b ω,t , b max )) ∆t + v bω,t (6.27)
The stochastic linearisation gives the equivalent gain N eq as found previously. The equation used to propagate the covariance matrix is now written as:

P b ω,t+1 = F Neq P bω,t F T Neq + ∆t V bω F Neq = 1 - ∆t τ b (1 -N eq ) (6.28) with N eq = erf b max /(σ b ω,k √ 2) .
How to tune b max and τ b ?

The tuning of the parameters b max and τ b is unfortunately not very intuitive. The rst order desaturation time constant τ b can be tuned to match the desired angular convergence of the attitude lter.

This would essentially mean that the attitude lter will not suer from the slow dynamics of the bias estimation when converging after a measurement disturbance. Alternatively it can be tuned according to the trust in the saturation limit b max . If one is sure of the saturation limit, τ b should be small, and inversely if one is sure of b max , τ b should be small.

Advantages of using a saturated bias model

To summarise, the gains of using a saturated bias model for the gyroscope bias instead of an unsaturated random walk are the following:

Limitation of the bias covariance and the growth rate of the attitude covariance in case of unobservability.

Robustness of the bias estimation to attitude measurement perturbations. For an attitude lter, measurement disturbances will impact not only the angles but also the bias estimation. A saturated gyroscope bias model can limit the impact on the gyroscope bias and in doing so, improve the convergence of the lter and the drift of the attitude estimation.

Potential intuitive detection of gyroscope anomalies with a properly tuned b max . If attitude measurement disturbances can be detected and isolated, the bias saturation can be used to used to determine whether the gyroscope bias becomes unreasonably large, likely due to a malfunctioning of the sensor itself. This point is however outside the scope of this thesis.

An illustration of the eect of using a saturated bias model is shown in Figures 6.1 and 6.2.

The simulated scenario corresponds to a drone in steady ight entering a zone of magnetic disturbance lasting 80s. The magnetic disturbance is considered detected and rejected completely, leading to a loss of the observability of the heading, and by consequence the z-axis gyroscope bias.

The contending lters are two EKF based the same model apart from the bias model used:

Model synthesis, "Standard EKF"

       q t+1 = I 4 + 1 2 Ω(ω)∆t q t , with ω = ω m -b ω -v ω q t+1 = q t+1 /||q t+1 || b ω,t+1 = b ω,t + v bω √ ∆t a m = R(q) T a ref + w a m m = R(q) T m ref + w m
Model synthesis, "EKF with bias saturation" The bias drift rate in this simulation is at the limit of what can be assumed as reasonable for low-cost sensors. However in this case the idea is to push the Kalman to its extreme behaviour to showcase the use of a saturated bias model in a nominal worst case scenario. Note also that when low-cost sensors are used in drone applications, the manufacturer specications regarding noise levels and drift rates are severely underestimated. Comments: The limited growth of the state covariance together with the saturation dynamics limit the bias instabilities of the lter when the observability is regained. The eect is also seen on the angular convergence which is signicantly improved in terms of oscillations and overshoot.

           q t+1 = I 4 + 1 2 Ω(ω)∆t q t , with ω = ω m -b ω -v ω q t+1 = q t+1 /||q t+1 || b ω,t+1 = b ω,k - 1 τ b (b ω,k -sat (b ω,k , b max )) ∆t + v bω √ ∆t a m = R(q) T a ref + w a m m = R(q) T m ref + w m

Conclusion

The saturated bias model can add signicant robustness and consistency to a Kalman lter with a negligible computation burden. The stochastic linearisation presented herein allow for direct implementation in any linearisation based lter. In the case of an Unscented Kalman lter being used, the stochastic linearisation becomes superuous and the nonlinear model will be used directly, retaining the same properties in terms of covariance consistency and stability.

Development of two fault tolerant attitude lters

In this section the previously developed methods for decoupling and bias saturation are combined to form two dierent attitude estimators.

The rst one, named NL-KF, is based on a discretised version of the continuous time model (2.14), with a modied bias prediction model (6.27) to account for bias saturation. The correction stage consists of the partial quaternion corrections of Section 6.2.1 where separate correction quaternions are calculated for the inclination correction and the heading correction using constant gains. This supposedly structurally decouples the lter in terms of disturbances, meaning that an acceleration disturbance will not impact the heading estimation and a magnetic disturbance will not impact the inclination estimation.

The second estimator, called DEC-IEKF for decoupled IEKF, is based on the continuous model (2.14), modied with the saturated bias model (6.23). The state correction uses the baseline accelerometer model for inclination correction and the decoupled pseudo measurement from Section 6.2.2 for heading correction. The pseudo measurement decoupling makes this IEKF more robust to magnetic disturbances and the saturated bias model improves the behaviour in the case of prolonged unobservability.

We see that one lter is posed in discrete time and one in continuous time. In practical implementation, the discrete lter can be implemented as is, however the continuous time lter must be discretised before implementation, using any standard numerical integration method.

This dierence has very little impact on the attitude ans bias estimation apart from the enforcement of the quaternion norm constraint.

In the discrete time NL-KF, the quaternion is normalised after the prediction step, and the covariance is corrected accordingly. In the continuous DEC-IEKF, the standard trick to enforce the norm constraint by adding an additional term -k ||q|| (||q|| -1) in the propagation of q and then discretise the ensemble, enforcing the norm constraint implicitly. Alternatively, one can discretise the lter and then normalise the quaternion a posteriori after each integration step and correct the covariance accordingly.

A nonlinear Kalman lter (NL-KF) with measurement decoupling and bias saturation

The NL-KF uses gyroscope measurements (ω m ), accelerometer measurements (a m ), and magnetometer measurement (m m ), to produce an attitude quaternion ( q) and an estimation of the gyroscope bias ( b). In this section we assume, from a fault tolerant point of view, that the input gyroscope bias, as well the accelerometer and magnetometer measurements have passed some sort of preliminary qualication, rendering the consolidated data ( bω,c , a c , and a c ) with associated status booleans a_status and m_status. An overview of the NL-KF is seen in Figure 6.3. As presented in the above, the NL-KF is divided into a a prediction step, an inclination correction step, and a heading correction step. The prediction step has nothing out of the ordinary compared to a classical EKF, apart from the model of the gyroscope bias being replaced by a saturated one. At the end of the prediction step, the quaternion is normalised, and a correction is applied to the lter covariance to account for this. The correction steps are essentially multiplicative quaternion corrections and additive bias corrections with the ad-hoc covariance correction previously described.

To start o, an algorithmic overview of the prediction step is given below.

Nonlinear decoupled Kalman lter (NL-KF)

Prediction

x+ = q+ b+ ω, =   I + 1 2 Ω(ω m -bω )∆t q bω -∆t τ b bω -sat bω , b max   =   f q q, bω , ω m f bω q, bω , ω m   P + = FPF T + Q k where F =      I 4 + Ω ω m -bω ∆t 1 2 -v T ŝ I 3 + v× 0 3×4 I 3 -∆t τ b (I 3 -N eq )      , q ŝ v Q =     ∆t ∂fq ∂ωm V ω ∂fq ∂ωm T ∆t 0 0 V bω ∆t     Normalisation q+ = q+ /||q + ||
Measurement: y q = q+,T q+ -1 + w q = h q (x, w q ) , w q ∼ ( , W )

Covariance correction: P = I 7 -K H T P with: K = P H T H P H T + W q -1 , and H = ∂h q ∂ x Note that we have here dened the equivalent stochastic gain N eq as a matrix instead of a scalar as in (6.26) in Section 6.3. The extension to a matrix gain is straightforward since the estimated standard deviation of b ω if taken as the square root of the diagonal elements of P corresponding to the bias estimation, i.e. P bω in (6.1).

The state correction is performed on condition of the measurement status booleans a_status and m_status. These were introduced in Chapter 5 to indicate whether a measurement can be used for attitude update or not. The state and covariance corrections have been detailed in Section 6. , P α = P +

Heading correction :

If m_status = 1:

q bω, = δq ψ,e 3 ⊗ q α b α ω, + e 3 k ψ δψ = g ψ (q α , m m )
(6.17)-(6.20)

P = ∂g ψ ∂ x P α ∂g ψ ∂ x T + ∂g ψ ∂m m W m ∂g ψ ∂m m T Else: q bω, = qα bα ω,
, P = P α

Remarks

We can make a few observations regarding the NL-KF which will be illustrated at the end of this chapter.

The decoupling between the inclination estimation and the heading estimation is weaker at larger inclination since the hypotheses of a decoupled bias estimation and the heading pseudo measurement no longer apply. However the corrections using the partial quaternion correction will still only impact the intended angles, i.e. the inclination for δq α and the heading for δq ψ . This means that the short term decoupling is assured at all angles but the long term is not since the bias estimation is coupled with the estimation of all angles through the prediction step.

Near exponential convergence of inclination errors at zero inclination. Since the convergence of the inclination is invariant of the heading error, the estimated inclination will converge at zero inclination according to the tuning of the gains k α , and k bα Asymptotic convergence of the heading error at zero inclination error.

Steady state error due to erroneous inclination axis estimation

The NL-KF as presented here and in [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF] has one major drawback. Simulations have shown that the fact that the fact that the lter uses the current estimated attitude to nd the inclination axis and then the error angle about this axis makes it prone to static attitude errors. The lter does not always converge properly in both roll and pitch due to the estimated inclination axis being "locked" along an axis and only converging the error with respect to that axis. This is especially a problem at zero estimated inclination, where the estimated inclination axis is very sensitive since the the problem (6.9) is badly conditioned.

To potentially remedy this problem we can instead determine the rotation axis as

       ûT e 3 = 0 ûT a m = 0 ûT û = 1 . (6.29)
This essentially mean that we use the measured vertical axis (a m ) instead of the estimated one (R( q)e 3 ).

If the estimated inclination is zero and the real inclination is non-zero, solving this problem will provide a non-ambiguous (but noisy!) solution to û.

The decoupling properties of the inclination correction described above still hold since the acceleration measurement is invariant of the heading. A disadvantage is that both the accelerometer-and the magnetometer measurements must be available at the same time, which is not necessarily the case in real time applications.

Convergence time at zero inclination

The fact that the inclination and heading estimations are structurally decoupled can be illustrated with a Monte-Carlo simulation batch. A Monte-Carlo simulation was carried out where additive attitude errors from 0 to 180 degrees were randomly distributed as an error in α, and ψ. The real attitude was kept as the zero attitude (α = ψ = 0 rad). The convergence time in α and ψ was then calculated at the 3τ threshold of the maximum error angle Since the inclination estimation does not depend on the yaw, the convergence time should be at max the one prescribed via the gain tuning. The heading estimation convergence as we have seen, depends on the correctness of the inclination. The maximum convergence for the heading should therefore be roughly the sum of the tuned inclination and heading estimation times.

For large initial errors, this assumption is weaker since the bias estimation dynamics have a larger impact on the convergence of the absolute error.

Here we have set the convergence times of the inclination and heading estimations to τ α = 5 s and τ ψ = 5 s respectively, with a damping of ζ = 0.7, this gives the gains k α = k ψ = 0.006 and k bα = k b ψ = 0.000018. We see that the convergence time for the inclination estimation is indeed at maximum around 5s, which is the prescribed value for τ α . The convergence of the heading estimation on the other hand is conditioned by the convergence of the inclination estimation, which is why we see convergence times up to around 10 s.

We see that the convergence in α is independent of the convergence in ψ since the convergence time for the inclination estimation is indeed at maximum around 5 s, which is the prescribed value for τ α . This is expected since the correction quaternion δq α, û. On the other hand the convergence in ψ assumes that α = α, thus its convergence time is at maximum τ α + τ ψ . This is however not true for large attitude errors since the bias estimates must also converge for the attitude estimate to converge asymptotically. In addition, the few outliers seen when the heading error approaches zero are due to the absolute inclination error being very large so that even at 3τ convergence, the assumption that α = α does not hold for the heading correction.

Conclusion

The fact that the correction gains do not depend on the covariance estimation in the NL-KF as is the case in a standard Kalman lter means that nothing can be claimed about the covariance in terms of optimality. It is simply a rough estimation of the accuracy and not locally minimized as in the case of an EKF. The debatable convergence properties and steady state error at attitudes dierent from zero, related to the computation of û is another factor that limits the utility of the presented lter.

Motivated by these points, we propose another decoupled lter in the next section focused on achieving a more accurate covariance estimation and a globally unambiguous convergence.

A decoupled Invariant Extended Kalman Filter with bias saturation (DEC-IEKF)

A further development of the EKF for attitude lter is the Multiplicative EKF (MEKF) [Markley, 2003] that takes the form of a continuous time lter using a 3-dimensional representation of the attitude covariance to achieve a fully non-singular attitude estimation. An issue with the MEKF is that the gains tend to vary a lot since they depend directly on the linearised rotation. A further reconstruction of the MEKF was proposed in [START_REF] Bonnabel | Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem[END_REF], named Generalized Multiplicative EKF (GMEKF), or invariant extended Kalman lter (IEKF), in order to achieve smoother gain-and attitude trajectories.

Here we use the IEKF formalism as the stepping stone for dening a multiplicative EKF with decoupling properties and bias saturation.

Model synthesis

The IEKF being multiplicative, we use the continuous models 2.14 and (6.23) for quaternion-and bias propagation. For the measurement models, we use the same one as the additive EKF for the acceleration. For the magnetic eld we use the pseudo measurement decoupling described in Section 6.2.2.

In order to synthesise the IEKF, some modication must be made to the model. Notably the noises entering the system (v ω and v bω ) are dened in the inertial frame.

The model synthesis is given below:

Decoupled IEKF with bias saturation, model synthesis

q = 1 2 Ω(ω)q, ω = ω m -b ω -R (q) T v ω ḃω = -1 τ b (b ω -sat (b ω , b max )) + R (q) T v bω a m = R(q) T a ref + w a ν m,⊥ = T R(q) R(q) T (e 3 × m ref ) -R(q) T e 3 × m m where m m = R(q) T m ref + w m (6.30)
Filter synthesis

The methodology used to dene an invariant lter is quite cumbersome. Some references to familiarise with the application are found in [START_REF] Barrau | Three examples of the stability properties of the invariant extended Kalman lter[END_REF][START_REF] Silva | The Right Invariant Nonlinear Complementary Filter for Low Cost Attitude and Heading Estimation of Platforms[END_REF][START_REF] Barczyk | Invariant observer design of attitude and heading reference system[END_REF][START_REF] Barrau | The invariant extended Kalman lter as a stable observer[END_REF].

The rst step is to search for transformations (of the state, input, and output) that remain unchanged under the system equations. Then from these transformation we deduce:

-The invariants of the system. Here we have found:

Îa = a ref , Îm = T m ref , Îω = R(q) ω m -bω .
η = q -1 ⊗ q bω -b ω
For details on the choice of these invariants, see e.g. [START_REF] Bonnabel | Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem[END_REF].

With our model, the invariant lter is written:

q ḃω =   1 2 Ω ω m -bω + R (q) T K q E q -1 τ b bω -sat bω , b max + R (q) T K b E   (6.32)
A key property of this equation is that its error dynamics depends only on the error η and the invariants (and the state noise v and the output noise w): , v, w (6.33) One can search for gains (which can depend of E and Î) that stabilize this this nonlinear error dynamics, or use the Kalman approach to tune the gains online in an optimal way. We here follow this later method.

η = F η, Î
The gain matrix K = (K q , K b ) T is calculated as a normal continuous time Kalman lter, i.e.

K = P C T W -1
where the error covariance P evolves according to

Ṗ = A P + P A T + D Q D T -P C T W -1 C P
The matrices C and A are found by setting up the error dynamics (6.33) and linearising around zero error. However, instead of using a fourth order representation of the attitude error ( q ⊗ q -1 ), we use a third order representation δγ, see e.g. [Markley, 2003], using the fact that when the attitude error is small, R T R ≈ I + (δγ) × . We nd the the matrices A, B, C, D through the linearised invariant error dynamics:

A -K C = ∂F ∂η , D = ∂F ∂v , -K C = ∂F ∂w , C = ∂E ∂η
This yields the matrices

A =   -Îω × -I 3×3 0 3×3 -1 τ b (I 3×3 -N eq )   C =     -Îa × 0 3×3 - Îm 0 × 0 3×3     B = diag (-I 3 , I 3 ) D = -I 6×6 . (6.34)
where we have used the stochastic linearisation gain N eq of the bias saturation.

Analysis for large errors

Filters based on the MEKF formalism are degraded when the attitude error is large since the linearisation assumption relying on small errors is far from true. For example, at a yaw error of π/2 rad, roll becomes pitch and pitch becomes negative roll. Furthermore, an attitude error of 180 degrees is only weakly observable and will converge very slowly, this is a characteristic shared with the standard EKF. Covariance majoration through multiple linearisation A potential remedy to improve the behaviour of the lter at large angular errors is to linearise the system around several error angles (not just zero error), calculate the propagated covariance for all linearisation points, and then calculate a majorant covariance matrix among the propagated ones.

For a small number of extra linearisation points, and given the limited dimension of the state covariance, this operation has a reasonable computational cost.

To illustrate the eect of this covariance majoration, the same simulated scenarios as shown in Figure 6.5 are replayed for the cases where several linearisation points are used. The linearisation points are given as an error in the yaw component of the attitude, that is to say the rotation matrix R(q) used in the computation of the invariants 6.31 is replaced by R(q)R(q δψ,e 3 ), where δψ is the linearisation error point in yaw.

Comments: The DEC-IEKF using a covariance majorations shows a faster convergence time than the DEC-IEKF linearised around zero although the dierence is very slight. We can also note that the dierence between using one supplementary linearisation point and using several seems to have a limited impact.

Comments: The DEC-IEKF using a covariance majorations shows a faster convergence time than the DEC-IEKF linearised around zero. The time gain is larger than for the error of 30 • , which is expected since the convergence time itself is longer.

Comments: The DEC-IEKF using a covariance majorations shows a faster convergence time than the DEC-IEKF linearised around zero. The trend of larger absolute time gain than for the smaller errors also persists.

Comments: The DEC-IEKF using a covariance majorations is signicantly faster than the DEC-IEKF linearised around zero. We also note that there is a slight dierence between the dierent covariance majorations. If more linearisation points are used, the convergence time improves. However the absolute dierence is negligible with respect to the gain of using two points instead of one. Improvement oered by the saturated bias model Interestingly, a faster angular convergence compared to the baseline IEKF is also achieved with the saturated bias model (see Figure 6.10). As explained in Section 6.3, the stochastic linearisation imitates the behaviour of an unscented transform, thereby "inheriting" certain characteristics of an UKF but at a vastly lower computational cost.

The main reason for this faster long term convergence, noting that the initial convergence is approximately equal in the two cases, is that the impact of the slow dynamics of the bias estimation is limited thanks to the saturation function and the equivalent stochastic linearisation.

Conclusion

Conclusively, we can say that there is an interest in using a majorated covariance through multiple linearisation. This conclusion might be trivial since this is basically what is done in an UKF, which is known to have a signicantly faster convergence than EKF-based lters.

Typical decoupling results

In order to showcase the decoupling properties of the presented lters, two scenarios are presented: 1. Accelerations and magnetic disturbances. This scenario aims to show the decoupling properties of the lter in a nominal case, i.e. when the attitude estimation error is small. This is the classical decoupling test and corresponds to nominal operation.

2. Magnetic disturbance with inclination error. This scenario illustrates the decoupling properties at large attitude errors. This property is important since if the disturbance decoupling is degraded at large attitude errors, the stability of the lter can be endangered.

The two lters are tuned to have roughly similar dynamic behaviour (response time) in steady state.

That is, the tuning parameters of the NL-KF, k α , k bα , k ψ , and k b ψ are tuned to the same response times found in steady state for the IEKF. The IEKF in turn is tuned by default with the theoretical measurement covariances W a , W m , V ω , and the bias stability covariance V bω is set to 0.01 • / √ s.

The sensors are simulated with a realistic noise level and the disturbances have a dynamic and magnitude that can be expected from physical phenomena (dynamic ight and y-by of a perturbed magnetic eld).

Scenario 1: Accelerations and magnetic disturbances

In this scenario, two temporally separated disturbances are simulated separated by a short disturbance free period. The idea is to show how the two lters react to disturbances on the accelerometer and magnetometer separately. In the ideal case, the accelerometer when disturbed, should only impact the inclination (φ and θ). Inversely, a disturbed magnetometer should only impact the heading (ψ). Both lters are fairly decoupled in both magnetic-to-inclination and acceleration-to-heading. When the accelerometer is disturbed (t ∈ [12, 38] s), the estimated yaw angle remains correct (apart from some minor transients). Inversely when the magnetometer is disturbed (t ∈ [50, 90] s), the inclination estimation remains correct.

The main reason for this is the availability of the two measurements (accelerometer and magnetometer).

The coupling eect from one state correction is thus counteracted by the correction of the other measurement. The NL-KF however clearly shows an overestimation of the covariance, which is expected as described ins Section 6.2.1.

Scenario 2: Magnetic disturbance with large inclination error

In this scenario, only the magnetometer is supposed to be subject to an enduring perturbation. The inclination estimation has articially been oset and the lters are prohibited from using the accelerometer during the entire simulation. The idea of the scenario is, although not operationally realistic, to

show the decoupling properties at large attitude errors. A secondary objective is to investigate the long-term stability of the lters.

Comments: The NL-KF handles the decoupling as well as in the nominal case. This is likely thanks to the structural decoupling through the orthogonalisation of the correction axis (recall û ⊥ e 3 ). In the case of the NL-KF, the correction gains do not depend on the covariance, and by extension, the cross coupling terms. Thus, the decoupling properties of the lter are invariant of the covariance. This is a feature shared with constant gain observers. A peculiar feature of the NL-KF is however the cross terms in the covariance leading to cross terms in the correction gain.

Conclusion

In this chapter we have developed some fault tolerant aspects of common attitude lters:

• A bias saturation model with a stochastic linearisation procedure.

• Two signicantly dierent disturbance decoupling methods for heading and inclination estimation.

• Two dierent attitude estimators, based on the EKF and IEKF formalism respectively, integrating the saturated bias model and the two disturbance decoupling methods.

Regarding the contribution of each fault tolerant mechanism developed herein, some key results can be noted:

1. A saturated bias model linearised with a stochastic linearisation is shown to improve the stability of standard EKF attitude lters during periods of unobservability.

2. Disturbance decoupling is an essential feature for fault tolerant attitude lters. Magnetic disturbance decoupling can be eortlessly implemented in Kalman lter based estimators via pseudo measurements, whereas acceleration to heading decoupling requires more intricate lter manipulation.

Furthermore, two dierent approaches to decoupling of the heading-and inclination estimation are developed. This results in two quaternion based attitude lters, one EKF based (NL-KF) and one IEKF based (DEC-IEKF), incorporating the previously developed bias saturation and decoupling methods.

From a disturbance decoupling point of view, both lters seem to perform equally well under nominal conditions. The only dierence we can deduce is that since the IEKF gains are covariance based, the decoupling properties are degraded over time if the inclination is not corrected for a long time due to prolonged dynamic trajectories for example.

The NL-KF provides uniform convergence for all heading errors at zero inclination which is not the case for the IEKF (whose covariance propagation, and by consequence gain, assumes small errors).

In short, from a fault tolerance perspective there is no clear winner between the lters we have investigated in this chapter but the choice must be made depending on the application and the kind of errors and attitudes which are likely to be encountered.

The NL-KF works well for attitude angles close to zero (which is typical for multi-rotor drones), the disturbance decoupling is also eective even at large attitude errors. However it suers from steady state errors at large attitude angles.

The DEC-IEKF on the other hand works well for all attitudes, however the magnetic disturbance decoupling is degraded at inclination estimation errors. The convergence time also increases with increasing attitude errors due to the linearisation around zero error becoming increasingly false. Notably an error of 180 • /s about any axis is only weakly observable.

Introduction

In the previous chapters we have introduced and discussed dierent mechanisms that improve the fault tolerance of attitude lters. We have seen that sensor output estimators can be eectively used to distinguish erroneous measurements even in the case of severe noise when using advanced detection techniques. Furthermore we have developed some fault tolerant aspects of the attitude lters through bias saturation and measurement decoupling resulting in two robust Kalman lters, one unconventional nonlinear and one based on the well known MEKF.

Motivation and principles

So far, we have seen that individual fault tolerant mechanisms can be used to improve various aspects of the fault tolerant attitude estimation problem (erroneous measurement detection and consolidation, attitude lter consistency and stability, attitude drift, etc.). We have simply put created a toolbox for composing a fault tolerant attitude estimator.

In this chapter we will combine those individual tools into a complete architecture [Öman Lundin et al., 2019a]. The architecture gathers the presented solutions categorised as measurement consolidation and fault tolerant attitude ltering to improve the error and consistency of an attitude estimation even in the event of measurement perturbations.

The working principle of the complete architecture is to handle severe enduring attitude measurement perturbations in a systematic manner:

-Detect the measurement disturbance using measurement and model based techniques.

-Consolidate the attitude measurement by either rejecting it completely or replacing it by a sensor output estimation.

-Consolidate the attitude rate measurement by replacing the current gyroscope bias estimation from the attitude lter by a redundant estimation from a low-pass lter.

-Update the attitude estimation with the consolidated measurements, using a decoupled attitude lter.

-If any of the attitude measurements are rejected, predict the attitude and gyroscope bias using a consistent prediction model for the attitude and gyroscope bias.

Figure 7.1 shows how the proposed attitude estimation architecture systematically treats the initial problem and the ensuing problems subsequently appearing as the original problem is solved.

It is hoped that a systematic decoupling of the dierent fault tolerant mechanisms of the architecture allows for an easier tuning since the dierent parts can be tuned independently. However nding a global optimal tuning might prove cumbersome. Another gain of dividing the solution into several sub-solutions is that the dierent parts (sensor estimation, fault detection, attitude estimation) can easily be replaced by other methods if deemed better.

For example the attitude lter can be replaced by any other attitude estimator providing an estimate of the attitude, gyroscope bias, and the associated covariance. A greedy design could replace the EKF-based estimator with a UKF [START_REF] Vinther | Inexpensive cubesat attitude estimation using quaternions and unscented Kalman ltering[END_REF][START_REF] Soken | Robust Kalman ltering for small satellite attitude estimation in the presence of measurement faults[END_REF] or if a convergence proof is demanded, a lter such as the sensor based [Batista et al., 2012a]. Care must be taken however, by choosing an attitude lter which implements a measurement decoupling and bias saturation to retain the level of fault tolerance.

The bias saturation and disturbance decoupling is as of now an open problem for Kalman-based attitude lters, however the ad-hoc solutions presented in Section 6.2.2 and 6.3 seem to be promising from a consistency point of view. Likewise the χ 2 fault detection can easily be replaced by a CUSUM, GMA, GLR, or any other more powerful test [START_REF] Gustafsson | Adaptive ltering and change detection[END_REF].

In the work of this thesis, we aim to address the problems depicted in Figure 7.1 by using dierent techniques introduced in the previous chapters. This approach can be illustrated by a "problemsolution" tree describing how upcoming problems are subsequently solved when solutions are proposed to the initial problem.

The initial problem is that of an enduring measurement perturbation.

Development of fault tolerant architectures: FT-IEKF-χ 2 /GLR

In order to deals with all the problem previously described, we designed a fault tolerant architecture based on a measurement consolidation stage followed by a decoupled and resilient attitude lter.

The objective of the measurement consolidation mechanisms is to provide the attitude estimation lter with only good/corrected measurements, in order to avoid attitude drift and bias. This is achieved by a cascade of several blocks, each serving a unique purpose in the safeguarding of the attitude estimation:

• Sensor output lters. Used to predict the sensor outputs and provide an analytically redundant data in case of disturbances:

Accelerometer and and magnetometer estimator consisting of EKFs.

The gyroscope bias lter is a saturated default low-pass lter.

• A Fault detection stage. • A data Consolidation stage: Deduces from the set of fault ags which measurement has to be rejected or replaced before being used by the attitude estimation lter.

As mentioned above, the architecture combines the attitude measurement consolidation of Section 5.2.1, the attitude rate measurement consolidation of Section 5.3, and the attitude lter dened in Section 6.4.2. This architecture uses either χ 2 -or GLR tests for fault detection. An overview of the architecture is seen in Figure 7.2.

As previously mentioned, the measurement consolidation step aims to detect, reject, or replace erroneous measurements and relies on a number of tests we summarise below:

Measurement norm rejection: Rejection of measurements not respecting the reference norm, based on χ 2 or GLR testing (Block "Fault detection").

Disturbed reference rejection: Rejection of the estimated sensor output due to temporarily unmodelled reference change, based on χ 2 or GLR testing (Blocks "EKF Mag.", "EKF Acc.", and "Fault detection").

Measurement fault rejection: Sensor model based fault rejection based on sensor models and χ 2 or GLR based detection (Blocks "EKF Mag.", "EKF Acc.", and "Fault detection").

Gyroscope bias consolidation: An open loop gyroscope bias estimation combined with the estimation from the attitude lter to provide a redundant bias estimation during magnetic and acceleration disturbances (Blocks "Gyroscope LP-lter" and "Data Consolid. Logic").

The attitude estimation stage aims to estimate both the attitude and the gyroscope bias, but also a consistent covariance (i.e. an uncertainty which enclose as accurately as possible the maximum possible error). It relies on: Gyroscope bias saturation: Consistent bias estimation during periods of unobservability (Block "Attitude lter").

Measurement decoupling: Decoupling of the inclination estimation from magnetic disturbances and of the heading estimation from acceleration disturbances (Block "Attitude lter").

On a sidenote, this architecture was presented in [Öman Lundin et al., 2019a] as an improvement of the architecture rst presented in [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF]. The dierences between this architecture and the rst one are the use of a dierent attitude lter, the idea of a redundant gyroscope bias estimation, and the use of GLR tests for fault detection. As a matter of fact, evaluation of the rst architecture using experimental data from a quadcopter platform [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF] unveiled a number of weaknesses:

1. Consistency: The covariance of the attitude lter was not consistent with the estimation during enduring perturbations since the gyroscope bias was not corrected. Under nominal conditions, the covariance of the attitude estimate was overestimated due to the non-optimality of the attitude lter being used.

2. Attitude drift: The lack of bias estimation updates during detected perturbations led to attitude drifts.

3. Missed detections: The simple χ 2 -test was judged insucient in the low signal-to-noise ratio measurements present in the multirotor platform. This typically led to perturbations causing disturbances on the attitude and gyroscope bias estimation before being detected, in turn leading to attitude drift in the absence of corrections.

The rst point was partially handled by switching the attitude lter previously developed in Section In the next section we will see the incremental value of each of the fault tolerant mechanisms of the nal architecture.

Step-by-step evaluation of fault tolerant mechanisms

In this section we will start with the bare attitude lter (DEC-IEKF) from Section 6.4.2 and illustrate what happens when it is impacted by a measurement perturbation in the form of a simulated magnetic disturbance. To this end, we dene a number of attitude lters, each implementing a higher level of fault tolerance than the previous:

• DEC-IEKF 1: The bare attitude lter.

• DEC-IEKF 2: This lter implements a norm based measurement rejection, representing the lowest level of fault tolerance (e.g. [START_REF] Wu | Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors[END_REF][START_REF] Sabatini | Quaternion-based extended Kalman lter for determining orientation by inertial and magnetic sensing[END_REF][START_REF] Valenti | Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs[END_REF].

• DEC-IEKF 3: The next step is adding the model based fault detection and measurement consolidation, based on χ 2 testing, roughly equating to the level of fault tolerance found in [Öman [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF][START_REF] Costanzi | An attitude estimation algorithm for mobile robots under unknown magnetic disturbances[END_REF][START_REF] Soken | Robust Kalman ltering for small satellite attitude estimation in the presence of measurement faults[END_REF].

• DEC-IEKF 4: Here we add the redundant gyroscope bias estimation.

• DEC-IEKF 5: The nal lter implements the GLR-based fault detection [Öman Lundin et al., 2019a].

The evaluation criteria are twofold. First of all the attitude error, expressed as the quaternion error angle, should be minimised. The error quaternion is expressed as δq = q -1 q = (cos(δε/2), e sin(δε/2)) T , where δε is the angular attitude error.

Secondly, the consistency of the attitude error should be maximised. When we talk about consistency, it is essentially that the absolute error should be lower than the uncertainty (at a certain level expressed in standard deviations). For magnetic disturbances, we are mainly interested in the heading consistency. In the results below, we compare the heading error ε ψ with the estimated standard deviation σψ .

The simulated accelerometer, magnetometer, and gyroscope measurements are seen in Figures 7.3 However since the detection is still based on a simple χ 2 test, the rejection is very irregular and the attitude drifts due to missed detections as the sinusoidal approaches zero. A very slight dierence can be seen in this case between DEC-IEKF-3 and DEC-IEKF-4, where the redundant bias estimation of the latter, removes the attitude drift during the phases of measurement rejection.

The GLR-based detector improves the disturbance rejection for the DEC-IEKF-5 in two ways. Firstly, it manifests a faster detection leading to a smaller attitude drift at the beginning of the disturbance.

Secondly, it is less sensitive to the intermittent periods of close-to-zero disturbance, leading to a much smoother attitude error. consistent than the rest of the architectures. However, as we can see in the near constant uncertainty estimation, it makes use of the sensor output estimation, leading to a slight drift over time. This drift is due to the unobservability of the measurement disturbance, leading to a slow drift of the sensor output estimation, meaning that at best the attitude drift can be limited over time, and not completely rejected.

Magnetic disturbance example 1: Stepwise disturbance Comments: In this case the dierence between the norm-only rejection of DEC-IEKF-2 and the model based rejection of DEC-IEKF-3/4/5 is even more agrant than in the previous example. We note that the DEC-IEKF-3 has a signicant drift over time, something which is remedied by the DEC-IEKF-4 using a redundant gyroscope bias estimation.

Another striking detail in this example is the error peak for the DEC-IEKF-3 and DEC-IEKF-4 at around 73 s. The reason for this peak is to be found in the Figure 7.8. The quick explanation is that the covariance decreases over time due to measurement rejection, and the correction gain when the observability is regained is very large, leading to a high innovation sensitivity.

The reason why this is not seen in the GLR-based DEC-IEKF-5 is thanks to the limited drift of the sensor output estimation, leading to the use of this redundant sensor information being privileged over complete measurement rejection. However, as for the rst example, this strategy can only limit the drift of the attitude and not eliminate it completely, which is why a small drift is still present.

Comments: This second example highlights the problems of the consistency as a sole evaluation criteria. To start of with, we see that the DEC-IEKF-2 is completely lost in terms of consistency thanks to its poor rejection behaviour. Even the DEC-IEKF-5, which is very ecient in rejecting the measurement disturbance, is objectively

Conclusion

In this chapter, we have developed an attitude estimation architecture consisting of a measurement consolidation stage and an attitude estimation stage.

The architecture proposes a step-by-step solution to safeguard the attitude estimation and its covariance against measurement errors. A simple evaluation shows that common measurement rejection methods described in literature [START_REF] Wu | Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors[END_REF][START_REF] Costanzi | An attitude estimation algorithm for mobile robots under unknown magnetic disturbances[END_REF][START_REF] Soken | Robust Kalman ltering for small satellite attitude estimation in the presence of measurement faults[END_REF][START_REF] Sabatini | Quaternion-based extended Kalman lter for determining orientation by inertial and magnetic sensing[END_REF] are not sucient to maintain low attitude estimation error and a good estimation consistency during enduring perturbations which are likely encountered in urban environments.

While the results regarding the estimation error are strictly conclusive -the more fault tolerant mechanisms implemented, the smaller the attitude error and drift. To give the Devil his due, the results regarding the attitude consistency are less obvious.

From a consistency point of view, measurement rejection, rather than measurement replacement, can sometimes be the better decision. Since the rejection increases the attitude uncertainty estimation and the measurement replacement can introduce attitude drift while maintaining the uncertainty, the consistency in the former case is sometimes better than in the latter one.

This critical point highlights the importance of estimation integrity. The sole evaluation of the attitude error and the attitude consistency is not sucient to evaluate the fault tolerance of an attitude estimator. While minimising the attitude error, and maximising the consistency, we must also maximise the integrity margin in order to keep the estimate practically useful.

On a higher level, the structure of the architecture also allows for easy identication of faulty sensors which can ease the fault identication when integrated with an inertial velocity and position estimator.

In the upcoming chapter, an extensive experimental evaluation of the proposed architecture is performed including a comparison with some current state of the art methods.

Let the games begin! Bane, The Dark Knight Rises (2012)

Introduction

In order to asses the fault tolerant capacities of the proposed architecture, a series of ight tests with a Mikrokopter Mk7 quadcopter embedding a PX4 autopilot module with low-cost MEMS sensors was performed. The intention was to y aggressive trajectories in order to bias the accelerometer measurement (i.e. create a non-zero specic acceleration so that the accelerometer measurement signicantly deviates from gravity), and also to y via a current loop placed adjacent to the drone in order to create a perturbed magnetic eld to be traversed.

Experimental setup

The experimental validation was performed at the indoor ight arena at ISAE-SupAéro equipped with an optical motion tracking system [Optitrack -Motion capture systems ] providing a ground truth of the attitude. this case we separate the full attitude into its inclination part (consisting of φ and θ) and heading part (ψ). The covariance matrix of interest can then be seen as block diagonal , i.e.

P euler = P α * * P ψ .

The integrity test is then dened as (Appendix B)

P L ξ = λ (P ξ ) F -1 χ 2 (1 -P ir , q δ , 0) P L ξ Integrity failure ≷ Integrate AL ξ (8.2)
where P ξ is the corresponding covariance matrix (P α or P ψ ), and q δ the dimension of the error (2 for α and 1 for ψ).

The two parameters to x are P ir , the integrity risk, and AL, the alert limit. In the case where no fault is considered, P ir is simply the probability for the error to exceed the alert limit when no fault is present. The alert limit is tightly coupled to the operational conditions and classically used in GNSS [START_REF] Fouque | Tightly coupled GIS data in GNSS x computation with integrity test[END_REF][START_REF] Marais | A survey of GNSS-based research and developments for the European railway signaling[END_REF][START_REF] Wörner | Integrity for autonomous driving: A survey[END_REF] to designate an integrity sphere or cylinder around the vehicle (usually and aircraft or a car). Details on how the protection limit is calculated are given in Appendix B.

In the case of angular estimation integrity, especially for drones, calculating the alert limit becomes more delicate. Since the euler angles do not directly give us an information on the risk of deviating from the nominal trajectory we have to associate them with an information that does so. Appendix B shows two examples of how one can calculate the angular alert limit for drones.

Filter parametrisation and initialisation

Modelling. The reference vectors were taken as a ref = (0, 0, -9.81) T m/s 2 and m ref = (-0.5, 0.1, 0.8) T G, the magnetic eld measured in the zero-yaw direction of the ight arena at 1 m above the oor on the starting point of the UAV.

The bias covariance was set to σ bω = (0.167, 0.167, 0.167)(deg/ Initializing. The lter states were initialised as x0 = (q T 0 , bT 0 ) T , with q0 = (1, 0, 0, 0) T and b0 as the on-ground static low pass gyroscope bias estimates.

Tuning. The attitude lter was congured with measured in-ight measurement standard deviations σ a = (3.27, 3.27, 3.27)m/s 2 , σ m = (0.082, 0.226, 0.21)G, σ ω = (8.33, 4.00, 1.33)(deg/s).

The measured covariances for W a = diag(σ a ) 2 and W m = diag(σ m ) 2 were used for the measurement fault detection while they were multiplied by a factor 10 for W a and 50 for W m for the attitude estimation in KF/IEKF. This is a practical consideration in order to take into account the unreliability of the actual measurement covariance and above all to better lter out vibration eects. eect on the error in neither of the three trajectories. The FT-KF has the worst results, large peaks on the inclination error. Other lters are equivalent from this point of view. On trajectory #1 a quite large mean error appears on the roll angle in the second part of the trial. Roll drifts also appear on trajectory #1 and #3 but transiently. We think it is due to coning and sculling errors.

As regards the heading estimation error the FT-KT has a bad behaviour. IEKF and FT-IEKF-χ 2

give similar results characterised by large error, inconsistent and non-integrate estimation. The best response is achieved with the FT-IEKF-GLR. It is the only lter that gives an integrate estimation of the heading. It fails however to remain consistent on the rst and third trials.

The heading estimation is signicantly improved when the GLR detector is applied, although far less when using χ 2 -tests. The drift seen in the heading estimation is likely due to small drift disturbances in the magnetometer measurement due to irregularities in the magnetic eld of the ight arena. These small drifts are not detected by normal χ 2 -tests but only by the GLR-test congured to detect measurement drifts.

Consistency: The inclination error is overall consistent for the IEKF-based architectures whereas the FT-KF architecture has some diculties with the transients. We also note the transient drifts which seem to be due to coning and sculling errors and not related to the dynamics of the quadcopter.

The dynamic phases are identied as a transient increase of the accelerometer covariance, particularly visible in a z .

The heading estimation error is not very consistent for the rst three architectures. However as mentioned regarding the error, the GLR-based rejection eectively eliminates most of the heading drifts.

In trajectory #1 we note that even the GLR-based architecture is transiently inconsistent. This is likely due to the drifts not being detected around 30-40 s into the test because of poor magnetometer measurements.

Integrity: The integrity of the inclination estimation is well under the alert limit in all cases and all three trajectories. Although the FT-KF which slightly overestimates the covariance, approaches the alert limit transiently. And except for the FT-KF case, the protection limit P L α is eectively an upper bound of the inclination error.

The heading estimation integrity shows the same pattern. In almost all cases and trajectories, the protection level is on a safe distance from the alert limit. For the FT-KF however, the margin is overall smaller.

It must be pointed out that for the rst three lters, the eective heading error becomes far much larger than the protection limit P L ψ . The lter said its estimation is integrate but it is not. This is the most dangerous situation from an integrity point of view. For the GLR-based architecture, a blatant overestimation of the covariance due to the measurement rejection takes P L worryingly close to AL in Trajectory #2.

Fast cross trajectory with magnetic disturbance

We only show the sensor data and estimation results for Trajectory 4. Comments:

Measurements: The magnetic disturbance clearly appears comparing the measurement and its reference. We see very large deviations of the measurement from its reference. They occur quite regularly, in fact every time the drone passes through the area disturbed by our magnetic eld disturbance device. This is also valid for the heading estimation however for the FT-IEKF-GLR it appears that the reason for the inconsistency is the detection delay since after the detection the error remains stable. The inconsistency of the FT-IEKF-χ 2 is also related to the poorer detection rate as compare to the FT- IEKF-GLR (especially for trajectory #9). The nominal IEKF with only norm rejection is completely inconsistent due to its failure to reject the disturbance.

Integrity: The inclination estimation remains integrate for the IEKF-based architectures. The FT-KF is rather sensitive to the unstable accelerometer covariance seen in all three trajectories. This leads to breaches of the alert limit on several occasions, especially around 120-160s.

The heading estimation integrity highlights dierent problems for the dierent architectures. The FT-KF detects and integrity loss quickly due to consecutive measurement rejection in all three trajectories.

The nominal IEKF is completely lost due to its large error, however the protection level itself remains low due to the missed detections, leading to a hazardously misleading estimation. The FT-IEKF architectures both remain fairly integrate. However the eective heading error may sometimes exceed the protection level. This is much more pronounced for the FT-IEKF-χ 2 than for the FT-IEKF-GLR. We again note the importance of a quick and precise detection where the GLR-detector helps keeping the estimate integrate where the χ 2 -detector struggles.

Additionally in this scenario we see the gain of using a redundant gyroscope bias estimation independent of the magnetometer measurement. The limited drift of the attitude due to the redundant bias estimation help the sensor output estimation to remain consistent and useful for attitude update. This in turn limits the growth of the attitude covariance and keeps the estimation integrate.

Comments

Estimation error: The inclination estimation error remains small for all architectures and both trajectories. The only exception is the dynamic part which induced larger errors for the FT-KF architecture at the beginning of both trajectory #10 and #11. This error is likely due to the quaternion estimators poor convergence at large inclination angles (see discussion in Section 6.4.1).

The heading estimation error follows a similar pattern as the one for the trajectories #7, #8, and #9.

The FT-KF detects and rejects the disturbance but suers from serious drift in trajectory #10. The The FT-IEKF-GLR remains integrate during both trajectories.

Estimation error statistics

To get a quantitative overview of the experimental results. The absolute maximum, the root mean square, and the standard deviation of the estimation errors in inclination and heading have been gathered into In short we can summarise the evaluation as follows:

The fault tolerant attitude estimation architecture from [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF] shows a signicant improvement over the nominal solution in accuracy during transient (#1-#6) and prolonged (#7-#9) perturbations. It does not however, handle lasting perturbations too well, as was stated in the motivations for the further development. We can also see that it gets tricked by the cases where the magnetometer norm only changes slightly (#9-#11) due to the orientation of the perturbation.

Notable is also the similarity between the precision of the FT-KF and the FT-IEKF-χ 2 where the only real dierence is that the latter uses a backup gyroscope bias estimation. This can especially be seen in the scenarios with enduring perturbations (#7-#9) where the FT-IEKF-χ 2 is signicantly better (roughly 50% of the error is shaved o ).

As expected, the standard IEKF using only norm rejection handles transient acceleration perturbations fairly well with a small degradation of accuracy (#1-#3). We also note the eective decoupling between roll/pitch and yaw axes during magnetic perturbations (#4-#9) . However this solution fails (looses accuracy) once the perturbations are long lasting and building up slowly (see #7-#9 and partially #10-#11).

The architecture with GLR based fault detection presented herein shows an additional increase in precision as compared to the other architectures in all tests. Notably we see that the accuracy in yaw is roughly equivalent for the cases with and without magnetic perturbations.

Conclusion

In this chapter we have performed an experimental validation of the fault tolerant architectures developed in Chapter 7. The evaluation concerns the ability of dierent attitude estimation architectures paired with an increasingly complex (and a priori abler) fault rejection stage. The test trajectories concern high acceleration phases and transient or enduring magnetic disturbances. Three evaluation criteria have been specied: estimation error; error consistency; estimation integrity.

The results indicate that using only a norm-based rejection is not enough to guarantee any of the three criteria (IEKF). The addition of a model based rejection method signicantly improves all criteria (FT-KF), however the risk of an ecient fault rejection is attitude drift. This residual drift is handled by adding a redundant gyroscope bias estimation to maintain a correct bias estimation during perturbations (FT-IEKF-χ

2 ). The nal problem that arises is the poor fault detection performance of the standard χ 2 -test in high noise scenarios. A nal architecture implementing a GLR-based measurement drift detection improves the rejection in cases of low perturbation-to-noise ratios and slowly appearing perturbations.

The integrity is a particularly tricky question for attitude lters. We eectively see that rejecting measurements can quickly degrade the integrity of the estimate. However we also know that pure gyroscope integration, under the assumption that the bias is well estimated, is the best way to handle with transient perturbations. This means that ltering the attitude measurements or rejecting faulty measurements, which would normally lead to a better (and more consistent) attitude estimate, eectively leads to a less reliable estimate (from an integrity point of view). The idea of using estimated sensor outputs during periods of unobservability, as for the FT-IEKF-χ 2 /GLR architectures, is therefore promising. Not only form a consistency point of view, but also to attempt to reconcile the demand for consistency and integrity during enduring perturbations.

Hasta la vista, baby

T-800, Terminator 2: Judgement day (1991)

9.1 Discussion -Where are we now?

This thesis has presented developments in the eld of fault tolerant navigation for drones. The contributions have been focused on the problems of fault tolerant position and attitude estimation in the specic case of non-redundant sensors.

In position estimation this implied having only a single localisation sensor to rely on, alternatively that all position sensors involved are subject to biases.

In attitude estimation this meant attitude estimation using IMU and magnetometer only. Alternatively any other non-redundant sensor suite. It also implied banning any secondary sensors such as GNSS, velocity sensors, or magnetic gradiometers.

Main contributions -Fault tolerant position estimation

The contributions in this eld concern extensions of the classical GLR algorithm to better handle multiple consecutive sensor biases by continuous re-identication and compensation of detected biases.

Furthermore, contributions have been made to the estimation integrity where it is proposed to use methods for detecting the disappearance of biases and a return to bias free measurements.

Main contributions -Fault tolerant attitude estimation

In the eld of fault tolerant attitude estimation, this thesis has contributed to evaluating and combining dierent techniques (some existing, some new) to achieve eective disturbance rejection for generic attitude sensors. Notably we have proposed to combine techniques from the signal processing domain with classical modelling and estimation techniques from automatic control for highly eective disturbance rejection.

We have also investigated the question of attitude estimation integrity, a hitherto scarcely researched eld. Our contributions in this eld concern the possibility of using performance models to provide articial measurements during periods of measurement disturbance rejection in order to limit covariance divergence. Another contribution in this domain is the application of a saturated bias model and its stochastic linearisation to limit the growth of the covariance when no attitude measurements are available.

Final comments on the experimental evaluation

The experimental evaluation has, just like the contributions, focused on one hand on position estimation, and on the other hand on attitude estimation.

Regarding the position estimation, it is clear that the presented evaluation has signicant problems, although it does conrm basic hypotheses underlying the proposed MGLR development. Notably the usefulness, and the limits of the re-identication step, and the criticality of the bias elimination for maintained estimation integrity.

We shall make no secret out of the fact that the tuning plays a huge role in the experimental evaluation. The probability of false alarm and the prescribed measurement covariances have a signicant impact on the estimation result and in the present cases they have been set more or less by "engineering judgement".

It is also clear that the absence of a ground truth (except for the vision based approach) greatly devalued the signicance of the results from a quantitative point of view. However the reconstruction of a ground truth at least allowed to get a rough idea of the actual error. A proper evaluation in the context of this thesis could have included a drone (of type multi-rotor) ying outdoors in or near a built environment in a predened and mapped pattern, equipped with an IMU and a dual GNSS receivers, one working in RTK, and another one in SBAS or single mode.

The signal in SBAS/single would be replayed as the measurement, and the RTK as the ground truth.

Such an evaluation would likely have given signicantly more quantiable results.

The campaign for attitude estimation evaluation was fairly well planned and executed. However as we have already noted, insucient preparation meant using gyroscope and accelerometer measurements uncorrected for coning and sculling. Furthermore, the errors of the optical tracking system were not assessed beforehand, leading to some odd error spikes in the estimated attitude errors.

On the bright side, the evaluation concerning the heading estimation part was very successful in the sense that the associated gyroscope measurement was barely impacted by coning, and the articial magnetic perturbation very well exposed the weaknesses of the state of the art.

The sensor data used in the evaluation together with the optical ground truth are available at:

https://gitlab.com/gustavlu/attitude-estimation-evaluation

Identied limits and drawbacks

Concerning fault-tolerant position estimation, the MGLR algorithm (Chapter 3) has been shown to exhibit a some major drawbacks:

Sensitivity to non-Gaussian noise in the re-identication step (inherited from the classical GLR).

The covariance compensation is not strongly statistically founded and the consequences in terms of false alarms are unknown.

The bias elimination methods proposed are very rudimentary and the conditions under which they work well are not established.

The proposed attitude estimation architecture has also shown some crucial weaknesses:

The tuning is complicated and the exact eect of each tuning (performance model time constants, probabilities of false alarm, etc.) is dicult to establish.

The stability of the full FT-IEKF architecture is dicult to asses since it contains a detection and decision step and thus a mix of discrete and continuous states.

The inclination/heading estimation decoupling has no formal proof of working at all attitudes and for all attitude errors.

The Achilles heel of the architecture is the gyroscope measurement which is considered fault-free apart from a quasi-constant bias.

Despite these drawbacks, we have shown operational functionality in various realistic scenarios, and it is probable that they won't manifest themselves in common operational scenarios for drones.

How does the attitude estimation architecture developed herein stand up against external sensor aided dittos?

From a fault-tolerance point of view, what is nally the gain of introducing a model of the platform dynamics compared to the mainly kinematics-based methods developed in this thesis?

Is there a gain in integrating non-redundant sensor state estimation techniques in the case of apparently redundant state estimation (e.g. GNSS + vision + mapping or other sources)?

The interest of using non-redundant techniques for fault tolerant state estimation for drones lies in the quantication of the trust that is put into the individual sensors as a function of the operational scenario. If indeed future low cost sensors for drones become reliable to the point where redundancy is sucient to assure a high level of estimation integrity, then non-redundant techniques such as the ones presented herein will be superuous. On the other hand, if the trend continues with relatively unreliable sensors and increasing on-board computing power, then the future is bright for non-redundant fault tolerant state estimation for drones.

A.1 The non-redundant measurement problem and the GLR algorithm Consider a (linearised) system subject to Gaussian state and measurement disturbances v t and w t+1 with covariances Q t and R t and an unknown bias b described by the discrete state representation (A.1):

x t+1 = A x t + B u t + F u Γ u, t-k b + v t We easily see that for the translation kinematics problem with input velocity and a biased position measurement, i.e. A = 0, B = 1, F u = 0, C = (1 1), F y = 1, the state x a is not uniformly observable and thus normal Kalman-or Luenberger observers cannot be used to estimate the position and the bias simultaneously but will render a result similar to Figure A.1. However since the system (A.1) is observable for t < k if the pair (A, C) is observable, the system is weakly observable. Since the apparition time is not known in general, this leads us into the domain of change detection to determine k. In order to nd k we need to rst set up an observer for the bias free case, i.e. for the zero hypothesis H 0 . The observer residuals can then be used, under the assumption that they are zero-mean under H 0 , to detect when measurements violating H 0 appear.

Φ t+1,k = A Φ t,k + F u Y t-k + K t+1 φ t+1,k φ t+1,k = F y Y t+1-k -C(A Φ t,k + F u Y t-k ) (A.8)
These signatures will be used in the GLR algorithm to create a set of matched lters used for fault detection. They are both zero for t < k so that xf t = 0 and ν f t = 0 because we assume that no biases are present before the instant k (which needs to be estimated).

A.2.2 Compensation

The objective of the change detection is to nd estimates k and b corresponding to the fault occurrence time and its amplitude. Estimations of the fault signatures can be then computed and used as correction terms to be removed from the (faulty) Kalman estimates. The associated covariance matrices must be adjusted accordingly. In the next section b is estimated recursively and we will see that bt, k ∼ N (b, Λ -1 t, k ), The compensation can be also applied to the innovation: Nevertheless it must be pointed out that, for an accurate recursive estimation of b, this compensation should not be done in a closed loop fashion. Indeed, removing the fault signature estimated at time t will hinder the improvement of the estimate at date t + 1. On the other hand, correcting the innovation with the previously detected bias is the only way to detect and estimate multiple biases appearing in rapid succession.

ν c t = ν t -

A.3 The GLR detector A.3.1 Likelihood ratio

Consider the state-space model (A.1) producing an innovation signal ν t given by (A.5). In order to identify whether a bias is present in the measurement, the two hypotheses "No bias present" (H 0 ) and "Bias present" (H 1 ) are introduced. Using the expressions derived above, we can express these hypotheses in terms of the innovation, i.e. As we can see, the impact of the bias is rather dicult to distinguish from the noise. Example of an application of the GLR algorithm to estimate a measurement bias. Note that the bias estimation converges as long as the bias remains within the sliding window, i.e. as long as it is observable.

Clearly the GLR algorithm manages to detect and estimate the bias fairly well as long as it is observable. Once the bias leaves the sliding window it is no longer observable and the estimation is degraded to eventually return to zero.

In order to continue correcting the state estimation even after the observability of the bias disappears, one would have to monitor when the bias leaves the sliding window and then x the value of the bias as the nal value of the estimate. This method is not shown here as the example is only meant as a pedagogical support to understand the GLR algorithm.

A.3.5 Conclusion

The GLR algorithm was developed for detecting bias jumps in linear systems. It builds on a linear regression over a sliding window where the regressor (fault signatures) are calculated from a predened bias form (constant, drift, etc.) supposed to appear at a given point in the sliding window, together with the Kalman lter recursion.

The algorithm assumes that a single bias appears on the sliding window, which is why it cannot be applied directly to detect measurement biases appearing arbitrarily frequently. In order to detect and estimate consecutive measurement bias the algorithm must be reset every time a bias is detected. This naturally leads to a compromise between detection delay and estimation precision.

Another disadvantage of the algorithm is its heavy reliance on Gaussian noise. This was remedied in [START_REF] Faurie | Combining generalized likelihood ratio and M-estimation for the detection/compensation of GPS measurement biases[END_REF] by replacing the RLS estimation with an M-estimation in the form of an iterated recursive least squares (IRLS) scheme, showing a signicantly better result in the case of non-Gaussian noise.

Newtons equation in the x-direction is:

F x = mg sin ε α cos ε α = m∆a H = m ∆x ⇒ ∆x = g tan ε α
Twice integrated we get the displacement, assuming ∆v 0 = ∆x 0 = 0: ∆x = 1 2 g tan ε∆t 2

We then get the alert limit AL, given as the admissible maximum error, expressed as a function of ∆x and ∆t: AL ε α = atan2(2∆x, g∆t 2 ) .

B.4.2 Example 2: Reprojection error limit during visual navigation

A second example particularly relevant for vision based navigation where reprojection of points (or point clouds) is important. In this case the angular limit is calculated from the maximum admissible reprojection error ∆d given the distance h from the observed point. The true horizontal distance d between the drone and the observed point is found as d = h tan α .

The perceived distance d + ∆d is given by d + ∆d = h tan(α + εα) = h tan α + tan ε α 1 -tan α tan ε α .

Contents 1 . 1

 11 What is the matter with these drones, anyway? . . . . . . . . . . . . . . . . 3 1.2 Drone navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 1.3 Orientation and position estimation . . . . . . . . . . . . . . . . . . . . . . . 7 1.4 Sensor faults and disturbances . . . . . . . . . . . . . . . . . . . . . . . . . . 9 1.5 Fault tolerant navigation for drones . . . . . . . . . . . . . . . . . . . . . . . 11 1.6 Main contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 1.7 How did I actually do all this? . . . . . . . . . . . . . . . . . . . . . . . . . . 14 1.8 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 1.9 Publications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

Figure 1

 1 Figure 1.1: Total UAS Forecast in the U.S., 2015-2035. Source: U.S. Department of Transportation, John A. Volpe National Transportation Center, Unmanned Aircraft System (UAS) Service Demand 2015-2035: Literature Review & Projections of Future Use, September 2013, DOTVNTSC-DoD-13-01.

Figure 1 .

 1 Figure 1.2 shows an example of a xed wing drone equipped with an array of dierent navigation sensors [H2020 VISION -Validation of Integrated Safety-enhanced Intelligent ight cONtrol ].

Figure 1 . 2 :

 12 Figure 1.2: Example of a drone equipped with an array of navigation sensors. Image courtesy: EC H2020 Project VISION

Figure 1 . 3 :

 13 Figure 1.3: Schematic overview of a combined position and attitude estimator.

Figure 1 . 4 :

 14 Figure 1.4: Schematic overview of a position estimator.

  Figure 1.6 shows the conceptual dierence between the dierent types of bias.

Figure 1 . 6 :

 16 Figure 1.6: Dierent types of sensor bias: transient; step; constant; drift.

Figure 1

 1 Figure 1.7: Typical biased GNSS position trajectory.

Figure 1 . 8 :

 18 Figure 1.8: Magnetometer subject to magnetic disturbance compared to the magnetic reference in the sensor frame.

  ensuing consolidation logic then allows to choose whether the measurement or the sensor output estimation should be used for attitude update and whether or not attitude correction should be performed at all with the consolidated data. This development also includes a consolidation for the attitude rate sensor bias based on a saturated low-pass lter. Development of generic fault tolerant aspects of attitude estimators through the development of two dierent disturbance decoupling techniques for Kalman lters, and the development of a stochastic linearisation for the implementation of a saturated bias model. These techniques have been applied to two classical attitude estimation lters to showcase the improvement in stability and performance during attitude measurement disturbances.Simulation and experimental assesmentThese developments have shown in simulation and experimental testing that accuracy, consistency and integrity of the attitude estimation are signicantly improved compared to classical techniques. The improvement is especially seen during long lasting disturbances since the measurement consolidation allows for the correction of the attitude estimate despite the attitude sensors being perturbed.

  thesis starts in Chapter 2 with an introduction to fault tolerant position and attitude estimation and the corresponding state of the art. Chapter 3 describes the development of a GLR based algorithm for multiple consecutive position bias estimation. In Chapter 4 the developed algorithm is tested in a number of dierent experimental settings. The fault tolerant attitude estimation starts in Chapter 5 with the development of the attitude and attitude rate measurement consolidation method. Chapter 6 proposes some fault tolerant aspects for classical attitude lters and two attitude lters implementing these aspects. Chapter 7 proceeds to combine the measurement consolidation and the fault tolerant attitude lters into a complete fault tolerant attitude estimation architecture. Chapter 8 presents an extensive experimental test campaign for the fault tolerant attitude estimation architecture including severe enduring attitude measurement disturbances and aggressive trajectories. Finally in Chapter 9, the obtained results are discussed in the larger perspective of fault tolerant navigation and potential future developments are discussed.

Figure 2 . 1 :

 21 Figure 2.1: Impact of a single bias on a INS/GNSS Kalman lter innovation with a non-redundant position measurement.

Figure 2 . 3 :

 23 Figure 2.3: Illustration of an inclination angle α about a vector u.

Figure 3

 3 Figure 3.1: Impact of three consecutive biases on a INS/GNSS Kalman lter innovation with a non-redundant position measurement.

Figure 3 .Figure 3 . 2 :

 332 Figure 3.1 we get an estimation result as in Figure 3.2.

Figure 3 .

 3 Figure 3.3 shows that, even if a fault detection/correction procedure is used and works well (here Willsky's GLR), the uncertainty of the corrected state estimate increases due to the detection and estimation of multiple appearing and disappearing biases. Note that for clarity, a dierent bias sequence that in Figure3.1 is shown.

Figure 3 . 3 :

 33 Figure 3.3: Increased state uncertainty due to multiple bias detections, estimations, and corrections by Willsky's GLR in multiple bias conguration.

Figure 3 .

 3 Figure 3.4: a) Classical GLR algorithm congured for consecutive bias detection and estimation. b) Proposed GLR algorithm (MGLR) for multiple bias detection and estimation

A

  comparison is made between the classical GLR by Willsky, our proposed MGLR with LS estimation, and our proposed MGLR with RLS estimation. In the rst two examples (single and multiple biases) the MGLR algorithms function without the bias elimination part to showcase the detection and estimation capabilities in open loop. Willsky's algorithm is congured for consecutive bias detection and estimation (i.e. with a Kalman lter reset after each detection).

Figure 3 .

 3 Figure 3.6 shows the typical behaviour of the classical GLR algorithm congured for multiple biases.

Figure 3 . 9 :

 39 Figure 3.9: Example of bias estimation and GLR test statistic for Willsky's algorithm in the multiple bias case. Sliding windows, L and L est , shown for scale.

Figure 3 .

 3 Figure 3.10: Example of bias estimation and GLR test statistic for the MGLR algorithm using RLS re-identication in the multiple bias case. Sliding windows, L and L est , shown for scale.

Figure 3 .

 3 Figure 3.11: Example of bias estimation and GLR test statistic for the MGLR algorithm using LS re-identication in the multiple bias case. Sliding windows, L and L est , shown for scale.

Figure 3 .Figure 3 .Figure 3 .

 333 Figure 3.12: Example of global accumulated bias rejection applied to a sequence of biases. Sliding windows, L and L est , shown for scale.

  Figure 4.1: Input, measurement and measurement standard deviation used in Scenario 3.

Figure 4 . 2 :

 42 Figure 4.2: Measurement error in Scenario 3.

Figure 4 . 3 :

 43 Figure 4.3: Consistency, test statistic, and integrity results when using Willsky's GLR algorithm in Scenario 3.

Figure 4 . 4 :

 44 Figure 4.4: Consistency, test statistic, and integrity results when using the MGLR-LS algorithm in Scenario 3.

4. 3

 3 Application to a hand-held GNSS receiver4.3.1 Introduction Description In this scenario, a handheld GNSS-RTK receiver is walked around a ight club runway (open terrain, no buildings). At one point the RTK (and possibly the SBAS) correction is lost and the GNSS position measurement shows a large position jump and a series of smaller perturbations. After roughly 1 minute, the receiver again retrieves its RTK precision, with a smaller perturbation occurring roughly a minute later. The idea of this scenario is to see whether the jumps in the position measurement can be corrected using only the velocity measurement as an input to improve the accuracy and integrity of the measurement during the perturbed period. The three axes (north, east, and down) are tested independently. In this scenario we again compare Willsky's GLR algorithm against the MGLR-LS with the global bias elimination.

Figure 4 .

 4 Figure 4.5: Consistency, test statistic, and integrity results when using the MGLR-LS algorithm with L est = 2 s in Scenario 3.

  and 4.8): The condence interval is generally underestimated and it does not manage to reect the uncertainties of the multiple bias detections apart from the part where no bias is present.Integrity: Both algorithms have a globally well estimated protection level, apart from a single occasion. This point of an underestimated protection level is seen around 60 s for Willsky's GLR (Figure4.10), and around 125 s for the MGLR-LS. In both of these cases, the error remains signicantly lower

Figure 4 .

 4 Figure 4.6: Input, measurement and measurement error along the north axis.

Figure 4 . 7 :

 47 Figure 4.7: Estimation result for Willsky's GLR along the north axis. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 . 8 :Figure 4

 484 Figure 4.8: Estimation result for the MGLR-LS along the north axis. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 .

 4 Figure 4.10: Estimation result for Willsky's GLR along the east axis. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 .

 4 Figure 4.11: Estimation result for the MGLR-LS along the east axis. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 .

 4 Figure 4.12: Input, measurement and measurement error along the down axis.

Figure 4 .

 4 Figure 4.13: Estimation result for Willsky's GLR along the down axis. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 .

 4 Figure 4.14: Estimation result for the MGLR-LS along the down axis. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 .Figure 4 .

 44 Figure 4.15: Input and measurement along the north axis in the urban navigation scenario.

Figure 4 .

 4 Figure 4.17: Estimation result for Willsky's GLR along the north axis in the urban navigation scenario. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

4 .

 4 16); namely a signicant bias around 115-125 s and a rapid succession of bias around 155 s. In this axis we also identify a time varying bias around 220-245 s.Precision: The precision result recalls an expected dierence between Willsky's GLR (4.21) and the MGLR-LS(Figure 4.22). The estimation error of Willsky's GLR shows a tendency to drift, something which is not present for the MGLR-LS thanks to the re-identication step. Furthermore, we see

Figure 4 . 18 :

 418 Figure 4.18: Estimation result for the MGLR-LS along the north axis in the urban navigation scenario. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 .

 4 Figure 4.19: Input and measurement along the east axis in the urban navigation scenario.

Figure 4 .

 4 Figure 4.20: Measurement error along the east axis in the urban navigation scenario.

Figure 4 .

 4 Figure 4.21: Estimation result for Willsky's GLR along the east axis in the urban navigation scenario. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 .

 4 Figure 4.22: Estimation result for the MGLR-LS along the east axis in the urban navigation scenario. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 .Figure 4 .

 44 Figure 4.23: Input and measurement along the down axis in the urban navigation scenario.

Figure 4 .

 4 Figure 4.25: Estimation result for Willsky's GLR along the down axis in the urban navigation scenario. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 4 .

 4 Figure 4.26: Estimation result for the MGLR-LS along the down axis in the urban navigation scenario. Top: Estimation error (black), condence interval (blue dash-dotted), protection level (green), and alert level (red dashed). Bottom: GLR test statistic and detection threshold.

Figure 5 . 2 :

 52 Figure 5.2: Detailed view of the sensor output estimation and residual generation.

Figure 5 . 3 :

 53 Figure 5.3: Outlier mitigation through covariance bloating for sensor output estimation in 1D: a) Nominal case with uncertainty intersection; b) Covariance bloating when no intersection.

Figure 5 . 4

 54 Figure 5.4 shows an example of the sensor output estimation ( m) for the case of a perturbed magnetometer measurement (m m ). The measurement norm ag (m_norm) is also shown to clarify when the lter relies on pure prediction.

Figure 5 . 4 :Figure 5 . 5 :

 5455 Figure 5.4: Local magnetic eld (m) VS magnetic measurement (m m ) VS sensor output estimation ( m) during a simulated magnetic disturbance.

Figure 5

 5 Figure 5.6: Fault sensitive residuals response to the occurrence of a large attitude error.

←

  Figure 5.7: Test statistic comparison for χ 2 and GLR detection for the residuals in Figure 5.5.

←

  Figure 5.9: Measurement consolidation result .

Figure 5

 5 Figure 5.10: Scenario 1 (Acc. Disturbance): Accelerometer (a), magnetometer (m), and gyroscope (ω) measurements in the case of disturbances on the accelerometer.

Figure 5 .

 5 Figure 5.11: Scenario 1 (Acc. Disturbance): Bias estimation without the bias consolidation strategy when the accelerometer measurements are disturbed.

Figure 5 Figure 5

 55 Figure5.12: Scenario 1 (Acc. Disturbance): Bias estimation using the bias consolidation strategy when the accelerometer measurements are disturbed.

Figure 5

 5 Figure 5.14: Scenario 2 (Mag. Disturbance): Bias estimation without the bias consolidation strategy when the magnetometer measurements are disturbed.

Figure 5 .Figure 5

 55 Figure 5.15: Scenario 2 (Mag. Disturbance): Bias estimation using the bias consolidation strategy when the magnetometer measurements are disturbed.

Figure 5 .

 5 Figure 5.17: Scenario 3 (Acc. + Mag. Disturbance): Bias estimation without the bias consolidation strategy when both the accelerometer and magnetometer measurements are disturbed.

Figure 5 . 18 :

 518 Figure 5.18: Scenario 3 (Acc. + Mag. Disturbance): Bias estimation using the bias consolidation strategy when both the accelerometer and magnetometer measurements are disturbed.

6. 3

 3 Bias saturation model with stochastic linearisation . . . . . . . . . . . . . . 6.3.1 Covariance propagation through stochastic linearisation . . . . . . . . . . . . . 6.3.2 Advantages of using a saturated bias model . . . . . . . . . . . . . . . . . . . . 6.3.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.4 Development of two fault tolerant attitude lters . . . . . . . . . . . . . . . 6.4.1 A nonlinear Kalman lter (NL-KF) with measurement decoupling and bias saturation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.4.2 A decoupled Invariant Extended Kalman Filter with bias saturation (DEC-IEKF)146 6.4.3 Typical decoupling results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  When used by an EKF or a similar lter based on linearisation, this bias model will lead to an estimate of b ω that tries to satisfy | bω | ≤ b max . This estimate is unbiased as long as |b| ≤ b max , because the feedback towards zero disappears.

  In the following simulation, b max = 1 • /s and τ b = 10∆t, ∆t = 0.01 s. The bias covariance V bω is tuned to a 3σ drift rate of 0.1 • /s, giving V bω = I 3 (0.01 π 180 /3) 2 rad 2 /s. The measurement covariances used are V ω = I 3 (10 π 180 /3) 2 rad 2 /s 2 , W a = I 3 (||a ref ||/3) 2 m 2 /s 4 , and W m = I 3 (||m ref ||/3) 2 G 2 .

Figure 6 Figure 6

 66 Figure6.1: Heading-and bias standard deviation estimation comparison between an EKF using a random walk bias model and an EKF using a saturated bias model during an extended period of unobservability.

Figure 6 . 3 :

 63 Figure 6.3: Overview of the NL-KF attitude lter.

  2.1 and are given here in a condensed algorithmic form: Nonlinear decoupled Kalman lter (NL-KF) Inclination correction : If a_status = 1: xα = q+ ⊗ δq α,û bω, + ûk α δα = g α x+ ,

Figure 6 .

 6 Figure 6.4 shows the result of the Monte-Carlo batch.

Figure 6 . 4 :

 64 Figure 6.4: Estimated convergence times in the inclination (α) and heading (ψ) estimation versus tuned convergence times, τ α and τ ψ .

Î

  = { Îa , Îm , Îω } -The invariant output error: E = a_status I 3 a ref -R a m m_status I 2 ν m,⊥-The state error:

Figure 6 .

 6 Figure6.5 shows the quaternion error convergence for the DEC-IEKF as an angular error is simulated at 10 s with an increasing amplitude.

  Figure 6.6: Convergence comparison of DEC-IEKF linearised around zero error VS DEC-IEKF with covariance majoration through multiple linearisation points. Simulated error in φ = 30 •

  Figure 6.7: Convergence comparison of DEC-IEKF linearised around zero error VS DEC-IEKF with covariance majoration through multiple linearisation points. Simulated error in φ = 80 •

  Figure 6.8: Convergence comparison of DEC-IEKF linearised around zero error VS DEC-IEKF with covariance majoration through multiple linearisation points. Simulated error in φ = 130 •

Figure 6 Figure 6

 66 Figure 6.9: Convergence comparison of DEC-IEKF linearised around zero error VS DEC-IEKF with covariance majoration through multiple linearisation points. Simulated error in φ = 180 •

Figure 6 Figure 6 Figure 6 Figure 6 Figure 6

 66666 Figure 6.12: Scenario 1 (Acc. and Mag. Disturbances): Euler angles VS estimated Euler angles by NL-KF with 3σ-uncertainty bound.

Figure 7 . 2 :

 72 Figure 7.2: Overview of the fault tolerant architecture.

6. 4 .

 4 1 for the MEKF-based lter of Section 6.4.2. The rst point partially and second point were remedied by introducing the open-loop gyroscope bias estimation and the associated switching logic. The third and last point was handled by introducing the GLR-based fault detection step.

  and 7.6. The resulting quaternion errors are shown in Figures 7.4 and 7.7. The Magnetic disturbance example 1: Sinusoidal disturbance Comments: The sole norm-based rejection (DEC-IEKF-2) is clearly not sucient to reject this kind of a disturbance. Either the disturbance is detected too late, with a serious attitude drift as a consequence, or it is not detected at all. The model based rejection added in DEC-IEKF-3/4 signicantly improves the disturbance rejection.

Figure 7 . 4 :Figure 7 . 5 :

 7475 Figure 7.3: Magnetic disturbance example 1, sinusoidal disturbance.

Figure 7 . 7 :

 77 Figure 7.6: Magnetic disturbance example 2, stepwise disturbances.

  Figure 8.1: Experimental setup in the ight arena with the quadcopter, current loop and optical tracking system marked.

  5deg/s. The bias saturation was set to b = 3 deg/s, and the anti-windup gain τ b = 1 s. The low-pass gyroscope bias lter time constant was set to τ LP = 100 s. The sensor performance models' response times were set to 3 s in both the acceleration and magnetic eld estimators.

Figure 8 . 3 :Figure 8 . 4 :Figure 8 . 5 :Figure 8

 8384858 Figure 8.2: Reference attitude from optical tracking system: Trajectory 1.

Figure 8 . 7 :

 87 Figure 8.7: Reference attitude from optical tracking system: Trajectory 4.

Figure 8 Figure 8

 88 Figure 8.8: Accelerometer measurement and norm VS body frame reference: Trajectory 4.

Figure 8

 8 Figure 8.14: Magnetometer measurement and norm VS body frame reference: Trajectory 7.

Figure 8

 8 Figure 8.16: Heading error consistency: Trajectory 7.

8. 3 . 4 Figure 8

 348 Figure8.17: Reference attitude from optical tracking system: Trajectory 10.

Figure 8 . 18 :Figure 8

 8188 Figure 8.18: Accelerometer measurement and norm VS body frame reference: Trajectory 10.

y

  t+1 = C x t+1 + w t+1 + F y Γ y, t+1-k b (A.1)The functions Γ u, t-k and Γ y, t-k are the input and output fault signatures, representing the appearance of a bias at time k. A constant bias is modelled as Γ * , t-k = IY t-k , and a drift asΓ * , t-k = (t-k)Y t-k ∆t.b is the magnitude of the bias (e.g. amplitude or slope), and F u and F y are the static distribution matrices of the bias on the input and output respectively. Clearly if we position ourselves at a time t ≥ k and augment the state such that x a = (x, b) T we getx a,t+1 = Aa A F u 0 I x t + B u t + v t y t+1 = (C F y ) Ca x a,t+1 + w t+1 . (A.2)Given that the system is linear, it is trivial to construct the observability matrix:

  For a system as (A.1), we can write the Kalman lter recursion when t < k as: Predictionx+ t+1 = A xt + B u t P + t+1 = A P t A T + Q Correction xt+1 = x+ t+1 + K t+1 ν t+1 P t+1 = [I -K t+1 C] P + t+1 (A.4)Where P t denotes the state covariance. The innovation, its covariance and the Kalman gain are in turn given by:ν t+1 = y t+1 -C x+ t+1 S t+1 = C P + t+1 C T + R t+1 K t+1 = P + t+1 C T S -1 t+1 (A.5)matrices on the state estimation and the innovation Φ t,k and φ t,k respectively. (A.1) through (A.7), we get a recursive expression for calculating the fault signatures on the innovation and the state (that is, the time response of the Kalman lter's innovation to the occurrence of a bias).

  Figure A.2: Measurement and biased innovation at the appearance of a constant bias on the measurement.

Figure A. 3 shows

 3 Figure A.3: Example of an application of the GLR algorithm to estimate a measurement bias. Note that the bias estimation converges as long as the bias remains within the sliding window, i.e. as long as it is observable.

  Figure B.4 illustrates this case for a multi-rotor drone observing a point on the ground, for example during an autonomous landing. For a UAV navigating in a tight space, this maximum error might be in the order of 0.1 m while for a drone performing a vision based landing it might be in the order of 5 m.A reprojection error due to an inclination error is depicted inFigure B.4

Figure B. 4 :

 4 Figure B.4: Example of alert limit, AL ε α , for a multi-rotor drone during visual navigation, calculated from a reprojection limit ∆d and the height above ground h.

  The position estimation problem . . . . . . . . . . . . . . . . . . . . . . . . . 19

	Chapter 2
	State of the art
	Contents
	2.1
	on Guidance, Navigation, and Control, EUROGNC, 2019, Milano, Italy
	[Öman Lundin et al., 2019a], G. Öman Lundin, P. Mouyon, A. Manecy; A fault tolerant attitude
	estimation architecture with GLR-based disturbance rejection; 4th International Conference on
	Control and Fault-Tolerant Systems, SYSTOL, 2019, Casablanca, Morocco

Mouyon, A. Manecy; A GLR algorithm for multiple consecutive measurement bias estimation; 4th International Workshop on Research, Education, and Development on Unmanned Aerial Systems, REDUAS, 2019, Linköping, Sweden [Öman Lundin et al., 2019b], G. Öman Lundin, P. Mouyon, A. Manecy; Experimental study of an attitude estimator with measurement disturbance rejection; Proceedings of 5th CEAS Conference 2.1.1 Position estimation without position sensor bias . . . . . . . . . . . . . . . . . 19 2.1.2 Impact of position sensor bias . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.1.3 The Generalized Likelihood Ratio (GLR) algorithm . . . . . . . . . . . . . . . 21 2.1.4 Position estimation integrity with non-redundant measurements . . . . . . . . . 22 2.2 State of the art -Fault-tolerant position estimation . . . . . . . . . . . . . . 22 2.2.1 Bias estimation using the Generalized Likelihood Ratio . . . . . . . . . . . . . 23 2.2.2 Bias estimation using nonlinear ltering . . . . . . . . . . . . . . . . . . . . . . 24 2.2.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2.3 The attitude estimation problem . . . . . . . . . . . . . . . . . . . . . . . . . 25 2.3.1 Attitude representations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 2.3.2 Modeling for attitude estimation . . . . . . . . . . . . . . . . . . . . . . . . . . 29 2.3.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 2.4 State of the art -Attitude estimation algorithms . . . . . . . . . . . . . . . 32 2.4.1 Fault-free attitude estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 2.4.2 Fault-tolerant attitude estimation . . . . . . . . . . . . . . . . . . . . . . . . . . 37 2.4.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

  norm. In this case, we want to test the zero hypothesis ||r m ||| -||r * ref ||. For that purpose, we introduce a norm sensitive residual, i.e.

ε ||rm|| = ||rm|| -||r * ref || .

Table 5

 5

.1: Short-and long term eect of various erroneous signals on the fault sensitive residuals.

  Figure 5.8: Principle of detection boolean generation from a fault sensitive residual. Test if ||r m || is OK r_outlier: Test if r m is OK r_warning: Test if r is OK r_status: Test if r OR r m is OK

The fault detection booleans are directly calculated by statistical testing of the fault sensitive residuals ε ||rm|| , ε rm , and ε r ref according to Figure

5

.8. We denominate the detection booleans r_norm for ε ||rm|| , r_outlier for ε rm , and r_warning for ε r ref . These booleans are then used to calculate a status boolean, r_status, which indicates whether the consolidated sensor data, denoted r c , is good or not for attitude update. The hypotheses associated these booleans are the following: r_norm:

Table 5

 5

.2: Sensor data consolidation from status booleans

  .

	Following this reasoning, the attitude rate bias consolidation logic is dened as:
	if a_norm == 1 OR a_outlier == 1 :
	bω,x,c		bω,LP,x
	bω,y,c	=	bω,LP,y	(5.21)
	if m_norm == 1 OR m_outlier == 1 :
	bω,z,c = bω,LP,z

  Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1.1 Motivation and principles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1.2 Choice of baseline attitude lters . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1.3 Chapter structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

	Chapter 6
	Development of fault tolerant attitude
	estimation lters
	Contents
	6.1

6.2 Disturbance decoupling strategies for attitude estimation Kalman lters . 6.2.1 Disturbance decoupling through partial inclination/heading quaternion correction129 6.2.2 Disturbance decoupling through pseudo measurements . . . . . . . . . . . . . . 6.2.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  Used to detect erroneous measurements based on the residuals from the sensor output lters and a predened probability of false alarm using:

	Simple χ 2 -tests
	GLR tests
	Any other statistical tests
	This yields a set of fault detection ags, indicating a problem with the measurement or the sensor
	output lters.

  Table 8.1. The best (smallest) result in each metric is presented in bold font. .28 / 14.21 / 14.21 / 14.21 4.74 / 5.16 / 5.31 / 5.74 15.50 / 22.72 / 19.78 / 5.81 7.06 / 11.16 / 9.29 / 3.11 || 17.17 / 15.68 / 15.67 / 16.00 4.56 / 4.80 / 4.69 / 5.09 15.55 / 21.48 / 17.84 / 6.00 8.40 / 12.26 / 8.97 / 1.36 || 12.26 / 13.57 / 12.11 / 13.14 4.31 / 5.52 / 4.85 / 5.37 7.57 / 12.37 / 9.32 / 4.15 3.46 / 7.26 / 5.28 / 1.71 Cross + Mag. dist. 12.28 / 11.58 / 10.31 / 10.45 4.19 / 4.55 / 3.74 / 3.89 9.59 / 20.67 / 7.89 / 2.88 3.35 / 11.50 / 3.26 / 1.55 || 17.80 / 17.60 / 17.60 / 17.60 5.68 / 7.37 / 6.02 / 6.64 10.27 / 32.33 / 5.94 / 3.02 5.02 / 15.62 / 2.34 / 1.44 || 13.07 / 11.97 / 12.03 / 11.84 4.25 / 3.82 / 4.07 / 4.18 9.68 / 30.34 / 8.91 / 3.02 4.02 / 12.31 / 4.71 / 1.57 Slow + Mag. dist. 8.44 / 16.25 / 10.26 / 10.92 3.21 / 9.95 / 4.08 / 4.15 14.54 / 159.03 / 4.54 / 4.01 6.63 / 68.91 / 2.90 / 2.57 || 7.82 / 17.65 / 9.45 / 11.48 2.95 / 8.00 / 4.11 / 4.90 13.32 / 160.07 / 7.40 / 4.95 8.42 / 73.17 / 5.30 / 3.27 || 10.75 / 10.65 / 12.07 / 12.73 3.28 / 3.34 / 3.92 / 4.11 18.94 / 72.35 / 11.82 / 4.01 10.79 / 34.85 / 7.34 / 2.29 Fast + Mag. dist. 10.60 / 11.17 / 11.89 / 12.32 4.04 / 4.70 / 4.16 / 4.69 16.98 / 81.35 / 11.03 / 5.86 12.02 / 46.01 / 8.05 / 3.94 || 10.62 / 7.68 / 9.80 / 9.73 3.94 / 3.36 / 4.57 / 4.84 7.37 / 70.88 / 3.76 / 1.52 1.98 / 39.04 / 0.90 / 0.54 Table 8.1: Error statistics for FT-KF / IEKF / FT-IEKF-χ 2 / FT-IEKF-GLR

				|θ -θ| + |φ -φ|		|ψ -ψ|
	#	Description	max	rms	max	rms
		Cross traj.

14

  H 0 : ν t = ν n t No bias present (k > t) H 1 : ν t = ν n t + φ t,k b Bias present (k ≤ t)

							Willsky GLR
		3
			y
	Output [m]	0 1 2	y m	L
		-1
		0	2	4	6	8	10	12	14	16	18
		0	2	4	6	8	10	12	14	16	18
							Time [s]
											(A.11)

If we use this measurement in a Kalman lter, the magnetic estimation error (innovation) is dened as:ν = R(q) T (a ref × m ref ) -m m,⊥ = R(q) T I -R R T (a ref × m ref ) .

Acknowledgements

Acknowledgements v

List of Abbreviations

General Chapter 6. Development of fault tolerant attitude estimation lters Summary T his chapter discusses fault tolerant strategies for attitude ltering. After having seen a strategy for measurement consolidation in the previous chapter, we focus on fault tolerance aspects of attitude lters.

The rst topic is the saturation of bias estimates and how to properly propagate the covariance of the estimate.

The second one concerns the problem of decoupling the roll and pitch axes from the yaw axis.

Based on the former analyses, two attitude lters adopting these fault tolerant strategies are proposed to illustrate the improvements with regards to fault tolerance.

The rst one is based on the EKF formalism using quaternion corrections whilst the second one is based on the IEKF formalism.

The chapter ends with small comparative study of the decoupling properties of the proposed lters and some lters found in the literature.

The second one uses a pseudo measurement that transforms the magnetic measurement to only provide information in the plane perpendicular to gravity, eectively eliminating the information on the magnetic dip angle, i.e. the inclination.

Disturbance decoupling through partial inclination/heading quaternion correction

Just as it is possible to structurally decouple the estimation of the roll/pitch angles from the yaw angles, we can also decouple the inclination and heading attitude representation.

Indeed if we write the estimated attitude as a quaternion using the inclination/heading representation, we have: q = q ψ ,e 3 ⊗ q α,u .

(6.7)

If we left-multiply by a heading error quaternion δq (cos(∆ψ/2), 0, 0, sin(∆ψ)) T calculated from the current quaternion and the magnetometer measurement we can eectively correct the heading without correcting the inclination: qc = δq ∆ ψ ,e 3 ⊗ q ψ ,e 3 ⊗ q α,u = q ∆ ψ + ψ ,e 3 ⊗ q α,u .

In the same manner, if the inclination axis u (u 1 , u 2 , 0) can be found, we can produce a correction quaternion δq (cos(∆α/2), u 1 sin(∆α), u 1 sin(∆α), 0) T which will be invariant of the heading error, i.e. qc = δq ∆ ψ ,e 3 ⊗ q α,u ⊗ δq ∆ α,u = q ψ ,e 3 ⊗ q α+∆α,u .

If the error quaternions δq e 3 ,∆ ψ and δq u,∆ α can be found, it should be possible to correct the heading without impacting the inclination and vice versa. In order to accomplish this, we propose the following decoupling procedure [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF]:

1. Decoupled inclination correction (a) Find the estimated inclination axis û invariant of yaw.

(b) Find the inclination angle α using Rodrigues' formula.

(c) Calculate the inclination matrix R α, û using Rodrigues' formula.

(d) Calculate the inclination error δα, i.e. the innovation.

(e) Calculate the correction quaternion δq α from δα and û.

(f ) Calculate the bias corrections for bω,x and bω,y using the error angle δα.

Decoupled Heading correction

(a) Find the planar magnetometer error y (y 1 , y 2 ) from q, m ref , and m m .

(b) Calculate the heading error angle δψ, i.e. the innovation, from the normalised magnetometer error components.

(c) Calculate the correction quaternion δq ψ from δψ and e 3 .

(d) Calculate the bias correction for bω,z using the error angle δψ. less consistent than the DEC-IEKF-3 and DEC-IEKF-4. One might argue that it is better to be marginally consistent with a small absolute error, than completely consistent with a relatively larger error.

Evaluation conclusion

The above two examples are by no means an exhaustive evaluation of the fault tolerant mechanisms and the complete architecture presented in this chapter. Having said so, it does cast the light on some key points that reinforce our choices regarding the individual fault tolerant mechanisms:

• Norm rejection based on single sample testing is nearly useless in realistic noise scenarios and disturbances with limited impact on the measurement norm.

• Model based, or innovation based rejection, is a must for any seriously fault tolerant architecture.

• Redundant gyroscope bias estimation through low-pass ltering is a promising method for reducing attitude drift during long-lasting perturbations in UAV applications.

• Sensor output estimation based on sensor performance models are useful for limiting the growth of uncertainties during long-lasting perturbations. However the trade-o is a risk of attitude drift due to the unobservability of the measurement disturbance.

• Powerful statistical tests such as the GLR test are useful to achieve fast detection and limit attitude drift due to poor rejection of transient disturbances.

Notably the consistency question raises the related question of estimation integrity. Simply speaking, an estimation integrity test allows to set a limit on the estimated uncertainty. All ight tests were performed with the same quadcopter and the data was recorded on-board with a PX4 module. The recorded data were the raw gyroscope, accelerometer, and magnetometer signals, plus the position and angles from the optical tracking system. The sampling rate of the accelerometer and gyroscope was on average 190Hz and for the magnetometer roughly 90Hz. The Optitrack measurements (position and angles) are sampled at 50Hz. An a posteriori analysis of the recorded data indicated that the gyroscope and accelerometer signals were unfortunately recorded without coning and sculling corrections respectively. This should be kept in mind when assessing the inclination estimation results. Suspicion regarding this was raised as the on-board attitude estimation (deliver by the PX4 autopilot) under nominal conditions had a standard deviation which was a magnitude lower than the one found with the replayed data. This dierence persisted even with pure gyrometer integration compensated o-line for the low frequency bias. This is the reason why no comparison with the on-board PX4 estimation is presented.

The collected IMU data was replayed o-line with the following lters:

1. The fault tolerant architecture from [START_REF] Lundin | A fault tolerant attitude estimation architecture with GLR-based disturbance rejection[END_REF]. (FT-KF) 2. Standard IEKF with only norm rejection of measurements (IEKF). This lter correspond to the lowest level of fault tolerance, and the computationally cheapest, found in the literature.

Evaluation criteria

In order to evaluate the attitude estimations we investigate three evaluation criteria. These three criteria are the estimation error, the estimation consistency, and the estimation integrity. The objective is to nd the estimator which passes the three steps of evaluation with the largest possible margin. The estimation error should be as low as possible (small bias and standard deviation), consistent and with integrity as often as possible, and with the lowest possible level of protection.

• Estimation error When evaluating the rst criterion we look at the absolute angular errors in roll, pitch, and yaw, i.e.

When analysing trials, we often look at the maxima of this criterion over time. Their occurrences generally correspond to the appearance of faults. Obviously its mean value and its standard deviation (σ ξ ) are also important.

• Estimation consistency The second criterion is the coherence of the estimation error ξ along with the estimated standard deviation σ ξ delivered by the Kalman lter. To be consistent, the estimation error ξ should remain lower than σ ξ .

• Estimation integrity When evaluating the integrity of the estimation, we calculate the protection level (P L), and perform an integrity test by comparing it to a predened alert limit (AL). In Chapter 8. Experimental evaluation of a fault tolerant attitude estimation architecture

The FT-KF was tuned with 10 s response time in heading, 5 s in inclination and 100 s in gyroscope bias estimation.

Supervision. The statistical thresholds of the sensor FDI were all set to a false alarm rate corresponding to 3σ for the magnetometer and the accelerometer (roughly one accelerometer measurement per second is rejected, and one magnetometer measurements is rejected each two seconds). The GLR window length is 50 samples (≈0.25 s for acc. and ≈0.5 s for mag.). The alert limit in inclination (AL α ) was set to 15 • , corresponding to a displacement of 1.5 m in 1 s from standstill. The alert limit in heading (AL ψ ) was set to 10 • . The integrity risk in both cases was set to P ir = 10 -7 .

Benchmark trajectories

A total of 11 ight tests of 4 dierent types were performed:

1. Fast cross trajectory without disturbance (# 1, 2, 3): A dynamical cross trajectory is own in order to provoke transient inertial accelerations.

2. Fast cross trajectory with magnetic disturbance (# 4, 5, 6): The same dynamical scenario as above but overying a zone of magnetic perturbations that additionally provoke transient perturbations in the magnetometer.

3. Slow trajectory over magnetic disturbance (# 7, 8, 9): A slow approach to a magnetically disturbed zone and a prolonged hover-like state within the zone. This resembles a UAV ying into for example a built environment or near a high voltage installation or industry for inspection.

Magnetic disturbance (# 7-8) -mainly oriented along inertial z-axis, #9 -mainly oriented along inertial y-axis.

4. Fast trajectory past magnetic disturbance and lingering in a disturbed environment (# 10, 11): A one axis fast trajectory past a magnetically perturbed zone, followed by fast approach and a 30 second residence within the zone. This is a scenario that mixes the above scenarios and can be imagined as a patrolling drone suddenly encountering a disturbed magnetic environment.

For each type of trajectory, the sensor data and estimation results are only shown for one case.

The estimation error, consistency, and integrity for the cases not shown are very similar to the ones shown for each trajectory and discrepancies are only commented in the text. The error statistics for all trajectories are summarised in Table 8.1.

Fast cross trajectory without magnetic disturbance

We only show the sensor data and estimation results for Trajectory 1.

Trajectory 1

Comments

Measurements: Trajectories (#1, #2, and #3) are dynamical cross trajectories without disturbance.

The drone rst y along the x-axis as it can be seen on the reference acceleration a ref,B,x until t ≈ 20 s. Consistency: The inclination estimation consistency is largely equivalent to the one found for the Trajectories #1, #2, and #3. That is to say in general consistent apart from a few transients. The IEKF-based architectures are in general more consistent than the FT-KF since the latter is subject to larger transient errors and shows a higher variability in its estimated standard deviation.

The heading estimation consistency is also signicantly improved with the addition of the measurement consolidation. However this improvement is mostly due to the decrease of the mean estimation error with the increasing complexity of the fault rejection. The best heading consistency is achieved with the FT-IEKF-GLR architecture, which is the only one with perfect consistency in all three trajectories.

The χ 2 -based architectures, FT-KF and FT-IEKF-χ 2 are on par with each other. Both slightly overestimate the uncertainty under nominal conditions and suer from some transient drifts due to missed detections during perturbations.

Integrity: The inclination integrity remains equivalent to Trajectories #1, #2, and #3 with no integrity alert.

The heading estimation is detected as integrate for the three IEKF architectures for Trajectories #4

and #5. The FT-KF architecture presents a drift due to measurement rejection, leading to an integrity alert in the same trajectories. In Trajectory #6, the FT-KF protection level is clearly underestimated at several occasions and the error draws dangerously close to the alert limit while the protection level remains low.

We also see an overestimation of the standard deviation for the FT-IEKF-GLR in trajectory #6.

This overestimation is partly due the articial increase in covariance already mentioned in the lter parametrisation in Section 8.2.2.

The IEKF is behaving very badly since the eective heading error far exceeds its estimated protection limit for all trials.

Slow trajectory over magnetic disturbance

We only show the sensor data and estimation results for Trajectory 7.

Trajectory 7

Comments

Measurements: The slow and large variations of the magnetic disturbance are easily seen on the gures. The acceleration measurements remains centered.

Estimation error: The inclination estimation error remains small throughout all three trajectories.

The heading estimation error varies greatly between the dierent architectures although it is coherent with the incremental developments described in Chapter 7. The nominal IEKF does not manage to reject the disturbance at all. The FT-KF rejects most of it but suers from a residual drift, and the FT-IEKF-χ 2 rejects most of the disturbance with less drift. Finally the FT-IEKF-GLR reject almost the entire disturbance and shows no sign of drift thanks to the early detection with the GLR-test. We also see a better rejection for trajectories #7 and #8 than for #9 since the magnetometer norm is more Chapter 9

Discussion and perspectives T his chapter discussed the obtained results and the pros and cons and utility of the developed techniques in a global fault tolerant navigation framework. In the end, some possible future openings of the proposed work are proposed and are put in perspective with other potential methods that have not been the scope of this thesis.

Perspectives

The contributions contained in this thesis are only a small step in the direction of generalised safe and integrate drone navigation. We can extend some lines for further investigation.

For the fault-tolerant position estimation in the short term it would be interesting to look at the following aspects:

Clarify the expression for the post-detection state estimation covariance. This is important to properly eliminate residual bias and to asses the consistency of the state estimate.

Real-time implementation and evaluation of the developed MGLR algorithm on a drone platform with GNSS-RTK ground truth.

Development of a mechanism to detect biases older than the last detected bias. In their current forms, the GLR and MGLR suppose that the any new bias will have appeared after the last detection, up to the current time.

In the long term, some major modications of the algorithm should be assessed:

The re-identication step in Chapter 3 can be replaced by an estimator robust to non-Gaussian disturbances (e.g. an M-estimator or a particle lter).

The bias elimination step is Chapter 3 can be reworked to minimise the a posteriori probability of false alarms.

For the fault-tolerant attitude estimation, the short term perspectives revolve mainly around the real-time application:

Real-time implementation of the FT-IEKF-GLR version of the algorithm and evaluation in an operational scenario.

Rene the tuning considerations for τ in the performance models, eventually tending towards an automatic tuning based on environmental information or ight point.

In the long term, some key points remain to be evaluated:

Asses the elephant in the room, the dependence on gyroscope. In this aspect it could be fruitful to integrate detection and accommodation of gyroscope errors other than a quasi-constant bias.

In Chapter 6, dierent sensor performance models can be used, for example based on dynamic models. This would in turn require to develop or integrate actuator fault detection schemes to limit the propagation of actuator faults to the sensor fault detection.

Extensions of the fault detection analysis can include the smallest disturbance to be detected to guarantee that the angular error stays below a given limit. This is particularly important from an integrity point of view and crucial for certication procedures.

On a higher level of analysis, some other perspectives appear, which can extend beyond the scope of the algorithms developed herein:

The MGLR algorithm can also be use to detect input biases. Can this be used to prime the attitude estimator or is it redundant?

Résumé

T his chapter introduces the problem of estimating sensor or actuator biases in the non- redundant case for linear systems. A brief analysis of the implications for the observability and controllability of the biased system shows that it is only observable/controllable if the bias apparition time is known. It then continues by presenting the GLR algorithm which handles this problem by xing the apparition time through multiple hypothesis testing, and xing the bias amplitude through a maximum likelihood estimation. The drawbacks of extending the GLR algorithm to a multiple bias case are discussed at the end. A.2 The GLR algorithm for non-redundant state estimation

As mentioned above, the state and the bias can be conditionally estimated if the bias apparition time k is known. The GLR algorithm solves this by setting up a bank of bias estimation lters annex to the Kalman lter working under the zero-hypothesis t < k, i.e. the system is fault-free (Kalman lter under H 0 ). Each of the bias estimation lters assumes an apparition time k i < t, corresponding to a fault signature which can be calculated thanks to the linearity of the Kalman lter. The fault signature of each bias lter is then matched to the innovation through LS estimation to nd the most likely bias amplitude. A global maximization is then performed over all bias lters to nd the maximum likelihood estimate of k, denoted k. The bias estimation is taken as the estimation of the bias lter given k i = k.

When the apparition time k and the bias amplitude b have been determined, the correction of the state estimate and its covariance, ∆x and ∆P , can be calculated and the Kalman lter under H 0 be re-initialised.

A.2.1 Fault signatures

The key component of the GLR algorithm is the fault signature [START_REF] Willsky | A generalized likelihood ratio approach to state estimation in linear systems subjects to abrupt changes[END_REF][START_REF] Palmqvist | Integrity monitoring of integrated satellite/inertial navigation systems using the likelihood ratio[END_REF],

i.e. the normalised lter response to a fault of a known form. The role of the fault signatures is to provide a deterministic response of the observer which can then be used to deduce the amplitude of the actual fault. In the linear case we can separate the fault-free response and the faulty one through superposition. For the system dynamics (A.1) it is written as

where n corresponds to the nominal stochastic dynamics and f to the deterministic fault eect.

Thanks to linearity we can also isolate in the Kalman lter dynamics the deterministic eect caused by the occurrence of a bias from the stochastic eect due to noise. We denote the fault signature Statistical hypothesis testing classically relies on the likelihood ratio LR t,k,b . Since the Kalman lter delivers statistically uncorrelated innovations (recall that b is assumed to be deterministic), this translates into a product of the likelihood ratios at each time instant. Hence,

The noise aecting the innovation is Gaussian with covariance S, thus the probability density function is written as:

The same stands for p(ν j |H 0 ) but with b = 0. The log-likelihood ratio, given as twice the logarithm of (A.12) is then

where (A.16)

The maximum with respect to b is easily found by dierentiation. It corresponds to:

bt

With this optimal value, the log-likelihood ratio and the generalized likelihood ratio become

The time instant k where the fault occurs is estimated by

Obviously the calculation of f t,k and Λ t,k may be done recursively. The same is true for bt,k . It can be shown that:

A.3. The GLR detector 207 These equations correspond to the estimation of a constant bias b, under the hypothesis that the fault occurs at time k, by a Kalman lter written in the information form.

A.3.3 Detection

By inverting (A.17), the likelihood ratio statistic in (A.18) can be written as:

Since bt,k is Gaussian with covariance matrix Λ -1 t,k , we conclude that, at each time t, the l LR,t,k is distributed as a χ 2 (n b ) where n b = dim(b). The same is obviously true for the GLR statistic l GLR,t = l LR,t, k.

From the cumulative distribution function of the χ 2 a detection threshold l GLR,det is then easily calculated given a probability of false alarm P F A . We can write the hypothesis test as follows:

The false alarm probability P F A and the non-detection probability P N D are:

Under the hypothesis H 0 (no bias present), the statistic is a centred χ 2 while it is not centred under H 1 . Its cumulative distribution function is denoted by F χ 2 (x; n b , µ b ) where µ b = b T Λ t, kb is the non-centrality parameter. So from their denitions, we see that:

The rst relation allows to compute the detection threshold given the false alarm rate.

Note that this detection threshold gives information about the hypothesis H 1 . Indeed there is no detection until bT Λ b < l GLR,det . So the greatest undetectable bias is:

In a similar way we can also dene the minimal detectable bias b. They are both equal in the scalar case, but may dier otherwise. The solution of the optimisation problem is:

The detection threshold being xed by the choice of P F A , the second relation in A.23 shows there is a unambiguous link between P N D and µ b . Indeed the noncentral chi-squared distribution F χ 2 considered as a function of µ b is a strictly decreasing function (we shall denote Fχ 2 ).

If one xes both P F A and P N D , it is also possible to compute the non-centrality parameter µ b .

Because by denition µ b = b T Λ t, kb, this yields to a similar denition of the greatest undetectable bias but with l GLR,det replaced by µ b .

In case of detection, we denote the corresponding lter date kdet , and the lter state bmax , and Λ -1 max its covariance matrix. The correction of the state and the measurement is then applied according to (A.9).

By nature, the GLR algorithm presented here-above allows only the detection of a single failure.

Its exact extension to the multi-fault case is complex because the number of hypotheses to be tested increases as well as the number of statistics to be computed. We will therefore propose a modication which allows the classical GLR to search for only a single bias at a time but simultaneously re-identify all previously detected biases. In practice, we will eectively replace the hypothesis "H 0 : No bias present before t k " of the GLR detector by "H k-1 : All biases present before t k detected and estimated".

Clearly the GLR detector will in this case loose its optimality since the fundamental hypothesis of no bias present is replaced by an approximate one which relies on a precise identication of detected biases.

A.3.4 Example of bias detection with GLR

Consider the position estimation problem in 1-D where the available sensors are an accelerometer and a position sensor (such as a GNSS receiver). The accelerometer is considered fault free but provides a noisy output with a Gaussian distributed white noise. The position sensor is subject to a Gaussian white noise and an unknown constant bias appearing at an unknown time. The model used is:

p ) and v a ∼ N (0, σ 2 a ). In the example below, the noise standard deviations are set to σ p = 1/3 m and σ a = 1/3 m/s 2 . The GLR parameters are the sliding window length L, and the probability of false alarm P F A . Here we have chosen L = 4s and P F A = 10 -6 . This appendix is divided into four part parts. The rst parts is an introduction to the qualication of a navigation system, in terms of estimation, precision, and integrity. The second part concerns the case where the state estimation is obtained with only fault-free sensors. This is the hypothesis used for checking the integrity of the attitude estimation since the information used by the attitude lter is already consolidated, and therefore fault-free by hypothesis. The third part considers the case where the sensors used are subject to randomly appearing biases. The approach in this case is slightly dierent since the method of detection enters into the calculation, which complicates the calculation of the integrity. The last part explains how integrity alarm levels can be dened for drone attitude estimates. For that purpose we make a link between attitude and translation errors, since alarm levels are commonly dened in the translation domain.

For simplicity we will investigate the integrity for a linear Kalman lter based position estimation.

The extension of the results to nonlinear estimation, as for the example of attitude estimation, is straightforward but the results are only approximative under the linearisation hypotheses of the EKF.

B.1 Qualication of a navigation system

We recall herein the denitions of the main quantities used to qualify the behaviour of a navigation system [START_REF] Mouyon | Navigation intègre pour l'atterrissage basé vision[END_REF].

B.1.1 Positioning

• The estimated position

The considered position estimation X is the one given by the navigation system. The positioning error δ = X -X is often projected on the horizontal plane (δ H : north n, and east e axes) and on the vertical axis (δ V : down axis d).

• Precision

The precision corresponds to the F OM limits (Figure Of Merit ) on the RMS level σ of the error. These limits generally translate the precision into a condence interval of 95%: F OM = 2 σ. They are dierent according to the axis considered (σ H , σ V , F OM H , F OM V ).

• Alert limits

The safe accomplishment of a mission supposes that the navigation error does not cross certain limits. These limits (denoted AL H and AL V ) are the maximal admissible positioning errors for the considered operations.

• Positioning failure A positioning failure is declared once the positioning error exceeds one of the alert limits, i.e.

B.1.2 Integrity

Other than the position estimation, and its associated precision, the navigation system consists of a stage of supervision in charge of emitting alerts as soon as it is incapable of guaranteeing the absence of a position failure. These alerts must be emitted wisely and on timely.

But as with all decision taking, one may be wrong: the supervision stage may not issue an alert when it should have, or may issue an alert in an untimely manner. These are the problematic events in navigation. These situations are characterized by a probability of non-detection (called integrity risk P ir ), and a probability of false alarm (called probability of false alert P f a ). The quantities introduced below are used to describe these events.

• Problematic events

The navigation system should thus emit alerts when and only when the uncertainty of the positioning is too large. Erroneous decisions constitute the problematic events:

Non-detection of a position failure, on time to alert T T A horizon, |δ| > AL, given that no positioning alert has been emitted before T T A seconds have elapsed. We associate to it the integrity risk P ir False alert of positioning failure |δ| < AL, given that a positioning alert has been emitted. We associate to it the probability of false alert P f a

The loss of integrity (|δ| > AL) is obviously dangerous. But the false alert leads to the loss of the navigation system and thus the abandonment of the ongoing operation (mapping, landing, refuelling, etc.) despite that it could have been continued.

• Integrity characteristics

The integrity is the capacity of the system to emit alerts when the error crosses an alert limit (AL H or AL V ) in an allotted time (T T A), and with a given risk (probability of non-detection, P ir ). The integrity is therefore characterised by divers quantities. The alert delay T T A is the maximum time lapse authorised to detect a positioning failure before integrity is lost. For the sake of concision, we have introduced acronyms to replace the expressions relative to the emission or non-emission of a positioning failure alert.

Integrity

B.1.3 Availability and continuity of service

In order for the navigation function to remain useful, it is not enough for the position estimate to be integrate. The periods during which the integrity is veried must cover a as much time as possible of the mission, and should not be too fragmented. Availability and continuity are the two quantities used to qualify this.

• Availability

The availability is the capacity of the system to detect a positioning failure while respecting the performance constraints. It is measured as a percentage of time AV AIL.

• Continuity of service

The continuity is the capacity of the system to detect and correct a positioning failure while respecting the performance constraints. It is measured by the probability of interruption of service per operation P is .

B.2 Integrity with fault-free sensors

In this section we investigate integrity in the case of a fault-free system. Supposing perfect sensors simplies the problem of navigation integrity. The analysis of the ideal case allows for one part to introduce the functions for position estimation and integrity verication. It also allows to study the nominal performance. We will also recall some fundamental mathematical which will be used in the general case of navigation using sensors subject to faults.

B.2.1 Operational procedure

When the sensors are fault-free the position estimation and integrity verication are fairly simple to develop.

-The estimation uses a Kalman lter which provides an estimation of the position ( X) and a description of the associated uncertainty (V ).

-The verication of the integrity is made via a comparative test. The value of the uncertainty allows to calculate a limit for the position error, called protection level (P L), which is a threshold guaranteed by the chosen probability 1 -P ir . If this level crosses the chosen alert limit (AL), the integrity is no longer guaranteed and an alarm is emitted.

• Position estimation

From the measurements and a model, the estimation funtion is :

• Calculate the position ( X)

• Calculate the precision of the positioning (V )

• Integrity testing

From the innovations and a model the integrity test function is : • Verify that the measurement allow to guarantee the integrity (AL, P ir )

The procedure of integrity verication described here-above is called direct. It is the one we will use be using henceforth. It is also possible to proceed in an indirect fashion. In this case the uncertainty (V ) is used jointly with the chosen alert limit (AL) to calculate the probability to exceed this limit.

We then verify that this probability does not exceed the chosen integrity risk (P ir ).

B.2.2 Position estimation

In order to analyse the position estimation function we proceed in continuous time, even though the implementation is of course done in discrete time. This allows a much simpler notation and writing of many expressions.

• System modelling

In continuous time the dynamics of the system is governed by : ẋ = A x + v z = C x + w v and w are centered white noises with covariances Q and R, supposed independent to simplify the demonstration.

• State estimator

The estimation function is constructed from the model above.

• Available information

We suppose that the sensors provide integrate measurements z. These measurements are processed via optimal ltering to estimate the state x. On the output we thus dispose of the following quantities :

We are interested in the positioning error. To study it, let us introduce the matrix D which allows to extract the position X from the state x of the model, i.e. we suppose that : X = Dx. The matrix D might concern only the horizontal position (in which case we denote it D H ), or the vertical position (with notation D V ). The position error is dened by :

B.2.3 Integrity test

• Some fundamental elements

In order to establish a test procedure we use some technical elements recalled below :

Any cumulative distribution function is increasing

In these formulas, λ(V ) and λ(V ) are the minimum and maximum eigenvalues of V respectively. χ 2 (q δ , µ δ ) represents the probability density function of a χ 2 random variable, with q δ degrees of liberty, and µ δ as non centrality parameter. We shall denote F χ 2 (x; q δ , µ δ ) its cumulatice distribution function.

P (A) designates the probability of the event (or the set) A.

• The reasoning

The objective is to guarantee a given integrity risk P ir . That is to say that the probability of non-detection of an exceeding of the alert limit (AL) should be maximum P ir .

P (|δ| > AL | no positioning alert) ≤ P ir

The translation of this objective makes it possible to characterize the absence of a positioning alert.

The reasoning is a succession of necessary and sucient conditions, sometimes only sucient. It leads to explicit the integrity test by dening a level of protection. To start of with, we have :

From which we deduce :

We then use the fact that δ T V -1 δ is a χ 2 :

In the last inequality all the elements are known : they are either given (AL, P ir , q δ ), or calculated from the measurements (V ). We may thus propose a procedure for integrity control that rests on the surveillance of λ(V ), or equivalently on the protection level dened by : Protection Level

The non-centrality parameter µ δ is null since there is no fault by assumption.

• The alert procedure

The matrix V measures the dispersion of the position error. For example if we consider the vertical axis, i.e. δ V , then λ(V ) is the standard deviation of δ V . As long as the eigenvalue λ(V )

is small enough, we know that the dispersion is small and thus the precision of the estimate is high. Hence we do not raise any alarm.

In an equivalent fashion, the matrix V -1 measures the quantity of information relative to the position X, provided by the measurements. P L increases when this information quantity decreases.

The alarm is emitted when the quantity of information is insucient with regards to the level necessary to maintain a sucient precision of the estimation of X.

The alert procedure for the positioning is thus :

Alert procedure

We note that our objective is to x an upper bound on the probability to cross the alert limit AL without emitting an alarm. Now that the alert procedure is dened, let us verify that this upper bound is true. For this we recall the previous elements, but this time with the conditioning :

The key of this demonstration is in the rst inequality. The probability is obtained by integration of the probability density of |δ| on the interval [AL , +∞]. In addition, the absence of an alert is directly related to the lower limit of this interval : AL > P L. By integrating from P L we set the upper bound of the probability.

The other points of the demonstration are similar to that which has been seen in the reasoning for establishing the alert procedure, but by replacing AL by P L.

• Summary : Integrate navigation with fault-free sensors

In the case where we use fault-free sensors, the results on the integrity test are summarised as follows :

Summary : Integrate navigation with fault-free sensors

State estimation

Kalman lter

Integrity test

Remarks

• Reduction of the conservatism

The test here-above uses only the knowledge of V to raise or not the alarm. However we dispose of much more knowledge, in particular we know ν, which is is general correlated with δ. Also for example if C = D, then ν = δ + w. That is to say, in this case, ν is a measurement of δ. Indeed it is a noisy measurement, but if R is small then we almost know δ. It is therefore legitimate to ask whether this information could not be used directly to reduce the conservatism of the integrity test.

The answer to this interrogation is negative. In fact, the Kalman lter has already integrated all the information, and the value of ν in particular has been used to correct the state. In addition, the Kalman lter is optimal, in the sense of minimising the covariance of the estimation (P is minimal). We can thus not do better than what has already been done with regards to P . We can also show that the optimality is attained regardless of the considered state combination : V , which is the covariance of D(x -x) is thus minimal. It is useless to try to do better.

• Indirect procedure

In the procedure described here-above we calculate a protection level P L from V and P ir , we then compare it to the alret limit AL. When P L is inferior to AL, the integrity is guaranteed with a risk P ir . It is also possible to proceed in an indirect way. We calculate in this case an upper limit of the integrity risk from V and AL via :

And the test compares this value to the maximal tolerated risk P ir . The two procedures are by all means equivalent.

B.3 Integrity with faulty sensors

When the navigation sensors are not fault-free, the innovation can no longer be assumed to be centred all the time. The position error obeys to δ ∼ N (δ f , V ), where δ f is the impact of the faults. Then the statistic δ T V -1 δ is still a χ 2 but not centred, with noncentral parameter µ δ = δ T f V -1 δ f . The protection level to be used is thus:

A priori µ δ is unknown, and this formula is not usable as it is. However, since the function F -1 χ 2 is decreasing w.r.t. µ δ , a solution is to nd out and use an upper bound of µ δ .

When the sensors are prone to failure, the navigation system usually contains, in addition to an estimator, algorithms for fault detection, identication, and exclusion of faulty sensors. The evaluation of the integrity can therefore also take into account the potential faults and the way they are processed.

The quantities introduced hereafter serve to characterise this process.

B.3.1 Faults and integrity risk allocation

• Fault detection

When the sensors are subject to faults, a fault detection test may be used to detect (and correct) the faults. To this test a probability of false alarm and a probability of missed detection are associated.

Probabilities relative to the fault detection We note that the detection of a fault does not necessarily lead to an integrity alert. This will depend on the estimation of the fault and of its impact on the positioning. That is to say P N D does not necessarily coincide with P ir , and P F A does not necessarily coincide with P f a .

• Integrity risk allocation (rst level)

In addition to the integrity risk, which we recall is the probability of a positioning failure occurrence not detected (on the T T A horizon), the navigation systems also use the following probabilities:

Integrity risk conditional on faults We thus write:

where P f is the probability of fault occurrence

We can of course extend this decomposition by showing the various individual faults.

The integrity risk allocation (Figure B.2) consists of choosing values of P f f and P md from the given P ir and P f such that the equation below is satised. We will allocate a certain proportion α of the risk to the case with fault, and 1 -α to the case without fault.

Integrity risk allocation

The choice of α is in reality a little more constrained than what is indicated in the above box. In fact, P f f and P md are probabilities, and so belong to [0 , 1]. The choice of α must then be chosen as:

However, and since generally P ir P f and P ir 1, the bounds are almost 0 and 1.

• Decomposition of the protection level

The decomposition of the integrity risk allows to calculate the protection levels by considering The global protection level (designated here by the generic term P L) to trigger positioning alerts will be taken as the maximum of these levels.

P L = max (P L f f , P L md )

The decomposition formula of the integrity risk shows that we guarantee it even though the integrity risk is majorated by P ir when P L < AL. We have in fact:

The integrity test P L < AL is very well a proof of integrity.

• Interest of the integrity risk allocation

The rst advantage to proceed in this manner, i.e. by decomposing the risk, comes from the fact that the conditioning (by the presence or absence of a fault) limits the scope of validity of the level computations. This allows then to simplify the search for their expressions. We simply decompose the problem into two sub-problem which are more precise and thus simpler.

Another advantage of this procedure is that the protection level P L md to satisfy in the presence of faults corresponds to a risk P md whose value is much higher than P ir , since P f is small. Given a higher authorised risk, the protection level P L md decreases, and the integrity becomes easier to verify.

For P L f f , the risk allocation does not change a lot since 1 -P f ≈ 1. So P L f f will be of the same magnitude as the protection level found in the case of a system using fault free sensors.

B.3.2 Protection level computation

In order to establish the expressions allowing the calculation of the protection levels P L f f and P L md , one must rst analyse the inuence of faults on the positioning error δ. This work has already be done when introducing the GLR algorithm A. The estimation of the state is divided into a nominal fault-free part xn and a faulty part xf , i.e. x = xn + xf . This decomposition can be exploited to analyse the positioning error. We have:

The integrity risk will therefore be analysed by considering the statistic δ T V -1 δ, and not bT Λ T b. This statistic still follows a law of χ 2 but its non-centrality parameter is µ δ and no longer µ b .

In order to nd out the values of P L f f and P L md one has to consider the unfaulty and faulty cases respectively. Without any fault present, we have:

This xes the protection level which is thus :

When a fault is present, and still taking into account the risk allocation, we need to check:

The associated protection level is thus :

This protection level depends on the non centrality parameter µ δ . From there, two ways may be followed to evaluate P L md . One can:

• Use an estimation μδ of µ δ .

• Use an upper bound µ δ of µ δ .

In this latter case, the computer protection level is larger than the one here above because F -1 χ 2 is an increasing function with respect to µ δ . Both approaches uses the link there exists between µ δ and µ b .

B.3.3 Non centrality parameter

The non centrality parameter µ δ depends on the fault value, since µ δ = δ T f V -1 δ f , where δ f is the impact of the fault projected onto the position. Using an estimation of the fault, one can thus estimate the non centrality parameter by:

Obviously this value is highly sensitive to the instantaneous performance of the fault detection test, and the way it estimates the fault.

We can also use the relationship between δ f and the estimated fault amplitude ( b). We have

From the fault detection test we know that any undetectable fault satises b T Λ T b ≤ l GLR,det . Then it is possible to compute an upper bound of µ δ as:

Solving the optimisation problem yields to:

A slightly more conservative bound, but with a clearer physical sense, is: µ δ = b T b λ M T , where b is the greatest undetectable bias.

A lower bound of µ δ can be compute in the same way.

It corresponds to the smallest detectable bias.

B.3.4 Introduction of a double risk allocation

The double risk allocation is introduced to take advantage of the knowledge of the failure detection test result. The idea is to analyse more specic situations, yielding thus to more accurate results.

We denote by D and D the cases where the fault detection test is positive and negative respectively.

The second risk decomposition writes:

P md = P md,D P D + P md,D (1 -P D )

where P D = (1 -P N D ) P f + P F A (1 -P f ) is the probability than the detection test is positive.

We introduce the second risk allocation depending on whether the detection test is positive or not:

Double risk allocation

With these bounds it is very easy to get lower bounds of the probabilities P f f,D , P f f,D , P md,D and P md,D . We shall denote them as P f f,D , etc. . . . Using these lower bounds increases the associated protection levels (recall that F χ 2 is increasing, so the same for its inverse).

B.3.5 Protection levels with the double risk allocation

In the fault-free case (case f f ), and whatever the result of the fault detection test is, we have µ δ = 0. In the missed detection case (case md), if a fault has been detected (case D), it has been estimated and corrected. So the residual impact of the fault is less than the minimum detectable fault. And we will use µ δ . Still in the missed detection case, if no fault has been detected (case D), then the fault is less that the maximal undetectable fault. And we shall use µ δ .

They are now four protection level, each one associated to one of the four cases. Due to the allocation the protection levels are smaller. It is hoped that the lack of knowledge of P D and µ δ (which are replaced by bounds) should not lead to a signicant deterioration in results. The protection level to be used are: P L = max {P L f f , P L md } P L f f = max P L f f,D , P L f f,D P L f f,D = λ(V ) F -1 χ 2 1 -P f f,D ; q δ , 0 P L f f,D = λ(V ) F -1 χ 2 1 -P f f,D ; q δ , 0 P L md = max P L md,D , P L md,D P L md,D = λ(V ) F -1 χ 2 1 -P md,D ; q δ , µ δ P L md,D = λ(V ) F -1 χ 2 1 -P md,D ; q δ , µ δ B.4 Examples of angular alert limit calculations

The alert limit (AL) for angular integrity is a bit more complicated to determine than for the translational position case. We detail two examples of how sensible alert limits can be found in the angular case.

B.4.1 Example 1: Multi-rotor transverse acceleration limit

A rst example is the multi-rotor drone at hover (stationary level ight) subject to an inclination error.

The angular error ε α in this case is proportionate to the uncompensated inertial acceleration ∆a. We can therefore calculate a maximum allowable acceleration ∆ā from a maximum allowable trajectory deviation ∆x during a given time ∆t, see Figure B.3. For small errors, ∆ā is directly proportionate to the angular error limit ε α . In this case we neglect phenomena such as drag for simplicity and to set an upper limit on the angle. By rearranging the expression and solving for the error angle ε α , we get the solution AL ε α = atan2(∆d, h + h tan 2 α + ∆d tan α)

In this general case, we require the inclination angle α. Since we do not know it exactly we can replace it by its estimate α. If the angles α and ε α are small, the expression simplies signicantly. In this case, tan α ≈ α, tan ε α ≈ ε α , and αε α ≈ 0, hence ∆d ≈ h ε α ⇒ AL ε α = ∆d/h .

B.4.3 Example of integrity evaluation

An example of the integrity criterion for a heading estimation in the case of an unobservability due to a magnetic disturbance can be seen in Figure B.5. The estimation error becomes larger due to unobservability. Simultaneously the 3σ-condence interval increases, and the same for the protection level. In the example, the estimation remains consistent since the estimation error is less than 3σ. However it does not remain integrate since P L exceeds AL. We nally note that in the end when the heading becomes observable once again, the integrity of the estimation is re-established. Given a discrete nonlinear state space model, with a state vector x, an input vector u, and a known output vector y, also aected by zero-mean Gaussian process and measurement noises v t ∼ N (0, V t ) and w t+1 ∼ N (0, W t+1 ):

x t+1 = f (x t , u t , v t ) y t+1 = h (x t+1 , w t+1 ) +

we can write an equivalent linearised model about the current state, input, and measurement as: An estimation of the state, x, and its covariance, P , can be found with the following predictioncorrection iteration (given an initial condition (x 0 , P 0 ):