# A Fault-Tolerant Multiple Sensor Fusion Approach Applied to UAV Attitude Estimation

Authors: Yu Gu; Jason N. Gross; Matthew B. Rhudy; Kyle Lassak
Journal: International Journal of Aerospace Engineering
 (2016)

---

## Abstract

A novel sensor fusion design framework is presented with the objective of improving the overall multisensor measurement system performance and achieving graceful degradation following individual sensor failures. The Unscented Information Filter (UIF) is used to provide a useful tool for combining information from multiple sources. A two-step off-line and on-line calibration procedure refines sensor error models and improves the measurement performance. A Fault Detection and Identification (FDI) scheme crosschecks sensor measurements and simultaneously monitors sensor biases. Low-quality or faulty sensor readings are then rejected from the final sensor fusion process. The attitude estimation problem is used as a case study for the multiple sensor fusion algorithm design, with information provided by a set of low-cost rate gyroscopes, accelerometers, magnetometers, and a single-frequency GPS receiver’s position and velocity solution. Flight data collected with an Unmanned Aerial Vehicle (UAV) research test bed verifies the sensor fusion, adaptation, and fault-tolerance capabilities of the designed sensor fusion algorithm.


---

## Body

## 1. Introduction

Sensing through a fusion of diverse but interrelated sensory data could reveal information that is difficult to measure directly. Having complementary and multiperspective viewpoints also allows for real-time evaluation of individual sensors’ performance and limitations, further enabling a reconfiguration of the measurement system if necessary. From this perspective, sensor fusion can be viewed as a process of refining internal models of both the measured phenomenon (for improved performance) and the measurement system (for improved reliability) through processing a heterogeneous set of sensory data. Within this context, a three-step sensor fusion design framework is presented in this paper.Step 1.
Combine sensory data from diverse and redundant sources to derive a fused solution that is difficult to measure directly and/or has better quality than the output of each participating sensor.Step 2.
Refine sensor error models with feedback from the sensor fusion algorithm.Step 3.
Reconfigure the measurement system to achieve performance enhancement under nominal conditions and graceful degradation following sensor failures.Step1 reflects a “traditional” view of sensor fusion. Many existing approaches are model based, which relies on a set of mathematical models to relate individual measurements to the fused solution. These models representa priori knowledge of the system to be measured and are based either on known relationships or on assumptions and heuristics. For numerical sensory data, a Bayesian filter [1, 2] is often used to derive a fused solution. This solution can either be computed at a centralized location or distributed among sensor nodes [3–5].Step2 enables the measurement system to maintain an updated knowledge of its states. At the sensor level, redundant information sources provide references for crosschecking and calibrating individual sensors used within the measurement system. This extends the traditional off-line sensor calibration [6] process to an on-line process. If a certain mathematical structure of the sensor model is assumed, the dynamic sensor calibration reduces to a parameter identification [7, 8] problem. For example, sensor bias or scaling factors are often estimated along with other states within a sensor fusion algorithm [9].Step3 integrates results from the first two levels to further improve the performance and robustness of the measurement system in several aspects. First, the sensor error models refined through the dynamic calibration process could improve the overall estimation performance and provide indications of the health condition of each sensor. Second, a comparison of sensory data collected from diverse but interrelated sources provides information for sensor Fault Detection, Identification, and Accommodation (FDIA) [10, 11]. Finally, the robustness of the measurement system could be improved by rejecting [4] low quality or faulty sensor measurements from the measurement update.The goal of this paper is to demonstrate this three-step sensor fusion approach through a practical application: to achieve reliable and accurate attitude estimation with a low-cost Inertial Measurement Unit (IMU), Global Positioning System (GPS) receiver, and triaxial magnetometers. As an important navigation problem, the 3D attitude for Unmanned Aerial Vehicles (UAVs), mobile robots, and mobile devices were estimated using a variety of information sources. This includes but not limited to dead reckoning with rate gyroscopes, sensing of earth’s gravity [12] and magnetic vectors [13], angular position of celestial objects [14], horizon line [15], terrain shape [16], optical flow [17], and known radio sources [18] such as GPS [19] and cellular network [20]. The selection of IMU, GPS, and magnetometers as the primary sensors for this study was mainly due to their widespread availability and popularity in various platforms, as well as the low-computational requirement of implementing these sensors when compared with methods such as computer vision or Lidar based mapping.The novel contributions of this effort include the following:(1)
A systematic design approach that integrates both sensor calibration and fault tolerance into a multiple sensor fusion system.
(2)
A FDI method based on information crosschecking and sensor bias tracking.The performance of all presented algorithms is evaluated with flight datasets, through processing of real flight data taken from an experimental UAV. Portions of this paper, including some figures and tables, are contained within a chapter of the second author’s graduate thesis [21].The rest of the paper is organized as follows. Section2 introduces four different sensor fusion formulations for vehicle attitude estimation using Unscented Information Filter (UIF) as the nonlinear estimator. The sensor calibration process for refining sensor error models is described in Section 3. Section 4 presents the FDIA methods. Section 5 discusses the experimental setup used in this study. The main results of this work are presented in Section 6, followed by a conclusion in Section 7.


## 2. Fusion of GPS, IMU, and Magnetometer Measurements

### 2.1. Coordinate Systems

Two coordinate systems are used throughout this paper. A local-level Cartesian navigation frame (L) is defined with its origin O
L at an arbitrary point near the vehicle, positive x
L axis pointing toward the geographic north, positive y
L axis pointing east, and positive z
L axis pointing to the center of the earth. A vehicle body-axes coordinate system (B) is defined with its origin at vehicle’s Center of Gravity (CG), with positive x
B pointing forward of the vehicle, positive y
B axis toward right, and positive z
B axis toward the bottom of the vehicle. Each sensor on-board of the UAV is assumed are assumed to be aligned with the body axes and to be located relatively close to each other (i.e., a very small lever arm between the GPS and IMU).The rotation of measurements between the two coordinate systems is calculated through the use of three attitude (Euler) angles: yaw (ψ), pitch (θ), and roll (ϕ). For example, the earth’s gravity vector and centrifugal acceleration due to the earth’s rotation (i.e., sensed “plumb-bob” acceleration) components in the vehicle body axis are found by assuming that it is parallel to the navigation frame z
L axis and rotating with a Direction Cosine Matrix (DCM):(1)
g
x
B
g
y
B
g
z
B
=
D
C
M
ϕ
,
θ
,
ψ
T
·
0
0
gthat is defined by the body-axis attitude:(2)
D
C
M
ϕ
,
θ
,
ψ
=
c
ψ
c
θ
-
s
ψ
c
ϕ
+
c
ψ
s
θ
s
ϕ
s
ψ
s
ϕ
+
c
ψ
s
θ
c
ϕ
s
ψ
c
θ
c
ψ
c
ϕ
+
s
ψ
s
θ
s
ϕ
-
c
ψ
s
ϕ
+
s
ψ
s
θ
c
ϕ
-
s
θ
c
θ
s
ϕ
c
θ
c
ϕ
,where “s” and “c” are abbreviated sine and cosine functions, respectively.


### 2.2. Information Sources

