IMU Networks for Trajectory Reconstruction in Logistics Applications

Abstract

This paper discusses the use of networks of Inertial Measurement Units (IMUs) for the reconstruction of trajectories from sensor data. Logistics is a natural application domain to verify the quality of the handling of goods. This is a mass application and the economics of logistics impose that the IMUs to be used must be low-cost and use basic computational devices. The approach in this paper converts a strategy from the literature, used in the multi-target following problem, to reach a consensus in a network of IMUs. This paper presents results on how to achieve the consensus in trajectory reconstruction, along with covariance intersection data fusion of the information obtained by all the nodes in the network.

1. Introduction

Logistics is a booming service industry, including the flow of goods between origin and consumption points (see, for instance, [1]). With the diversity of goods being transported between any two points around the world, one of the quality indicators used by logistics operators is the way goods are manipulated during their transportation. Poor handling may result in damages and/or liability costs to insurance and/or logistics operators. Being able to analyze the movement of the goods upon delivery, i.e., having a traceability property, is recognized as strategic [2].

In engineering terms, this amounts to analyzing the trajectories the goods are subject to during transportation, i.e., the trajectories resulting from the way they are manipulated during transportation.

The trajectory reconstruction problem is well known in, for example, the aerospace industry. Using information from an IMU attached to the body of the spacecraft, its trajectory can be reconstructed using the velocity and acceleration data and assuming a good model of the spacecraft. Essentially, this amounts to an optimization problem, where the reconstructed trajectory must be such that the generated data is close to the observed data. The literature on this problem is vast, see, for instance, [3] on using multiple models and Unscented Kalman Filtering (UKF), [4] on using Extended Kalman Filtering (EKF) techniques and inertial measurements data, [5] on using image data, and [6] on using Gaussian regression techniques and GPS data.

The application to this logistics problem introduces additional constraints; namely, the sensing must be really cheap (i.e., the quality of the accelerometers and rate-gyros is nowhere close to that used in the aerospace industry), and the rigidity of the goods is only loosely verified. For example, goods such as food and domestic appliances are often transported in cardboard boxes and, hence, rigidity is valid under the assumption that the handling forces are relatively small or highly localized to avoid damages. Moreover, the placement of the sensors will, in general, be made by unskilled/carefree operators, which may lead to devices malfunctioning and, hence, redundancy, as with using multiple (similar) devices, is likely to increase the amount of valid information for trajectory reconstruction.

The vision of this paper is to implement this redundancy in that of a network of low-cost IMUs that can be glued and forgotten in typical packages (made out of cardboard, wood, plastic, metal, etc.) preserving, as much as possible, the rigidity property. These IMUs can either be placed: (i) forming a regular, pre-defined pattern, with the packages specifying precise areas of where to “glue” the devices, or (ii) distributed through the package arbitrarily. Placing the devices must not require any special expertise. The first scenario is possible in packages of standardized dimensions and limits the need for calibration procedures. In the second scenario, some calibration procedures may be necessary prior to the start of the transportation. For the sake of simplicity, this paper assumes that the whole IMUs in the network are placed at the same point. This does not imply any lack of generality; in a real scenario, with all the IMUs placed in different (and rigid) locations, the reconstructed trajectories would differ among them by constant homogeneous transformations.

Having multiple IMUs gathering data and combining it to reconstruct the trajectory may, in principle, improve the robustness of the solution due to the following reasons: (i) the amount of data increases, possibly containing richer information, and (ii) redundancy helps deal with sensor fails. Combining a posteriori the solutions found, from the data collected from each single IMU, e.g., using covariance intersection or some other averaging method, is a possibility and does not require the exchange of information among the nodes.

The alternative, followed in this paper, is to have a network of nodes, with each including: (i) sensory information from a local IMU, and (ii) communication with other distributed nodes (of a similar architecture) to exchange information and combine it locally to improve the reconstruction (this paper refers to a network of IMUs as these devices are the key component of the nodes). The main research question addressed is then whether or not using networks of IMUs improves trajectory reconstruction quality over a single IMU. A secondary research question is related to the framework defining the admissible protocols to exchange information among the IMU network.

The novel approach presented here follows from a multi-target tracking problem first presented in [7], re-interpreted as a consensus generation in a network of IMUs. The motivation to use this formulation comes from the proven optimality of the framework in [7] in the context of the multi-tracking problem.

