A Decentralized Sensor Fusion Scheme for Multi Sensorial Fault Resilient Pose Estimation

Abstract

This article proposes a novel decentralized two-layered and multi-sensorial based fusion architecture for establishing a novel resilient pose estimation scheme. As it will be presented, the first layer of the fusion architecture considers a set of distributed nodes. All the possible combinations of pose information, appearing from different sensors, are integrated to acquire various possibilities of estimated pose obtained by involving multiple extended Kalman filters. Based on the estimated poses, obtained from the first layer, a Fault Resilient Optimal Information Fusion (FR-OIF) paradigm is introduced in the second layer to provide a trusted pose estimation. The second layer incorporates the output of each node (constructed in the first layer) in a weighted linear combination form, while explicitly accounting for the maximum likelihood fusion criterion. Moreover, in the case of inaccurate measurements, the proposed FR-OIF formulation enables a self resiliency by embedding a built-in fault isolation mechanism. Additionally, the FR-OIF scheme is also able to address accurate localization in the presence of sensor failures or erroneous measurements. To demonstrate the effectiveness of the proposed fusion architecture, extensive experimental studies have been conducted with a micro aerial vehicle, equipped with various onboard pose sensors, such as a 3D lidar, a real-sense camera, an ultra wide band node, and an IMU. The efficiency of the proposed novel framework is extensively evaluated through multiple experimental results, while its superiority is also demonstrated through a comparison with the classical multi-sensorial centralized fusion approach.

1. Introduction

State estimation is a challenging problem in the field of robotics that has been significantly explored in the recent years and in different scientific and technological oriented communities, such as: robotics [1], aerospace [2], automatic control [3], artificial intelligence [4], and computer vision [5]. In this framework, one of the most interesting problems is the one that is related to the estimation of the pose of a robot, especially for the case that multi-sensors are utilized for the pose determination problem, with related sensor fusion schemes, in order to increase the overall accuracy of the estimation but also at the same time introduce the proper resiliency.

Towards this direction, lately the research on sensor fusion has been evolving in a rapid manner, since multi-sensor fusion has the ability to integrate or to combine data streams from different sources and simultaneously to increase the quality of the measurements, while decreasing the corrupting noise from the measurements. Thus, in the multi-sensorial fusion architectures, such for example the case of the pose estimation, it is very common to have multiple sensors that are providing, e.g., full pose state estimations or partial pose estimations (translation or orientation) and to have an overall sensor fusion scheme that is commonly realized in a centralized [6] or distributed approaches [6,7]. In centralized fusion architectures, a central node or a single node is utilized, where direct measurement data or raw data from multiple sensors are used to fuse using several type of Kalman filters, depending upon the system whether it is linear or nonlinear. In this context, the Extended Kalman Filter (EKF) has taken considerable attention in numerous research works involving the case of multi-sensor fusion, while utilizing real data from sensors for localization of ground robots and Micro Aerial Vehicle (MAV) [8,9,10,11]. However, the EKF based fusion algorithms involve local linearization of the system. In order to explicitly account for the nonlinearities, involved in the dynamical model, few progressive approaches consider the Unscented Kalman Filter (UKF) [12,13] and the Particle Filter (PF) [14] based multi-sensor fusion for robotic localization. Even though the UKF and PF are superior for addressing nonlinearities, the advantage comes with additional complexity in the computational burden. Comparison studies reported in [15,16] demonstrated that for robot localization, the performance of EKF is comparable in all practical purposes. Apart from the conventional Kalman based approaches, various innovative numerical optimization based multi-sensor fusion algorithms, such as moving horizon estimation [17,18], set membership function [19], graph optimization [20], while MAV localization in GPS denied environment have recently been investigated in [19,21]. The numerical optimization based estimation framework has the capability to incorporate the non-linearity of the dynamical model, as well as various physical constraints. However, it imposes additional computation complexity and limitations due to the theoretical guarantee on convergence properties.

In general, a centralized structure with multi inputs and outputs works very well for data fusion. Though it is not sufficient for all the cases, sometimes one of the sensor can fail suddenly for a certain period of time in total operating duration, in such a situation, the centralized approach follows the faulty sensor’s data, while having the disadvantage that it can not detect or eliminate the fault occurred by the sensors. Although it is possible to attain an almost optimal solution in the centralized fusion framework, in real and field utilizations, processing all the sensors at a node or a point is most likely ineffective and has the potential to lead into direct failures in case of a sensor defects or temporary performance degradation. On the other hand, the distributed fusion [22] typically consists of at least two layers, where in the first layer, the raw data are collected from different sensor measurement units to create the local estimates and in the sequel are being forwarded to the second layer for further fusion from the corresponding nodes. Typically, in the first layer a Kalman filter is utilized to provide the local estimates from the sensors [23]. However, depending on the data assimilation procedures, in the second layer various decentralized fusion methods have been investigated [24]. In this direction, an information filter based decentralized fusion [22] is considered for the localization of mobile robots and MAVs in [24,25]. However, these formulations do not explicitly incorporates the correlation between local estimates. Contemplating the dependency of the local nodes, various covariance intersection based decentralize fusion algorithms for collaborative localization have been investigated in [26,27,28,29,30]. In this context, an innovative maximum likelihood criterion for performing the decentralized fusion was presented in [31], where the information from the local nodes were integrated as a weighted combination to provide the fused states. The weighting matrices were judiciously determined based on the cross covariance of the local estimates. However, these decentralized fusion approaches, have been implemented based on the assumption that the measurements from sensors are only influenced by an inherent unbiased Gaussian noise, while in reality, and for field robotic applications, the related measurements are spurious [30] due to unexpected uncertainties, such as a temporal inoperative surrounding (such as low illumination condition, presence of smoke/dust etc.) fault, spike and sensor glitches. In such situations, the magnitude of inaccuracy in the measurements is much larger when compared to the normal noise [32]. In order to address this issue, various fault detection and isolation methods, in combination with the decentralized fusion, are investigated in the related state of the art literature [28,33,34].