Two types of information sources are generally available for estimating a vehicle’s attitude angles:(
1
) the time-integration of rate gyroscope measurements; (
2
) the measurement of external vector fields of well-known directions. With readings from a set of 3-axis strap-down rate gyroscopes, the attitude angles are computed with a set of attitude kinematic equations [22]:(3)
ϕ
˙
=
p
+
q
sin
⁡
ϕ
tan
⁡
θ
+
r
cos
⁡
ϕ
tan
⁡
θ
,
θ
˙
=
q
cos
⁡
ϕ
-
r
sin
⁡
ϕ
,
ψ
˙
=
q
sin
⁡
ϕ
+
r
cos
⁡
ϕ
sec
⁡
θ
,where p, q, and r are the roll rate, pitch rate, and yaw rate measured in the body axis, respectively. This attitude solution will diverge over the time due to the accumulation of rate gyroscope biases during the integration process. For typical low-cost microelectromechanical systems- (MEMS-) based gyroscopes, the uncorrected attitude solutions are not directly usable after a short period of time.A standard approach for controlling the attitude error growth is to regulate it with nondrifting information sources, such as the known direction of an external vector field. Examples of commonly used aiding information include the earth’s gravity vector [12], the earth’s magnetic vector [13], GPS [19], cellular network [20], celestial map [14], horizon line [15], terrain map [16], computer vision [17], and known radio sources [18]. From this list, earth’s gravity and magnetic vector fields are often the easiest to measure and therefore are discussed in detail in this paper. Four sensor fusion formulations are presented in the rest of this section to show that a fusion of any combination of GPS, IMU, and magnetometers is capable of providing nondrifting Euler angle estimates.


### 2.3. Sensor Fusion Formulation #1: GPS/IMU

The attitude of a stationary vehicle can be directly solved from (1) using measurements from a set of 3-axis accelerometers. For a moving vehicle, the accelerometers measure both the acceleration due to gravity and the vehicle’s acceleration in the inertial frame. To isolate the gravity vector from inertial acceleration, a GPS receiver’s velocity solution provides an independent observation of the vehicle’s inertial acceleration in the local Cartesian coordinates. Since the GPS will not sense the earth’s gravity, the relationship between accelerometer measurements a
B and GPS velocity measurements V
L can be described by(4)
V
˙
x
L
V
˙
y
L
V
˙
z
L
G
P
S
=
D
C
M
ϕ
,
θ
,
ψ
·
a
x
B
a
y
B
a
z
B
I
M
U
+
0
0
g
.Using the heading information provided by the GPS,(5)
ψ
=
t
a
n
-
1
V
y
L
V
x
L
.Equation (4) can be explicitly solved to calculate Euler angles [12]. However, a better approach exists with the use of the stationary gravity vector to regulate the INS integration error with a recursive estimator. Within this formulation, the state, input, and measurement vectors are, respectively, x
=
ϕ
θ
ψ
T, u
=
p
q
r
T, and z
=
a
L
=
a
x
L
a
y
L
a
z
L
T. The nonlinear continuous-time state transition equations x
˙
=
f
c
(
x
,
u
,
w
) are directly based on (3):(6)
ϕ
˙
θ
˙
ψ
˙
=
p
+
w
p
+
q
+
w
q
sin
⁡
ϕ
tan
⁡
θ
+
r
+
w
r
cos
⁡
ϕ
tan
⁡
θ
q
+
w
q
cos
⁡
ϕ
-
r
+
w
r
sin
⁡
ϕ
q
+
w
q
sin
⁡
ϕ
+
r
+
w
r
cos
⁡
ϕ
sec
⁡
θ
,where w
p
,
q
,
r are the noises associated with the corresponding rate gyroscope measurements, which are assumed to be zero mean white Gaussian: w
1
=
w
p
w
q
w
r
T
≈
N
(
0
,
Q
1
). Note that w
p
,
q
,
r are implemented as nonadditive input noises instead of modeling process noise additive on the state estimates, which is commonly used in Kalman filter formulations.The continuousobservation equationsz
=
h
c
(
x
,
v
) are also nonlinear and are modeled by(7)
V
˙
x
L
+
v
g
x
V
˙
y
L
+
v
g
y
V
˙
z
L
+
v
g
z
G
P
S
=
D
C
M
ϕ
,
θ
,
ψ
·
a
x
B
+
v
a
x
a
y
B
+
v
a
y
a
z
B
+
v
a
z
I
M
U
+
0
0
g
,where v
g
x
,
g
y
,
g
z are the noises assumed to be white for the GPS estimated accelerations and v
a
x
,
a
y
,
a
z are noises associated with the accelerometer measurements, with v
1
=
v
g
x
v
g
y
v
g
z
v
a
x
v
a
y
v
a
z
T
≈
N
(
0
,
R
1
).


### 2.4. Sensor Fusion Formulation #2: IMU/Magnetometers

The gravity vector used in Formulation #1 can be directly substituted with the Earth’s magnetic vector in regulating the growth of the rate gyroscope integration error. For this formulation, the state, input, and measurement vectors are given, respectively, byx
=
ϕ
θ
ψ
T, u
=
p
q
r
T, and z
=
M
x
B
M
y
B
M
z
B
T, where M
x
,
y
,
z
B are the magnetometer measurements. The state transition equations are the same as in (6), and the nonlinear observation equations are given by(8)
M
x
B
+
v
M
x
M
y
B
+
v
M
y
M
z
B
+
v
M
z
=
D
C
M
ϕ
,
θ
,
ψ
T
·
M
e
x
L
M
e
y
L
M
e
z
L
,where M
e
x
,
y
,
z
L is the local magnetic vector determined with National Oceanic and Atmospheric Administration’s Geomagnetic Online Calculator [23], and v
2
=
v
M
x
v
M
y
v
M
z
T
≈
N
(
0
,
R
2
) are the noises associated with the magnetometer measurements, which are assumed to be zero mean, white, and Gaussian. An appealing feature for the IMU/magnetometers formulation is that it can operate indoors or within other GPS-denied environments. However, the local magnetic field can be distorted by the existence of ferromagnetic materials in the close vicinity, affecting the attitude estimation performance. A solution to this problem is discussed in Section 3.


### 2.5. Sensor Fusion Formulation #3: GPS/Magnetometers

The body-axis magnetic field measurements, coupled with GPS heading, also provide adequate information for Euler angle estimation at most places on earth that are not near the magnetic poles. In this formulation, the state, input, and measurement vectors are, respectively,x
=
ϕ
θ
T, u
=
0
0
T, and z
=
M
x
B
M
y
B
M
z
B
T. Since the rate gyroscope measurements are not available in this case, the state transition equations are simply defined as follows:(9)
ϕ
˙
θ
˙
=
0
0
+
w
2
,where the unknowns ϕ and θ are assumed to be perturbed with white noises w
2
≈
N
(
0
,
Q
2
). The observation equations are the same as in (8), where ψ is calculated with (5). The observation equations could also be augmented with (7) to incorporate additional gravity vector constraints during the measurement update. The benefit of this formulation is that it provides an independent attitude estimate without the rate gyroscopes; therefore, this approach would not cause stability issues [24] in control systems that rely on rate gyroscopes for inner-loop feedback.


### 2.6. Sensor Fusion Formulation #4: GPS/IMU/Mag

