Compensation Algorithm Based on Estimation State for Transmission Delay in Cooperative Localization＊

Wang Leigang（王雷钢），Zhang Tao（张涛），Yang Weifeng（杨伟锋）

1.Department of Automation，Tsinghua University，Beijing，100084，P.R.China；2.Luoyang Electronic Equipment Test Center of China，Luoyang，471003，P.R.China

1 Introduction

In the cooperative localization（CL），by integrating and sharing the absolute measurements（e.g.，GPS，landmark，etc.）and the relative measurement（distance／bearing），the accuracy and reliability of the estimation position of each agent in a group are improved drastically［1－2］.The advantage of the CL stems from the individual measurement information can be shared among the members of a group［3－4］.However，this advantage comes at the cost of increased computation and communication requirements［5］.For example，the agents equipped with high precision position device or luckily receiving GPS need to broadcast their own state information to other agents in a timely and accurate communication manner.

When there exists the communication link，the transmission delay is inevitable for the environment or information processing.Based on Kalman filter，there is some research on this is－sue［6－7］，such as extrapolation of measurements［8］，re－organized innovation［9］，state augmented and so on.In Ref.［10］，by establishing a delay，state－space model for state prediction and delay information is fed back to the model.In Ref.［11］，the history state parameters are reserved for observation update，by which the predicted estimation state is acquired and then used to substitute the present state estimation.While the relevance of the information is not considered and the available information is thus utilized incompletely.A delayed extended Kalman filter（DEKF）is designed by state reverse in Ref.［12］，where the relevance of the information is considered.But big storage space is consumed and the estimation is conservative.In Ref.［13］，based on the path prediction of master unmanned underwater vehicles（UUV），an unequal interval filtering algorithm without state reverse is proposed.However，the state of the master UUV need to be propagated in each slave UUV.Though it is suitable for leader－follower cooperative structure，it does not fit for the distributed structure in which all the agents are identical in a group.

2 Related Models

Let us consider a group of n mobile agents which is moving in two－dimension（2D）.And the proprioceptive sensors（e.g.，wheel encoders），exteroceptive sensors（e.g.，laser rangefinder，radio ranging）and GPS receiver are mounted on each agent and all of these sensors are homogeneous.The proprioceptive sensors measure the self motion of the agent and monitor the environment for localization features.And the exteroceptive sensors detect and identify other agents moving in its vicinity and measure their respective distance.The objective of CL is to combine these measurements with the equation of motion to obtain more accurate position estimation.

（1）Agent motion model

The ith agent equation of motion is as follows［14］

where xi＝ ［xi，yi，ψi］∈R3×1is the agent state，including agent location（xi，yi）and agent headingψi.It is assumed that onboard introspective sensors measure the linear speed Viand angular speedωiof the agent.Each agent has a GPS receiver and an exteroceptive sensor to measure agent－to－agent distance.

（2）Measurement model

where kis the agent index.¯Vi，¯ωi，¯xiare the true values.zi，zijare the absolute measurements and the relative measurements.Suppose thatεi，ζi，wi，vijare subject to the standard normal distribution.For normal distribution，there are moment parameter and information parameter two expressions.In this paper，the former，namely，the mean and covariance｛＾X，P｝are adopted.

For the nonlinear system，the extended Kalman filter（EKF）is used to estimate the moment parameters.By Taylor series expansion，the approximate transition matrixΦ，the control matrix B，and the observation matrix Hare acquired as follows

And thus state propagation is given as

where Tsis the simulation cycle and u＝V，［ ω］Tand＾xthe estimated state vector.~x and~z denote the prediction state and observation，respectively.

Kalman filter estimation includes propagation and observation update cycles.In the perception cycle，based on the evolution of the system equations，the measured control inputs and the statistical description of the system noise，the knowledge about the system state is propagated to the next step.In the observation update cycle，where relative pose measurements are processed to update the propagated estimates calculated during the previous cycle.In the CL，most of the time the agents propagate their position and covariance estimation based on their own perception，while the observation updates are usually rare，and they take place only when the measurements occur［3］.

3 Compensation Based on Estimation State

The transmission delay issue caused by poor communication in CL is focused in this paper.Assume that the agent i gets the GPS or the relative measurements to other agent at time step k and these measurements are processed and packed.Consequently，the information packet Ci（k）is formed and then repeatedly broadcasts until it is received by the others in the group or a new measurement occurs.If Ci（k）can be received by other agents immediately，the observation update will be done in the receiver as usual［3－4］.How ever，in sheltered areas such as buildings，forests，valleys or a variety of hostile jamming environment，the information packets Ci（k）may be delayed for some time before being received by the agent j.If the available Ci（k）at time step k＋m is discarded and not used to the present state updates of agent j，it will be a great waste.The primary contribution of the proposed algorithm is that the k－moment observation information of the agents i can be used to update the state of agent j at time step k＋m.And the compensation manoeuvre is based on the estimated state rather than extrapolating or re－organizing the observed information as mentioned above.