In summary, distributed fusion is a robust framework to failures and can indicate the proper resiliency for critical applications, while eliminating the risk of single failures. Specifically in robotic applications, the approach of perception, in the direction of decentralized fusion, can handle the fundamental problem addressed in numerous implementations like multi-robot tracking, cooperative localization [35] and navigation [35], multi-robot Simultaneous Localization and Mapping (SLAM) [36], distributed multi-view 3D reconstruction and mapping, and multi-robot monitoring [37]. The map merging, data association, and robot localization can potentially be efficient only if the robots are enough capable to perceive autonomously the world. Therefore, centralized and decentralized fusion or in other words fusion in general, plays an important role in all robotic applications.

In this article, a unique decentralized multi-sensor fusion approach is proposed that introduces a novel flexible fault resilient structure, by isolating the information from the faulty sensor, while enabling the information provided by the other sensor measurements in an optimal information fusion framework. Furthermore, the classical optimal information fusion (OIF) presented in [31] has a provision to incorporate a sensor level fault isolation, which operates as a separate unit. However, an increasing number of sensors/local nodes, along with the presence of temporal sporadic (spikes, temporary failure) measurements, demands a more flexible fusion architecture to account for fault resiliency in real time. In the present work, an innovative fault isolation mechanism is embedded with the OIF architecture by representing the isolation operation, as a constraint optimization problem. Thus, the novelty of this article stems from the novel application of an optimal information two-layered sensor fusion method, which has been modified ideally for handling sensor breakdown during operation, as it will be described in the sequel.

The main focus of this article is to develop a two-layered, multi-sensor fusion with self resiliency with the assistance of a unique optimal isolation algorithm. The technique works in several stages. In the first step, information from multiple asynchronous sensors is exhaustively exploited in an orderly sequence by introducing the concept of nodes. Each node individually fuses position and orientation information from independent sensors separately. In a way, the nodal architecture introduces the feasibility of distinguishing partially defective measurements and thereby broaden out the possibility of fusing all the acceptable information. For instance, if a sensor provides accurate position and defective orientation measurement for a short duration, utilization of the position measurement would be beneficial in lieu of discarding the entire sensor information. Next, in the second layer, the information from each node is blended in a weighted combination by employing an maximum likelihood estimator to obtain the most accurate pose collectively. Moreover, the proposed fault resilient optimal information fusion architecture incorporates an inbuilt fault isolation mechanism to discard disputed outcome from sensors. Once the disputed outcomes are observed, the weighing parameters of the optimal information filter are adjusted to accommodate the fault isolation. The second contribution stems from the effectiveness evaluation of the second layer fusion architecture when a sensor is not working correctly for a certain period or when the system receives inaccurate measurements from the sensors. In this case, an optimal information filter with fault handling capability is proposed and incorporated to get more robust and accurate responses from the corrupted outcomes. In this case, a modified co-variance weight scheme is introduced for combining all possible nodes in the second layer that is utilized to resolve the effect of erroneous measurements. The final contribution of the article is towards the performance comparison of the existing centralized and decentralized fusion techniques with the proposed novel decentralized fusion approach. Both existing methods methods perform well when measured sensor data are faultless, otherwise, the proposed fault resilient decentralized fusion scheme works far better than the existing centralized and decentralized filter estimation. The comparison of these different fusion architectures ha been carried out by using experimentally collected data sets.

2. Problem Formulation

Aiming towards a real-time, resilient and accurate navigation for autonomous exploration, through an unknown environment, aerial robotic vehicles equipped with multiple asynchronous real senors, as the one depicted in Figure 1 will be considered as the base line of this novel work, without a loss of generality, since the overall framework has the merit to be platform agnostic. In this case, the considered sensor suite of the MAV contains a Velodyne Puck LITE, the Intel realsense camera T265, the IMU of a Pixhawk 4 flight controller, and a single UWB node. The collected point-cloud from the 3D-lidar is processed online to provide odometry based on [38]. Essentially, the 3D-lidar odometry and the real sense camera T265 are capable of providing the real time 3D pose of the MAV independently, whereas, the IMU provides the measurements of angular velocity, as well as the acceleration of the MAV. In addition, a network of 5 UWB nodes is set strategically around the utilized flying arena, for estimating the position of the MAV based on the mounted UWB node. The overall objective is to provide a novel decentralized multi-sensor fusion architecture that can blend the information from various real sensor measurements to obtain the most accurate pose of the MAV in real-time. A schematic representation of the MAV pose is depicted in Figure 2.

The pose is described using two reference frames, namely the world frame $(\mathbb{W}=\{X_W,Y_W,Z_W\})$ and the body-fixed frame $(\mathbb{B}=\{X_B,Y_B,Z_B\})$. The body-fixed frame is attached to the MAV’s centre of mass, while the inertial frame is assumed to be attached at a point on the ground with its $X,Y$ and Z axis directed along the East, North and Up (ENU) directions, respectively. The sensors are mounted in the body fixed frame $\mathbb{B}$ of the MAV, and provide the information regarding its pose. The position of the MAV (essentially the origin of the body-fixed frame) is defined as $\overrightarrow{p}$, which is described with respect to the world frame $\mathbb{W}$ as shown in Figure 2. The orientation of MAV with respect to the world frame can be visualized using the Euler angle representation $\{\varphi ,\theta ,\psi \}$, denoting the roll, pitch and yaw rotations, respectively. In order to avoid the singularity associated with Euler angle, the orientation of the MAV is considered to be represented by quaternions [39] denoted with q. The position of the MAV, p and its orientation, q together designate the pose of the vehicle.