This paper is organized as follows. Section 2 illustrates a single, simulated IMU, establishing a baseline performance for comparison with the consensus approach. Section 3 presents a quick view of the work in [7], which serves as a basis for this work, focusing on the consensus features and clarifying the re-interpretation of the key terms. Moreover, the feasibility problem, i.e., the existence of a strategy for the exchange of information, such that all the nodes in the network can provide estimates of the reconstruction, is discussed using the Linear Matrix Inequalities (LMI) framework. Section 4 illustrates the evolution of the performance indicators of the simulated nodes recovered from the network framework when the communications among them are inhibited. Section 5 presents simulation results with simple networks with two and three nodes. Section 6 discusses the improvement of the network consensus over the reconstruction by the independent nodes. Section 7 presents the results of the fusion of the trajectories reconstructed by each node using covariance intersection. Finally, Section 8 concludes this paper with a discussion on the feasibility of the approach and points to future research.

2. Baseline IMU

Given an arbitrary trajectory that serves as the ground truth, the corresponding linear and angular accelerations can be easily obtained if the inertia properties of the body (assumed rigid) moving over the trajectory are known. Thus, to accurately simulate the sensors to be glued to the logistics packages, knowledge of the inertia properties of the package is required beforehand.

In practical terms, full knowledge about the package inertia will seldom be available (though the mass and the physical dimensions can be easily known a priori as they are the parameters that directly influence the transportation cost). Therefore, while the observations reflect the real properties of the packages, the reconstruction of the trajectories has only a limited amount of that knowledge available. However, it should be emphasized that the estimation of the mass/inertia properties can also be completed using IMU data. This is a well-known problem in the aerospace industry, see, for instance, Ref. [8] on using recursive least squares, Ref. [9] on using continuous system identification techniques to identify the dynamics of the Hubble telescope, Ref. [10] on using the LMI framework and formulating a least squares problem, and Refs. [11,12] on using the least squares formulation combined with S-estimators for the identification of spacecraft parameters.

The basic approach to estimating the trajectory from the velocities and accelerations is to use simple integrator models. Given a trajectory expressed in terms of the position and orientation coordinates, $x, y, z, \alpha , \beta , \gamma$, the corresponding signals from the accelerometers and rate-gyros can be obtained from basic expressions describing the geometry of the trajectory:

$$\begin{array}{c}\ddot{x}=a_x\hfill \\ \ddot{y}=a_y\hfill \\ \ddot{z}=a_z\hfill \end{array}$$

(1)

for the translation and

$$\dot{R}=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$

(2)

for the rotation, with R, the rotation matrix, expressing the orientation at each point in terms of Euler ZYX angles, which can be written as follows:

$$\left[\begin{array}{c}\dot{\alpha }\\ \dot{\beta }\\ \dot{\gamma }\end{array}\right]={\left[\begin{array}{ccc}0& -s_{\alpha }& c_{\alpha }{c}_{\beta }\\ s_{\alpha }{c}_{\beta }(c_{\alpha }-c_{\alpha })& c_{\alpha }& s_{\alpha }{c}_{\beta }\\ 1& 0& -s_{\beta }\end{array}\right]}^{-1}\left[\begin{array}{c}{\omega }_x\hfill \\ {\omega }_y\hfill \\ {\omega }_z\hfill \end{array}\right]$$

(3)

where $a_x, a_y, a_z, {\omega }_x, {\omega }_y, {\omega }_z$ are the corresponding sensor outputs, i.e., the linear accelerations and angular velocities, respectively. Also, $s_{\theta }, c_{\theta }$ stand for $sin\left(\theta \right)$ and $cos\left(\theta \right)$, respectively. The purpose of these expressions is to yield a simulation tool and, without the lack of generality, they implicitly assume a unit mass body. To improve realism, noise can be added to the outputs of these sensors.

2.1. Dead Reckoning Experiments

The purpose of these experiments is as follows: (i) to show the effects of noise when no compensation strategy is used, and (ii) to serve as the baseline to compare with the network experiments. Using sensor data obtained from (1) and (3), corresponding to some reference trajectory, and a dead reckoning model, the goal is to reconstruct the reference trajectory (this amounts to using an inverse model, as shown in Figure 1).

Sensor uncertainties make the reconstructed trajectory deviate from the real one. Figure 2 illustrates the effects of such situations for two levels of position noise.

From the perspective of a logistics application, the dead reckoning approach above may still be a valid option if the disturbances are limited and only specific events are relevant to measure the quality of the handling, e.g., if the existence of strong non-smooth/disturbances areas is the quality indicator. In such cases, even if the reconstructed trajectory in the Cartesian 3D space differs significantly from the real one, the presence of the areas of strong disturbances can still be recognized.