The distribution algorithm is the foundation in the CL.And a large body of work has been done in the recent past.Since other agent′s position is used for new position estimation，the correlation between them arises naturally.The quantitative representation of the correlation is a covariance term，which can be regarded as an indicator about how much information has been shared by different agents.And the key of distributed implement is to assign the correlation to different agents and keep them update，such as singular value decomposition（SVD）［3］，augmented information filter（AIF）［4］，covariance intersection（CI）algorithm［15－16］and so on.

In order to implement the distributed computing of the covariance term，in this paper a bank of propagation calculators（i＝1，…，n）are established in each agent.When the first relative measurement occurs between the master and slave agents（the master can detect other agents and receive the state estimation information from others and the slave is just opposite.This relationship is relative and it will determine whether original base cells should be set to identity matrix or not in an observation update cycle），a new covariance is calculated in the master agent and then retained in it as the original base cells of propagation calculators.Accordingly，in the slave agent，the counterpart is set to identity matrix automatically.In a time update cycle，all these original base cells in propagation calculators are used for update of covariance factors，and only its own state transition matrix is required.In the observation update cycle，as new original base cells，the new covariance matrix and identity matrix are re－assigned to the master and slave agents respectively.In the time update cycle，covariance factors are dependently propagated in each agent.By this method，not only the independence is guaranteed and the covariance update is maintained，but also the error propagation of matrix decomposition and calculation consumption is avoided.The above－mentioned method is used as basis for delay effect analysis in the following.

3.1 No transmission delay

In this case，firstly，the following assumptions are stated：

（1）The observed information is available instantaneously and the agent j receives the information packet Ci（k）（at least including vi，Hi，Si，，where the innovation is vi＝zi（k）－hi（（k）），and the covariance of innovation is Si＝Hi（k）（k）（Hi（k））T＋Ri）from agent i.

（2）The agent jis the master and the agent i is the slave during the last progress of transmission，and before update the moment parameters of agent i and j are（k），（k），x（k），（k），respectively.

And the covariance factors of them are thus updated as Eqs.（5－8）.

Agent i

Agent j

Based on Eqs.（5－8），in the following time update phrase，the covariance factors of agents are updated respectively.At time step k＋m，the covariance factors are updated as

whereΓi（k，m）＝Φi（k＋m）…Φi（k＋1）is defined as the state transition matrix impacting factor.

Considering Eqs.（6，8，9，10），the state of agent i and the covariance between agents i and j are obtained as

3.2 mtransmission delay cycles

In this case，the agent j gets the information packet Ci（k）from agent i at time step k＋mnot k.So in the agent j，the time update is still going on at time step k.（In principle，the observation update should be done at this time.）In the following time update phrase，the role of the agent i is still the master relative to agent j.However，under normal communication，the master－slave relationship between agents i and j should be changed at time step k.Therefore，this relationship is erroneously remained for mtransmission delay cycles.At time step k＋m，the covariance（k＋m）（k＋m）are updated as

综上所述，各分类方案的优先排序为：方案一＞方案三＞方案二，即方案一（基于有害垃圾单独投放的干湿（易腐垃圾、其他垃圾） 两分法）为现阶段最适合于海口市居民区的生活垃圾分类投放方案。

Agent i

Agent j

Contrasting Eqs.（11，15），it obtains the difference quantity of estimated state parameters between no delay and mdelay cycles as follows

Notice that in the estimation ofthe knowledge of~Pji（k）is required.Assume that the state transition matrix impacting factorΓis known.And then the covariance factorP（k）in agent jat time step kcan be deduced by Eq.（14）.Furthermore，the prediction covariance（k）is acquired.So how to getΓbecomes critical for the issue at hand.From the original form ofΓ，it needs historical state transition matrix to be multiplied continuously.As these can be prohibitively expensive in calculation and storage，it is motivated to pursue a reduced－complexity equivalent expression that is introduced next.

Since

Then

The equivalent expression of the state transition matrix impacting factorΓis shown in Eq.（17）.The expression form is transformed from multiplication to addition，which has two advantages.On the one hand，the consumption of matrix multiplication is saved.On the other，only measured velocity，angular velocity are needed for Γ.It is not necessary to record all the state transition matrix from the previous time of Ci（k）arrival to now.