2.1. Asynchronous Sensors Associated with the Present Study

The under consideration MAV, equipped with various sensors, is presented in Figure 1. The sensors that are considered in the present context are: (a) the IMU of a Pixhawk 4 flight controller that provides the acceleration $a_m$, the angular velocity ${\omega }_m$ and the orientation $q_{IMU}$ of the MAV, (b) the Velodyne Puck LITE radar assisted with Lidar Odometry (LIO) [38] to provide a 3D pose of the MAV denoted by $p_{LIO},q_{LIO}$, (c) the Intel Real-sense T265 visual sensor integrated with visual odometry (VIO) that provides a position and orientation denoted with $p_{VIO},q_{VIO}$, and (d) the Ultra-Wideband (UWB) transceivers that provide the position of the MAV, denoted with $p_{UWB}$.

2.2. The MAV’s Utilized Kinematic Model

The decentralized sensor fusion architecture at its core utilizes the model based estimation framework, where the nonlinear kinematic model of the MAV is considered as [40]:

$$\begin{array}{cc}\hfill & \dot{p}=v_t\hfill \end{array}$$

(1a)

$$\begin{array}{cc}\hfill & {\dot{v}}_t=a_t\hfill \end{array}$$

(1b)

$$\begin{array}{cc}\hfill & \dot{q}=\frac{1}{2}q\otimes {\omega }_t\hfill \end{array}$$

(1c)

where, $p\in {\mathbb{R}}^{3\times 1}$ denotes the position, $v_t\in {\mathbb{R}}^{3\times 1}$ represents the velocity, $q\in {\mathbb{R}}^{4\times 1}$ stands for the orientation of the MAV in the form of quaternions representation. Here, $a_t\in {\mathbb{R}}^{3\times 1}$ represents the total acceleration of the vehicle expressed in the inertial frame of references, where as ${\omega }_t\in {\mathbb{R}}^{3\times 1}$ denotes the body rate experienced by the vehicle. Physically, these parameters (acceleration and body rates) are characterized as an input to the system, which are typically measured using an IMU denoted as $a_m$ and ${\omega }_m$. In general, the measurements from sensors are noisy, which includes sensor bias as well. In order to establish this fact, the measurement signals are represented as:

$$\begin{array}{cc}\hfill & a_m=R_t^T(a_t-g_t)+a_{b_t}+a_n\hfill \end{array}$$

(2a)

$$\begin{array}{cc}\hfill & {\omega }_m={\omega }_t+{\omega }_{b_t}+{\omega }_n\hfill \end{array}$$

(2b)

where, $a_{b_t}\in {\mathbb{R}}^{3\times 1}$ denotes the accelerometer bias and ${\omega }_{b_t}\in {\mathbb{R}}^{3\times 1}$ represents the gyroscopic bias terms, $a_n$ and ${\omega }_n$ signify the additive noise for acceleration and angular rate, respectively, $R_t\triangleq R\left(q\right)\in SO\left(3\right)$ denotes the transformation matrix, from the body to the world frame, and $g_t\in {\mathbb{R}}^{3\times 1}$ denotes the gravity bias. Moreover, it has also been accounted that the bias factors $a_{b_t},{\omega }_{b_t}$ are driven by a process noise dynamically represented as:

$$\begin{array}{cc}\hfill {\dot{a}}_{b_t}& =a_{\omega }\hfill \end{array}$$

(3a)

$$\begin{array}{cc}\hfill {\dot{\omega }}_{b_t}& ={\omega }_{\omega }\hfill \end{array}$$

(3b)

$$\begin{array}{cc}\hfill {\dot{g}}_t& =0_{3\times 1}\hfill \end{array}$$

(3c)

where, $a_{\omega }\in {\mathbb{R}}^{3\times 1}$ and ${\omega }_{\omega }\in {\mathbb{R}}^{3\times 1}$ are the accelerometer and gyroscopic process noise, respectively. Rearranging Equation (4a,b), the total acceleration can be expressed as:

$$\begin{array}{cc}\hfill a_t& =R_t(a_m-a_{b_t}-a_n)+g_t\hfill \end{array}$$

(4a)

$$\begin{array}{cc}\hfill {\omega }_t& ={\omega }_m-{\omega }_{b_t}-{\omega }_n\hfill \end{array}$$

(4b)

Substituting, Equations (4a,b) and (1b,c), respectively, yields:

$$\begin{array}{cc}\hfill {\dot{v}}_t& =R_t\left(a_m-a_{b_t}-a_n\right)+g_t\hfill \end{array}$$

(5a)

$$\begin{array}{cc}\hfill \dot{q}& =\frac{1}{2}q\otimes ({\omega }_m-{\omega }_{b_t}-{\omega }_n)\hfill \end{array}$$

(5b)

The above equation of motion of MAV is expressed in a compact mathematical notation given as:

$$\begin{array}{cc}\hfill {\dot{X}}_t& =f_t(X_t,u_m,w)\hfill \end{array}$$

(6a)

$$\begin{array}{cc}\hfill y_t& =[I_{7\times 7}\mid 0_{7\times 12}]X_t\hfill \end{array}$$

(6b)

where, the state vector is denoted as $X_t={[p_t,v_t,q,a_{bt},{\omega }_{bt},g_t]}^T\in R^{19\times 1}$, the noisy measured input based on IMU reading is denoted as $u_m\in R^{6\times 1}$ and the random process noise $w\in R^{6\times 1}$ is defined as:

$$u_m=\left[\begin{array}{c}{a}_m-a_n\\ {\omega }_m-{\omega }_n\end{array}\right],w=\left[\begin{array}{c}{a}_w\\ {\omega }_w\end{array}\right]$$