So far, combinations of any two sensors from the set of GPS, IMU, and magnetometers have been used for attitude estimation. Particularly, the differences between Formulations #1 and #2 are only present in the observation equations. Combining the two sets of observation equations could lead to a tighter regulation of the error growth in the strap-down INS equations. In fact, any measureable external vector field of known direction could be added to the observation equations in a similar fashion. For the GPS/IMU/magnetometers formulation, the state, input, and measurement vectors are, respectively,x
=
ϕ
θ
ψ
T, u
=
p
q
r
T, and z
=
a
x
L
a
y
L
a
z
L
M
x
B
M
y
B
M
z
B
T. The state transition equations are the same as in (6), and the observation equations are simply a combination of (7) and (8).


### 2.7. Unscented Information Filter

An information filter approach [5] is used for the fusion of multiple sensor measurements. The main advantage of using the information filter instead of Kalman filter is that the information update can be expressed as a sum: (10)
I
k
∣
k
=
I
k
∣
k
-
1
+
∑
j
=
1
N
I
j
,
k
,
i
^
k
∣
k
=
i
^
k
∣
k
-
1
+
∑
j
=
1
N
i
j
,
k
,where I
k
∣
k
-
1 is the predicted information matrix, i
^
k
∣
k
-
1 is the predicted information state vector, and I
j
,
k, i
j
,
k are the measurement information matrix and information vector, respectively, associated with the jth measurement out of a total of N independent measurements. This simple relationship in information update creates a suitable framework for adding/removing sensors and handling unsynchronized measurement updates within a multisensor fusion system.The state estimation problems presented in Formulations #1–#4 are solved with an Unscented Information Filter (UIF) [3]. The UIF uses the same prediction model as an Unscented Kalman Filter (UKF) [25, 26] to calculate the predicted error covariance matrix, P
k
∣
k
-
1, and state estimation, x
^
k
∣
k
-
1. The predicted information matrix, I
k
∣
k
-
1, and predicted information state vector, i
^
k
∣
k
-
1, are simply defined as follows:(11)
I
k
∣
k
-
1
=
P
k
∣
k
-
1
-
1
,
i
^
k
∣
k
-
1
=
P
k
∣
k
-
1
-
1
x
^
k
∣
k
-
1
.For the measurement update, the nonlinear observation equation for each independent measurementz
j
,
k
=
h
j
x
^
k
∣
k
-
1
,
v
j
,
k is locally linearized with a statistical linear regression method [27]:(12)
z
j
,
k
≈
H
x
j
,
k
H
v
j
,
k
︸
H
j
,
k
x
^
k
∣
k
-
1
v
j
,
k
︸
x
k
∣
k
-
1
a
+
b
j
,
kthat minimizes the sum of squared errors e
j
,
i
=
χ
j
,
k
i
-
(
H
j
,
k
χ
j
,
k
∣
k
-
1
i
+
b
j
,
k
):(13)
H
j
,
k
,
b
j
,
k
=
a
r
g
m
i
n
H
,
b
∑
i
=
0
2
L
e
j
,
i
T
e
j
,
i
,where H
j
,
k
=
P
x
k
z
j
,
k
T
P
k
∣
k
-
1
a
-
1 and b
j
,
k
=
z
^
j
,
k
-
H
j
,
k
x
k
∣
k
-
1
a. The mean and covariance of e
j are given by(14)
e
-
j
=
0
,
P
j
,
e
e
=
P
z
j
,
k
z
j
,
k
-
H
j
,
k
P
k
∣
k
-
1
a
H
j
,
k
T
.The measurement information matrixI
j
,
k and information vector i
j
,
k for the jth measurement [3] can then be provided by(15)
i
j
,
k
=
H
x
j
,
k
T
R
-
j
,
k
-
1
z
j
,
k
-
b
j
,
k
.
I
j
,
k
=
H
x
j
,
k
T
R
-
j
,
k
-
1
H
x
j
,
k
,where R
-
j
,
k is the covariance matrix for the sum of the linearized actual observation noise R
j and the linearization noise:(16)
R
-
j
,
k
=
H
z
j
,
k
R
j
H
z
j
,
k
T
+
P
j
,
e
e
=
P
z
j
,
k
z
j
,
k
-
H
x
j
,
k
P
k
∣
k
-
1
H
x
j
,
k
T
.The procedure for deriving (14)–(16) is outlined in [3].Following the information update equations (10) with all the measurements during each time frame, the estimated posterior covariance matrix and state vector can be recovered using(17)
P
k
∣
k
=
I
k
∣
k
-
1
,
x
^
k
∣
k
=
P
k
∣
k
i
^
k
∣
k
.


## 2.1. Coordinate Systems

Two coordinate systems are used throughout this paper. A local-level Cartesian navigation frame (L) is defined with its origin O
L at an arbitrary point near the vehicle, positive x
L axis pointing toward the geographic north, positive y
L axis pointing east, and positive z
L axis pointing to the center of the earth. A vehicle body-axes coordinate system (B) is defined with its origin at vehicle’s Center of Gravity (CG), with positive x
B pointing forward of the vehicle, positive y
B axis toward right, and positive z
B axis toward the bottom of the vehicle. Each sensor on-board of the UAV is assumed are assumed to be aligned with the body axes and to be located relatively close to each other (i.e., a very small lever arm between the GPS and IMU).The rotation of measurements between the two coordinate systems is calculated through the use of three attitude (Euler) angles: yaw (ψ), pitch (θ), and roll (ϕ). For example, the earth’s gravity vector and centrifugal acceleration due to the earth’s rotation (i.e., sensed “plumb-bob” acceleration) components in the vehicle body axis are found by assuming that it is parallel to the navigation frame z
L axis and rotating with a Direction Cosine Matrix (DCM):(1)
g
x
B
g
y
B
g
z
B
=
D
C
M
ϕ
,
θ
,
ψ
T
·
0
0
gthat is defined by the body-axis attitude:(2)
D
C
M
ϕ
,
θ
,
ψ
=
c
ψ
c
θ
-
s
ψ
c
ϕ
+
c
ψ
s
θ
s
ϕ
s
ψ
s
ϕ
+
c
ψ
s
θ
c
ϕ
s
ψ
c
θ
c
ψ
c
ϕ
+
s
ψ
s
θ
s
ϕ
-
c
ψ
s
ϕ
+
s
ψ
s
θ
c
ϕ
-
s
θ
c
θ
s
ϕ
c
θ
c
ϕ
,where “s” and “c” are abbreviated sine and cosine functions, respectively.


## 2.2. Information Sources

