DISTRIBUTED ITERATIVE MULTIMODAL SENSOR FUSION METHOD FOR IMPROVED COLLABORATIVE LOCALIZATION AND NAVIGATION

• US 20090212995A1
• Filed: 12/16/2008
• Published: 08/27/2009
• Est. Priority Date: 12/20/2007
• Status: Active Grant

• Current Assignee: SRI International, Inc.
• Original Assignee: SRI International, Inc.
• Inventors: Zhao, Tao, Mau, Suin-Chuon, Kaba, James, Wu, Shunguang
• Granted Patent: US 7,821,453 B2
• Time in Patent Office: Days
• US Class Current: 342/109
• CPC Class Codes: G01S 2013/466 by Trilateration, i.e. two antennas or two sensors determine separately the distance to a target, whereby with the knowledge of the baseline length, i.e. the distance between the antennas or sensors, the position data of the target is determined
• CPC Class Codes: G01S 5/0278 involving statistical or probabilistic considerations G01S5/0252, G01S5/0294 take precedence
• CPC Class Codes: G01S 5/0289 of multiple transceivers, e.g. in ad hoc networks
• CPC Class Codes: G01S 5/0294 Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering

Abstract

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).

Claims

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.