3. Decentralized Sensor Fusion Architecture

In order to make use of all the available information from the sensor measurements in the best possible way, a two-layered fusion architecture is considered here. A schematic overview of the proposed fusion architecture is presented in Figure 3. Since the primary focus here is to determine the MAV pose, the sensors involved in this process are capable of measuring either the position and/or orientation, while the IMU is the only exception, which provides the acceleration and body rates of the vehicle. In the First layer, information from multiple asynchronous sensors is exhaustively exploited in an orderly sequence by introducing the concept of nodes. Potentially, each node provides the pose of the MAV, which is obtained by involving its position from one sensor and orientation from another. In the second layer, the information from each node is used in a weighted combination to collectively obtain the most accurate pose of the MAV in a maximum likelihood hood manner. A complete overview of the fusion architecture in detail is presented in the sequel.

3.1. First Layered Decentralized Fusion Architecture

In the first layer, we have introduced the concept of the decentralized nodes. In the context of the multi-sensor framework, the position and orientation of the MAV is obtained by two distinct sensors that are arbitrarily selected to construct a node. Incorporating all of such possible combinations of sensor measurements collectively, a total number of seven nodes are constructed in the present setup and alphabetically denoted as node-l, where $l\in \{A,B,\cdots ,G\}$. The position and orientation information accounted for the individual nodes are described in

Table 1. Combination of sensor information used to construct the nodes of the proposed first layer

Pose	A	B	C	D	E	F	G
Position	Real sense Camera	3D LiDar	UWB	UWB	3D LiDar	Real sense Camera	UWB
Orientation	IMU	Real sense Camera	IMU	3D LiDar	IMU	3D- LiDar	Real sense Camera

. In the process of constructing the node, each decentralized node is associated with an Extended Kalman Filters (EKF) [41], which blends the measurements from various sensors in all possible combinations. However, each node receives the IMU information, as measured actuation/control input to the kinematic model associated with the Kalman filter. Moreover, position and orientation from two distinct sensors are utilized as the measurement information. In the context of the present setup under consideration, Figure 4 describes all the possible decentralized nodes and the associated measurements [42].

Figure 4. Combination of position and orientation information flow from various sensors in constituting different nodes of the first layer.

Figure 4. Combination of position and orientation information flow from various sensors in constituting different nodes of the first layer.

Previously, in the Section 2.2, the kinematic model of a MAV in continuous time form is presented in Equation (6a). In order to describe the decentralized nodes, associated with the first layer, in a compact mathematical form (in correlations with EKF), an equivalent of Equation (6b) in discrete time representation (using Euler [43] discretization) is provided by:

$$\begin{array}{cc}\hfill x_k& =f_{k-1}(x_{k-1},u_{k-1},{\omega }_{k-1})\hfill \end{array}$$

(7a)

$$\begin{array}{cc}\hfill y_k& =h_k(x_k,v_k)=[I_{7\times 7}\mid 0_{7\times 12}]x_k+v_k\hfill \end{array}$$

(7b)

where, k denotes the discrete time instants. It should be noted that in order to account for model inaccuracy, we have considered a process noise ${\omega }_k\in R^{19}$. Moreover, an additional measurement noise vector $v_k\in R^7$ is introduced in Equation (7b) to encapsulate a realistic output model, under the influence of noisy measurement appearing from real sensors. The process and measurement noise are assumed to follow the Gaussian distribution as:

$$\begin{array}{c}\hfill {\omega }_k\sim \mathcal{N}(0,Q_k),v_k\sim \mathcal{N}(0,R_k)\end{array}$$

(8)

where $Q_k$ and $R_k$ represents the process noise co-variance matrix and the measurement noise co-variance matrix, respectively. The mathematical operator E denotes the expectation and the superscript T indicates the transpose. Starting with an initial guess of a posteriori estimate ${\widehat{x}}_{l_0}^{+}=E\left(x_{l_0}\right)$ and $P_{l_0}^{+}=E\left[(x_l-{\widehat{x}}_{l_0}^{+}){(x_l-{\widehat{x}}_{l_0}^{+})}^T\right]$, along with the assumption in Equation (8), the $l^{\mathrm{th}}$ node is described as a local EKF with the following prediction-correction formalism:

Prediction Steps:

$$\begin{array}{cc}\hfill {\widehat{x}}_{l_0}^{+}& =E\left(x_{l_0}\right),P_{l_0}^{+}=E\left[(x_l-{\widehat{x}}_{l_0}^{+}){(x_l-{\widehat{x}}_{l_0}^{+})}^T\right]\hfill \end{array}$$

(9a)

$$\begin{array}{cc}\hfill {\widehat{x}}_{l_k}^{-}& =f_{l_{k-1}}({\widehat{x}}_{l_{k-1}}^{+},u_{k-1},0)\hfill \end{array}$$

(9b)

$$\begin{array}{cc}\hfill K_{l_k}& =P_{l_k}^{-}{H}_{l_k}^T{(H_{l_k}{P}_{l_k}^{-}{H}_{l_k}^T+R_{k_l}^T)}^{-1}\hfill \end{array}$$

(9c)

$$\begin{array}{cc}\hfill P_{l_k}^{-}& =F_{l_k}{P}_{l_k}^{+}{F}_{l_k}^T+L_{l_k}{Q}_{l_k}{L}_{l_k}\hfill \end{array}$$

(9d)

where the ‘+’ symbol is used to denote an a priori estimate, the ‘−’ symbol is designated to a posteriori estimate, the subscript l indicates the corresponding variable of the lth node, where $l\in \{A,B,\cdots ,G\}$. The Jacobian matrices are defined as:

$$\begin{array}{c}\hfill F_{l_k}=\frac{\partial f_{l_{k-1}}}{\partial x_{l_k}},L_{l_k}=\frac{\partial f_{l_k}}{\partial u_k},H_{l_k}=\frac{\partial h_{l_k}}{\partial x{l}_k}\end{array}$$

(10)

while the inputs excitation $u_k$ (linear acceleration and angular velocity), used in the prediction process, are essentially obtained from the IMU measurements.

Correction Steps:

$$\begin{array}{cc}\hfill {\widehat{x}}_{l_k}^{+}& ={\widehat{x}}_{l_k}^{-}+K_{l_k}[y_{l_k}-h_{l_k}(x_{l_k}^{-},0)]\hfill \end{array}$$

(11a)

$$\begin{array}{cc}\hfill P_{l_k}^{+}& =(I-K_{l_k}{H}_{l_k})P_{l_k}^{-}\hfill \end{array}$$

(11b)

Note that, in the process of constructing the nodes, the associated EKF cleans out the noisy measurements appearing from the actual sensor unit. It is apparent that, individually each node is potentially capable of proving the information regarding the pose of the MAV. However, the accuracy of pose information, obtained from each decentralized node, varies depending on the accuracy of the sensors that are involved to constitute the node. Hence, to obtain a best possible pose estimate, a second layer architecture is presented in the sequel.

3.2. Second Layer Decentralized Fusion Architecture

The estimated states ${\widehat{x}}_l$, $l\in \{A,B,\cdots ,G\}$ from each node are placed together in a weighted combination to jointly obtain an accurate estimate of the MAV pose, while using an Optimal Information Filter (OIF) by maximum likelihood estimation. Thus, the FR-OIF is used in the second layer, which incorporates the capability of choosing optimal weights based on the co-variances that is obtained from the first layer fusion. Moreover, the proposed formulation embeds a fault isolation mechanism with the OIF architecture. The collective estimate of the fused state vector, as the output of the second layer, is expressed as:

$${\widehat{x}}_k=\sum _{l\in \{A,\cdots ,G\}}{\overline{A}}_{l_k}\widehat{x_{l_k}}$$

(12)

where, ${\overline{A}}_{l_k},l\in \{A,B,\cdots ,G\}$ represents the arbitrary weight associated with the corresponding node. Here ${\widehat{x}}_k$ and ${\widehat{x}}_{l_k}$ denote the outcome of the second layered fusion and estimated states of the $l^{\mathrm{th}}$ node from the first layer, respectively. These weigh parameters are optimally determined based on a minimum variance (maximum likelihood) criterion.

Assuming that both the FR-OIF, as well as the EKF act as an unbiased estimator, i.e., $E\left({\widehat{x}}_k\right)=E\left(x_k\right)$, $E\left({\widehat{x}}_{l_k}\right)=E\left(x_k\right)$, and taking the expectation of both sides of Equation (12) it yields:

$${\overline{A}}_{A_k}+{\overline{A}}_{B_k}+......+{\overline{A}}_{G_k}=I$$

(13)

where $x_k$ represents the actual state of the MAV. The estimation error for FR-OIF, i.e., $x_k-{\widehat{x}}_k$ is expressed as:

$${\tilde{x}}_k=x_k-\sum _{l\in \{A_k,\cdots ,G\}}{\overline{A}}_{l_k}{\widehat{x}}_{l_k}$$

(14)

Using the constraint relation from Equation (13), the estimation error in Equation (14) is rewritten as:

$$\widetilde{x_k}=\sum _{l\in \{A,\cdots ,G\}}\overline{A_{l_k}}(x_k-{\widehat{x}}_{l_k})=\sum _{l\in \{A,\cdots ,G\}}{\overline{A}}_l{\tilde{x}}_{l_k}=W_k{\tilde{x}}_L$$

(15)

where $W_k={[{\overline{A}}_{A_k},{\overline{A}}_{B_k},\cdots {\overline{A}}_{G_k}]}^T$ and ${\tilde{x}}_L=[{\tilde{x}}_{A_k},{\tilde{x}}_{B_k},\cdots ,{\tilde{x}}_{G_k}]$. Hence, the error co-variance matrix of the second layer of the FR-OIF is expressed as:

$$P_k=E\left({\tilde{x}}_k{\tilde{x}}_k^T\right)=W_k^T{\Sigma }_k{W}_k$$

(16)

where ${\Sigma }_k=E\left({\tilde{x}}_L{\tilde{x}}_L^T\right)=P_{{(l,m)}_k},\forall l=m=\{A,B,\cdots ,G\}$ represents the cross co-variance matrix [31] between lth and mth node, expressed as:

$$P_{{(l,m)}_k}^{-}=\left[I-K_{m_k}{H}_{m_k}\right]\times \left[F_{l_k}{P}_{{(l,m)}_k}^{+}{F}_{l_k}^{\mathrm{T}}+Q_k\right]\times {\left[I-K_{m_k}{H}_{m_k}\right]}^{\mathrm{T}}$$

(17)

At this point, it is possible to obtain the weight parameter matrix W by solving the following static optimization, as described in [31]:

$$\begin{array}{cc}\hfill & \underset{A_{A_k},\cdots ,A_{G_k}}{min}{J}_k=\frac{1}{2}tr\left(P_k\right)=\frac{1}{2}tr\left(W_k^T{\Sigma }_k{W}_k^T\right)\hfill \\ \hfill & \mathrm{subjected}\mathrm{to}\left({\overline{A}}_{A_k}+{\overline{A}}_{B_k}+.......+{\overline{A}}_{G_k}\right)=I\hfill \end{array}$$

(18)