Two types of information sources are generally available for estimating a vehicle’s attitude angles:(
1
) the time-integration of rate gyroscope measurements; (
2
) the measurement of external vector fields of well-known directions. With readings from a set of 3-axis strap-down rate gyroscopes, the attitude angles are computed with a set of attitude kinematic equations [22]:(3)
ϕ
˙
=
p
+
q
sin
⁡
ϕ
tan
⁡
θ
+
r
cos
⁡
ϕ
tan
⁡
θ
,
θ
˙
=
q
cos
⁡
ϕ
-
r
sin
⁡
ϕ
,
ψ
˙
=
q
sin
⁡
ϕ
+
r
cos
⁡
ϕ
sec
⁡
θ
,where p, q, and r are the roll rate, pitch rate, and yaw rate measured in the body axis, respectively. This attitude solution will diverge over the time due to the accumulation of rate gyroscope biases during the integration process. For typical low-cost microelectromechanical systems- (MEMS-) based gyroscopes, the uncorrected attitude solutions are not directly usable after a short period of time.A standard approach for controlling the attitude error growth is to regulate it with nondrifting information sources, such as the known direction of an external vector field. Examples of commonly used aiding information include the earth’s gravity vector [12], the earth’s magnetic vector [13], GPS [19], cellular network [20], celestial map [14], horizon line [15], terrain map [16], computer vision [17], and known radio sources [18]. From this list, earth’s gravity and magnetic vector fields are often the easiest to measure and therefore are discussed in detail in this paper. Four sensor fusion formulations are presented in the rest of this section to show that a fusion of any combination of GPS, IMU, and magnetometers is capable of providing nondrifting Euler angle estimates.


## 2.3. Sensor Fusion Formulation #1: GPS/IMU

The attitude of a stationary vehicle can be directly solved from (1) using measurements from a set of 3-axis accelerometers. For a moving vehicle, the accelerometers measure both the acceleration due to gravity and the vehicle’s acceleration in the inertial frame. To isolate the gravity vector from inertial acceleration, a GPS receiver’s velocity solution provides an independent observation of the vehicle’s inertial acceleration in the local Cartesian coordinates. Since the GPS will not sense the earth’s gravity, the relationship between accelerometer measurements a
B and GPS velocity measurements V
L can be described by(4)
V
˙
x
L
V
˙
y
L
V
˙
z
L
G
P
S
=
D
C
M
ϕ
,
θ
,
ψ
·
a
x
B
a
y
B
a
z
B
I
M
U
+
0
0
g
.Using the heading information provided by the GPS,(5)
ψ
=
t
a
n
-
1
V
y
L
V
x
L
.Equation (4) can be explicitly solved to calculate Euler angles [12]. However, a better approach exists with the use of the stationary gravity vector to regulate the INS integration error with a recursive estimator. Within this formulation, the state, input, and measurement vectors are, respectively, x
=
ϕ
θ
ψ
T, u
=
p
q
r
T, and z
=
a
L
=
a
x
L
a
y
L
a
z
L
T. The nonlinear continuous-time state transition equations x
˙
=
f
c
(
x
,
u
,
w
) are directly based on (3):(6)
ϕ
˙
θ
˙
ψ
˙
=
p
+
w
p
+
q
+
w
q
sin
⁡
ϕ
tan
⁡
θ
+
r
+
w
r
cos
⁡
ϕ
tan
⁡
θ
q
+
w
q
cos
⁡
ϕ
-
r
+
w
r
sin
⁡
ϕ
q
+
w
q
sin
⁡
ϕ
+
r
+
w
r
cos
⁡
ϕ
sec
⁡
θ
,where w
p
,
q
,
r are the noises associated with the corresponding rate gyroscope measurements, which are assumed to be zero mean white Gaussian: w
1
=
w
p
w
q
w
r
T
≈
N
(
0
,
Q
1
). Note that w
p
,
q
,
r are implemented as nonadditive input noises instead of modeling process noise additive on the state estimates, which is commonly used in Kalman filter formulations.The continuousobservation equationsz
=
h
c
(
x
,
v
) are also nonlinear and are modeled by(7)
V
˙
x
L
+
v
g
x
V
˙
y
L
+
v
g
y
V
˙
z
L
+
v
g
z
G
P
S
=
D
C
M
ϕ
,
θ
,
ψ
·
a
x
B
+
v
a
x
a
y
B
+
v
a
y
a
z
B
+
v
a
z
I
M
U
+
0
0
g
,where v
g
x
,
g
y
,
g
z are the noises assumed to be white for the GPS estimated accelerations and v
a
x
,
a
y
,
a
z are noises associated with the accelerometer measurements, with v
1
=
v
g
x
v
g
y
v
g
z
v
a
x
v
a
y
v
a
z
T
≈
N
(
0
,
R
1
).


## 2.4. Sensor Fusion Formulation #2: IMU/Magnetometers

The gravity vector used in Formulation #1 can be directly substituted with the Earth’s magnetic vector in regulating the growth of the rate gyroscope integration error. For this formulation, the state, input, and measurement vectors are given, respectively, byx
=
ϕ
θ
ψ
T, u
=
p
q
r
T, and z
=
M
x
B
M
y
B
M
z
B
T, where M
x
,
y
,
z
B are the magnetometer measurements. The state transition equations are the same as in (6), and the nonlinear observation equations are given by(8)
M
x
B
+
v
M
x
M
y
B
+
v
M
y
M
z
B
+
v
M
z
=
D
C
M
ϕ
,
θ
,
ψ
T
·
M
e
x
L
M
e
y
L
M
e
z
L
,where M
e
x
,
y
,
z
L is the local magnetic vector determined with National Oceanic and Atmospheric Administration’s Geomagnetic Online Calculator [23], and v
2
=
v
M
x
v
M
y
v
M
z
T
≈
N
(
0
,
R
2
) are the noises associated with the magnetometer measurements, which are assumed to be zero mean, white, and Gaussian. An appealing feature for the IMU/magnetometers formulation is that it can operate indoors or within other GPS-denied environments. However, the local magnetic field can be distorted by the existence of ferromagnetic materials in the close vicinity, affecting the attitude estimation performance. A solution to this problem is discussed in Section 3.


## 2.5. Sensor Fusion Formulation #3: GPS/Magnetometers

The body-axis magnetic field measurements, coupled with GPS heading, also provide adequate information for Euler angle estimation at most places on earth that are not near the magnetic poles. In this formulation, the state, input, and measurement vectors are, respectively,x
=
ϕ
θ
T, u
=
0
0
T, and z
=
M
x
B
M
y
B
M
z
B
T. Since the rate gyroscope measurements are not available in this case, the state transition equations are simply defined as follows:(9)
ϕ
˙
θ
˙
=
0
0
+
w
2
,where the unknowns ϕ and θ are assumed to be perturbed with white noises w
2
≈
N
(
0
,
Q
2
). The observation equations are the same as in (8), where ψ is calculated with (5). The observation equations could also be augmented with (7) to incorporate additional gravity vector constraints during the measurement update. The benefit of this formulation is that it provides an independent attitude estimate without the rate gyroscopes; therefore, this approach would not cause stability issues [24] in control systems that rely on rate gyroscopes for inner-loop feedback.


## 2.6. Sensor Fusion Formulation #4: GPS/IMU/Mag

So far, combinations of any two sensors from the set of GPS, IMU, and magnetometers have been used for attitude estimation. Particularly, the differences between Formulations #1 and #2 are only present in the observation equations. Combining the two sets of observation equations could lead to a tighter regulation of the error growth in the strap-down INS equations. In fact, any measureable external vector field of known direction could be added to the observation equations in a similar fashion. For the GPS/IMU/magnetometers formulation, the state, input, and measurement vectors are, respectively,x
=
ϕ
θ
ψ
T, u
=
p
q
r
T, and z
=
a
x
L
a
y
L
a
z
L
M
x
B
M
y
B
M
z
B
T. The state transition equations are the same as in (6), and the observation equations are simply a combination of (7) and (8).