The existence of drifts in the observed variables is another important disturbance factor. The poor assembly of a sensor and electronics issues are common causes of drifts. In the accelerometer, drift means that the dynamics of the acceleration are constantly integrating some value that is not being observed by the accelerometer. In the case of the rate-gyro, this means that the dynamics of the rotational velocity are constantly integrating some value that is not being directly observed by the gyro.

Figure 3 illustrates two experiments of the dead reckoning approach under constant drift conditions. The disturbances considered only affect the linear acceleration. In the first sample, they are given as follows:

$$a_{offse{t}_k}=a_{offse{t}_{k-1}}+[0.05\ast \mathrm{randn},0,0]$$

where “randn” stands for the usual function returning a random value with an $N(0,1)$ distribution. In the second sample, all the coordinates are similarly affected by $0.05\ast \mathrm{randn}$.

This is an example where the estimated trajectory is too different from the original one. However, depending on the specific requirements of the logistics application, the drift disturbances may not be an issue, e.g., as mentioned above if the purpose is the detection of harsh handling. Clearly, drifts are highly relevant if accurate trajectory reconstruction is required, which is the focus of the remainder of this paper.

2.2. Single IMU

In general, an IMU will include a filtering stage, often formed by an Extended Kalman Filter. The process and observation models can be easily obtained from (1) and (3). Figure 4 shows a sample of the reconstruction for a biased spiral reference trajectory, with the position and orientation $N(0,0.1)$ process noises and $N(0,0.01)$ and $N(0,0.001)$ observation noises for the position and orientation, respectively.

From the results in Figure 4, a single IMU can already provide quality information about the real trajectory, even in the presence of mild disturbances. Similar to the dead reckoning option, the basic IMU in this example can already be useful in the logistics domain. However, the research goal for this paper is aimed at expanding the performance of the single IMU.

3. Background

This paper adopts the formulation from [7], developed in the context of a multi-target tracking problem, with the targets and measurements assumed to have a one-to-one relation. The notation follows the usual form of the continuous-discrete (CD) extended Kalman framework (see, for instance, [13] for a critical comment and the implementation issues of this form), with the prediction stage for the ith IMU node given by the following (see also [14] for a face-off between five different formulations):

$$\begin{array}{c}{x}_{k|k-1}^i=f_{k-1}^i\left(x_{k-1|k-1}^i\right)\hfill \\ z_k^i=h_k^i\left(x_{k|k-1}^i\right)\hfill \\ P_{k|k-1}^i=F_{k-1}^i{P}_{k-1|k-1}^i{\left(F_{k-1}^i\right)}^T+Q_{k-1}^i,\hfill \end{array}$$

(4)

where $x^i$ stands for the state variables, $z^i$ the observations, $P^i$ the covariance matrix, $Q^i$ the observation noise covariance, F the Jacobian of f, and the modified update stage is as follows:

$$\begin{array}{c}{S}_k^i=H_k^i{P}_{k|k-1}^i{\left(H_k^i\right)}^T+R_k^i\end{array}$$

(5)

$$\begin{array}{c}{K}_k^i=P_{k|k-1}^i{\left(H_k^i\right)}^T{\left(S_k^i\right)}^{-1}\end{array}$$

(6)

$$\begin{array}{c}{x}_{k|k}^i=x_{k|k-1}^i+K_k^i{\tilde{z}}_k^i\end{array}$$

(7)

$$\begin{array}{c}{P}_{k|k}^i=P_{k|k-1}^i-K_k^i\left(1-{\beta }_0^i\right)S_k^i{\left(K_k^i\right)}^T+K_k^i{\overline{P}}_k^i{\left(K_k^i\right)}^T\end{array}$$

(8)

where H is the Jacobian of h, $R^i$ is the process noise covariance matrix, and the networking and consensus are expressed through the measurement uncertainties:

$${\overline{P}}_k^i=\sum _{j=1}^{M_k}{\beta }_j^i{\tilde{z}}_{j,k}^i{\left({\tilde{z}}_{j,k}^i\right)}^T-{\tilde{z}}_k^i{\left({\tilde{z}}_k^i\right)}^T,$$

(9)

using the $M_k$ measurements, ${\tilde{z}}_{j,k}^i$ (received by the ith IMU from the jth IMU, i.e., the ith IMU is connected to $M_k$, which are the other IMUs), and with the weighted innovations, is as follows:

$$\begin{array}{c}{\tilde{z}}_k^i=\sum _{j=1}^{M_k}{\beta }_j^i\left(z_{j,k}^i-{\widehat{z}}_k^i\right)\hfill \\ {\tilde{z}}_{j,k}^i=z_k^i-{\widehat{z}}_{j,k}^i,\hfill \end{array}$$