However, the solution of Equation (18), obtained from the classical OIF in [31], is unable to provide a sufficient resiliency in the presence of inaccurate measurements obtained from one or a group of sensors. However, such problems of corrupted sensor measurements for a short duration of the operation period are often encountered in reality. For example, in the absence of sufficient visual features for some part of the surrounding environment, the real-sense camera fails to determine the MAV’s position, or in the presence of a bright moving object in the lidar’s field of view, it fails to provide an accurate pose.

In order to overcome such shortcomings, a separate fault isolation technique is proposed as an enhancement to the proposed decentralized estimation scheme, where the classical OIF formulation is modified to incorporate an inbuilt fault isolation mechanism with the OIF structure. For a time interval, if a group of sensors is identified to be corrupted, based on a fault detection method, the corresponding nodes associated with the faulty sensor need to be eliminated from the second layer architecture during the defective period of operation. In this article and without a loss of generality, the mechanisms for identifying the fault occurrence will not be considered and it will be assumed that the time of the fault and the faulty node can be identified. However, a straightforward nullifying of the weight matrices linked with the corrupted nodes, without altering other weights, leads to violation of the constraint in Equation (13). Hence, it is required to reformulate the optimization problem to bring in the flexibility of enabling/disabling a group of nodes online, while the MAV is operating in real-life applications. Let us consider the ith node, where $i\in A_k,B_k,\cdots ,G_k$, is found to be corrupted for a short time interval. This brings an additional constraint collectively presented as:

$$\sum _{l\in \{A,\cdots ,G\}}{\overline{A}}_{l_k}=I,\sum _{i\in \{A,\cdots ,G\}}{\overline{A}}_{i_k}=0,i\notin l$$

(19)

The above constraints are combined and with a compact mathematical notation and can be represented as:

$$\sum _{l\in \{A,\cdots ,G\}}{\delta }_{l_k}{\overline{A}}_{l_k}=I$$

(20)

where, ${\delta }_{l_k}\in \{0,1\}$ is a scalar multiplying factor. We will impose that ${\delta }_{l_k}=0$, if the lth node is found to be corrupted, otherwise ${\delta }_{l_k}=1$. The modified optimization problem is presented as:

$$\begin{array}{cc}\hfill & \underset{A_A,\cdots ,A_G}{min}{J}_k=\frac{1}{2}tr\left(P_k\right)=\frac{1}{2}tr\left(W_k^T{\Sigma }_k{W}_k^T\right)\hfill \\ \hfill & \mathrm{subjected}\mathrm{to}\left(W_k^T{e}_{{\delta }_k}-I\right)=0\hfill \end{array}$$

(21)

where, $e_{{\delta }_k}={\left[{\delta }_{A_k}I,{\delta }_{B_k}I,\cdots ,{\delta }_{G_k}I\right]}^T$. Following the solution approach of the optimization problem with an equality constraint using the Lagrange multiplayer method [44], the augmented cost function is presented as:

$${\overline{J}}_k=\frac{1}{2}tr\left(W_k^T{\Sigma }_k{W}_k\right)+tr\left[{\Lambda }_k\left(W_k^T{e}_{{\delta }_k}-I\right)\right]$$

(22)

where, ${\Lambda }_k\in R^{19\times 19}$ represents the Lagrange multiplayer. Evaluating the necessary conditions of optimality, i.e., $\frac{\partial {\overline{J}}_k}{\partial W_k}=0$ and $\frac{\partial {\overline{J}}_k}{\partial {\Lambda }_k}=0$, yields:

$$\left[\begin{array}{cc}{\Sigma }_k& e_{{\delta }_k}\\ {e_{{\delta }_k}}^T& 0\end{array}\right]\left[\begin{array}{c}{W}_k\\ {\Lambda }_k\end{array}\right] = \left[\begin{array}{c}0\\ I\end{array}\right]$$

(23)

From the solution of Equation (23), the optimal weight matrix $W_k$ is obtained as:

$$W_k={\Sigma }_k^{-1}{e}_{{\delta }_k}{\left({e_{{\delta }_k}}^T{\Sigma }_k^{-1}{e}_{{\delta }_k}\right)}^{-1}$$

(24)

Using the optimal weight matrix $W_k$ into Equation (12), one can obtain the estimated state from the second layer fusion architecture. The revised $W_k$, as a function of $e_{{\delta }_k}$, enables the modified OIF to be resilient in presence of a faulty measurement from a group of sensors.

Fault Detection

In the presented methodology, the corruption or the fault of a sensor is diagnosed by the first layer fusion, while the Kalman filter innovation from Equation (11a) is utilized to detect the fault or locate the sensor failure at the corresponding time frame. The innovation can be written as $I_{D,l}=[y_{v_k}-h_k({\widehat{x}}_k,0)]$. In this way, the innovation vector $I_{D,l}$ depends on the estimated poses from the individual nodes with the pose obtained from a Vicon motion capturing system. In this case the sub-scripted letter D denotes the detected fault, while the sub-scripted letter v indicates the pose obtained from a Vicon. Furthermore, Vicon provides the most accurate pose, which can be considered as a ground truth. Once the innovation for all the nodes is computed, it is compared with a threshold value. The logical rule is given as:

$$\left\{\begin{array}{c}\left|I_{D,l}\right|⩾\Delta :{\delta }_l=0,lth\mathit{node}\mathit{is}\mathit{faulty}\\ 0\le \left|I_{D,l}\right|<\Delta :{\delta }_l=1,lth\mathit{node}\mathit{is}\mathit{legitimate}\end{array}\right.$$

(25)

The fault $f_l$ occurs. $\left|.\right|$ is denoted for absolute value operator and $\Delta$ is the threshold that can be chosen arbitrary.

4. Experimental Framework Evaluation

For the experimental evaluation of the proposed scheme, collected data from a MAV under a manual flight is utilized. The platform and its components have been depicted in Figure 1. In this case, the sensor suite of the MAV consists of the Velodyne Puck LITE based Lidar odometry, the intel real-sense camera T265 for visual odometry, the IMU of a Pixhawk 4 flight controller, and a single UWB node. The detail about the sensor suit is described in Section 2. The MAV is manually flown in an approximate rectangular trajectory. During the experiment, information from the multiple sensors are recorded, which is used to evaluate the efficacy of the proposed FR-OIF framework. Apart form the on-board sensor suit, a Vicon motion capture system is used to provide the most accurate pose of MAV, which is considered as the ground truth in the present context.

The fusion method works in three different stages when sensor outcomes are faulty. In the first step, various nodes generate their equivalent estimated states and associated innovation (as presented in Equation (11a)). The actuation input (Angular velocity ${\omega }_m$ and linear acceleration $a_m$) for all the nodes are obtained from IMU, which is shared among all the nodes. In the second step, innovation terms for various nodes are compared with a constant threshold. The process essentially identifies the defective node. Eventually, in the last effort, the resilient fault isolation mechanism of FR-OIF architecture eliminates the faulty measurements. The corresponding numerical values for the various systems and design parameters that are considered in the present article are: initial guess for error co-variance $P_{l_0}=I_{19\times 19}$, initial state $X_{l_0}={[0_{1\times 6},1,0_{1\times 10},9.81]}^T\forall l\in (A,\cdots ,G)$, process noise co-variance $Q_{l_k}=1000\times I_{19\times 19}$, measurement noise co-variance $R_{l_k}=10\times I_{18\times 18}$, threshold tolerance for innovation $\Delta =0.4$. The experimental results, along with a comparison study based on the centralized EKF approach will be presented in the sequel.

The estimated trajectory of the MAV is presented in Figure 5. From the obtained results, it is evident that the FR-OIF provides a pose estimate that is approximately close to the ground truth trajectory obtained from the Vicon system. The variation of the estimated MAV position along $X,Y,Z$ are presented in Figure 6. It can be observed in Figure 5 that, in the absence of faulty measurement, the estimated trajectory obtained from the Centralized fusion (CF) as well as the decentralized Optimal information fusion (OIF) are approximately equivalent. However, during the operating region, where a faulty measurement is encountered, both the estimated trajectories obtained based on CF and OIF significantly deviated from the ground truth. The variation of the MAV’s orientation in Euler angle representation is shown in Figure 7. The estimated orientation, obtained from the FR-OIF, is approximately close to the Vicon based ground truth. However, during the experiment, when the aerial robot was roaming around the rectangular path of trajectory, it was manoeuvring with small-angle variation along with the roll, pitch and yaw. Hence, the noticeable performance improvement for FR-OIF is prominent in transnational motion compared to that of the rotations.

In order to demonstrate the effectiveness of the proposed fault resilient framework, momentary faults are synthetically introduced into the measured sensor data obtained during the experiment. The evaluation is carried out in multiple scenarios depicting presence of spurious measurement from multiple sensors, as follows:

• Case-1: Temporal fault only in LIO measurement in between (20–30) s, while measurement from all other sensors are unaltered.
• Case-2: Temporal fault only in VIO measurement in between (50–60) s, while measurement from all other sensors are unaltered.
• Case-3: Temporal fault in both LIO and UWB measurements are itroduced in different operating points. The UWB measurement is faulty during (20–30) s, whereas for LIO reports faulty data during (35–45) s. Hence, this case evaluates multiple faults from different sensors in separate operating point.
• Case-4: Simultaneous temporal failure of LIO and UWB appeared during (20–30) s.

Note that the sensor selected for reporting faulty operation and the corresponding time duration are arbitrarily selected without loss of generality. The variation of the estimated positions for all the possible cases under consideration is depicted in Figure 8, Figure 9, Figure 10 and Figure 11. It is evident from the results that the proposed FR-OIF is successfully capable of determining the MAV position in the presence of various possible failure conditions and temporal faulty sensor measurements. More significantly, from Figure 11 one can visualize that the FR-OIF demonstrated its efficacy for a simultaneous failure of multiple sensors at a time, as described in case-4.

The variation of estimated position, obtained from the different nodes $A\cdots G$ are also depicted in Figure 8, Figure 9, Figure 10 and Figure 11, corresponding to the various cases under consideration. Each of the node, individually employing an EKF, indeed eventually removes the measurement noise from the estimated pose. However, the accuracy of the corresponding node depends on the measurement of sensor associated with it. For example, if we consider the situation as described in the Case-1, the faulty measurement is associated with the LIO. Hence, the estimated position from the node B and E, which are using the position information based on LIO, (refer to Table 1) are inaccurate, as shown in Figure 6. Similarly, one can observe here that the estimated orientation, obtained from the nodes D and F, are erroneous during (20–30) s the period of the faulty operation of LIO, since these nodes are using the LIO based orientation as a measurements. The equivalent analysis also holds for the remaining case studies.

4.1. Comparison of FR-OIF with Centralized and Distributed Fusion Approaches

In order to demonstrate the efficiency of the proposed FR-OIF, comparative study with the EKF based centralized multi-sensor fusion as well as decentralized optimal information fusion is presented here. For the sake of completeness, a brief description of the EKF based centralized fusion architecture is described in the sequel. The centralized EKF follows exactly the same steps of the prediction and correction approach, as described in Equations (Section 3.1)–(Section 3.1), with the only exception that the measured pose from the multiple sensors are collectively taken into account. Hence, the output equation is described as:

$$y_k\in {\mathbb{R}}^{21\times 1}={\left[p_{{}_{LIO}},q_{{}_{LIO}},p_{{}_{VIO}},q_{{}_{VIO}},p_{{}_{UWB}},q_{{}_{IMU}}\right]}^T$$

(26)

The dimension of the measurement noise co-variance matrix $R_k$, Kalman gain $K_k$ and output gradient $H_k$ are redefined accordingly.

Apart from the EKF based centralized approach, a comparison study has been carried out with the existing decentralized OIF [31] method. The formulation presented in [31] considers a weighted sum of the individual node in the second layer fusion, as described in Equation (18). Both the CF and OIF are evaluated for all the scenarios presented in the Case 1–4 with faulty sensor measurements from multiple sensors. The variations of the estimated pose for CF, OIF and FR-OIF are presented in Figure 5, Figure 6, Figure 7 and Figure 8. The comparison study reveals that in the absence of sporadic measurements, the performance of all the methods (CF, OIF and FR-OIF) under consideration are approximately equivalent. However, in the presence of faulty measurements (Case 1–4), it can be observed that the estimated trajectory, obtained from the CF and OIF approach, deviates from the ground truth. Moreover, the presented experimental study brings out another interesting fact where the performance of the OIF closely resembles with the CF. This is highlighted in Figure 8, which is an emphasized version of Figure 6 for a time duration of operation under the faulty measurements (and the fact is also evident in other figures as well). In contrast, FR-OIF is capable of providing an accurate pose estimation even in the presence of fault from multiple sensors.

4.2. Accuracy in Terms of the Root Mean Square Error

In this section, the accuracy of the fusion algorithms are evaluated in terms of Root Mean Square Error (RMSE). In this case, a comparison is made in two steps. Firstly, the estimated poses are compared using a sliding window RMSE where the corresponding plots are depicted in Figure 12 and Figure 13 in a logarithmic scale. Secondly, to compare the performance of the estimated pose, obtained based on various nodes and fusion approaches, the single value RMSE is computed and illustrated in

Table 2. RMSE comparison for the estimation of the positions in meters.

Axis	A	B	C	D	E	F	G	CF	OIF	FR-OIF
X	0.1114	0.5740	0.2509	0.2509	0.5740	0.1114	0.2509	0.2503	0.2340	0.1394
Y	0.0806	2.1856	0.0793	0.0793	2.1856	0.0806	0.0793	0.7564	0.6452	0.0650
Z	0.0502	0.4616	0.1122	0.1122	0.4616	0.0502	0.1122	0.1751	0.1557	0.0499

and

Table 3. RMSE comparison for the estimation of the orientation in Euler angles.

Angles	A	B	C	D	E	F	G	CF	OIF	FR-OIF
$\psi$	0.4667	0.7522	0.4667	0.7473	0.4667	0.7473	0.0168	0.5343	0.5154	0.5340
$\theta$	2.0663	2.1664	2.0663	1.9324	2.0663	1.9324	0.0188	1.9618	1.9618	1.9505
$\varphi$	1.9387	1.9246	1.9387	1.9357	1.9387	1.9357	0.0066	1.8365	1.8456	1.8341

.

Note that the RMSE errors are computed by considering the Vicon based ground truth as reference. The RMSE comparison table proves that the second layered FR-OIF method is superior when the sensor measurements are erroneous. In order to visualize the variation of RMSE error along the trajectory, a sliding-window logarithmic RMSE (RMSLE) is considered with a window size of 100 samples. Since, the RMSLE provides the error in a logarithmic scale, the smaller the magnitude is (more negative) the more signifies for a higher accuracy. From the variation of RMSLE, as presented in Figure 12 and Figure 13. Even though the root mean square error for orientations has slightly varied with the efficiency of the estimators.

Hence, transnational motion during the experiment was made significant changes in the RMSE Table 2, as well as Figure 12, however it is not significantly visible in Table 3 and Figure 13.

The proposed FR-OIF provides excellent performance in terms of RMSE, compared to the centralized fusion approach. Moreover, based on the experimental results it can concluded that the proposed multi-sensor fusion is capable to provide a resilient pose estimation in the presence of faulty measurements and it has a great potential for various practical applications, involving multiple sensors and with a sufficient redundancy.

In the present context, the evaluation of the proposed fault resilient fusion is carried out with the experimental data, where temporal sensor fault are synthetically injected for the purpose of validation. However, part of the future work will consider evaluating the proposed FR-OIF sensor fusion framework in a field robotic experiment, where a momentary sensor failure is unavoidable in presence of dusty/smokey and dark environment. Additionally it is to be noted that, In the presented approach, and as in the case of most of the fault detection approaches in the related literature, the $\Delta$ has been ad hoc selected to a constant number and without loosing generality, while part of future work is also related to the adaptive determination of this value.

5. Conclusions

In this article, a novel decentralized multi-sensor fusion framework for resilient pose estimation of MAV is presented. The proposed multi-sensor fusion considered a two layered architecture. In the first layer, by combining the information from different sensors and by using an EKF a set of nodes are constructed. Each node provides an estimate of the MAV pose, which are collectively integrated by using OIF to provide an optimal estimate of it. Moreover, an unique fault isolation is embedded with the classical OIF formulation to incorporate the resiliency in presence of faulty measurements. Based on the experimental study an interesting fact has been established that, without an external fault isolation mechanism, the performance of the classical OIF closely resembles with the centralized EKF based multi-sensor fusion approach. Hence, these two methods are not sufficient to eliminate the fault accurately. In contrast, the proposed fault resilient optimal isolation technique is adequately capable to overcome such shortcomings. Even though the proposed FR-OIF is presented in this article is considered pose estimation of MAV, the formulation is quite generic and it can be applied in various autonomous navigation and with different robotic platforms and involving multiple sensors.