The present covariance can be acquired by Eqs.（13，14）as follows

Contrasting Eqs.（12，18），the difference of the covariance between no transmission delay and mdelay cycles is obtained as

Notice that（where kpdenotes the time when the last observation occurs before step kand（kp）is derived from the covariance matrix decomposition）and＝（）Andkis thus in－vertible matrix and Eq.（19）is valid.

By far，both the state and covariance compensation amount can be obtained by Eqs.（16，19）.The present estimated state can be updated byΔ~xandΔPaccordingly.The state and covariance factor of agents i and j are updated respectively as follows

Agent i

Agent j

In summary，the algorithm flow on CL is described as Fig.1considering the transmission delay.When the information packet arrives，it should be determined whether there exits time delay or not（each agent requires rigorously time synchronization）.If not，the state parameters and covariance factors are updated naturally.Otherwise，the compensation should be considered and then the compensation update is completed.

Fig.1 Flowchart of CL considering transmission delay

By the algorithm，there are several characteristics as follows：

（1）During each observation cycle as well as“delay”observation，the total communication cost per agent is reduced fromO（（n－1）（t2－t1）／Ts）to O（n－1）（where t1，t2denote the moments when last and present observation take place，respectively）.Even so，the relation among agents can be well－kept.

（2）The state transition matrix impacting factorΓin“delay”observation cycle can be obtained by addition rather than matrix multiplication.The computation cost per agent is O（2m）.

（3）Since the moment when the observation occurs is not available for the other receiver agents in advance，theoretically，all state informationΦ（k）should be recorded for later use.By Eq.（12），it shows that history velocity，angular velocity instead ofΦ（k）are enough forΓ.

4 Simulation Results

It is supposed that three robots are adopted in simulation，since the generalization to an arbitrary number of robots is straightforward and all of them move along different circles in 2D.The relative positions are shown in Fig.2.The starting positions are（0，－0.4），（1.5，－0.4），（0，1.9）.Robot 1is moving counter－clockwise.And Robot 2and Robot 3are moving clockwise.The simulation period is Ts＝0.4s.The odometry and radio measurements of linear and rotational velocity and agent－to－agent distance are corrupted by zero－mean Gaussian noise withσV＝0.05，σω＝0.1，σr＝0.001.It should be noted that since the performance of navigation unit（e.g.，accelerometer，gyroscope）is not considered in this paper，they are assumed to be ideal in drift herein and accumulated error caused by drift is omitted advisedly for the purpose of this paper.

In simulation，Agent 2receives GPS at certain intervals.Agents 1and 3cannot obtain GPS but they can detect Agent 2at some time.The scheme is designed as follows：

（1）The time series when Agent 2gets GPS are 0.4，3.6，…，19.6，with an interval of 3.2s（the simulation lasts 20s）.

Fig.2 Trajectories of three agents with delay compensation

（2）The time series when Agent 1detects Agent 2are 2，3.2，4.8，9.6，14.4.

（3）The time series when Agent 3detects Agent 2read 2.4，4，6，11.2，16.

It is supposed that at t＝6.8s（k＝18），the observation information of Agent 2cannot be received by Agent 1and Agent 3immediately.Until five cycles pass（m＝5），the observation is accepted by Agents 1and 3.

Fig.2shows the relative layout and the trajectories of three agents，where the red solid curve represents the revised trajectory by GPS and relative distance with transmission delay.Although Agents 1and 3receive the observation information from Agent 2with transmission delay，by the compensation the delay effect on position estimation can be eliminated.In Fig.3，the comparisons of the position error of Agent 1under no－delay，and delay with／without compensation are demonstrated.The black dashed curve represents the transmission delay，which is compensated by the compensation algorithm in this paper.It can be seen from Figs.3－5that the position estimation precision will be reduced（As in Fig.5，there is at least no increase error）if delay information can be utilized.By the proposed method，the estimation curve is drawn back to the circumstances with no delay once compensation occurs.Otherwise，it will continue to be the green curve.After compensation，the estimated position curve is almost in accord with that of no transmission delay.

Fig.3 Comparison of estimated position of Agent 1 with／without transmission delay

Fig.4 Comparison of estimated xcoordinate of Agent 1 with／without transmission delay

Fig.5 Comparison of estimated ycoordinate of Agent 1 with／without transmission delay