(10)

where ${\beta }_j^i$ is the weight expressing the relevance of the measurement from node j to the estimate of the measurement by node i, ${\beta }_0^i$ is the weight expressing the relevance of the poor measurements (${\beta }_0^i\to 1$ amounts to reducing the norm of $P_{k|k}^i$, forcing the covariance to decrease even if poor measurements are being fed into the system), ${\widehat{z}}_k^i$ is the estimate of the measurement of the ith node at instant k, that is, ${\widehat{z}}_k^i=h_k^i\left(x_{k|k-1}^i\right)$, and $z_{j,k}^i$ is the actual measurement of node j at instant k reaching the ith IMU.

The topology of the network is assumed fixed. In [7], the ${\beta }_j^i$ parameters were interpreted in terms of a marginal probability of association between node j and target i. The ${\beta }_0^i$ is used to quantify the misdetection of a target (1/0 for misdetected/detected). However, for the purpose of this paper, this interpretation is not useful as the association between the nodes and measurements can be known a priori. Instead, these parameters can be seen as follows: (i) the communications protocol (as in consensus problems), and (ii) scaling weights for the measurements. Also, as in the original problem, the number of measurements, $M_k$, can vary along time (or measurement scans) k.

The formulation from [7] is, essentially, an EKF with a modified update of the covariance matrix (through the ${\beta }_0$ factor that scales the innovation residual covariance in (5), and a weighted combination of the innovations from the whole network weighted by the ${\beta }_j^i$. Also, (8) dumps the term explicit in the noise $K_k^i{R}_k^i{\left(K_k^i\right)}^T$, and the symmetric term ${\left(K_k^i{H}_k^i\right)}^T+K_k^i{H}_k^i$ affecting $P_{k|k-1}^i$ in the CD formulation of the EKF.

This innovates relative to the CD-EFK procedure in that the covariance of each unit now has feedback from the innovations produced by the network. Moreover, the tuning “knob” $R_k^i$ in the CD-EKF formulation is now replaced with the ${\beta }_j^i$ and ${\beta }_0^i$ parameters. These can also be seen as the mediator/protocol/scaling factors between the nodes.

For the sake of readability, the expression for the covariance update in the CD-EKF formulation, and a re-writing of (8) to highlight the differences to the CD-EKF covariance update equation, is presented below.

$$\begin{array}{c}{P}_{k|k}^i=\left(I-K_k^i{H}_k^i\right)P_{k|k-1}^i{\left(I-K_k^i{H}_k^i\right)}^T+K_k^i{R}_k^i{\left(K_k^i\right)}^T\\ =P_{k|k-1}^i+K_k^i{S}_k^i{\left(K_k^i\right)}^T-K_k^i{H}_k^i{P}_{k|k-1}^i-P_{k|k-1}^i{\left(K_k^i{H}_k^i\right)}^T\end{array}$$

(11)

The renewed one, where ${\tilde{z}}_{j,k}$ and ${\tilde{z}}_{j,k}^i$ are $N\times M_k$ matrices is as follows:

$$\begin{array}{c}{P}_{k|k}^i=P_{k|k-1}^i-K_k^i{S}_k^i{\left(K_k^i\right)}^T+\\ +K_k^i\left(\sum _{j=1}^{M_k}{\beta }_j^i{\tilde{z}}_{j,k}^i{\left({\tilde{z}}_{j,k}^i\right)}^T\right){\left(K_k^i\right)}^T-K_k^i{\tilde{z}}_k^i{\left({\tilde{z}}_k^i\right)}^T{\left(K_k^i\right)}^T+{\beta }_0^i{K}_k^i{S}_k^i{\left(K_k^i\right)}^T\end{array}$$

(12)

The first two terms on the righthand side of (12) are also present in (11). The third and fourth terms account for the network exchanges and are the additional “tuning knob”. The fifth term dampens the second term and provides additional control.

The convergence of (12) can be achieved by the careful selection of ${\beta }_0^i$ and ${\beta }_j^i$, namely ensuring that $P_{k|k}^i$ is a contracting sequence converging to some fixed point (see [15]). This means:

$$\exists k_0>0,\alpha >0,\gamma >0:\forall k>k_0,∥P_{k|k}^i-P_{k|k-1}^i∥\le \alpha e^{-\gamma (k-k_0)},$$

or, alternatively,

$$\begin{array}{c}\hfill \exists k_0>0,\alpha >0,\gamma >0:\forall k>k_0,\\ \hfill ∥-(1-{\beta }_0^i)K_k^i{S}_k^i{\left(K_k^i\right)}^T+\sum _{j=1}^{M_k}{\beta }_j^i\left(K_k^i{\tilde{z}}_{j,k}^i{\left({\tilde{z}}_{j,k}^i\right)}^T{\left(K_k^i\right)}^T\right)-K_k^i{\tilde{z}}_k^i{\left({\tilde{z}}_k^i\right)}^T{\left(K_k^i\right)}^T∥\le \alpha e^{-\gamma (k-k_0)},\end{array}$$

(13)

or, in a more compact form to highlight the terms involved in the consensus, with $A_k^i=(1-{\beta }_0^i)K_k^i{S}_k^i{\left(K_k^i\right)}^T$, $C_k^i=K_k^i{\tilde{z}}_k^i{\left({\tilde{z}}_k^i\right)}^T{\left(K_k^i\right)}^T$, and $F_{j,k}^i=K_k^i{\tilde{z}}_{j,k}^i{\left({\tilde{z}}_{j,k}^i\right)}^T{\left(K_k^i\right)}^T$,

$$\begin{array}{c}\hfill \exists k_0>0,\alpha >0,\gamma >0:\forall k>k_0,\\ \hfill ∥-A_k^i-C_k^i+\sum _{j=1}^{M_k}{\beta }_j^i{F}_{j,k}^i∥<\alpha e^{-\gamma (k-k_0)}.\end{array}$$

(14)

At each time step, a new solution must be produced so that the convergence of the covariance matrix can be controlled. This means: (i) the consensus will be adjusting over time, as the ${\beta }_j^i$ will (in general) be changing, and (ii) the tradeoff values for $\alpha ,\gamma$ (that can be constant over time) have to be selected. However, one must ensure that the $\alpha ,\gamma$ can be made unique during the whole duration of the trajectory, despite the changes in the ${\beta }_j^i$.

This makes:

$$Z_k^i=\frac{1}{\alpha }{e}^{\gamma (k-k_0)}\left(-A_k^i-C_k^i+\sum _{j=1}^{M_k}{\beta }_j^i{F}_{j,k}^i\right),$$

(15)

the solution of

$$∥Z_k^i∥<1,$$

(16)

which can be obtained using the LMI form (see, for instance, [16], pp. 7–8, on computing maximum singular values) as

$$\left[\begin{array}{cc}I& Z_k^i\\ {\left(Z_k^i\right)}^T& I\end{array}\right]>0.$$

(17)

The complete solution for the convergence of (12) requires an additional LMI that imposes the positive definiteness of $P_{k|k}^i$ given that $P_{k|k-1}^i$ is assumed positive definite is as follows:

$$P_{k|k-1}^i-A_k^i-C_k^i+\sum _{j=1}^{M_k}{\beta }_j^i{F}_{j,k}^i\succ 0,$$

(18)

or, more compactly, with $D_k^i=P_{k|k-1}^i-A_k^i-C_k^i$,

$$D_k^i+\sum _{j=1}^{M_k}{\beta }_j^i{F}_{j,k}^i\succ 0.$$

(19)

Therefore, if (17) and (19), with the ${\beta }^i$ variables, are both feasible at each time step, k, for some choice of ${\beta }_j^i$, then there is a consensus solution to the network from the point of view of the ith node, which ensures the convergence of $P_{k|k}^i$. A solution for the full network requires the feasibility of the above LMI problems for all the nodes.

The LMI feasibility can be tested by multiple algorithms from general ones, Refs. [17,18] on ellipsoid algorithms, to specific ones, [19] for large and sparse LMIs or Ref. [20] on stochastic algorithms. Several software packages to solve LMIs are available, i.e., the Scilab-based LMITOOL [21], the Matlab-based YALMIP [22], and the Matlab LMI toolbox.

These LMI problems are non-convex (the presence of the affine term in (19) is enough to make it non-convex) and, hence, finding optimal parameters using standard LMI solvers is, in general, an issue. Relaxation techniques are commonly used to solve non-convex problems and obtain either good solutions or pseudo-solutions from which the approximations to the good ones can be derived (see, for instance, [23,24,25]). The approach proposed here uses a basic form of relaxation, consisting in scaling some terms until a solution can be found.

4. Simulation Experiments

Making ${\beta }_0^i=0$ and ${\beta }_j^i=1,\mathrm{if}i=j,\mathrm{and}0,\mathrm{otherwise}$, then ${\tilde{z}}_k^i={\tilde{z}}_{j,k}^i$ for $j=i$, recovering a set of isolated nodes:

$$\begin{array}{c}{E}_k^i\equiv P_{k|k-1}^i-K_k^i{S}_k^i{\left(K_k^i\right)}^T>0\end{array}$$

(20)

and the convergence condition becomes:

$$\begin{array}{c}∥\frac{1}{\alpha }{e}^{\gamma (k-k_0)}\left(-K_k^i{S}_k^i{\left(K_k^i\right)}^T\right)∥<1.\end{array}$$

(21)

Given that all the matrices in (20) are norm-bounded, then the second condition can always be verified for a large enough $\alpha$ and a small enough $\gamma$.

The noise covariance $R_k^i$ bounds the definiteness of (20) (in the best possible scenario $R_k^i=0$; in general, this is not a free parameter as it must be chosen in connection with the sensors, though it can also act as a tuning knob to some extent). The matrix $S_k^i$ is, by construction, always positive definite and, hence, the quadratic term $K_k^i{S}_k^i{\left(K_k^i\right)}^T$ is also positive definite. By introducing a scale factor ${\xi }_k^i\in [-1,1]$ in $K_k^i$, the LMI (21) dominates that in (20), i.e., the set of solutions $P_{k|k-1}^i$ of (21) contains those of (20) (see [26] for detailed analysis and the results related to LMI dominance).

Given the following scaled expression:

$$P_{k|k-1}^i-{\left({\xi }_k^i\right)}^2{K}_k^i{S}_k^i{\left(K_k^i\right)}^T>0,$$

(22)

as ${\xi }_k^i\to \pm 1$, the solution $P_{k|k-1}^i$ converges to that of the unscaled expression. Also, ${\xi }_k^i\to 0$, $P_{k|k}^i\to P_{k|k-1}^i$, which is construction positive definite.

Given the low complexity of the scaled expression, its feasibility relative to the ${\xi }_k^i$ parameter can be obtained from the eigenvalues/eigenvectors of the matrix:

$$G_k^i\equiv P_{k|k-1}^i{\left({\xi }_k^i\right)}^{-2}{\left(K_k^i{S}_k^i{\left(K_k^i\right)}^T\right)}^{-1}.$$

(23)

The positiveness of all the eigenvalues means $G_k^i>0$. As the ${\xi }_k^i$ varies, the eigenvalues of the relaxed matrix are linearly related to those of $G_k^i$.

Figure 5 shows an example of the evolution of the eigenvalues using ${\xi }_k^i=1$.

As expected, the quality of this solution, using the EKF, is clearly superior to that in the example of dead reckoning.

Whenever the nodes are not independent and the network must reach a consensus, the feasibility must be checked directly using (17) and (19).

5. Network Consensus Experiments

Figure 6 shows the 3D space trajectories for the two nodes operating independently and as a network. All the IMUs are assumed to be time-synchronized, i.e., the simulation loop time is the same for all of them. For the independent nodes test, ${\beta }_0^i=0, i=1,2$, ${\beta }_1^1=1, {\beta }_2^1=0, {\beta }_1^2=0, {\beta }_2^2=1$. For the networked nodes test, ${\beta }_0^i=0.01, i=1,2$, ${\beta }_1^1=0.6, {\beta }_2^1=0.4, {\beta }_1^2=0.4, {\beta }_2^2=0.6$. Any of the IMU trajectories can be used for reconstruction purposes.

Figure 7 shows the evolution of the two-norm of the $xyz$ trajectory and of the respective mean. Clearly, the mean error when the nodes operate cooperatively is lower than when operating independently.

Figure 8 and Figure 9 show a similar experiment with three nodes. For the independent nodes test, ${\beta }_0^i=0, i=1,2,3$, ${\beta }_1^1=1, {\beta }_1^2=0, {\beta }_2^1=0, {\beta }_2^2=1, {\beta }_2^3=0, {\beta }_3^1=0, {\beta }_3^2=0, {\beta }_3^3=1$. For the networked nodes test, ${\beta }_0^i=0.01, i=1,2,3$, ${\beta }_1^1=0.7, {\beta }_2^1=0.01, {\beta }_3^1=0.01$, ${\beta }_1^2=0.01, {\beta }_2^2=0.7, {\beta }_3^2=0.01, {\beta }_1^3=0.01, {\beta }_2^3=0.01, {\beta }_3^3=0.8$. The protocol for the network experiments, i.e., the set of ${\beta }^i$ gains, was chosen to have each IMU privileging its own estimates but still accounting for the other nodes.

For the three-node network, the mean error of the $xyz$ trajectory of the networked nodes is also lower than the error for the independent nodes. However, the increase in the number of IMUs does not immediately result in an increase in performance, i.e., a lower mean error norm.

The evolution of the feasibility conditions, i.e., (17) and (19), is shown in Figure 10.

Alternative protocol values, e.g., increasing the contributions of the surrounding nodes with ${\beta }_0^i=0.01, i=1,2,3$, ${\beta }_1^1=0.5, {\beta }_2^1=0.2, {\beta }_3^1=0.1, {\beta }_1^2=0.2, {\beta }_2^2=0.5, {\beta }_3^2=0.2$, ${\beta }_1^3=0.1, {\beta }_2^3=0.2, {\beta }_3^3=0.5$, continues to yield a lower error for the consensus version (see Figure 11). These values were defined empirically, and they ensure LMI feasibility. Given that the trajectory reconstruction will, in general, be made offline, finding alternative admissible protocols is not a problem. The alternative workflow to finding a solution to the LMI feasibility problem is to use an empirical approach, starting with independent nodes and gradually increasing the contribution of each node to the estimates of the neighbors, checking the LMI feasibility at each step.

Figure 9b and Figure 11b illustrate the dependence of the mean error norm from the ${\beta }^i$ values. Moreover, it provides the ground for an optimization problem on the space of the feasible ${\beta }^i$. It is worth noting that increasing the exchange of information, i.e., increasing the ${\beta }^i$, had the effect of improving the performance.

From a practical perspective, if, during an online reconstruction, the LMI problem is unfeasible, then simply use one of the independent units as a solution.

6. Networking for Improvement over Independent Nodes

As seen before, the feasibility of a network can be tied to an LMI. Also, the experiments in the previous section empirically show that networks have an advantage over independent nodes. The purpose of this section is to demonstrate this advantage, i.e., the difference between the tracking errors obtained in the networked and independent versions:

$$\begin{array}{c}{e}_k^i=∥e_k^{i^{net}}∥-∥e_k^{i^{ind}}∥=∥r-x_{k|k}^{i^{net}}∥-∥r-x_{k|k}^{i^{ind}}∥\hfill \\ \hfill \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}=∥r-x_{k|k-1}^{i^{net}}+K_k^{i^{net}}{\tilde{z}}_k^{i^{net}}∥-∥r-x_{k|k-1}^{i^{ind}}+K_k^{i^{ind}}{\tilde{z}}_k^{i^{ind}}∥,\end{array}$$