## 2.7. Unscented Information Filter

An information filter approach [5] is used for the fusion of multiple sensor measurements. The main advantage of using the information filter instead of Kalman filter is that the information update can be expressed as a sum: (10)
I
k
∣
k
=
I
k
∣
k
-
1
+
∑
j
=
1
N
I
j
,
k
,
i
^
k
∣
k
=
i
^
k
∣
k
-
1
+
∑
j
=
1
N
i
j
,
k
,where I
k
∣
k
-
1 is the predicted information matrix, i
^
k
∣
k
-
1 is the predicted information state vector, and I
j
,
k, i
j
,
k are the measurement information matrix and information vector, respectively, associated with the jth measurement out of a total of N independent measurements. This simple relationship in information update creates a suitable framework for adding/removing sensors and handling unsynchronized measurement updates within a multisensor fusion system.The state estimation problems presented in Formulations #1–#4 are solved with an Unscented Information Filter (UIF) [3]. The UIF uses the same prediction model as an Unscented Kalman Filter (UKF) [25, 26] to calculate the predicted error covariance matrix, P
k
∣
k
-
1, and state estimation, x
^
k
∣
k
-
1. The predicted information matrix, I
k
∣
k
-
1, and predicted information state vector, i
^
k
∣
k
-
1, are simply defined as follows:(11)
I
k
∣
k
-
1
=
P
k
∣
k
-
1
-
1
,
i
^
k
∣
k
-
1
=
P
k
∣
k
-
1
-
1
x
^
k
∣
k
-
1
.For the measurement update, the nonlinear observation equation for each independent measurementz
j
,
k
=
h
j
x
^
k
∣
k
-
1
,
v
j
,
k is locally linearized with a statistical linear regression method [27]:(12)
z
j
,
k
≈
H
x
j
,
k
H
v
j
,
k
︸
H
j
,
k
x
^
k
∣
k
-
1
v
j
,
k
︸
x
k
∣
k
-
1
a
+
b
j
,
kthat minimizes the sum of squared errors e
j
,
i
=
χ
j
,
k
i
-
(
H
j
,
k
χ
j
,
k
∣
k
-
1
i
+
b
j
,
k
):(13)
H
j
,
k
,
b
j
,
k
=
a
r
g
m
i
n
H
,
b
∑
i
=
0
2
L
e
j
,
i
T
e
j
,
i
,where H
j
,
k
=
P
x
k
z
j
,
k
T
P
k
∣
k
-
1
a
-
1 and b
j
,
k
=
z
^
j
,
k
-
H
j
,
k
x
k
∣
k
-
1
a. The mean and covariance of e
j are given by(14)
e
-
j
=
0
,
P
j
,
e
e
=
P
z
j
,
k
z
j
,
k
-
H
j
,
k
P
k
∣
k
-
1
a
H
j
,
k
T
.The measurement information matrixI
j
,
k and information vector i
j
,
k for the jth measurement [3] can then be provided by(15)
i
j
,
k
=
H
x
j
,
k
T
R
-
j
,
k
-
1
z
j
,
k
-
b
j
,
k
.
I
j
,
k
=
H
x
j
,
k
T
R
-
j
,
k
-
1
H
x
j
,
k
,where R
-
j
,
k is the covariance matrix for the sum of the linearized actual observation noise R
j and the linearization noise:(16)
R
-
j
,
k
=
H
z
j
,
k
R
j
H
z
j
,
k
T
+
P
j
,
e
e
=
P
z
j
,
k
z
j
,
k
-
H
x
j
,
k
P
k
∣
k
-
1
H
x
j
,
k
T
.The procedure for deriving (14)–(16) is outlined in [3].Following the information update equations (10) with all the measurements during each time frame, the estimated posterior covariance matrix and state vector can be recovered using(17)
P
k
∣
k
=
I
k
∣
k
-
1
,
x
^
k
∣
k
=
P
k
∣
k
i
^
k
∣
k
.


## 3. Sensor Calibration

The existence of redundant information from different sources provides an opportunity for calibrating each individual sensor within the measurement system. The calibration process may be performed off-line [6], on-line [7, 28], or using a combination of the two. The latter approach is used in this paper where a batch off-line calibration is first performed for the 3-axis magnetometers which then provide the initial condition for the recursive on-line calibration. This procedure has several advantages:(1)
The off-line calibration process is less restricted by the availability of computational resources; therefore, a large set of calibration parameters can be evaluated. Additionally, the off-line calibration can be performed through comparing with temporary sensors of higher quality (to be removed before the operation) and data from deliberately performed maneuvers.
(2)
The off-line calibrated sensor parameters providea priori knowledge of the sensor error model. The on-line calibration then starts from off-line calibrated parameters, minimizing the impact of the transient response based on better initial estimates.
(3)
The on-line calibration provides the capability for dealing with time varying parameters.
(4)
The on-line calibrated parameters can be restricted within a set of prespecified bounds and the calibration can be turned off or revert to the off-line values in the event of unstable conditions.The concept of this 2-step calibration process is similar to the adaptive augmentation of a baseline controller [29] in control theory. Within this effort, the magnetometers are first calibrated off-line, followed by an on-line estimation of nine sensor biases associated with rate gyroscopes, accelerometers, and magnetometers.


### 3.1. Magnetometer Error Model

The magnetometer readings of earth’s magnetic field are often distorted by the existence of ferromagnetic materials in the local area, as well as the imperfection in the measurement system itself. The calibration of the magnetometer is a well-studied problem in the literature [27, 28, 30]. However, most calibration research in the past was performed off-line, due to a lack of reference information during the vehicle operation.Without a loss of generality, the 3-axis magnetometer calibration process is formulated as a nonlinear parameter identification (PID) problem:(18)
M
^
x
B
M
^
y
B
M
^
z
B
=
D
C
M
R
M
ϕ
,
R
M
θ
,
R
M
ψ
T
︸
R
M
·
S
M
x
0
0
0
S
M
y
0
0
0
S
M
z
︸
S
M
·
M
x
B
M
y
B
M
z
B
-
b
M
x
b
M
y
b
M
z
︸
b
M
,where M
^
x
,
y
,
z
B are the calibrated magnetometer measurements, R
M is a rotation matrix parameterized by the three rotation angles: R
M
ϕ, R
M
θ, and R
M
ψ, S
M is a diagonal scaling matrix, and b
M is a bias vector. The nine parameters (19)
Θ
M
=
R
M
ϕ
R
M
θ
R
M
ψ
S
M
x
S
M
y
S
M
z
b
M
x
b
M
y
b
M
z
Tto be estimated capture all of the soft iron effects, hard iron effects, sensor nonorthogonality, bias, and scaling factor [27].


### 3.2. Magnetometer Off-Line Calibration

