A Minimum Energy Filter for Distributed Multirobot Localisation
AA Minimum Energy Filter for DistributedMultirobot Localisation (cid:63)
Jack Henderson ∗ Jochen Trumpf ∗ Mohammad Zamani ∗∗∗
Research School of Electrical, Energy and Materials Engineering,Australian National UniversityE-mail: { jack.henderson, jochen.trumpf } @anu.edu.au ∗∗ Land Division, Defence Science and Technology Group, AustraliaE-mail: [email protected]
Abstract:
We present a new approach to the cooperative localisation problem by applying thetheory of minimum energy filtering. We consider the problem of estimating the pose of a groupof mobile robots in an environment where robots can perceive fixed landmarks and neighbouringrobots as well as share information with others over a communication channel. Whereas the vastmajority of the existing literature applies some variant of a Kalman Filter, we derive a set offilter equations for the global state estimate based on the principle of minimum energy filtering.We show how the filter equations can be decoupled and the calculations distributed amongthe robots in the network without requiring a central processing node. Finally, we provide ademonstration of the filter’s performance in simulation.
Keywords:
Nonlinear observers and filter design, Localization, Cooperative perception,Autonomous Mobile Robots, Multi-vehicle systems1. INTRODUCTIONIn a wide range of robotics applications, an accurateestimation of the current position and orientation (pose)of a robot is essential for its primary mission. However,in environments where sensor performance is degraded,traditional approaches to state estimation begin to fail,for example when GNSS systems are actively jammed orrobots have very limited access to external landmarks.Collaborative localisation (CL) is an approach that can beutilised where multiple robots are operating in the sameenvironment. The core concept is that the informationgained by sensors on one robot can be shared with otherrobots in the environment thereby increasing the accuracyof the pose estimates. This is useful in the aforementionedcase of sensor degradation, and can also be used forgroups of heterogeneous robots where robots have differentsensing capabilities.When information such as local state estimates are sharedbetween robots in a filtering framework, the pose estimatesof each robot are no longer independent. If this state-dependency is not properly accounted for, it can lead todata incest and over-confidence problems (Howard et al.,2003).One solution to this problem is presented by Roumeliotisand Bekey (2002), who derive a centralised ExtendedKalman Filter (EKF) which jointly estimates the pose of (cid:63)
This research is supported by the Commonwealth of Australiaas represented by the Defence Science and Technology Group ofthe Department of Defence and by the Australian Research CouncilDiscovery Project DP190103615: “Control of Network Systems withSigned Dynamical Interconnections” all robots in the network and where all of the informationdependencies are tracked in a single covariance matrix.They go on to show that the joint filter equations canbe decoupled into a set of smaller, communicating filtersdistributed among the robots in the network.There are two drawbacks to this type of approach. Firstly,the entire joint covariance matrix must be tracked, eitherin a centralised or distributed arrangement. Secondly, aftereach measurement step, a robot must communicate to allother robots in the network to update the estimates andcovariance terms that are tracked by other robots. Thisis often impractical in most scenarios as a robust, fully-connected communications network topology cannot beguaranteed.Recent approaches to CL have focused on reducing thedata that is tracked by each robot and relaxing the com-munication network topology constraints. For example,Carrillo-Arce et al. (2013) proposes a filter local to eachrobot which only tracks the local state. Cross-covarianceterms are not tracked and are instead estimated locallywhen two robots meet and share information. While thisreduces the communication overhead, it comes at the costof being too conservative in the dependency estimationand not utilising available data to the maximal possibleextent. Their experimental results show the performanceof this type of filter is worse than the joint EKF. Furtherwork by Luft et al. (2018) aims to better approximatethe cross covariance terms, and demonstrates performancevery similar to, although slightly worse than, the jointEKF from Roumeliotis and Bekey (2002).The common element in these works is that they all utilisethe Extended Kalman Filter and are all compared to the a r X i v : . [ ee ss . S Y ] M a y oint EKF presented by Roumeliotis and Bekey (2002) as itprovides a baseline estimate given no restrictions on com-putation or communication. While the EKF and variantssuch as the Multiplicative EKF (MEKF) (Markley, 2003)are industry standard in terms of pose filtering algorithms,there are potentially better alternatives. All variants of theEKF rely on linearising the system, which can result ininstability and convergence issues when estimating highlynon-linear systems such as robot pose kinematics. Morerecently, the approach of minimum energy filtering hasbeen demonstrated by Zamani et al. (2013) as a moreaccurate and robust algorithm for pose estimation. A col-laborative minimum-energy pose estimation algorithm hasbeen proposed by Zamani and Hunjet (2019), but opts forestimating cross-covariance terms rather than tracking thefull state of the system.In this paper, we present the derivation of a centralisedgeometric approximate minimum-energy (GAME) filterto estimate the poses of a network of robots using bothinteroceptive and exteroceptive measurements. We showhow this filter can be equivalently derived as a set ofcollaborative filters which run locally on each robot in thenetwork. The decoupled filters provide exactly the samepose estimates as the centralised filter and information-sharing is only necessary during the filter update step,where exteroceptive measurements are processed.The filter we derive provides a baseline which can beused as a benchmark for future implementations of otherminimum-energy filters, much in the same way thatRoumeliotis and Bekey (2002) has been used as the base-line for further developments of EKF-based filters.The remainder of this paper is organised as follows. InSection 2 we briefly introduce a number of concepts andthe notation used in the paper. Section 3 formally statesthe problem we aim to solve and we present our solutionin Section 4. We then demonstrate an implementation ofthe filter through a simulation in Section 5 and concludethe paper in Section 6.2. PRELIMINARIESIn this section we introduce the notation and conventionsused throughout the paper. ( . ) (cid:62) denotes the matrix transpose, I n denotes an n × n identity matrix. The operators exp and log denote thematrix exponential and matrix logarithm respectively.We use the matrix Lie groups SO(3) to represent rotationsand SE(3) to represent poses in homogeneous coordinates.The corresponding Lie algebras are so (3) and se (3), re-spectively.SO(3) = (cid:8) R ∈ R × | R (cid:62) R = I , det R = 1 (cid:9) (1)SE(3) = (cid:26) X = (cid:20) R p × (cid:21) | R ∈ SO(3) , p ∈ R (cid:27) (2) so (3) = (cid:8) Ψ ∈ R × | Ψ (cid:62) = − Ψ (cid:9) (3) se (3) = (cid:26) Γ = (cid:20) Ψ v × (cid:21) | Ψ ∈ so (3) , v ∈ R (cid:27) (4) We define the following maps which allow us to switchbetween matrix and vector representations.( . ) × : R → so (3) ω × := (cid:34) − ω ω ω − ω − ω ω (cid:35) (5)vex : so (3) → R vex(Ω) := vex( ω × ) = ω (6)( . ) ∧ : R → se (3) γ ∧ := (cid:20) ω × v (cid:21) = Γ (7)( . ) ∨ : se (3) → R Γ ∨ := ( γ ∧ ) ∨ = γ (8)( . ) (cid:103) : se (3) n → R n Γ (cid:103) = (cid:2) Γ ∨(cid:62) , . . . , Γ ∨(cid:62) n (cid:3) (cid:62) (9)( . ) (cid:102) : R n → se (3) n γ (cid:102) := (cid:0) Γ (cid:103) (cid:1) (cid:102) = Γ (10)where γ = (cid:20) ωv (cid:21) , ω, v ∈ R Γ ∈ se (3) (11) γ ∈ R n Γ = (Γ , . . . , Γ n ) ∈ se (3) n (12)The following maps are useful when working in homoge-neous coordinates.¯( . ) : R → R ¯ v := (cid:2) v (cid:62) (cid:3) (cid:62) (13)˚( . ) : R → R ˚ v := (cid:2) v (cid:62) (cid:3) (cid:62) (14)˜( . ) : R × → R × ˜ M := (cid:20) M
00 1 (cid:21) (15)Observe the following identities for Γ ∈ se (3) , v ∈ R .Γ¯ v = F ( v )Γ ∨ F ( v ) := (cid:20) − v × I (cid:21) (16)Γ (cid:62) ¯ v = G ( v )Γ ∨ G ( v ) := (cid:20) v × v (cid:62) (cid:21) (17)We define the symmetric and skew-symmetric projectionsof R n × n , P s and P a respectively, and the unique orthogonalprojection, P , of R × onto se (3) with respect to theFrobenius inner product. P s : R n × n → sym( n ) P s ( M ) := 12 ( M + M (cid:62) ) (18) P a : R n × n → so ( n ) P a ( M ) := 12 ( M − M (cid:62) ) (19) P : R × → se (3) P (cid:18)(cid:20) A × B × C × D × (cid:21)(cid:19) := (cid:20) P a ( A ) B (cid:21) (20)Lastly, we define the element-wise multiplication operator, (cid:12) , for a general group, G . (cid:12) : G n × G n → G n (21)( x , . . . , x n ) (cid:12) ( y , . . . , y n ) := ( x y , . . . , x n y n ) (22)We will omit the (cid:12) symbol when the meaning is clear fromcontext. Let T X SE(3) denote the tangent space to the manifoldSE(3) at the point X . Note that the Lie algebra se (3)coincides with T I SE(3) and that for all Γ ∈ se (3), thetangent vector X Γ ∈ T X SE(3).Let the metric (cid:104) ., . (cid:105) X : T X SE(3) × T X SE(3) → R denotethe standard left-invariant Riemannian metric on SE(3),that is X Γ , X Ψ (cid:105) X = (cid:104) Γ , Ψ (cid:105) I = (cid:104) Γ , Ψ (cid:105) (23)= tr (cid:18)(cid:20) . I
00 1 (cid:21) Γ (cid:62) Ψ (cid:19) (24)= (cid:104) Γ ∨ , Ψ ∨ (cid:105) = (Γ ∨ ) (cid:62) Ψ ∨ (25)for Γ , Ψ ∈ se (3).We define the distance function, d P , applied to two ele-ments X , X ∈ SE(3) n , weighted by a positive definitematrix P ∈ R n × n (cid:31) d P ( X , X ) := (cid:113) (cid:104) P log( X -12 (cid:12) X ) (cid:103) , log( X -12 (cid:12) X ) (cid:103) (cid:105) . (26) Let f : SE(3) → R denote a differentiable map. Then D X f ( X ) : T X SE(3) → R denotes the Fr´echet derivativeand we have D X f ( X ) ◦ ( X Γ) = (cid:104)∇ X f ( X ) , X Γ (cid:105) X (27)where X Γ ∈ T X SE(3) denotes the tangent direction inwhich the derivative is evaluated and ∇ X f ( X ) denotes thegradient at the point X with respect to the metric (cid:104) ., . (cid:105) X .The second order differential map D X f ( x ) : T X SE(3) × T X SE(3) → R is defined as D X f ( x ) ◦ ( X Γ , X Ψ) = (cid:104)
Hess X f ( X ) ◦ ( X Ψ) , X Γ (cid:105) X (28)= (cid:104) Hess X f ( X ) ◦ ( X Γ) , X Ψ (cid:105) X (29)where Hess X f ( X ) denotes the Hessian operator. The mapcan also be written in terms of first-order derivatives: D X f ( x ) ◦ ( X Γ , X Ψ) = D X ( D X f ( X ) ◦ ( X Γ)) ◦ ( X Ψ) −(cid:104)∇ X f ( X ) , X Λ Ψ (Γ) (cid:105) X (30)where Λ Ψ : se (3) → se (3) is the connection function. Inthis paper, we use the symmetric Cartan connection: Λ Ψ (Γ) := 12 (ΨΓ − ΓΨ) (31)3. PROBLEM FORMULATIONWe consider n mobile robots in a fully-connected networkwith node set N := { , . . . , n } . A set of r landmarks L := { l i ∈ R | i = 1 , . . . , r } are placed in the environmentat fixed locations. Each robot is equipped with a suiteof interoceptive and exteroceptive sensors as well as amethod to communicate directly to other robots in thenetwork. We aim to derive a deterministic second-orderapproximate minimum energy filter to estimate the pose ofeach robot in the network. Initially, this will be formulatedas a set of centralised equations but we will show how thefilter can be decoupled and distributed among the robotsin the network. The rotation, R i , and translation, p i , of each robot i ∈ N with respect to a fixed reference frame is represented as a4 × X i . The pose has the followingleft-invariant kinematics. X i = (cid:20) R i p i (cid:21) ∈ SE(3) Ω i = (cid:20) ω i v i (cid:21) ∧ ∈ se (3) (32)˙ X i = X i Ω i X i (0) = X i, (33) where ω i and v i are the angular and linear velocities of therobot with respect to the reference frame. A robot, i , can independently measure its own velocity.The measurement, u i , is corrupted by zero-mean sensornoise, (cid:15) i ∈ R . u i = (cid:20) ω i v i (cid:21) + B i (cid:15) i (34)where B i ∈ R × is determined by the sensor properties.Each robot is equipped with a sensor that measures therelative translation between the robot and landmarks inthe environment. A measurement, y ∈ R , of the landmark l ∈ L taken by robot i is corrupted by zero-mean sensornoise, δ ∈ R . ¯ y = X -1 i ¯ l + ˜ C ˚ δ (35)where C ∈ R × is determined by the sensor properties.A similar sensor on each robot also measures the relativetranslation to other robots in the network. A robot, i ,senses and identifies a known marker point, m j , affixed toanother robot, j . The measurement, z ij ∈ R , is corruptedby zero-mean sensor noise, η ∈ R .¯ z ij = X -1 i X j ¯ m j + ˜ D ˚ η (36)where D ∈ R × is determined by the sensor properties.The marker point, m j , is known and is defined with respectto the body-fixed frame of robot j .Landmark and robot measurements are not necessarilyavailable at all times or to all robots. Measurements maybe intermittent and a robot may only be able to observea subset of L and N at any given time. We introduce the global state variable, X , which comprisesof the states of all robots in the network. X := ( X , . . . , X n ) ∈ SE(3) n (37)We then have˙ X := X (cid:12) (Ω , . . . , Ω n ) , X (0) = X , (38)and denote u := ( u , . . . , u n ) , (cid:15) := ( (cid:15) , . . . , (cid:15) n ) . (39) As discussed in Section 3.2, each robot can receive infor-mation from three different sensors to provide measure-ments of velocity, positions of landmarks and positions ofother robots. We follow the approach taken by Zamaniand Trumpf (2019) to define the problem in terms of acontinuous-time propagation step that uses the velocitymeasurements, and a discrete time update step, usingeither the landmark or robot measurements.Following Mortensen’s formulation of the deterministicminimum energy problem (Mortensen, 1968), we introducethe following continuous-time cost functional, J t . J t ( X , (cid:15) ) := 12 d P (cid:16) X (0) , ˆ X (cid:17) + 12 (cid:88) i ∈ N (cid:90) t (cid:107) (cid:15) i (cid:107) dτ (40)here P is a positive definite matrix which weightsthe initial estimate. We assume that, relative to theexteroceptive measurements, the velocity measurementsare available at a high enough frequency that they canbe regarded as a continuous signal.We can now formally define the minimum energy filteringproblem: Given a sequence of velocity measurements, u [0 , t ], find an estimate, ˆ X ( t ) ∈ SE(3) n , of the state of thesystem, X ( t ), that minimises the cost functional J t ( ˆ X , (cid:15) )and is consistent with the kinematics described in (38).The estimate must also be formulated as a recursiveequation, dependent only on the measurements and thestate estimate at the current time.Minimising J t is performed in two steps – firstly byminimising over (cid:15) , and then minimising over a point X on the trajectory. We introduce the value function, V , torepresent the first step in this process. V ( X , t ) := min (cid:15) [0 ,t ] J t ( X , (cid:15) ) (41) V ( X (0) ,
0) = 12 d P (cid:16) X (0) − ˆ X (cid:17) (42)The optimal state estimate is then given byˆ X ( t ) := arg min X V ( X , t ) . (43)We now consider the exteroceptive landmark and robotmeasurements. As in Zamani and Trumpf (2019), we intro-duce a discrete-update value function, V + , for landmarkmeasurements. Additionally, we introduce a second valuefunction, V ++ , for measurements of other robots. V + ( X , t ) := V ( X , t ) + 12 (cid:13)(cid:13) X i ¯ y i − ¯ l (cid:13)(cid:13) P -1¯ y (44) V ++ ( X , t ) := V ( X , t ) + 12 (cid:107) X i ¯ z ij − X j ¯ m j (cid:107) P -1¯ z (45) P ¯ y := ˜ C ˜ C (cid:62) , P ¯ z := ˜ D ˜ D (cid:62) (46)The optimal minimum-energy state estimate is given byˆ X + ( t ) or ˆ X ++ ( t ), respectivelyˆ X + ( t ) := arg min X V + ( X , t ) (47)ˆ X ++ ( t ) := arg min X V ++ ( X , t ) (48)While these equations have been formulated for a singlemeasurement, they are applied to each landmark or robotmeasurement at the time they are received.4. RESULTSIn this section, we derive the filter equations for thecentralised state estimation problem and then show howthey can be decoupled and distributed among the robotsin the network. The following lemma is a simple consequence of therelevant definitions.
Lemma 1.
Given any two tangent directions X Γ , X Ψ ∈ T X SE(3) n , the Hessian of the value function, acting as asymmetric mapping with respect to the inner product is equivalently represented with a positive definite matrix, P ∈ R n × n , operating on vectors Γ (cid:103) , Ψ (cid:103) ∈ R n . (cid:10) P Ψ (cid:103) , Γ (cid:103) (cid:11) := (cid:104) Hess X V ( X , t ) ◦ X Ψ , X Γ (cid:105) | X = ˆ X ( t ) (49) Propagation of Velocity Measurements.
Following themethodology in Theorem 1 and Theorem 2 of Zamaniand Trumpf (2019) results in the following filter statepropagation equations for the centralised system.˙ˆ X ( t ) = ˆ X ( t ) (cid:12) u ( t ) (50)˙ P ( t ) = − P BB (cid:62) P + P s ( P U ) , P (0) = P (51)where U := (cid:20) ( u ω ) × u v ) × ( u ω ) × (cid:21) , (52) U := blkdiag( U , U , . . . , U n ) , (53) B := blkdiag( B , B , . . . , B n ) . (54)We note here that B is block diagonal, indicating our as-sumption that velocity measurements on-board one robotare independent of all other robots. Landmark Measurement UpdateTheorem 2.
Consider a single relative position measure-ment of a landmark, y , as defined in (35). The approximateminimum-energy recursive solution to the estimate of thestate X , as defined in (47) isˆ X + = ˆ X (cid:12) Θ (55)where Θ = exp (cid:32)(cid:18) − ( P + ) -1 (cid:16) ˆ X -1 ∇ X V + ( ˆ X ( t ) , t ) (cid:17) (cid:103) (cid:19) (cid:102) (cid:33) (56) ∇ X i V + ( ˆ X ( t ) , t ) = ˆ X i P (cid:16) ˆ X (cid:62) i P -1¯ y ( ˆ X i ¯ y − ¯ l )¯ y (cid:62) ˜(2 I ) (cid:17) (57) ∇ X j V + ( ˆ X ( t ) , t ) = 0 ∀ j (cid:54) = i. (58) P + is the matrix equivalent to Hess X V + ( ˆ X ( t ) , t ), asdefined in Lemma 1, and can be calculated as P + = P + Q (59) Q ii = P s (cid:16) F (¯ y ) (cid:62) G ( ˆ X (cid:62) i P -1¯ y ( ˆ X i ¯ y − ¯ l )) (cid:17) + F (¯ y ) (cid:62) ˆ X (cid:62) i P -1¯ y ˆ X i F (¯ y ) (60) Q k = ∀ k (cid:54) = ( i, i ) . (61)Here, Q ∈ R n × n is indexed in blocks of 6 × Q ii refers to the i th 6 × Q . Recall the definitions of F and G from (16)and (17), respectively. Proof:
We first perform a Taylor expansion of V + ( X , t )to second order around the point X = ˆ X ( t ) along thegeodesic Ψ = log( ˆ X -1 (cid:12) X ). This yields an approximatesolution as the value function is not guaranteed to be ofsecond order. Ignoring the higher order terms, V + ( X , t ) = V + ( ˆ X , t ) + (cid:104)∇ X V + ( ˆ X , t ) , ˆ X Ψ (cid:105) + 12 (cid:104) Hess X V + ( ˆ X , t ) ◦ ˆ X Ψ , ˆ X Ψ (cid:105) (62)As a consequence of (47), we have {D X V + ( X , t ) ◦ X Γ } X = ˆ X + ( t ) = 0 (63)hich we can substitute in (62). Combined with theconsequence from (43) that ∇ X V ( ˆ X , t ) = 0, it followsthat0 = (cid:110) (cid:68) ∇ X V + ( ˆ X ( t ) , t ) , D X ( ˆ X Ψ ) ◦ ( ˆ X Γ ) (cid:69) + (cid:68) Hess X V + ( ˆ X ( t ) , t ) ◦ ˆ X Ψ , D X ( ˆ X Ψ ) ◦ ( ˆ X Γ ) (cid:69)(cid:111) X = ˆ X + ( t ) (64)Rearranging to solve for ˆ X + ( t ), together with (49), resultsin (55). Equation (57) is then derived by evaluating (cid:104)∇ X i V + ( ˆ X ( t ) , t ) , ˆ X i Γ (cid:105) (65)using (27), (44), and (63). We then evaluate the deriva-tive and reformulate in terms of (24) to solve for ∇ X i V + ( ˆ X ( t ) , t ). We calculate P + by observing thatHess X V + ( ˆ X ( t ) , t ) = Hess X V ( ˆ X ( t ) , t )+ Hess X (cid:18) (cid:13)(cid:13) X i ¯ y i − ¯ l (cid:13)(cid:13) P -1¯ y (cid:19) . (66) (cid:50) Robot Measurement UpdateTheorem 3.
In the same manner as Theorem 2, the ap-proximate minimum-energy recursive estimate for thestate X after a robot measurement, z ij , can be calculatedas ˆ X ++ = ˆ X (cid:12) Ξ (67)where Ξ = exp (cid:32)(cid:18) − ( P ++ ) -1 (cid:16) ˆ X -1 (cid:12) ∇ X V ++ ( ˆ X ( t ) , t ) (cid:17) (cid:103) (cid:19) (cid:102) (cid:33) (68) ∇ X i V ++ ( ˆ X ( t ) , t ) = ˆ X i P (cid:16) ˆ X (cid:62) i P -1¯ z (cid:16) ˆ X i ¯ z − ˆ X j ¯ m (cid:17) ¯ z (cid:62) ˜(2 I ) (cid:17) (69) ∇ X j V ++ ( ˆ X ( t ) , t ) = ˆ X j P (cid:16) ˆ X (cid:62) j P -1¯ z (cid:16) ˆ X i ¯ z − ˆ X j ¯ m (cid:17) ¯ m (cid:62) ˜(2 I ) (cid:17) (70) ∇ X k V ++ ( ˆ X ( t ) , t ) = 0 ∀ k / ∈ { i, j } (71) P ++ is the matrix equivalent to Hess X V ++ ( ˆ X ( t ) , t ), asdefined in Lemma 1, and can be calculated as P ++ = P + W (72) W ii = P s (cid:16) F (¯ z ) (cid:62) G ( ˆ X (cid:62) i P -1¯ z ( ˆ X i ¯ z − ˆ X j ¯ m )) (cid:17) + F (¯ z ) (cid:62) ˆ X (cid:62) i P -1¯ z ˆ X i F (¯ z ) (73) W ij = F (¯ z ) (cid:62) ˆ X (cid:62) i P -1¯ z ˆ X j F ( ¯ m ) , W ji = W (cid:62) ij (74) W jj = P s (cid:16) F ( ¯ m ) (cid:62) G ( ˆ X (cid:62) j P -1¯ z ( ˆ X j ¯ m − ˆ X i ¯ z )) (cid:17) + F (¯ z ) (cid:62) ˆ X (cid:62) j P -1¯ z ˆ X j F ( ¯ m ) (75) W k = ∀ k / ∈ { ( i, i ) , ( i, j ) , ( j, i ) , ( j, j ) } . (76)Here, W ∈ R n × n is indexed in the same way as Q fromthe previous section. Proof:
The proof follows along the same lines as the proofof Theorem 2.
Given the set of equations that define the propagationand update steps for the centralised GAME filter, we nowattempt to decouple the equations so that the calculationscan be distributed among the robots in the network. Wewill find that the decoupling of the filter equations is easierwhen working with the inverse of the Hessian, Σ := P -1 .In the following formulation each robot, i , tracks its ownstate estimate, ˆ X i , and an n × ki . Propagation Step
We note that the state propagationequation (50) is trivial to decouple.˙ˆ X i ( t ) = ˆ X i ( t ) u i ( t ) ∀ i ∈ N (77)To decouple the calculation for P , we reformulate (51) interms of Σ, which allows us to separate ˙Σ into components.˙Σ = BB (cid:62) − P s ( U Σ) (78)˙Σ ii = B i B (cid:62) i − P s ( U i Σ ii ) (79)˙Σ ij = − (cid:0) U i Σ ij + Σ ij U (cid:62) j (cid:1) (80)Here, we use the same 6 × ii ,only depend on data local to robot i , while the off-diagonalsub-matrices, ˙Σ ij , depend on data local to both robot i and j . Given that (80) is a homogeneous linear ODE, we canfind an explicit solution if we assume that U i and U j areconstant.Σ ij ( t ) = exp (cid:18) − t U i (cid:19) Σ ij (0) exp (cid:18) − t U (cid:62) j (cid:19) (81)In reality, the velocity measurements are received from asensor which updates at a fixed time interval, which meansthat U i and U j do remain constant for a time ∆ t , whichrepresents the time between two successive measurements.Thus, we can recursively evaluate Σ ij at a time t n , afterthe n -th measurement is recorded byΣ ij ( t n ) = exp (cid:0) − ∆ t U i ( t n ) (cid:1) Σ ij ( t n − ) exp (cid:0) − ∆ t U j ( t n ) (cid:62) (cid:1) (82)= K i ( t n )Σ ij (0) K j ( t n ) (cid:62) (83) K i ( t n ) = (cid:89) k = n exp (cid:18) − ∆ t U i ( t k ) (cid:19) (84)Based on this formulation, we observe that K i can becomputed independently by robot i and similarly K j canbe computed by robot j . Robot i can calculate Σ ji ( t n ) byreceiving a message from robot j that contains K j ( t n ).In this way, we show a parallel result to Roumeliotis andBekey (2002) whereby robots can propagate their stateindependently and only need to share information at atime where exteroceptive measurements are taken. Robot Measurement Update
In the robot measurementupdate step, we are required to decouple (67) and (72).This would be a straightforward task for (72) if P wasknown, however given that the propagation step has beencomputed in terms of Σ, this would require a full matrix-inversion of Σ, which is only possible in a centralisedsystem — not in our system where each robot is onlyracking a sub-matrix. Thus, the update step must alsobe reformulated and then decoupled in terms of Σ.Σ ++ = ( I n + Σ W ) -1 Σ (85)Recall the definition of W from (73) through (76). Weobserve that, because of the sparsity of W , the onlyelements of Σ that need to be known in order to compute( I n +Σ W ) -1 are Σ ki and Σ kj , k ∈ N . This corresponds tothe elements of Σ that are being tracked by robot i and j ,respectively, and means that the inverse can be computedlocally between robot i and j . Once calculated, this termcan then be shared with all other robots in the network tocalculate the value for Σ ++ .Similarly, (69) and (70) can be computed locally betweenrobots i and j , which also allows (68) to be computedlocally. Each component of Ξ can then be communicatedto the relevant robot such that (67) can be computedlocally to each robot.ˆ X ++ i = ˆ X i Ξ i ∀ i ∈ N (86)One of the issues with the current formulation is that(85) requires inverting a 6 n × n matrix. However, weobserve that rank( W ) (cid:54)
12 and we perform a singularvalue decomposition on Σ W as follows. T SV (cid:62) = Σ W (87) T T (cid:62) = V V (cid:62) = I n , S ∈ D , T, V ∈ R n × (88)We can then apply the matrix inversion lemma (Woodburymatrix identity) which reduces the size of the matrixthat is inverted from 6 n × n to a maximum of 12 × W . S is diagonal and canbe trivially inverted.Σ ++ = (cid:0) I n − T ( S -1 + V (cid:62) T ) -1 V (cid:62) (cid:1) Σ (89)This also reduces the size of messages that need to becommunicated, as instead of sending a 6 n × n matrix, T , S , and V can be sent individually, which is only 6 n × Landmark Measurement Update
The decoupling of thelandmark measurement update equations follows in asimilar way to the previous section, givingΣ + = ( I n + Σ Q ) -1 Σ . (90)Calculating Σ Q only requires Σ ki , k ∈ N to be known.Thus robot i can perform the matrix inversion locally, andthen communicate the required information for all otherrobots to update their state and respective componentsof Σ + . Similarly, Θ can be calculated by robot i anddistributed to each robot to perform the update of thestate estimate locally byˆ X + i = ˆ X i Θ i ∀ i ∈ N. (91)If we perform an SVD of Σ Q in a similar way to (87), wenote that rank( Q ) ≤ n × n ×
12 + 6.5. SIMULATIONSWe demonstrate the performance of the resulting filterin two Python simulations. The first considers the case Code is available at jackhenderson.com.au
Fig. 1. Average translation error across all robots for 2-DScenario. Best viewed in colour.where the robots’ poses are constrained to a 2-D plane,such as in the case of a network of ground based robots,while the second scenario considers the more general caseof 3-D trajectories.
We consider a network of n = 4 robots moving along circu-lar trajectories within an approximately 20m ×
20m area.There are r = 4 landmarks in the environment and eachrobot is only able to take measurements of one distinctlandmark at a rate of 10 Hz. Velocity measurements areavailable to each robot at a rate of 100 Hz. Robots canobserve only a single other distinct robot at a rate of 5 Hzbut can communicate freely to all. To be specific, Robot 1can observe Robot 2, R2 can observe R3, R3 can observeR4, and R4 can observe R1. The sensor properties aredefined as B = 0 . I , C = 0 . I , D = 0 . I , (92) (cid:15) ∼ N ( , I ) , δ ∼ N ( , I ) , η ∼ N ( , I ) . (93)Note, the sensor errors are constrained appropriately inthe 2-D case.We implement three filters, the centralised GAME fil-ter described in Section 4.1, the decoupled GAME filterdescribed in Section 4.2, and the collaborative GAMEfilter proposed by Zamani and Hunjet (2019), using theCovariance Intersection estimation method. The averagetranslation error over all robots is shown in Figure 1.We can observe that the centralised GAME filter and thedecoupled GAME filter provide identical state estimates,demonstrating that there is no loss of information whenthe centralised filter is decoupled. Our filter is able to accu-rately localise the network of robots from an initial averagetranslation error of 1.8m down to a long term average of0.08m. This simulation also highlights a weakness in thefilter from Zamani and Hunjet (2019) which stems fromthe asymmetry of the robot observations. As it does notshare the information gained from measurements to otherrobots, the filter is not able to accurately localise and itdiverges after approximately 15 seconds. We present a different scenario to highlight the differencein filter performance even when measurements are readilyavailable to all robots. We again consider a network of n =ig. 2. Average translation error across all robots for 3-DScenario. Best viewed in colour.4 robots with r = 4 fixed landmarks in the environment.Velocity measurements are available to each robot at arate of 100 Hz. In contrast to the previous scenario, eachrobot can observe all 4 landmarks at a rate of 10 Hz, andcan observe all other robots at a rate of 10 Hz. Robotsmove with continuously changing random velocities inan approximately 20m × ×
20m volume. The sensorproperties are the same as defined in (92) and (93). Theaverage translation error of the three different filters isshown in Figure 2. The covariance intersection method inthe Zamani and Hunjet (2019) filter was coarsely tuned toa value of ω = 0 . , 1412–1417. Howard, A., Mataric, M., and Sukhatme, G. (2003).Putting the ‘I’ in ‘team’: An ego-centric approach tocooperative localization. In , 868–874.Luft, L., Schubert, T., Roumeliotis, S.I., and Burgard, W.(2018). Recursive decentralized localization for multi-robot systems with asynchronous pairwise communica-tion. The International Journal of Robotics Research ,37(10), 1152–1167.Markley, F.L. (2003). Attitude Error Representations forKalman Filtering.
Journal of Guidance, Control, andDynamics , 26(2), 311–317.Mortensen, R.E. (1968). Maximum-likelihood recursivenonlinear filtering.
Journal of Optimization Theory andApplications , 2(6), 386–394.Roumeliotis, S. and Bekey, G. (2002). Distributed multi-robot localization.
IEEE Transactions on Robotics andAutomation , 18(5), 781–795.Zamani, M. and Hunjet, R. (2019). Collaborative PoseFiltering Using Relative Measurements and Communi-cations. In ,919–924.Zamani, M. and Trumpf, J. (2019). Discrete updatepose filter on the special Euclidean group SE(3). In
Proceedings of the 55th IEEE Conference on Decisionand Control (CDC) .Zamani, M., Trumpf, J., and Mahony, R. (2013).Minimum-Energy Filtering for Attitude Estimation.