(24)

where r is the reference trajectory, $x_{k|k}^{i^{net}}$ is the trajectory reconstructed by a network, and $x_{k|k}^{i^{ind}}$ is a trajectory reconstructed by one node operating independently, verifying the averaging relation:

$$\exists N:n>N, \frac{1}{n}\sum _{k=1}^n{e}_k^i<0.$$

(25)

For a discrete, time-varying, linear system, Lemma 3.1 in [27] states that $P_{k|k}^{i^{ind}}$ is bounded above and below, i.e., $\exists {\mu }_l,{\mu }_u>0:{\mu }_{l}I<P_{k|k}^{i^{ind}}<{\mu }_{u}I$. $S_k^{i^{ind}}$ is a linear operator on $P^{i^{ind}}$ and, hence, it is also bounded above and below. $K_k^{i^{ind}}$ is also bounded as it depends on the bounded $P^{i^{ind}}$ and on ${\left(S_k^{i^{ind}}\right)}^{-1}$, which is also upper and lower bounded. Therefore, $x_k^{i^{ind}}$ is bounded and, also, the whole rightmost term in (24) can be written as follows:

$$L_i\mathbb{I}<r-x_{k|k-1}^{i^{ind}}+K_k^{i^{ind}}{\tilde{z}}_k^{i^{ind}}<L_s\mathbb{I},$$