The off-line calibration process starts with the creation of a set of reference signals to be compared with the actual magnetometer measurements. The reference is created by rotating the known earth’s magnetic vectorM
e
L from the local Cartesian coordinates to the vehicle’s body axis M
e
B:(20)
M
e
x
B
M
e
y
B
M
e
z
B
=
D
C
M
ϕ
,
θ
,
ψ
T
·
M
e
x
L
M
e
y
L
M
e
z
L
,where the Euler angles are provided by a GPS/IMU sensor fusion algorithm discussed earlier. A set of estimated calibration parameters Θ
^
M is then acquired through minimizing a cost function, which spans over an entire set of flight data, using a quasi-Newton method:(21)
J
=
∑
M
^
x
B
-
M
e
x
B
2
+
M
^
y
B
-
M
e
y
B
2
+
M
^
y
B
-
M
e
y
B
2
,
Θ
^
M
=
a
r
g
m
i
n
Θ
M
∈
R
9
J
.The off-line calibration problem can also be solved with a maximum likelihood method similar to the one discussed in [27].


### 3.3. Mag., Gyro, and Accelerometer On-Line Calibration

The on-line calibration process is performed by augmenting the sensor fusion Formulation #4 with nine additional bias states,b
=
b
G
p
b
G
q
b
G
r
b
A
x
b
A
y
b
A
z
b
M
x
b
M
y
b
M
z
T, one for each magnetometer, rate gyroscope, and accelerometer. In this way, the attitude state estimation and sensor error model parameter identification [8] are performed simultaneously. During the state prediction stage, the state transition equations described in (6) are used for attitude states, with the exception that the estimated rate gyroscope biases are subtracted from the raw IMU measurements:(22)
u
=
p
-
b
^
G
p
q
-
b
^
G
q
r
-
b
^
G
r
T
.The dynamics of the nine bias states are modeled as random walk using:(23)
b
˙
=
0
+
w
3
,where the bias states are only assumed to be perturbed with white noises w
3
≈
N
(
0
,
Q
3
)
. The initial conditions for the bias states are set to be(24)
b
0
=
0
0
0
0
0
0
b
~
M
x
b
~
M
y
b
~
M
z
T
,where b
~
M
x
,
M
y
,
M
z are off-line calibrated magnetometer bias values.The nonlinear observation equations are derived from (7) and (8) with added bias terms on the accelerometer measurements as well as the rotated and scaled magnetometer measurements:(25)
V
˙
x
L
+
v
g
x
V
˙
y
L
+
v
g
y
V
˙
z
L
+
v
g
z
G
P
S
=
D
C
M
ϕ
,
θ
,
ψ
·
a
x
B
-
b
^
A
x
+
v
a
x
a
y
B
-
b
^
A
y
+
v
a
y
a
z
B
-
b
^
A
z
+
v
a
z
I
M
U
+
0
0
g
,
D
C
M
R
~
M
ϕ
,
R
~
M
θ
,
R
~
M
ψ
T
·
S
~
M
x
0
0
0
S
~
M
y
0
0
0
S
~
M
z
·
M
x
B
+
v
M
x
M
y
B
+
v
M
y
M
z
B
+
v
M
z
-
b
^
M
x
b
^
M
y
b
^
M
z
=
D
C
M
ϕ
,
θ
,
ψ
T
·
M
e
x
L
M
e
y
L
M
e
z
L
,where R
~
M and S
~
M are the rotation and scaling matrices acquired from the off-line magnetometer calibration.


## 3.1. Magnetometer Error Model

The magnetometer readings of earth’s magnetic field are often distorted by the existence of ferromagnetic materials in the local area, as well as the imperfection in the measurement system itself. The calibration of the magnetometer is a well-studied problem in the literature [27, 28, 30]. However, most calibration research in the past was performed off-line, due to a lack of reference information during the vehicle operation.Without a loss of generality, the 3-axis magnetometer calibration process is formulated as a nonlinear parameter identification (PID) problem:(18)
M
^
x
B
M
^
y
B
M
^
z
B
=
D
C
M
R
M
ϕ
,
R
M
θ
,
R
M
ψ
T
︸
R
M
·
S
M
x
0
0
0
S
M
y
0
0
0
S
M
z
︸
S
M
·
M
x
B
M
y
B
M
z
B
-
b
M
x
b
M
y
b
M
z
︸
b
M
,where M
^
x
,
y
,
z
B are the calibrated magnetometer measurements, R
M is a rotation matrix parameterized by the three rotation angles: R
M
ϕ, R
M
θ, and R
M
ψ, S
M is a diagonal scaling matrix, and b
M is a bias vector. The nine parameters (19)
Θ
M
=
R
M
ϕ
R
M
θ
R
M
ψ
S
M
x
S
M
y
S
M
z
b
M
x
b
M
y
b
M
z
Tto be estimated capture all of the soft iron effects, hard iron effects, sensor nonorthogonality, bias, and scaling factor [27].


## 3.2. Magnetometer Off-Line Calibration

The off-line calibration process starts with the creation of a set of reference signals to be compared with the actual magnetometer measurements. The reference is created by rotating the known earth’s magnetic vectorM
e
L from the local Cartesian coordinates to the vehicle’s body axis M
e
B:(20)
M
e
x
B
M
e
y
B
M
e
z
B
=
D
C
M
ϕ
,
θ
,
ψ
T
·
M
e
x
L
M
e
y
L
M
e
z
L
,where the Euler angles are provided by a GPS/IMU sensor fusion algorithm discussed earlier. A set of estimated calibration parameters Θ
^
M is then acquired through minimizing a cost function, which spans over an entire set of flight data, using a quasi-Newton method:(21)
J
=
∑
M
^
x
B
-
M
e
x
B
2
+
M
^
y
B
-
M
e
y
B
2
+
M
^
y
B
-
M
e
y
B
2
,
Θ
^
M
=
a
r
g
m
i
n
Θ
M
∈
R
9
J
.The off-line calibration problem can also be solved with a maximum likelihood method similar to the one discussed in [27].


## 3.3. Mag., Gyro, and Accelerometer On-Line Calibration

The on-line calibration process is performed by augmenting the sensor fusion Formulation #4 with nine additional bias states,b
=
b
G
p
b
G
q
b
G
r
b
A
x
b
A
y
b
A
z
b
M
x
b
M
y
b
M
z
T, one for each magnetometer, rate gyroscope, and accelerometer. In this way, the attitude state estimation and sensor error model parameter identification [8] are performed simultaneously. During the state prediction stage, the state transition equations described in (6) are used for attitude states, with the exception that the estimated rate gyroscope biases are subtracted from the raw IMU measurements:(22)
u
=
p
-
b
^
G
p
q
-
b
^
G
q
r
-
b
^
G
r
T
.The dynamics of the nine bias states are modeled as random walk using:(23)
b
˙
=
0
+
w
3
,where the bias states are only assumed to be perturbed with white noises w
3
≈
N
(
0
,
Q
3
)
. The initial conditions for the bias states are set to be(24)
b
0
=
0
0
0
0
0
0
b
~
M
x
b
~
M
y
b
~
M
z
T
,where b
~
M
x
,
M
y
,
M
z are off-line calibrated magnetometer bias values.The nonlinear observation equations are derived from (7) and (8) with added bias terms on the accelerometer measurements as well as the rotated and scaled magnetometer measurements:(25)
V
˙
x
L
+
v
g
x
V
˙
y
L
+
v
g
y
V
˙
z
L
+
v
g
z
G
P
S
=
D
C
M
ϕ
,
θ
,
ψ
·
a
x
B
-
b
^
A
x
+
v
a
x
a
y
B
-
b
^
A
y
+
v
a
y
a
z
B
-
b
^
A
z
+
v
a
z
I
M
U
+
0
0
g
,
D
C
M
R
~
M
ϕ
,
R
~
M
θ
,
R
~
M
ψ
T
·
S
~
M
x
0
0
0
S
~
M
y
0
0
0
S
~
M
z
·
M
x
B
+
v
M
x
M
y
B
+
v
M
y
M
z
B
+
v
M
z
-
b
^
M
x
b
^
M
y
b
^
M
z
=
D
C
M
ϕ
,
θ
,
ψ
T
·
M
e
x
L
M
e
y
L
M
e
z
L
,where R
~
M and S
~
M are the rotation and scaling matrices acquired from the off-line magnetometer calibration.


