Distributed iterative multimodal sensor fusion method for improved collaborative localization and navigation
A computer implemented method for fusing position and range measurements to determine the position of at least one node of a plurality of distributed nodes is disclosed. The method includes (a) measuring the position of at least one node; (b) computing an estimate of the position of the at least one node based on the measured position using a filter that takes account of past estimates of position; (c) receiving an estimate of position of at least a second node; (d) measuring inter-node range to the at least a second node; (e) combining the measured inter-node range with the estimate of position of at least a second node using a second filter that takes account of past estimates of position to generate a refined estimate of the position of the at least one node; and (f) when a change in the position of the at least one node is above a predetermined threshold value, setting the refined estimate of the position of the at least one node to the estimate of the position of the at least one node and repeating (c), (e), and (f).
1. A computer implemented method for fusing position and range measurements to determine the position of at least one node of a plurality of distributed nodes, comprising the steps of:
(a) measuring the position of at least one node,
(b) computing an estimate of the position of the at least one node based on the measured position using a filter that takes account of past estimates of position;
(c) receiving an estimate of position of at least a second node;
(d) measuring inter-node range to the at least a second node;
(e) combining the measured inter-node range with the estimate of position of at least a second node using a second filter that takes account of past estimates of position to generate a refined estimate of the position of the at least one node, and
(f) when a change in the position of the at least one node is above a predetermined threshold value, setting the refined estimate of the position of the at least one node to the estimate of the position of the at least one node and repeating steps (c), (e), and (f).
2. The method of claim 1 , further comprising the step of:
(g) estimating an initial position of the at least one node before step (a).
3. The method of claim 2 , wherein steps (b), (e), and (g) further comprise computing a measure of confidence in an estimate of position of the at least one node.
4. The method of claim 3 , wherein the measure of confidence is a covariance.
5. The method of claim 3 , wherein the measure of confidence is used as a weighting factor to combine current and previous position measurements of the at least one node and the at least a second node with past position measurements to arrive at the refined estimate of the position of the at least one node.
6. The method of claim 1 , wherein step (c) further comprises the step of sending the estimate of the position of the at least one node to the at least a second node.
7. The method of claim 1 , wherein step (f) further comprises the step of sending the refined estimate of the position of the at least one node to the at least a second node during the repetition of step (c).
8. The method of claim 1 , wherein the filters in steps (c) and (e) constrain noisy (local) position measurements with inter-node constraints.
9. The method of claim 1 , wherein the filter that takes account of past estimates of position in step (c) is a Kalman Filter.
10. The method of claim 1 , wherein the second filter that takes account of past estimates of position in step (e) is an Extended Kalman Filter.
11. The method of claim 1 , wherein the change in the position of the at least one node in step (f) is a change in distance between the refined estimate of the position of the at least one node and the estimate of the position of the at least one node.
12. The method of claim 1 , wherein step (f) terminates after a predetermined number of iterations.
13. The method of claim 1 , wherein the number of neighboring nodes in the plurality of distributed nodes changes dynamically.
14. An apparatus for fusing position and range measurements to determine the position of at least one node of a plurality of distributed nodes, comprising:
a local position measurement device for (a) measuring the position of the at least one node;
an inter-node data communication device for (b) receiving an estimate of position of at least a second node;
an inter-node measurement device (c) measuring inter-node range to the at least the second node; and
at least one processor configured for:
(d) computing an estimate of the position of the at least one node based on the measured position using a filter that takes account of past estimates of position;
(e) combining the measured inter-node range with the estimate of position of at least a second node using, a second filter that takes account of past estimates of position to generate a refined estimate of the position of the at least one node, and
(f) when a change in the position of the at least one node is above a predetermined threshold value, setting the refined estimate of the position of the at least one node to the estimate of the position of the at least one node and repeating steps (c), (e), and (f).
15. The apparatus of claim 14 , wherein the at least one processor is further configured for estimating an initial position of the at least one node before the local position measurement device measures the position of the at least one node.
16. The apparatus of claim 14 , wherein the local position measurement device is an internal navigation system (INU).
17. The apparatus of claim 14 , wherein the local position measurement device comprises at least one video camera working in conjunction with at least one the processor for measuring position visually.
18. The apparatus of claim 14 , wherein the local position measurement device is Doppler radar.
19. The apparatus of claim 14 , wherein the inter-node measurements device and the inter-node data communication device comprise an RF ranging device.
20. The apparatus of claim 14 , wherein the RF ranging device comprises:
radar working in conjunction with the at least one processor;
a radio transceiver; and
at least one antenna.
21. The apparatus of claim 14 , wherein the inter-node measurement device is a radio that measures the round trip time of flight of a radio signal exchanged between two radios.
22. The apparatus of claim 14 , wherein the at least one processor is further configured for computing a measure of confidence in an estimate of position of the at least one node.
23. The apparatus of claim 14 , wherein the inter-node data communication device is further configured for sending the second estimate of the position of the at least one node to the at least a second node.
24. The apparatus of claim 14 , wherein the inter-node data communication device is further configured for sending the refined estimate of the position of the at least one node to the at least a second node during the repetition of (b).
25. The apparatus of claim 14 , wherein the filter that takes account of past estimates of position in (d) is a Kalman Filter.
26. The apparatus of claim 14 , wherein the second filter that takes account of past estimates of position in (e) is an Extended Kalman Filter.
27. The apparatus of claim 14 , wherein the change in the position of the at least one node in (e) is a change in distance between the refined estimate of position and the second estimate of position.
28. The apparatus of claim 14 , wherein (f) terminates after a predetermined number of iterations.