for some $L_i,L_s>0$, and $\mathbb{I}$ is a suitable identity matrix.

Applying a two-norm on both sides of an inequality preserves the inequality, as this norm is monotone [28], and the righthand term can thus be assumed to be a bounded function:

$$L_i<∥r-x_{k|k-1}^{i^{ind}}+K_k^{i^{ind}}{\tilde{z}}_k^{i^{ind}}∥<L_s.$$

(26)

The leftmost term in (24) is a function of the ${\beta }_j^i$ parameters. Without losing generality, one can represent this term as follows:

$$∥r-x_{k|k-1}^{i^{net}}+K_k^{i^{net}}{\tilde{z}}_k^{i^{net}}∥\equiv ∥r-g\left(\beta \right)+\beta f\left(\beta \right)∥,$$

(27)

with $f(·),g(·)$ as the adequate functions. Therefore, (24) can be written as follows:

$$∥r-g\left(\beta \right)+\beta f\left(\beta \right)∥-L_s<e_k^i<∥r-g\left(\beta \right)+\beta f\left(\beta \right)∥-L_i.$$

(28)

Under the LMI feasibility conditions, $f(·)$ and $g\left(\beta \right)$ are bounded and, hence, by selecting small enough $\beta$ constants, the rightand side of (28) approaches:

$$∥r-g\left(\beta \right)∥-L_i.$$

(29)

The feasibility of the LMI problem ensures the asymptotic convergence of $g\left(\beta \right)\equiv x_{k|k-1}^i\to x_{k|k}^i$ and, hence, it is clear that, if the networked version converges to the reference trajectory r, the $e_k^i$ are negative.

The consensus approach can also be beneficial in the case of structured unmodeled disturbances, as in the case of unmodeled drift in the sensors. From Figure 12, the consensus approach retains the good property of low mean error when compared with the non-consensus approach.

Figure 13 illustrates the performance in the case of a piecewise linear (two trunks) reference trajectory. As before, the mean error property is verified.

7. Network Fusion Using Covariance Intersection

The framework above leads to each IMU producing its own estimate, each of which is better than the independent estimates. A single estimate can be obtained by applying the covariance intersection procedure [29] (the inverse covariance matrices are combined using a linear convex law, with a fixed parameter $\omega$). For the two-node case, the procedure can be applied directly; a parameter ${\omega }_1$ controls the relative importance between the two data sources being fused. For the three-node case, the expressions used for the two-node case can be cascaded, i.e., a first stage fuses two nodes and a second stage fuses the result with the third node. A ${\omega }_2$ parameter controls the importance between nodes one and two, and node three. In this case, ${\omega }_{1,2}=0.5$, meaning that the data sources in each stage have the same relative importance.