In order to verify the validity of the algo－rithm，three simulation modes are adopted in the simulation.In the first mode，the observation information from Agent 2at k＝18is received by others，which is a benchmark for the other two modes.The second mode is that the observation information cannot be received before the next new measurement.While the third mode is that the observation information is received after 3delay cycles pass（k＋m＝21）and then the receiver agents are compensated.The comparison results of the estimated state＾x，＾y，＾ψand the error of A－gent 3under no delay and delay with／without compensation are demonstrated in the Tables 1－3.By compensation，the error amount is reduced to about 10%.So the precision of the estimated state is improved dramatically due to the compen－sation.Moreover，the simulations also show that even though no relative measurement occurs among agents during time step k＝18—23，for relation which has been setup before，Agents 1and 3can benefit from the absolute measurement from Agent 2.

Table 1 Comparison of estimated＾xof Robot 3under three modes m

Table 2 Comparison of estimated＾yof Robot 3under three modes m

Table 3 Comparison of estimated＾ψof Robot 3under three modes m

5 Conclusions

The reliability of the algorithm in CL must be considered in poor communication scenario.The proposed method of state and covariance compensation algorithm for communication delay is useful，especially for the case where the observation information is subject to several times broadcasting and then is received by other agents.By using the method，the observation information is maximally utilized.The distinguishable feature of the algorithm is that it is based on the estimated state compensation rather than the observed information correction.However，it is assumed that the estimated moment parameters in the two different cases are approximate in a short period.Whereby，the state transition matrices at same moments in different cases are identical.But there are some differences in practice，which explains why the results in compensated states are not completely consistent with those of no transmission delay in the simulations.

［1］ Burgard W，Moors M，Fox D，et al.Collaborative multi－robot exploration［C］∥IEEE International Conference on Robotics and Automation.［S.l.］：IEEE，2000，1：476－481.

［2］ Nicosia J.Decentralized cooperative navigation for spacecraft［C］∥IEEE Aerospace Conference.［S.l.］：IEEE，2007：1－6.

［3］ Roumeliotis S I，Bekey G A.Distributed multirobot localization［J］.IEEE Transactions on Robotics and Automation，2002，18（5）：781－795.

［4］ Mu Hua.Decentralized algorithms of cooperative navigation for mobile platform［D］.Changsha：National University of Defense Technology，2010.（in Chinese）

［5］ Trawny N，Roumeliotis S I，Giannakis G B.Cooperative multi－robot localization under communication constraints［C］∥IEEE International Conference on Robotics and Automation.［S.l.］：IEEE，2009：4394－4400.

［6］ Wang Z D，Yang F W，Daniel W C Ho，et al.Robust H∞filtering for stochastic time－delay systems with missing measurements［J］.IEEE Transactions on Signal Processing，2006，54（7）：2579－2587.

［7］ Lu X，Xie L H，Zhang H S，et al.Robust Kalman filtering for discrete－time systems with measurement delay［J］.IEEE Transactions on Circuits and Systems，2007，54（6）：522－526.

［8］ Larsen T D，Andersen N A，Ravn O，et al.Incorporation of time delayed measurements in a discretetime Kalman filter［C］∥37th IEEE Conference on Decision and Control.Tampa，USA：［s.n.］，1998，4：3972－3977.

［9］ Lu X，Zhang H S，Wang W，et al.Kalman filtering for multiple time－delay systems［J］.Automatica，2005，41（8）：1455－1461.

［10］Chen H Y，Wang J D，Xu T.Flight delay state space model based on genetic EM algorithm［J］.Transactions of Nanjing University of Aeronautics and Astronautics，2011，28（3）：276－281.

［11］Maczka D K，Gadre A S，Stilwell D J.Implementation of a cooperative navigation algorithm on a platoon of autonomous underwater vehicles［C］∥IEEE International Conference on OCEANS.［S.l.］：IEEE，2007：1－6.

［12］Yao Y，Xu D，Yan W.Cooperative localization with communication delays for MAUVs［C］∥IEEE International Conference on Intelligent Computing and Intelligent Systems.［S.l.］：IEEE，2009，1：244－249.

［13］Yao Yao，Xu Demin.Cooperative localization of multiple UUVs with communication delays a real－time update method based on path prediction［J］.Robot，2011，33（2）：161－168.（in Chinese）

［14］Rajnikant Sharma.Bearing－only cooperative localization and path－planning of ground and aerial robots［D］.Provo，USA：Department of Electrical and Computer Engineering，Brigham Young University，2011.

［15］Chen L，Arambel P O，Mehra R K.Estimation under unknown correlation：Covariance intersection revisited［J］.IEEE Transactions on Automatic Control，2002，47（11）：1879－1882.

［16］Arambel P O，Rago C，Mehra R K.Covariance intersection algorithm for distributed spacecraft state estimation［C］∥IEEE Proceedings of the 2001A－merican Control Conference.［S.l.］：IEEE，2001，6：4398－4403.