## 4. Fault Detection, Identification, and Accommodation

The sensor FDIA is achieved through two independent approaches: information crosschecking and sensor bias tracking. The first approach detects discrepancies among all information sources to identify outliers, which could be due to temporary low-quality measurements or abrupt sensor failures. The second method allows detection and tracking of “soft” sensor failures that slowly develop over time.In our information crosschecking approach, it is important to mention that under nominal conditions, we assume that each independent information source can observe valid state estimates and that the optimal state estimate would be the result of fusing all sources. Therefore, because we have multiple redundant information sources, the goal is to identify outlier estimates and exclude their associated information sources from the state estimate altogether. A graphical representation of the information crosschecking approach is shown in Figure1.Figure 1
Fault Detection, Identification, and Accommodation approach.In Figure1, first, multiple Kalman updated states and error-covariance estimates (or equivalently information matrices and vectors) are derived using the independent sources of information available. In general, (S
k
,
I
k
,
i
^
k) are available at each time step k, where S
k is a set of N
+
1 independent information sources from either prediction or a sensor measurement, and I
k, i
^
k are sets of the associated Fisher information matrices and vectors. Next, to identify potential outlier information sources, the Mahalanobis distance [31] is used to evaluate the “closeness” between pairwise sets of estimates. For example, to determine if the sth information source is consistent with the rth information source, we evaluate(26)
P
s
,
k
=
I
s
,
k
-
1
,
x
^
s
,
k
=
P
s
,
k
i
^
s
,
k
,
P
r
,
k
=
I
r
,
k
-
1
,
x
^
r
,
k
=
P
r
,
k
i
^
r
,
kin order to calculate the square of the Mahalanobis distance between them. Consider (27)
F
s
∣
r
,
k
=
x
^
s
,
k
-
x
^
r
,
k
T
P
s
,
k
+
P
r
,
k
-
1
x
^
s
,
k
-
x
^
r
,
k
.We chose to use Mahalanobis distance because it is an intuitive metric that indicates the statistical agreement between state estimates. Additionally, it takes into consideration estimated error-covariance and is extensible to high dimension state-spaces.After considering all unique pairs of estimates from individual information sources a total ofN
+
1 unique values are available to base our fault detection and identification upon. However, these values represent the “closeness” between estimate pairs, which is not the most convenient for the goal of identifying individual faulty information sources. In particular, it would be more beneficial if we could evaluate how well each individual information source agrees with all other sources in a single metric. To accomplish this, we employ a simple vector in addition to combining the squared Mahalanobis distance values from each information source with respect to all others. The sum then represents how well a particular information source agrees with all others. As an example, for information source, s, we define all the unit vectors to other information sources:(28)
u
→
s
∣
i
=
e
s
k
-
e
~
i
k
e
s
k
-
e
~
i
k
,
i
=
1
:
N
,
i
≠
s
.Then, using the values calculated in (27), we use their inverse values to scale each unit vector, sum with vector addition, and find the resultant magnitude. The use of the inverse of the squared Mahalanobis distance is analogous to inverse square laws that govern power loss. The resultant e
s
k is a sum of a total of N
-
1 vectors associated with e
s
k and each component of E
~
k:(29)
F
→
s
=
∑
i
=
1
N
F
→
s
∣
i
,
i
≠
s
.Finally, the magnitude of the detection vector, F
→
s, provides a scalar assessment of the agreement between e
s
k and other estimates at the same time. If F
→
s is less than a prespecified threshold, Ω
s, that is, set based upon empirical tuning, a failure is declared for the sensor associated with e
s
k.For the attitude estimation problem described in this paper, three information sources are available at time stepk based on the posterior estimates from the previous step (I
k
-
1
∣
k
-
1
,
i
^
k
-
1
∣
k
-
1), (
1
) rate gyroscopes baseda priori estimates (I
k
∣
k
-
1
,
i
^
k
∣
k
-
1); (
2
) the posterior estimates, (I
k
∣
k
G
,
i
^
k
∣
k
G), updated with the gravity vector information; (
3
) the posterior estimates, (I
k
∣
k
M
,
i
^
k
∣
k
M), updated with the magnetic vector information. To ensure the independence of the three estimates, the calculation of (I
k
∣
k
G
,
i
^
k
∣
k
G) and (I
k
∣
k
M
,
i
^
k
∣
k
M) are based on a random walk assumption only instead of using the rate gyroscope measurements during the prediction step: (30)
x
˙
=
0
+
w
4
,where w
4
≈
N
(
0
,
Q
4
) are white noises.To complement the information crosschecking approach, the sensor bias tracking method monitors the bias states estimated by the on-line calibration scheme. Under nominal conditions, all sensor biases should be bounded within a prespecified envelopee. The size ofe can be determined based on a statistical evaluation of data collected in the past as well as common sensor error specifications, such as the bias instability for the case of accelerometers and rate gyroscopes. During the operation, if a sensor bias grows outside ofe, an anomaly warning for this particular sensor is declared and no additional action is taken. If the bias continues to grow beyond α ·e, where α
>
1 is a prespecified threshold value, a sensor failure status is declared. Under this condition, the bias states that are directly correlated or closely coupled with the faulty sensor are capped at or below their current value, and the associated error covariance matrix is scaled at each time step to represent an increasingly uncertain knowledge of the bias states:(31)
P
k
∣
k
-
1
f
=
β
·
P
k
∣
k
-
1
f
,where β
>
1 is empirically selected to be 1.005 in this implementation.Once a failure is declared with either FDI approach, fault accommodation is based on a simple concept: with the availability of redundant information and with the information update being a sum, the UIF has the freedom of rejecting any sensor measurement it considers a fault or of lower-quality. This approach allows the sensor fusion algorithm to be conservative and only uses the best information for state estimation. A block diagram for the UIF based fault-tolerant multiple sensor fusion algorithm is shown in Figure2.Figure 2
Overall sensor fusion algorithm architecture.


## 5. Experimental Setup