Figure 14 shows the resulting trajectories for the spiral reference trajectory in the case of two and three nodes, respectively. In both cases, a median filter with a sliding window of 20 samples is used to smooth the output of the fusion stage. In this example, the fusion of the three nodes yields a smaller tracking error trend when filtered.

The cascading technique can also be used for networks with a higher number of nodes. The adequate selection of the ${\omega }_i$ parameters of the different covariance intersection stages controls the relative importance of the nodes in the fusion process.

8. Conclusions

This paper presented a formulation for networks of IMUs, extending a representation of the multi-target problem discussed in the literature. The feasibility conditions are discussed based on the feasibility of the LMIs modeling the network. Moreover, the network with the consensus version (with the nodes exchanging information among them) is shown to have an edge over the non-consensus network (i.e., with independent nodes).

The results obtained show the advantage of the network consensus over the single IMU solutions. The limitations of the use of IMU networks are mainly related to the practical implementation and not to the theoretical framework. As referred in the introduction, the rigidity assumption in the relative positioning of the IMUs is likely not to hold permanently.

In this paper, it is implicitly assumed that data is acquired, synchronously, and immediately processed for reconstruction. However, it should be emphasized that data acquisition and reconstruction can occur sequentially in time, i.e., data is acquired during traveling, after which it can be downloaded and fed into the reconstruction stage. Moreover, this decoupled strategy allows the testing of different parameters for reconstruction. Also, synchronism in the data acquisition is likely not to hold for the whole duration of a mission (e.g., due to the mishandling of packages that can damage the devices, intrinsic failures, electronics interferences, etc.).

Future work involves multiple research directions, namely related to the following: (i) uncertainties, (ii) missing information, and (iii) data fusion strategies. A natural evolution is the testing of alternative formulations for the EKF, e.g., the discrete formulation (which, being computationally simpler, may lead to numerical problems). Also, more complex protocols need to be tested and stronger connections with the consensus theory need to be investigated. Essentially, this amounts to replacing the ${\beta }_j^i$ with adequate functions, being able to shape the interchange of information among the nodes.

The rigidity criteria that can be used to determine the performance bounds of the consensus approach will be a key research topic related to the presented framework. The non-rigid scenarios amount to unstructured uncertainties that need to be accommodated by the framework.

Also, the strategies to minimize any effects of missing data are to be investigated. This is mainly related to failures in the devices (or the network).

The advantage of combining covariance intersection and the consensus was illustrated in the simple example above when data is synchronized. Additional testing including asynchronous data scenarios, e.g., as in [30,31], is scheduled for future work.