Relative Pose Estimation using Range-only Measurements with Large Initial Uncertainty Anusna Chakraborty1 Kevin Brink2 Rajnikant Sharma1 Laith Sahawneh3
Abstract—In this paper, a relative frame localization problem is addressed for pairs of vehicles with significant initial pose uncertainties. A relative frame Extended Kalman filter (EKF) is developed for the case where vehicles share odometry (bodyframe delta position values), and are capable of getting intervehicle range measurements. The extended Kalman filter is not robust to large initial errors in this nonlinear system, therefore a Multi-Hypothesis approach is used to accommodate the unknown, up to approximate range, initial relative pose (relative position and orientation). This formulation provides a lightweight and robust relative initialization approach that identifies an accurate relative pose and would be appropriate for relative guidance, initialization of more complex (multivehicle) navigation approaches, or shared, synthetic aperturelike measurement processing. Results are presented for both simulated and hardware implementations of the filter.
I. INTRODUCTION Unmanned Vehicles (UVs) have several civilian and military applications. In such missions, it has been shown that instead of a single vehicle, it is advantageous to use multiple vehicles as it increases robustness, sensor coverage, sensor information, and efficiency and these benefits apply to several applications such as target tracking, border surveillance, aerial refueling [1], [2], docking [3], [4]. In order to deploy a team of UVs, the pose (position and heading) information is often necessary in absolute/inertial frame [5] for the team to move from one point to another. This information is usually available from Global Positioning System (GPS). However, in hostile environments, GPS signals may be jammed [6] or in urban areas due to congestion of buildings, the GPS signals may be very unreliable [7] However, in multi-agent coordination and control, the absolute vehicle pose is not necessarily critical. While a decent understanding of global pose may be important, in many applications, it is the relative position and orientation of the vehicles which is required for execution of coordinated actions. The relative position estimation problem has been solved using various techniques like EKF in both centralized form [8] and distributed form [9]. And relative localization has been used for swarm control [10], target tracking [11] and target localization [12] applications. Generally the estimation approaches used in these applications assume accurate a priori information regarding relative or global pose. This paper does 1 Anusna Chakraborty, and Rajnikant Sharma are with Aerospace Engineering and Engineering Mechanics department, University of Cincinnati
[email protected] 2 Kevin Brink is a Senior Research Engineer with the Air Force Research Laboratory, Eglin Air Force Base, Florida
[email protected] 3 Laith Sahawneh was with Mechanical and Aerospace Engineering department, University of Florida during development and is currently with Delphi Automotive PLC
[email protected]
not make this assumption and instead focuses on the estimation of relative position and orientation between cooperative vehicles even in cases when they have very poor a priori information. In a recent work, Bai and Beard [13] have used relative position measurements between cooperative UVs to address the target hand-off problem and the system has been shown to be observable for certain trajectories. That work assumed relative position measurements (px and py ) or equivalently a simultaneous range and bearing measurement. However, in this work we only assume access to inter-vehicle range from a source such as radios with time of flight capabilities, or even Received Signal Strength Indicator (RSSI) signals [14]. Additionally, the current literature generally assumes that the initial pose of the vehicles is well known, at least as a percentage of range between agents, and often within a few metres when initializing cooperative systems. For a number of reason, this may not always be the case; vehicles which “meet” after traveling for long distances without any access to GPS or landmarks, long communication outages, or cases where a reinitializion is needed due to prior failure. Arulampalam et al. discussed a modified version of the EKF in [15] designed to accommodate large initial uncertainties in the initial states. Assuming available inter-agent bearing measurements, polar coordinates were used with a bank of filters with multiple initial range estimates to cover the uncertainty in range. In this case a weighing formula was derived to assign weights to each of the filters and the estimated states at each point was a weighted sum of the estimates from all other filters. This weighted sum approach does not lend itself to the system assumptions in this paper, namely measured range and large uncertainties in relative bearing, but the bank of filters approach does provide inspiration. In this paper, we estimate relative pose (px , py and δψ) between vehicles assuming that vehicles have accurate intervehicle ranging capability and that they are able to communicate velocity and heading rate estimates without any data loss, but otherwise have no initial pose information. The assumption of good communication is limiting in practice, however, if EKF implementations are found to be effective under these assumptions then it is reasonable to believe that data dropouts can be handled using batch processing, such as a modified GTSAM [16] or G20 [17] if necessary. Using the initial range measurement, a bank of EKFs is initialized across the full 0 to 2π uncertainty range in the initial relative bearing. For simplicity, the paper initializes the relative heading with shared global heading measurements (from a source such as a magnetometer) which are noisy, but provide some bounding. If needed, the relative heading could also be sampled in the same fashion as the relative bearing shown
N
ψ1
v1 E
ρ21
px py δψ
N v2 ψ2
vˆi = vi +µv , and ωˆi = ωi +µω , where µv and µω are the zero mean Gaussian noise with σv and σω as standard deviation. The vehicles share vˆi and ωˆi and use an inter-vehicle range measurement, ρ˜ in order to estimate the relative pose of the other vehicle in their body frame. The pose includes [px , py , δψ]> , the relative x and y positions, and the relative heading of the vehicles. When attempting to estimate vehicle 1’s pose in vehicle 2’s frame, we have " # " #" #
η21 E
Fig. 1: Relative geometry between two vehicles later in this paper as global heading measurements are not used outside of initialization. After seeing the bank of filters, all hypothesis are then propagated using shared velocity and heading rate, and then updated using the range measurements and a simple metric is then used to identify the best performing filter. If initial uncertianties are properly sampled, the “best” filter provides an accurate and consistent estimates of the relative pose of the vehicles. Our contributions in this paper are listed as: • Development of a lightweight filter using range to determine relative pose (relative position and heading) between vehicles. • An initialization approach using a bank of filters which does not require a priori information on the relative bearing between vehicles (accommodating large global position uncertainty). • The identification of a simple metric to identify the accurate and consistent filters within the filter. Rest of the paper is organized as follows: The relative pose estimation problem with MHEKF and an algorithm is formulated in Section II and III followed by the results and discussion in Section IV and then the conclusion in Section V. II. P ROBLEM F ORMULATION Consider two vehicles moving in a horizontal plane as shown in Fig 1. Each vehicle is moving with velocity vi and heading angle ψi , i = 1, 2 where, η21 is the relative bearing of vehicle 1 with respect to vehicle 2 and ρ21 is the range between vehicles. Equations of motion for the ith vehicle can be written as pn˙ i vi cψi p˙ei = vi sψi , (1) ωi ψ˙i where, c[·] := cos(·) and s[·] := sin(·). [pni , pei ]> is the position vector in the inertial coordinate (NED) frame, ψi is the heading angle, and ωi is the turn rate. vˆi and ωˆi are estimated values from the individual vehicles navigation systems. For this, note that the estimates, vˆi and ωˆi , are modeled as the summation of true values and Gaussian noise,
=
cψ2 −sψ2 0
sψ2 cψ2 0
0 0 1
p n1 − p n2 pe1 − pe2 ψ1 − ψ2
.
(2)
Similar to [13], relative equations of motion in Vehicle 2’s frame can be written as p˙x ω2 py + v1 cδψ − v2 p˙y = −ω2 px + v1 sδψ . (3) ˙ ω1 − ω2 δψ The motion model can be written from (3) as x˙ = f (x, u, σu ) ωˆ2 py + vˆ1 cδψ − vˆ2 = −ωˆ2 px + vˆ1 sδψ , ωˆ1 − ωˆ2
(4) (5)
where, x is the state, vˆi = vi + µv , and ωˆi = ωi + µωi . Again, we assume that there is an inter-vehicle range measurement ρ˜ which can be written as ρ˜ = h(x) + µρ = ρ + µρ q = p2x + p2y + µρ
(6) (7) (8)
where, µρ is zero mean Gaussian noise with σρ standard deviation. A single EKF approach for this system, with the assumed lack of initial relative bearing data, is far too nonlinear and very often results in divergence. In order to tackle this large initial uncertainty, we define a Multi-Hypothesis approach; instead of a single initial guess with high uncertainty, we initialize multiple filters with relatively tight initial covariance values. The goal is to initialize enough filters such that one or more filters do, in fact, have accurate initial guesses (with respect to their tighter covariance bounds) and result in accurate and consistent estimates as the system propagates. It should be noted that the number of filters required will be system dependent, changing with quality of the odom-like inputs and ranging accuracy. III. M ULTI -H YPOTHESIS E XTENDED K ALMAN F ILTER Before explaining the Multi-Hypothesis filter, it is important to understand the functioning of a single EKF. A. Single Filter Here the individual EKF implementations are detailed following the development in [18]. The standard EKF propaga-
tion follows (9) and (10) where f (x, u) is the motion model from (4). (9)
and the input jacobian B, given as ∂f ∂u cδpsi = sδpsi 0
(12) −1 0 0
0 py 0 −px . 1 −1
(13)
One can then determine the system process noise Q using the relation Q = BQu B > which maps the process noise into the 3 states of interest. When inter-vehicular range measurements are available, we update the filter using the standard equations, x ˆ+ = x ˆ− + L(˜ ρ − ρˆ)
(14)
P + = (I − LH)P − ,
(15)
where, ρ˜ is the range measurement received, ρˆ is the inferred x,u) measurement from the filter, H = ∂h(ˆ is the measurement ∂x Jacobian and L is the Kalman gain. This EKF was found to be highly sensitive to nonlinearities related to the relative heading state, and even with very accurate initial estimates the filter is prone to divergence. Therefore a recent partial update technique [19] is applied to increase the robustness of the filter. The concept of the partial update is similar to a Schmidt-Kalman filter [20], but instead of holding problematic states/parameters fixed, it allows for those states to receive partial updates. This helps the otherwise divergent system respond “more linearly” than the full EKF otherwise would, while still estimating the states of interest. This approach increases the uncertainty levels the EKF can tolerate without diverging and was seen to be beneficial when applied to the relative heading states in this particular implementation of the EKF (reducing the number of hypothisis filters needed in the next section). In order to implement the partial update approach the EKF is implented normally, following (14) and (15). After applying the standard EKF updates, the relative heading state and associated covariance is remapped following the partial update equations, x ˆ++ = αxˆ3 − + (1 − α)xˆ3 + 3 ++ P33
2
−
2
(16) +
= α P33 + (1 − α )P33 .
Initial guesses True relative position Vehicle 2
Initial 2 bound
50
(10)
Here A = ∂f ∂x f (x, u). The process noise Q, used to propagate the covariance in (10), incorporates the effect of the input noises σv , and σω in the filter. To evaluate Q we start with the input uncertainties Qu , 2 σ v1 0 0 0 0 σv2 0 0 2 (11) Qu = 2 0 0 0 σω1 0 0 0 σω2 2
B=
100
(17)
Y (m)
Ts x ˆ =x ˆ +( ) f (x, u) NN Ts ) (AP + P AT + Q) P+ = P− + ( NN −
+
150
0
-50
-100
-150 -150
-100
-50
0
X (m)
50
100
150
Fig. 2: MHEKF initial guess values, associated 2 σ bounds, and true relative state values
Here, x3 and P33 is the relative heading state and it’s ++ associated covariance and x ˆ++ and P33 are the resulting, 3 partially updated values which are then used along with the rest of the entries of x ˆ+ and P + at the next propagation step. The weighing factor, α[0, 1], is related to how much of the full EKF update is applied. For this paper, α is set to 0.7, which effectively means that (1 − α) = 0.3, or 30% of the standard EKF update is applied to the heading state and at each measurement update, with the appropriate proportion of the covariance update also applied. With the partial update implemented, the relative pose EKF detailed in this section was seen to produce accurate, effective system estimates, when initialized with accurate initial estimates and reasonable uncertainty levels. However, with range as the only accurate system input, the EKF is still unable to accommodate the full uncertainty in the system and a multihypothesis approach is called for. B. Multi-Hypothesis filter Using ranging data only, we assume a full 2π relative bearing uncertainty (an uncertainty level the EKF from the prior section cannot accommodate). Therefore, a bank of initial guesses is placed on a narrow, circular path corresponding to the range measurement, while uniformly dividing the circle into Nη evenly spaced initial estimates using equations (18) and (19). An example of 36 such initial guesses can be seen in Fig 2 and 3. xi0 = ρcos(ηi ) y0i δψ0i
= ρsin(ηi ) = ψˆ2 − ψˆ1 ,
(18) (19) (20)
Again, we assume a relatively accurate global heading is known initially and is shared for each vehicle at the initial position. We do not use global heading directly to further propagate the relative heading, but instead communicate a heading rate and velocity, ωi and vi . This keeps the filter implementation fairly generic, and in the case were a rough global heading was not available, the same filter could be
Algorithm 1 Multi-Hypothesis Extended Kalman Filter using range-only measurements for Relative Pose Estimation Initial guesses True relative position Vehicle 2
30 Initial 2 bound
Y (m)
20
10
0
-10
-20
-30
70
80
90
100
X (m)
110
120
Fig. 3: MHEKF - Zoomed in plot of initial guess values, associated 2 σ bounds, and true relative state values applied with the relative heading sampled in a very similar manner to the bearing sampling used here. The angular separation between individual filter initial pose estimates is based on a couple of system factors. The aim is to have each filter with the largest covariance values such that the filter is still convergent when the initial guesses happen to be within two sigma, but the accuracy or limiting covariance bounds will be system dependent. This covariance bound and associated range drive the angular sample density for the initial guesses such that the 3σ covariance bounds overlap enough to guarantee the true system state is an appropriate match for at least one of the filter initial guesses. As the above system propagates, several of the filters will fail due to non-linearities associated with their poor initial guesses. However, other filters will track the system quite well, and we aim to choose the filter closest to the actual trajectory. To do this, we use a simple χ-squared inspired approach. We let ei = |ρ − ρˆ| and very simply, sum the number of times each filter’s range residual is below a threshold. Whenever it is below that threshold a count for that filter is incremented which means that larger the value of χi for the ith filter, the more the chances of that filter converging eventually. if |ei | ≤ cσρ , then χi = χi + 1
(21)
This identifies the filter with the largest χ value, and we use this as our “best” estimate moving forward. For the examples in this paper, a c value of 3 is used, although a smaller value was seen to more readily distinguish the top few filters, yet introducing more variance to the final selection. As demonstrated in Fig 2, a circular covariance for each initial pose was used. However, a tighter covariance could be applied leveraging the range measurement uncertainty if desired, likely resulting in rotated uncertainty ellipses for each xi0 and y0i . This can be valuable for both further constraining the individual filters and for more complex filter performance metrics which take both state and measurement uncertainties into account. An algorithm representing the combined approach of MultiHypothesis EKF with relative localization using range-only measurements is shown in Algorithm 1.
Initial inter-vehicle range = ρ0 . Set Nψ (number of different headings - in this paper it is 1) and Nη (number of different bearings) . for i in 1 - Nη do for j in 1 - Nψ do Initialize px , py and δψ based on (18)- (20) end for end for for k in 1 - Nη Nψ do (Prediction Step) for m in 1 - NN do (NN - number of predictions before a measurement update) s ) f (x, u) x ˆ− = x ˆ− + ( NTN ∂f A = ∂x f (x, u) s P− = P− + ( NTN ) (AP + P AT + Q) end for if ρ ≥ (some small value) then H = ∂h ∂x (as given in (6)) L = P − H T (σρ + HP − H T )−1 x ˆ+ = x ˆ− + L(˜ ρ − ρˆ) + P = (I − LH)P − (Partial Heading Update) − + x ˆ+ 3 = αxˆ3 + (1 − α)xˆ3 + + 2 − 2 P33 = α P33 + (1 − α )P33 end if e˙i = |ρ − ρˆ| if |e˙i | ≤ cσρ then χi = χi + 1 end if end for P Count = χi . Choose best filter using max(χi ) (as given in (21))
IV. R ESULTS A. Simulation Results 1) Using a Single Filter: For the simulation results, we consider two vehicles following random trajectories and vehicle 2 estimates the pose of vehicle 1. Fig 4 shows the true trajectories of the vehicles. These trajectories are used for each simulation example result shown in this section. First, we show the effects of very large initial uncertainty on the system and validate the need for a Multi-Hypothesis approach. As expected, when the filter has very large initial uncertainties, the filter can quickly diverge as shown in Figs 5 and 6. The simulation parameters used are provided in Table I. From Fig 6, it is clear that the relative EKF cannot handle such large uncertainy. Therefore, several initial guesses are required to provide the filter (at least one) with accurate enough initial conditions to result in an accurate, non-divergent filter. 2) Using Multi-Hypothesis EKF: Using the MultiHypothesis formulation from Section III, we run a batch of filters for a certain period of time and choose the best filters based on the elimination criteria previously discussed to overcome the large initial uncertainty.
Parameter Initial pose uncertainty Initial range measurement Range uncertainty Odometry uncertainty Update rate Total number of filters Distance between samples
Single Filter - Simulation [100 m, 100 m, 3 ◦ ] 100 m 1m [0.1 m/s, 0.01 rad/s] 10 Hz 1 N/A
MHEKF - Simulation [10 m, 10 m, 3 ◦ ] 100 m 1m [0.1 m/s, 0.01 rad/s] 10 Hz 36 17.4 m
MHEKF - Hardware [0.01 m, 0.01 m, 0.03 ◦ ] 1m 0.01 m [0.01 m/s, 0.001 rad/s] 50 Hz 36 0.1744 m
err (deg) py err(m) px err (m)
TABLE I: Simulation parameters used for Single filter, MHEKF in simulation or in hardware.
200
Vehicle 1 - True position Vehicle 2 - True Position
450 400
0
-200
350
0
10
20
30
40
50
60
70
80
N (m)
300 250
90
100
rel y-pos err +3 -3
500
rel x-pos err +3 -3
0
-500
200 150 100 50
0
10
20
30
100
200
300
60
70
80
90
100
0
-50 0
0
50
rel heading error +3 -3
10
20
30
40
50
60
70
80
90
100
time (s)
0 -100
40
50
400
E (m)
Fig. 6: Errors with 3σ for Large Initial Uncertainty.
Fig. 4: True trajectories for both the vehicles,trajectories start at bottom and move North
px (m)
200
0
-200 0
10
20
30
40
50
60
70
80
90
100
py (m)
200 100 True rel x-pos Est. rel x-pos
0
True rel y-pos Est. rel y-pos
-100
(deg)
0
10
20
30
40
50
60
70
80
90
100
100 50
Fig. 7: MHEKF - Pose errors from all 36 filters.
True rel heading Est. rel heading
0 -50 0
10
20
30
40
50
60
70
80
90
100
time (s)
Fig. 5: True vs Estimated pose for Large Initial Uncertainty.
The simulation parameters provided in I. The pose uncertainty in the list above is used to set the covariance values for every filter initialized in MHEKF. The same Multi-Hypothesis initialization from Section III was used here. The initial guesses displayed in Figs 2 and 3 are representative of the initial guesses used here. The error plots from the 36 filters are shown in Fig 7 and it is clear that most of the 36 hypotheses start out with very poor estimates. Others start out fairly well but are unable to handle the non-linearities and eventually diverge. However, there are few filters that both start well and continue to effectively estimate the relative pose. Fig 8 represents the cumulative sum of the χ-count for each filter over the entire run time. In the scenario shown 4 of the filters maintained a χ-count of 98%
χi acceptance percentage. The algorithm selects the filter with the highest χ-count as the “best” estimate. Fig 10 shows the results from the best estimate obtained from MHEKF and suggests the MHEKF is capable of generating and identifying an accurate relative pose estimate even in the case of significant initial uncertainty. B. Experimental Results For the hardware experiments, two Kobuki based Turtlebots [21] were used as shown in Fig 11. These are extremely robust platforms and provide enough space to place an Inteli5 powered notebook for all computational purposes. There were 8 infrared cameras in the motion capture (MOCAP) arena that provided us with the ground truth of the Turtlebots’ pose. Robot Operating System (ROS) [22], which is an open source software was used to connect to the two robots and also to the MOCAP system. A master-slave framework was established using ROS where the ground-station was the master and the
Cumulative sum for each filter
Cumulative sum plot for each filter based on -count
1000 900 800 700 600
Best Case
500 400 300 200
Worst cases
100 0 0
10
20
30
40
50
60
70
80
90
100
t(s) Fig. 8: MHEKF - Elimination Criteria based on χ-count
Fig. 10: MHEKF - Errors with 3σ for the best filter as identified my χmax .
px (m)
200
0
-200 0
10
20
30
40
50
60
70
80
90
100
0
10
20
30
40
50
60
70
80
90
100
0
10
20
30
40
50
60
70
80
py (m)
200 150 100 50
(deg)
100 50
True rel heading Est rel heading
0 -50
90
100
Fig. 11: Experimental Setup using 2 Turtlebot robots
t (s)
laptops on the Turtlebots were configured as slaves. This ensured that commands were sent at the same time. The range measurements were simulated from the MOCAP data and noise was added to it which was then fed into the filter. The parameters used for the hardware experiments for each filter initialization are listed in Table I. The two Turtlebots follow the same anti-clockwise circular trajectory as seen from Fig 12. Again, 36 filters are initialized and the highest χ-count is again selected, the results of which are displayed in Fig 14. The best filter is selected using the χcount metric and the resulting true and estimated trajectories follow each other very closely. Fig 13, and a reasonable covariance is achieved, Fig 14. It should be noted that there is a timing issue between estimates and the truth reference which is evident at the 40 second mark, however, the results clearly show the filter is tracking the relative frame quite well. From the results in both the simulation and hardware sections, the MHEKF approach appears to be an effective way to address the relative pose problem when faced with large initial uncertainties. This method provides a relative pose estimate which can then be used for guidance, shared sensing,
or to seed algorithms involving larger numbers of vehicles which could not otherwise tolerate such large relative pose uncertainties.
2
True Trajectory for Hardware Vehicle 1 Vehicle 2
1.5
1
Y (m)
Fig. 9: MHEKF - True vs Estimate pose plots for the best filter as identified my χmax .
0.5
0
-0.5
-1 -1.5
-1
-0.5
0
0.5
1
X (m)
Fig. 12: True Trajectory of Turtlebots - Both following anticlockwise trajectory, starting at top their respective circles and holding even speeds.
for a pair-wise case, which could then be used to seed the bigger multi-vehicle case as part of our future work. Through the simulation and experimental results, the approach was shown to produce and identify an accurate filter. R EFERENCES
Fig. 13: True (dotted line) and Estimated (solid line) x, y, ψ
Fig. 14: Errors (solid red line) with 3σ bounds (dotted blue lines)
C. Future Work Planned extensions of these results include leveraging observability analysis of the system to generate guidance and control commands designed to improve estimation quality. Further, investigating performance of batch filters when data drops and communication delays are present will also be explored. Finally, these results will be used to seed larger multiagent navigation systems to allow for large initial uncertainty in systems with three or more vehicles. V. C ONCLUSION In this paper, we have developed a relative pose estimation algorithm which can estimate the pose of a vehicle in its own body frame. This algorithm does not require any a priori data beyond an initial range measurement, then relies on range and shared odometry data. A Multi-Hypothesis approach is leveraged to sufficiently sample the relative pose space, and a simple evaluation heuristic is applied to identify the best filter
[1] J. P. Nalepka and J. L. Hinchman, “Automated aerial refueling: extending the effectiveness of unmanned air vehicles,” in AIAA Modeling and Simulation Technologies Conference and Exhibit, 2005, pp. 15–18. [2] P. Stevens and S. Sinha, “Autonomous system for the aerial refueling or decontamination of unmanned airborne vehicles,” Aug. 12 2003, uS Patent 6,604,711. [3] J. Kimmett, J. Valasek, and J. L. Junkins, “Autonomous aerial refueling utilizing a vision based navigation system,” in Proceedings of the 2002 AIAA GNC Conference, paper, vol. 5569, 2002. [4] P. T. Wingett, S. K. Brault, and C. C. Potter, “Unmanned underwater vehicle docking station coupling system and method,” Feb. 21 2006, uS Patent 7,000,560. [5] W.-W. Kao, “Integration of gps and dead-reckoning navigation systems,” in Vehicle Navigation and Information Systems Conference, 1991, vol. 2. IEEE, 1991, pp. 635–643. [6] B. Iyidir and Y. Ozkazanc, “Jamming of gps receivers,” in Signal Processing and Communications Applications Conference, 2004. Proceedings of the IEEE 12th. IEEE, 2004, pp. 747–750. [7] P. D. Groves, “Shadow matching: A new gnss positioning technique for urban canyons,” The journal of Navigation, vol. 64, no. 3, pp. 417–430, 2011. [8] A. Martinelli, F. Pont, and R. Siegwart, “Multi-robot localization using relative observations,” in Proceedings of the 2005 IEEE international conference on robotics and automation. IEEE, 2005, pp. 2797–2802. [9] D. Moore, J. Leonard, D. Rus, and S. Teller, “Robust distributed network localization with noisy range measurements,” in Proceedings of the 2nd international conference on Embedded networked sensor systems. ACM, 2004, pp. 50–61. [10] M. Saska, J. Vakula, and L. Pˇreu´cil, “Swarms of micro aerial vehicles stabilized under a visual relative localization,” in Robotics and Automation (ICRA), 2014 IEEE International Conference on. IEEE, 2014, pp. 3570–3575. [11] J. J. Leonard and H. F. Durrant-Whyte, “Mobile robot localization by tracking geometric beacons,” IEEE Transactions on robotics and Automation, vol. 7, no. 3, pp. 376–382, 1991. [12] Y. Zou and K. Chakrabarty, “Sensor deployment and target localization in distributed sensor networks,” ACM Transactions on Embedded Computing Systems (TECS), vol. 3, no. 1, pp. 61–91, 2004. [13] H. Bai and R. W. Beard, “Relative heading estimation for target handoff in gps-denied environments,” in 2016 American Control Conference (ACC). IEEE, 2016, pp. 336–341. [14] L. R. Sahawneh and K. M. Brink, “Factor graphs-based multi-robot cooperative localization: A study of shared information influence on optimization accuracy and consistency.” [15] S. Arulampalam and B. Ristic, “Comparison of the particle filter with range parameterized and modified polar ekfs for angle-only tracking,” in Proc. Spie, vol. 4048, 2000, pp. 288–299. [16] F. Dellaert, “Factor graphs and gtsam: A hands-on introduction,” Georgia Institute of Technology, Tech. Rep., 2012. [17] A. Ahmad, G. D. Tipaldi, P. Lima, and W. Burgard, “Cooperative robot localization and target tracking based on least squares minimization,” in Robotics and Automation (ICRA), 2013 IEEE International Conference on. IEEE, 2013, pp. 5696–5701. [18] R. W. Beard and T. W. McLain, Small unmanned aircraft: Theory and practice. Princeton university press, 2012. [19] K. M. Brink, “Partial-update schmidt–kalman filter,” Journal of Guidance, Control, and Dynamics, 2017. [20] S. J. Julier and J. K. Uhlmann, “New extension of the kalman filter to nonlinear systems,” in Signal processing, sensor fusion, and target recognition VI, vol. 3068. International Society for Optics and Photonics, 1997, pp. 182–194. [21] “TURTLEBOT 2 COMPLETE by clearpath robotics,” https://store.clearpathrobotics.com/products/turtlebot-2, accessed: 2017-09-20. [22] “ROS,” http://www.ros.org/, accessed: 2017-09-20.
