Multi-Vehicle System Localization by Distributed Moving Horizon Estimation over a Sensor Network with Sporadic Measurements

This is a repository copy of “Multi-Vehicle System Localization by Distributed Moving Horizon Estimation over a Sensor Network with Sporadic Measurements” in the Depósito de Investigación de la Universidad de Sevilla. Version: Author Accepted Version Citation: Antonello Venturino, Sylvain Bertrand, Cristina Stoica Maniu, Teodoro Alamo & Eduardo F. Camacho. 2022 IEEE Conference on Control Technology and Applications (CCTA). 23-25 Agosto 2022. ISBN 978-1-6654-7338-5. Trieste, Italy. http://doi.org/10.1109/CCTA49430.2022.9966151 To cite this publication, please use the final published version (if applicable). Please check the document version above. Copyright: Other than for strictly personal use, it is not permitted to download, forward or distribute the text or part of it, without the consent of the author(s) and/or copyright holder(s), unless the work is under an open content license such as Creative Commons. Takedown policy: Please contact us ([email protected]) and provide details if you believe this document breaches copyrights. We will remove access to the work immediately and investigate your claim. Multi-Vehicle System Localization by Distributed Moving Horizon Estimation over a Sensor Network with Sporadic Measurements Antonello Venturino, Sylvain Bertrand, Cristina Stoica Maniu, Teodoro Alamo, Eduardo F. Camacho Abstract— This paper proposes a Distributed Moving Horizon Estimator (DMHE) for the Multi-Vehicle system localization problem using Sensor Networks with sporadic measurements. Due to its capability to efficiently exploit environmental information via constraints, the proposed DHME technique is well-suited to better estimate the system state when measurements are available at time instants a priori unknown. Indeed, the use of output constraints can contribute to locally improve estimation accuracy, especially when dealing with sporadic measurements and biased sensors data. A realistic case study is proposed within the Robot Operating System framework and Gazebo to localize a Multi-Vehicle System using an inexpensive Sensor Network with low-computation capabilities. A comparative campaign simulation is performed to confirm the effectiveness of the proposed DMHE algorithm in terms of accuracy, computation time, and constraints handling with respect to existing results. I. INTRODUCTION Numerous studies have been dedicated to Distributed State Estimation (DSE) over Sensor Network (SN) [1]–[3] during the last few years since these schemes are suitable for diverse applications and contexts. Some examples focus on detecting and mitigating cyber-attacks [4], tracking intruders in a safe area [1], [5], estimating the state of large-scale systems [6], [7], mobile robot localization [8], [9], etc. Some of these works have conducted only theoretical developments or have exclusively numerically shown the effectiveness of the considered techniques. Indeed, there is still a judicious need for deep insights such as applying algorithms in real experiments and applications. For example, communication delays and losses [10], computation time [5], the timevarying topology of the network [9], sporadic measurements [11], [12] are still open problems to cope with in theory and much more in practice. Distributed State Estimation with a Moving Horizon Estimator (MHE) is one particular case for which the computation time of the state estimation algorithm matters since it involves a constrained optimization problem to be solved at each time instant [13]. The MHE paradigm consists in using an estimation window of fixed size, which moves forward in A. Venturino and C. Stoica Maniu are with Universit´ e Paris- Saclay, CNRS, CentraleSup´ elec, Laboratoire des signaux et syst` emes, 91190, Gif-sur-Yvette, France (e-mail: {antonello.venturino; cristina.stoica}@l2s.centralesupelec.fr). A. Venturino and S. Bertrand are with Universit´ e Paris-Saclay, ONERA, Traitement de l’information et syst` emes, 91123, Palaiseau, France (e-mail: {antonello.venturino;sylvain.bertrand}@onera.fr). T. Alamo and E.F. Camacho are with Department of Ingenier´ ıa de Sistemas y Autom´ atica, Universidad de Sevilla, Camino de los Descubrimientos, 41092 Sevilla, Spain (e-mail: [email protected], [email protected]). time hence the problem remains computationally tractable since only the most recent information is processed and the past evolution of the system is summarized in its so-called arrival cost [14]. The current paper focuses on Multi-Vehicle System (MVS) localization using Distributed MHE (DMHE) algorithms. Similar works have been conducted by the authors of [8] and [9]. In [8], the DMHE problem has been addressed by focusing on the non linearity of the model and on the possible local observability issues at the sensor level. In [9], the authors accounted for mobile nodes in the Sensor Network that led to deal with a dynamic topology. Indeed, using a flocking algorithm for the motion control, the mobile sensors attempt to move in a specific way in order to get the best positions to observe the target and to avoid collisions between neighboring agents. In the current paper, we focus on the computation time aspect, which is a key factor for real-time implementation. In previous research works by the authors [5], [15], [16], DMHE algorithms with pre-estimation have been proposed in order to reduce the computation time while preserving or improving the accuracy of the state estimation. To this aim, the input sequence of noise to be estimated has been replaced by a Luenberger observer leading to fewer optimization parameters to be accounted. Furthermore, an observability rankbased weights technique is used to enhance the accuracy. The contribution of the current paper is two-fold. First, in addition to a reduced computation time and an improved accuracy due to the pre-estimation, the proposed DMHE technique is designed for realistic large-scale systems scenarios involving sporadic measurements (i.e. available at time instants a priori unknown). To this aim, constraints on measurements (coming from the knowledge of the environment where the Multi-Vehicle System is evolving) are embodied using binary parameters in this novel Distributed Moving Horizon Estimation formulation. Thus, the environment information is exploited to better estimate the system state. Second, the current paper aims at evaluating the performance of the proposed DMHE approach (in terms of accuracy and computation time) on a realistic case study, i.e. the distributed localization of a Multi-Vehicle System by a static sensor network, developed within the Robot Operating System (ROS) framework and Gazebo environment. This realistic distributed implementation within ROS and Gazebo would enable the deployment on a hardware setup. To confirm its efficiency, the proposed DMHE constrained formulation is compared with the notable DMHE algorithm [2]. The paper is structured as follows. Section II describes the problem under investigation and introduces the main ingredients for the algorithm. The proposed DMHE algorithm is presented in Section III. Before concluding remarks (Section V), a realistic simulation scenario within the ROS and Gazebo environments is investigated in Section IV. II. DISTRIBUTED STATE ESTIMATION OVER SENSOR NETWORK WITH SPORADIC MEASUREMENTS This section describes the problem of Distributed State Estimation (DSE) of a Multi-Vehicle System (MVS) over a Sensor Network (SN) with sporadic measurements, the considered model, and the Sensor Network with its characteristics. A. Problem description The Distributed State Estimation algorithm uses a Sensor Network to estimate the states of the Multi-Vehicle System. In this context, we assume that the Sensor Network is composed of possibly nSheterogeneous sensors with sporadic measurements, i.e. the measurements may not be (partially or entirely) available all the time to each sensor. For instance, a mobile vehicle is detected by a camera only when it is within its field of view, or by a beacon when it is within its range, etc. A (formation of) vehicle(s) moving in an unknown direction can thus be detected by a given sensor belonging to a Sensor Network at time instants a priori unknown. The system under observation is composed of nVinterconnected ground vehicles which are limited to moving in specific areas, e.g. in an urban area, they can only move on the road (see Fig. 1). We further exploit this information as position constraints in the Distributed State Estimation optimization problem. B. Considered model The dynamical model of the ν-th vehicle (denoted by the left superscript) is described as a linear time-invariant (LTI) system νxt+1 =νAνxt+νwt, ν = 1, . . . , nV(1) where νxt∈νX ⊆ Rνnxis the state and νwt∈νW ⊆ Rνnx is an exogenous input (e.g. an unknown control input, state perturbation, etc.), with νXand νWconvex sets. Remark 1: Notice that, when referring to the global Multi- Vehicle System, the left superscript νis omitted, e.g. xt= [1x⊤ t,...,nVx⊤ t]⊤or A=diag(1A, . . . , nVA). Since each sensor i(right superscript) can provide measurements on each vehicle individually. The following mathematical expression1refers to the output carried out by sensor iwith respect to the ν-th vehicle νyi t=νCi ν xt+νvi t, i = 1, . . . , nS(2) where νyi t∈Rni yis the measurement vector and νvi t∈Rni y the measurement noise. Remark 2: Notice that in (2), the right superscript irefers to the i-th sensor and the left superscript νto the ν-th vehicle. 1Notice that a linear equation is used for the output vector. Fig. 1: Scenario illustrated in Gazebo: MVS with 5 vehicles in the starting place. In this respect, νCiis the output matrix specifying that the sensor iis measuring the vehicle νposition. The notation Ci without the left superscript νrefers to the output matrix of the global Multi-Vehicle System C=diag(1Ci,...,nVCi), similar to Remark 1. For the sake of simplicity in exposing the rest of the paper and without losing generality, we assume that each sensor can measure only the position of the vehicles. The following notation is necessary to denote the collective output matrix of the global system allowing to aggregate both the measuring and non-measuring situations of each sensor Ci αt=Di αtCi(3) where Di αtis a diagonal matrix having ναi t∈ {0,1}as components, with ν= 1, . . . , nV, leading to Di αt=diag(1αi tI1ny,...,nVαi tInVny)(4) with Iνnythe identity matrix of size νny. Remark 3: Notice that ναi tis a time-dependent binary parameter indicating if the sensor iis able to measure the ν-th vehicle at time t(i.e. ναi t= 1) or not (i.e. ναi t= 0). C. Constraints This subsection defines measurement constraints exploiting the a priori knowledge of the environment and the Sensor Network. First, denote by Rthe subset of positions corresponding to the road (assumed to be non-convex) on which the vehicles can move. Then, denote by Fithe set of the points forming the sensor ifield of view. The convex hull of the intersection of these two sets denoted by Si=Co(R∩Fi)(5) is further used to constrain the position of the vehicle when the mobile vehicle is within the field of view of the sensor i(see Fig. 2 for a graphic representation). Fig. 2: Road R(blue line), fields of view F1and F2 (yellow), and convexified constraints S1(red polytope) and S2(blue polytope). D. Sensor network In Distributed State Estimation schemes, the nearby sensors share data among each other. The Sensor Network is described by a directed graph G= (N,E), where N= {1,2, . . . , nS}is the set of all nodes (sensors) and E ⊆ N × Nis the set of all edges (communication links). Therefore, the pair (i, j)∈ E exists if and only if the sensor jcan receive information from the sensor i. The neighborhood Ni of the sensor iis defined as Ni={j∈ N : (i, j)∈ E} and its cardinality ni S=card(Ni). In this paper, we consider that the topology of the Sensor Network is fixed. We distinguish local information, i.e. referring only to the local sensor i, and regional information, i.e. referring to the entire neighborhood Ni. A general bar notation (¯ ·)is used to denote the regional information, e.g. the regional output of sensor iat time tis ¯yi t= [(yi t)⊤,(yj1 t)⊤, . . . , (y jni S t)⊤]⊤, {j1, . . . , jni S}∈Ni. The edges of the graph Gare weighted by the components of a stochastic matrix K, those values are given as follows kij >0if (j, i)∈ E,(6a) kij = 0 otherwise,(6b) nS X j=1 kij = 1,∀i= 1, . . . , nS.(6c) The values of kij can be chosen according to some criteria. For instance, in [16] the authors proposed a rank-based method that leads to a better accuracy for the estimations since Kwill be used in the DMHE algorithm to compute the consensus terms, as described further on in Section III. E. Problem statement Consider the discrete-time LTI system (1) and the sensor network Gwith the linear measurement equation (2), under the assumption that the graph G= (N,E)is strongly connected, i.e. every node is reachable from every other node. The role of each sensor i∈ N, at each time t, is to (possibly) get measurement on (part of) the Multi-Vehicle System, to exchange information among neighbor nodes of Niand to process locally available information in order to determine a local estimate ˆxi tof the real state xtof the Multi-Vehicle System. III. CONSTRAINED DMHE WITH SPORADIC MEASUREMENTS FOR MULTI-VEHICLE SYSTEMS This section recalls the Distributed Moving Horizon Estimation approach with pre-estimation and observability rankbased weights proposed in [16] and presents its novel formulation to handle the Multi-Vehicle localization application considered in the current paper. Thanks to the pre-estimation, the proposed DMHE technique reduces the computation time needed by the sensors (see [16]) to estimate the state of the system, compared to classical DMHE (see [2]). In fact, by replacing the input sequence of the noise to be estimated, the pre-estimation observer reduces the computation time required to solve the optimization problem due to a reduced number of optimization variables. Moreover, thanks to the observability rank-based weights, the accuracy of the estimates is improved and thus it enables the use of classic DMHEs for sporadic measurements. A. Local optimization problem At each time t, given an estimation horizon length N⩾ 1, each sensor i∈ N determines the estimate ˆxi t|tof the state xt|tby solving the following constrained minimization problem with pre-estimation: ˆxi t−N|t=arg min ˆxi t−N Ji αt(·)(7) s.t. ˆxi k+1 =Aˆxi k+Li αkˆvi k,(8) ¯yi k=¯ Ciˆxi k+ˆ ¯vi k,(9) ˆxi k∈ X ∩ Si αk,(10) ∀k=t−N, . . . , t. Notice that in (8), the estimate of the measurement noise ˆvi kis local, while in (9) ˆ ¯vi kis regional. The Amatrix in (8) refers to the global Multi-Vehicle System. The sequence of state estimates ˆxi t−N+1|t,...,ˆxi t|tis obtained from the optimal solution ˆxi t−N|tand using the dynamics (8). The main novelty w.r.t. [16] concerns the use of the binary parameter ναi tin (7) and (8), as it is detailed in the next paragraph. In addition, the constraints (10) are incorporated within the optimization problem as explained in section II-C. The parameter ναi tallows to deal with the sporadic measurements. First, it is useful to discern when the constraints Siare used by sensor iand when not. In particular, considering νSi αt, i.e. the projection of Si αtinto the subspace related to the ν-th vehicle leads to νSi αt=νSiif ναi t= 1 νSi αt=Rνnxif ναi t= 0 The Luenberger gain Liis computed such that Φi=A− LiCiis Schur stable when the Multi-Vehicle System is observable by sensor i. One may compute the gain associated to the global MVS or separately, since Li= [1Li,...,nVLi]. In addition, the dependence on ναi tis formulated via Li αt= LiDi αt, with Di αtdefined by (4). The binary parameter ναi tappears also in the objective function Ji αtdefined as Ji αt(·) = 1 2 t X k=t−N ¯yi k−¯ Ciˆxi k  2 ¯ Ri αt + Γi t−N(·),(11) where the weight matrix ¯ Ri αt= ( ¯ Ri)−1Di αtcan be chosen as the product between the covariance matrix of the measurement noise, and the diagonal matrix Di αtof appropriate dimensions. The last term of (11) is the initial penalty function Γi t(·), known in the MHE paradigm as arrival cost. This terms is non negative and it summarizes the effect of the past measurements, before time t−N. The initial penalty function Γi t(·)in (11) defined as follows: Γi t(·) = 1 2  ˆxi t−N−ˆ ¯xi t−N|t−1   2 (¯ Πi t−N|t−1)−1,(12) involves two consensus terms described below. We denote by ˆ ¯xi t−N|t−1the weighted average state estimation computed by the neighborhood Nias follows: ˆ ¯xi t−N|t−1=X j∈Ni kij ˆxj t−N|t−1,(13) where ˆxj t−N|t−1is the second estimated state in the sequence computed at the previous time by sensor j. Notice that the penalty function Γi t−Nincludes a consensus-on-estimates term, in the sense that it penalizes deviations of ˆxi t−Nfrom ˆ ¯xi t−N|t−1. It helps to improve the accuracy of the local estimates and it is necessary to guarantee convergence of the state estimates to the state of the observed system even if it lacks of regional observability [2]. The positive definite matrix ¯ Πi t−N|t−1is computed as in [2]. For the sake of completeness, we recall here the procedure to compute it by: ¯ Πi t−N|t−1=X j∈Ni nj Sk2 ijΠj t−N|t−1,(14) where the update of Πi t−N|t−1is performed by the sensor i on the basis of regionally available information. In particular, the matrix Πi t−N|t−1, with i∈ N, is given by one iteration of the difference Riccati equation associated to a Kalman filter for the system: (xt−N=Axt−N−1+wt−N−1 ¯zi t−N=¯ Oi Nxt−N+¯ Vi t−N where ¯ Vi t−Nrepresents the measurement noise and ¯ Oi N defines the i-th sensor regional observability matrix: ¯ Oi N=(¯ Ci)⊤(¯ CiA)⊤· · · (¯ CiAN−1)⊤⊤.(15) Then defining: Si N=     0 0 · · · 0 ¯ Ci0· · · 0 . . .. . ..... . . ¯ CiAN−2¯ CiAN−3· · · ¯ Ci      ∈R¯piN×n(N−1), (16) ¯ Ri N=diag(¯ Ri,..., ¯ Ri)∈R¯piNׯpiN,(17) QN−1=diag(Q, . . . , Q)∈Rn(N−1)×n(N−1),(18) Cov[¯ Vi t] = ¯ R∗i N=¯ Ri N+Si NQN−1(Si N)⊤,(19) and setting the covariance of the estimate ˆxi t−N−1as: Π∗i t−N−1|t−2=¯ Πi t−N−1|t−2−1+ ( ¯ Ci)⊤(¯ Ri)−1¯ Ci−1 , (20) the resulting Riccati recursive equation is given by: Πi t−N|t−1=AΠ∗i t−N−1|t−2A⊤+Q−AΠ∗i t−N−1|t−2¯ Oi N⊤ ·¯ Oi NΠ∗i t−N−1|t−2¯ Oi N⊤+¯ R∗i N−1 ·¯ Oi NΠ∗i t−N−1|t−2A⊤. (21) Since the communication network topology is assumed to be time-invariant, these equations can be computed off-line. However, once the matrices Πi t−N|t−1have been computed, we perform a consensus weights update in order to compute the matrices ¯ Πi t−N|t−1according to (14). B. Observability rank-based weights technique Here, we briefly recall the weights tuning technique in [16], for the stochastic matrix Kassociated to the graph G and highlight its adjustment for the considered Multi-Vehicle localization problem by DMHE over a Sensor Network with sporadic measurements. This method needs only local information available by each sensor to compute its own component of K. Thus, it is suitable for a distributed scheme, and more important for this application context, where the measurements are sporadic. Indeed, this technique enhances the accuracy of the estimates by means of exploiting observability properties of the neighborhoods. Since these properties are changing over time for this Sensor Network, the observability rank-based weights technique is an appropriate method to improve even more the accuracy and the convergence of the algorithm. Consider a sensor iat time t. Its regional observability matrix ¯ Oi n=(¯ Ci αt)⊤(¯ Ci αtA)⊤· · · (¯ Ci αtAn−1)⊤⊤(22) is of full rank if and only if the the pair (A, ¯ Ci αt)is completely observable, i.e. rank(¯ Oi n) = n. For the sake of simplicity, we denote by ρi O=rank(¯ Oi n). This information could be used as reliability of sensor iwhen choosing the weights, which according to (6) must be averaged among the neighbors. Moreover, since at some time instants a priori unknown, the entire neighborhoods could not have sensing capabilities at all, i.e. ρi O= 0. To avoid division by zero a lower bound smaller than 1(0.5in (23)) is chosen for kij , which results in kij =   0.5if Pj∈Niρj O= 0 ρj O Pj∈Niρj O otherwise (23) C. DMHE modus operandi Finally, the procedure of the proposed distributed scheme is described in Algorithm 1. Algorithm 1 DMHE procedure 1: Off-line: ∀i∈ N 2: receive from the nodes j∈ N i:Lj,Cj,Rj 3: compute the pre-observer gain Li 4: store the a priori initial estimation ˆxi 0|0= ˆx0of x0, where ˆx0is given, and the covariance matrix Π0of x0 5: Initialization: ∀i∈ N, at the first time step t= 0 6: collect a first local measurement yi 0 7: receive from the neighborhood j∈ N itheir measurements yj 0 8: Online: ∀i∈ N,∀t > 0 9: collect the local measurement yi t 10: receive from the neighbors j∈ N ithe collected data in the step 9 11: compute the Di αtmatrix according to (4) 12: compute the kij components according to (23) 13: if 1⩽t⩽Nthen 14: set the horizon length N=t, the covariance matrix ¯ Πi t−N|t−1=¯ Πi 0|t−1= Π0and the a priori initial estimation state ˆxi t−N|t−1= ˆxi 0|t−1 15: else 16: compute Πi t−N|t−1according to (19), (20) and (21) 17: receive Πj t−N|t−1from the nodes j∈ Ni 18: compute ¯ Πi t−N|t−1according to (14) 19: solve the local optimization problem of DMHE, minimizing Jias in (11) and (12) subject to the constraints (8)-(10) 20: store the solution ˆxi t−N|tand the corresponding estimate ˆxi t|t 21: receive from the neighbors j∈ Nitheir estimates ˆxj t−N+1|t The sporadic measurements constraints are integrated at step 19, with Di αtcomputed at step 11. Notice that the steps 10, 18 and 21 in the procedure regarding the exchanging information could be rearranged to include only one synchronization. However, the current formulation has been chosen for clarity reasons w.r.t. calculation details. IV. REALISTIC SIMULATIONS A. Scenario and simulation setup In this section, the proposed DMHE is applied to estimate the positions of a team of nV= 5 ground vehicles moving together. To evaluate its performance a realistic implementation in the ROS framework and in the Gazebo environment is proposed, see Fig. 1 and the associated video available at https://youtu.be/KRvlQgvHGEo. For the estimation models used in the DMHE optimization problem, each vehicle is modeled as single integrator, with a 2-dimensional state vector representing its Cartesian positions in the plane. The control input vector, i.e. Cartesian linear velocities, of each vehicle is assumed to be unknown, and it is further considered as an exogenous input, i.e. νwt∈R2a uniformly distributed noise with covariance matrix Q=I2. To simulate a realistic system, each vehicle is modeled in Gazebo as a differential drive robot (TurtleBot3). The Multi-Vehicle System goes from the starting point (1,−2) m towards the final point (11,11) m driving within the road and controlled by a leader-follower formation control strategy. The Sensor Network (SN) is composed of nS= 17 cameras measuring the Cartesian positions of the vehicles, and connected as in the Fig. 3 (see the red edges representing the communication links between the nodes depicting the cameras). Fig. 3 also shows the projection on the ground of the field of view of each camera (yellow rectangles), and the road (blue solid line). The start and finish position of the Multi-Vehicle System are clearly indicated in Figs. 1 and 3. Notice that the graph associated with the Sensor Network is not a complete graph, i.e. a graph in which every pair of distinct vertices is connected by a unique edge. The measurements refer to the reference frame associated with each camera. Thus, to have them in the absolute reference frame it is necessary to translate and rotate them with a transformation matrix. To make the scenario more realistic, we added different biases for each camera (via the measurement equation (2) for each sensor) on these translations and rotations, allowing to model uncertainties related to the cameras’ poses. Assuming that these biases can not be easily estimated and compensated in the considered scenario, the purpose is to investigate the robustness of the proposed Distributed Moving Horizon Estimation to this additional source of uncertainty (i.e. sensor biases) and to validate the usefulness of a priori known environment constraints considered in the DMHE optimization problem. A Monte Carlo simulation with 100 runs with different measurements noises (per run) normally distributed, i.e. νvi t is a white noise with zero mean and covariance matrix Ri= 0.5I2was performed. The estimators runs with a sampling time Ts= 0.5s and a horizon length N=4. The initial values of the algorithms have been set as νˆx0=0 0⊤, Π0= 105I2. The optimization problem was implemented by using the quadratic programming solver from [17] implemented in Python. The considered performance indexes are the computation time τneeded by the solver to estimate the positions of the Multi-Vehicle System, and the Root Mean Square Error (RMSE) computed as follows RMSEt=1 100 ·nS 100 X z=1 X i∈N   xt(z)−ˆxi t|t(z)  , 1 10 12 13 14 15 16 17 11 98 7 6 5 4 3 2 Fig. 3: Simulation scenario with road (blue line), cameras (numbered nodes), field of view of each camera (yellow rectangles), and communication link between the sensors (red arrows). both averaged among the trials and the sensors, where xt(z) and ˆxi t|t(z)are, respectively, the realization of the real state of the system and the estimated one, by sensor i, for the trial z. The simulation is carried out by a PC Linux Ubuntu 20.04 equipped with an Intel i9-11950H processor. We compare the proposed Distributed Moving Horizon Estimation without constraints (denoted by DMHE) and with constraints2(denoted by DMHES). We also compare the results with the algorithm in [2], denoted hereafter by DMHEF for the unconstrained case. We also added the constraints (5) to this approach, denoted hereafter by DMHES F. B. Results’ analysis Figure 4 illustrates the averaged RMSE among all the sensors and all the 100 trials. It shows that the proposed DMHE (red dotted curve) and DMHES(solid green curve) have better accuracy w.r.t. to the approach in [2], with constraints (solid cyan curve) or without constraints (dark blue dotted curve). Indeed, the RMSE obtained with the proposed estimation approaches (both DMHE and DMHES) are improved by a factor close to 30% w.r.t. the RMSE of DMHEFand DMHES F. This figure shows also the bounds (shaded colors) representing the minimum and the maximum RMSE of each trial and for each individual local observer. The same trend can be seen in Fig. 5 showing the computation time τaveraged among all the sensors and trials. Accounting for constraints is done at the cost of an increase of the computation time (close to a factor 2). The proposed pre-estimation mechanism enables to compensate that by drastically reducing the computation time. Here the 2The constraints are added as in (5). 0 30 60 90 120 150 180 210 0 5 10 15 Fig. 4: Averaged RMSE among all the sensors and all the trials. 0 30 60 90 120 150 180 210 0 2 4 6 810 -3 Fig. 5: Averaged computation time τamong all the sensors and all the trials. bounds, representing the minimum and the maximum τof each trial and for each individual local observer of the proposed DMHEs are tighter than the bounds obtained with the DMHEF(see [2]) and DMHES F. The constraints Siare used in the local optimization problem only when the camera is actually sensing a vehicle. In order to show the effects of these constraints Si(5), we picked up, for one random trial, the estimations of the position along the x-axis for the fourth vehicle. In particular, we consider to plot only the estimations during the time periods when their sensors are active, i.e. when the vehicle belongs to its field of view. Thus, Fig. 6 illustrates the real position 4px,t (in green solid line) coming from the true localization on the ground provided by the Gazebo simulator, the measurements of the cameras (cyan dots), the estimations using DMHE (red dotted curves) and DMHES(dark blue dotted curves). The zoomed parts also show the local constraints (black dashed lines). Due to the measurement noise and bias, some measurements (cyan dots) could not correspond to possible positions of the vehicle which are constrained to be within the road boundaries. Accounting explicitly for constraints in the estimation helps to improve the accuracy. Figure 6 illustrates that the estimations with the DMHES(dark blue dotted curves) method respect the constraints represented by black dashed lines in the zoomed parts. V. CONCLUSION AND PERSPECTIVES In this paper, we proposed a Distributed Moving Horizon Estimation (DMHE) algorithm for localizing a Multi-Vehicle System (MVS) over a static sensor camera network with sporadic measurements, i.e. available at time instants a 0 30 60 90 120 150 180 210 0 5 10 15 Fig. 6: Estimation of the position along the x-axis of the fourth vehicle 4ˆpx,t by all the active sensors. priori unknown. The proposed approach, which takes into account measurement constraints, has been implemented in a realistic distributed way in the Robotic Operator System (ROS) middleware with a Gazebo simulation environment. Indeed, thanks to the pre-estimation observer in the optimization problem, the proposed DMHE has shown half of the computation time needed by [2], since it replaces the input sequence of the noise to be estimated, thus leading to fewer optimization parameters. Moreover, for the reason that an optimization problem is used in the MHE paradigm, the proposed DMHE is prone to exploit a priori information as constraints to better estimate the state of the system. The proposed DMHE formulation is able to deal with sporadic measurements due to binary parameters inserted into the algorithm. In addition, it increases the accuracy of the estimation as a consequence of using the observability rankbased method to tune the components of the consensus matrix associated with the graph of the Sensor Network. This particular aspect is suitable in conditions of sporadic measurements. Current research work focuses on implementing the proposed method on hardware Multi-Vehicle System, involving TurtleBot3 mobile vehicles, and an inexpensive sensor camera network. VI. ACKNOWLEDGMENT This work was supported by the Agencia Estatal de Investigaci´ on (AEI)-Spain under Grant PID2019-106212RB- C41/AEI/10.13039/501100011033 and the European Research Council under the advanced grant OCONTSOLAR Grant agreement ID: 789051. REFERENCES [1] G. Battistelli, “Distributed moving-horizon estimation with arrival-cost consensus,” IEEE Transactions on Automatic Control, vol. 64, no. 8, pp. 3316–3323, 2018. [2] M. Farina, G. Ferrari-Trecate, and R. Scattolini, “Distributed moving horizon estimation for linear constrained systems,” IEEE Transactions on Automatic Control, vol. 55, no. 11, pp. 2462–2475, 2010. [3] M. Farina, G. Ferrari-Trecate, and R. Scattolini, “Distributed moving horizon estimation for nonlinear constrained systems,” International Journal of Robust and Nonlinear Control, vol. 22, no. 2, pp. 123– 143, 2012. [4] K. Gheitasi, M. Ghaderi, and W. Lucia, “A novel networked control scheme with safety guarantees for detection and mitigation of cyberattacks,” in 18th European Control Conference, 2019, pp. 1449–1454. [5] A. Venturino, S. Bertrand, C. Stoica Maniu, T. Alamo, and E. F. Camacho, “A new ℓ-step neighbourhood distributed moving horizon estimator,” in 60th IEEE Conference on Decision and Control, 2021, pp. 508–513. [6] A. Haber and M. Verhaegen, “Moving horizon estimation for largescale interconnected systems,” IEEE Transactions on Automatic Control, vol. 58, no. 11, pp. 2834–2847, 2013. [7] P. Segovia, V. Puig, and E. Duviella, “Set-membership-based distributed moving horizon estimation of large-scale systems,” ISA transactions, 2021. [8] A. Simonetto, D. Balzaretti, and T. Keviczky, “A distributed moving horizon estimator for mobile robot localization problems,” IFAC Proceedings Volumes, vol. 44, no. 1, pp. 8902–8907, 2011. [9] Z. R. Yousefi and M. B. Menhaj, “Constrained distributed estimation based on consensus algorithm for mobile robots tracking,” in IEEE International Conference on Control System, Computing and Engineering, 2014, pp. 124–129. [10] J. Zeng and J. Liu, “Distributed moving horizon estimation subject to communication delays and losses,” in American Control Conference, 2015, pp. 5533–5538. [11] R. Postoyan and D. Neˇ si´ c, “A framework for the observer design for networked control systems,” IEEE Transactions on Automatic Control, vol. 57, no. 5, pp. 1309–1314, 2011. [12] F. Ferrante, F. Gouaisbaut, R. G. Sanfelice, and S. Tarbouriech, “State estimation of linear systems in the presence of sporadic measurements,” Automatica, vol. 73, pp. 101–109, 2016. [13] K. R. Muske, J. B. Rawlings, and J. H. Lee, “Receding horizon recursive state estimation,” in American Control Conference, 1993, pp. 900–904. [14] C. V. Rao, J. B. Rawlings, and J. H. Lee, “Constrained linear state estimation — a moving horizon approach,” Automatica, vol. 37, no. 10, pp. 1619–1628, 2001. [15] A. Venturino, S. Bertrand, C. Stoica Maniu, T. Alamo, and E. F. Camacho, “Distributed moving horizon estimation with pre-estimating observer,” in 24th International Conference on System Theory, Control and Computing, 2020, pp. 174–179. [16] A. Venturino, C. Stoica Maniu, S. Bertrand, T. Alamo, and E. F. Camacho, “Distributed moving horizon state estimation for sensor networks with low computation capabilities,” System Theory, Control and Computing Journal, vol. 1, no. 1, pp. 81–87, 2021. [17] D. Goldfarb and A. Idnani, “A numerically stable dual method for solving strictly convex quadratic programs,” Mathematical programming, vol. 27, no. 1, pp. 1–33, 1983.