The sensor fusion algorithms outlined in the previous sections are evaluated with the actual flight data from YF-22 unmanned research aircraft [32] developed at West Virginia University. The vehicle, shown in Figure 3, is approximately 2.4 m long with a 2 m wing span and has a take-off weight of approximately 22.5 Kg. The aircraft is powered with a miniature turbine that provides 125 N of static thrust. The cruise speed for the aircraft is approximately 40 m/s.Figure 3
WVU YF-22 research aircraft and relevant sensors.The aircraft instrumentation [33] includes three sensors directly relevant to this study. An Analog Devices ADIS-16405® tri-axis inertial sensor with magnetometers is used to provide a 14-bit digital output of 3-axis acceleration, angular rate, and magnetic field measurements, with a full scale range of ±18 g, ±150°/s, and ±2.5 Gauss, respectively. The manufacturer reported 1
-
σ initial bias errors for the accelerometers, rate gyroscopes, and magnetometers that are ±50 mg, ±3°/s, and ±4 mGauss, respectively. A Novatel OEM4® GPS receiver provides an estimate of the aircraft 3D position and velocity in the Earth-Centered, Earth-Fixed (ECEF) coordinate system independent of the inertial and magnetic sensors, which is transformed into a local Cartesian coordinate system. The manufacturer reported GPS position and velocity accuracies are 1.8 meter Circular Error Probable (CEP) and 0.03 m/s Root Mean Square (RMS), respectively. A Goodrich VG34® mechanical vertical gyroscope is used to provide independent pitch and roll angle measurements and is used as the “truth data” for this sensor fusion study. The VG34 has a ±90° measurement range on the roll axis and a ±60° range on the pitch axis and is sampled with 16-bit resolution. The VG34 has a self-erection system and reported accuracy of within 0.25° of true vertical.


## 6. Results

The sensor fusion algorithms are validated using three sets of flight data. The first set is used for magnetometer off-line calibration. A GPS/IMU sensor fusion algorithm (Formulation #1) augmented with six rate gyroscope and accelerometer bias states is used to estimate the attitude angles required by the off-line magnetometer calibration algorithm. The remaining two sets of flight data collected from two different days were used to validate and compare different sensor fusion algorithm performance. Table1 lists the estimation performance of eight sensor fusion algorithms in terms of mean absolute error and error standard deviation for pitch and roll angle estimates. An average of two flights is used in calculating the value of each entry. The same stochastic noise modeling assumptions of the GPS, rate gyroscope, accelerometer, and magnetometer noises were used for each formulation and no individual tuning was performed.Table 1
Statistics of attitude estimation algorithms.
Formulation
E
ϕ
e
r
r
σ
ϕ
e
r
r
E
θ
e
r
r
σ
θ
e
r
r
GPS/IMU
2.990
1.671
1.590
1.865
GPS/IMU + 6 bias states
1.516
1.524
1.558
1.700
GPS/Mag. (Off-L. Cal.)
1.726
2.008
2.315
2.195
IMU/Mag. (Off-L. Cal.)
15.81
19.59
22.99
27.69
GPS/IMU/Mag. (Raw)
8.061
3.219
5.276
2.279
GPS/IMU/Mag.(Off-L. Cal.)
2.026
1.452
2.415
2.127
GPS/IMU/Mag.(Off-L. + On-L Cal.)
1.598
1.697
1.417
1.679Table1 shows that each algorithm is able to provide a pitch and roll estimate. The GPS/IMU and GPS/Mag. formulations both have good performance, but the IMU/Mag. performs poorly with off-line calibrated magnetometers. The performance of IMU/Mag. formulation is found to be highly sensitive to the quality of the calibration.The combination of all three sensors gives a good attitude estimation performance once a set of off-line calibrated magnetometer parameters are used. The introduction of an on-line calibration scheme to the GPS/IMU/Mag. sensor fusion algorithm provides an additional enhancement of its performance. Figure4 shows a section of the flight with vertical gyroscope pitch measurements along with estimates from the GPS/IMU/Mag. sensor fusion algorithm with raw magnetometer readings, off-line calibrated parameters, and a combined off-line and on-line sensor calibration.Figure 4
GPS/IMU/Mag. sensor fusion with raw, off-line calibration, and a two-stage off-line/on-line calibration.The fault-tolerant aspect of the sensor fusion algorithm is validated with simulated sensor failures superimposed on the actual flight data. The performance of the sensor fusion algorithm with and without FDI and fault accommodation schemes under nominal condition were first evaluated, along with the estimation performance under a set of simple sensor failure scenarios, where the outputs of GPS, rate gyroscopes, or magnetometers were lost for the second half (50%) of the flight. Table2 summarizes the results of this test.Table 2
Statistics of UIF-based fault tolerant attitude estimation.
Operating condition
E
ϕ
e
r
r
σ
ϕ
e
r
r
E
θ
e
r
r
σ
θ
e
r
r
Nominal w/o FDIA
1.598
1.697
1.417
1.679
Nominal w/FDIA
1.585
1.708
1.360
1.610
GPS failure w/FDIA
3.566
5.546
2.205
3.185
Gyro failure w/FDIA
1.814
2.211
1.622
1.821
Mag. failure w/FDIA
1.890
2.111
1.534
1.892An interesting observation is that the inclusion of the FDIA scheme slightly improves the overall performance of the sensor fusion algorithm even under nominal conditions without imposed sensor failures. This is due to the fact that the FDIA scheme monitors the quality of each measurement and rejects information of lower quality. This desirable feature is demonstrated in Figure5, where four GPS data points were rejected by the sensor fusion algorithm during a half-second section of the flight.Figure 5
Rejection of low-quality GPS measurements by the sensor fusion algorithm.The ability of the sensor fusion algorithm to detect, accommodate, and recover from different sensor failures is further demonstrated in Figure6, where a series of sequentially imposed GPS, rate gyroscope, and magnetometer failures are presented. The force field based failure detection signals for each set of sensors are also shown in Figure 7, along with the respective detection threshold indicated as a dotted line.Figure 6
Sensor fusion with no failure, GPS failure, rate gyroscope failure, and magnetometer failure.Figure 7
“Virtual Force Field” based failure detection signal.Figure6 demonstrates an advantage of this information filter based fault accommodation method for fault accommodation. The transition between nominal and failure conditions in terms of attitude estimation is smooth and seamless. The estimation performance gracefully degrades after a sensor failure and recovers after the failure is removed.In addition to large and abrupt sensor failures, the ability of the sensor fusion algorithm to handle slowly developing “soft” failures is demonstrated in Figure8. A random walk bias (with σ
=
0.02
d
e
g
/
s
e
c) is added to the pitch rate sensor measurements and the pitch rate bias state estimate is used for FDI. The sensor fusion algorithm compensates for the “soft” failure through sensor calibration when the bias is small and through sensor rejection when it becomes large.Figure 8
Detection and accommodation of slowly building up “soft” failure.


## 7. Conclusions and Discussion

In this paper, a general 3-step sensor fusion approach is proposed and is applied to a 3D attitude estimation problem. The validation results using sets of UAV flight data show that having multiple redundant information sources allows for on-line calibration of individual sensors within the measurement system, leading to both improved performance and improved understanding of sensor health conditions, especially under “soft” sensor failure conditions. Furthermore, a crosschecking among different information sources allows for identifying “hard” failures or instantaneous faulty measurements. The use of information filter provides a convenient and scalable platform for multiple sensor fusion, information crosschecking, and faulty sensor rejection.The attitude estimation problem discussed in this paper utilizes three information sources, which is the minimum number required for the presented FDIA approach. With an increased number of sensors, FDI could become more reliable and the overall estimation performance would be less sensitive to individual sensor failures. The development of a decentralized on-line sensor calibration scheme is the remaining bottleneck before a truly scalable multiple sensor fusion algorithm could be implemented.



---

*Source: 6217428-2016-02-24.xml*