Methods for geolocation using electronic distance measurement equipment

Abstract

Methods for geolocation utilizing electronic distance measurement equipment. The methods select a small subset of available data that is most trustworthy, and then mathematically treats the subset of data (preferably bilateration or trilateration) to compute one or more new measured position candidates. One measured position candidate may be chosen among the measure position candidates based on confidence metrics, relative position geometry, and the comparison with history-based predicted position and its respective confidence metric. In a case where no measured position candidates of sufficient confidence are available, the predicted position is taken as the new position.

Claims

1. A method of determining a new position for a node, the node having a distance measuring radio, the method comprising: a) starting with a previously determined position having a previously determined confidence C, and calculating a predicted position and an associated predicted confidence utilizing data from at least a 3-axis inertial sensor; b) electronically measuring a first distance to a first other node and a second distance to a second other node; c) obtaining a first current position of the first other node comprising at least X, Y, Z coordinates and a first confidence metric, and obtaining a second current position of the second other node comprising at least X, Y, Z coordinates and a second confidence metric; d) geometrically projecting the first distance and the second distance onto a common horizontal plane, to respectively determine a first projected distance and a second projected distance; e) calculating circle-circle intersections of the first projected distance and the second projected distance to respectively determine a first candidate position and a second candidate position; f) determining a candidate confidence as the lesser of the first confidence metric and the second confidence metric, then adjusting the candidate confidence based on geometric conditions of the calculations in step e); g) determining a first error distance between the predicted position and the first candidate position, and determining a second error distance between the predicted position and the second candidate position; h) comparing the first error distance of the first candidate position and the second error distance of the second candidate position, and selecting one with a lower error distance as a candidate position; and i) if the candidate confidence of the candidate position is greater than the predicted confidence, then the candidate position is selected as the new position and the candidate confidence is selected as a new confidence, otherwise the predicted position is selected as the new position and the predicted confidence is selected as the new confidence.

2. The method of claim 1, wherein the first other node and the second other node are selected from among a plurality of available nodes based at least on a corresponding confidence.

3. The method of claim 1, wherein the first other node and the second other node are selected from among a plurality of available nodes based at least on a geometry of their corresponding positions relative to a previous position of the node.

4. A method of determining a new position for a node, the node having a distance measuring radio, the method comprising: a) starting with a previously determined previous position having a previously determined confidence, and calculating a predicted position and an associated predicted confidence utilizing data from at least a 3-axis inertial sensor; b) electronically measuring a first distance to a first other node, a second distance to a second other node and a third distance to a third other node; c) obtaining a first current position of the first other node comprising at least X, Y, Z coordinates and a first confidence metric, obtaining a second current position of the second other node comprising at least X, Y, Z coordinates and a second confidence metric, and obtaining a third current position of the third other node comprising at least X, Y, Z coordinates and a third confidence metric; d) using trilateration with the first, second and third current positions and the corresponding first, second and third distances to determine at least a candidate position; e) determining a candidate confidence as the lesser of the first confidence, the second confidence and the third confidence, then adjusting the confidence based on geometric conditions of the calculations in step d); and f) if the candidate confidence is greater than the predicted confidence, then the candidate position is selected as the new position and the candidate confidence is selected as a new confidence, otherwise the predicted position is selected as the new position and the predicted confidence is selected as the new confidence C.

5. The method of claim 4, wherein the first other node and the second other node are selected from among a plurality of available nodes based at least on a corresponding confidence.

6. The method of claim 4, wherein the first other node and the second other node are selected from among a plurality of available nodes based at least on a geometry of their corresponding positions relative to a previous position of the node.

7. A method of determining a new position for a node, the node having a distance measuring radio, the method comprising: a) starting with a previously determined previous position having a previously determined confidence, and calculating a predicted position and an associated predicted confidence utilizing data from at least a 3-axis inertial sensor; b) electronically measuring a first distance to a first other node and a second distance o a second other node; c) obtaining a first current position of the first other node comprising at least X, Y, Z coordinates and a first confidence metric, and a second current position of the second other node comprising at least X, Y, Z coordinates and a second confidence metric; d) geometrically projecting the first distance and the second distance onto a common horizontal plane, to respectively determine a first projected distance and a second projected distance; e) calculating circle-circle intersections of the first projected distance and the second projected distance to respectively determine a first candidate position and a second candidate position; f) determining a candidate confidence as the lesser of the first confidence metric and the second confidence metric, then adjusting the candidate confidence based on geometric conditions of the calculations in step e); g) determining a first error distance between the predicted position and the first candidate position, and determining a second error distance between the predicted position and the second candidate position; h) comparing the first error distance of the first candidate position and the second error distance of the second candidate position, and selecting one with a lower error distance as a candidate position and the corresponding error distance as an error distance; i) calculating the new position as a weighted interpolation between the predicted position and the candidate position; and j) calculating a new confidence as a weighted interpolation of the predicted confidence and the candidate confidence.

8. The method of claim 7, wherein the first other node, the second other node and the third other node are selected from among a plurality of available nodes based at least on a corresponding confidence.

9. The method of claim 7, wherein the first other node, the second other node and third other node are chosen from among a plurality of available nodes based at least on a geometry of their corresponding positions relative to a previous position of the node.

10. A method of determining a new position for a node, the node having a distance measuring radio with an associated precision, the method comprising: a) starting with a previously determined previous position having a previously determined confidence, calculating a predicted position and an associated predicted confidence utilizing data from at least a 3-axis inertial sensor; b) electronically measuring a first distance to a first other node, a second distance to a second other node, and a third distance to a third other node; c) obtaining a first current position of the first other node comprising at least X, Y, Z coordinates and a first confidence metric, a second current position of the second other node comprising at least X, Y, Z coordinates and a second confidence metric, and a third current position of the third other node comprising at least X, Y, Z coordinates and a third confidence metric; d) using trilateration with the first, second and third positions and the corresponding first, second and third distances to determine at least a candidate position; e) determining a candidate confidence as the lesser of the first confidence, the second confidence and the third confidence, then adjusting the confidence based on geometric conditions of the calculations in step d); f) calculating the new position as a weighted interpolation between the predicted position and the candidate position; and g) calculating a new confidence as a weighted interpolation of the predicted confidence and the candidate confidence.

11. The method of claim 10, wherein the first other node, the second other node and the third other node are selected from among a plurality of available nodes based at least on a corresponding confidence.

12. The method of claim 10, wherein the first other node, the second other node and third other node are chosen from among a plurality of available nodes based at least on a geometry of their corresponding positions relative to a previous position of the node.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) The present invention is described herein with reference to the following drawings:

(2) FIG. 1 shows an overall data flow diagram of an embodiment of the present invention;

(3) FIG. 2 is a diagrammatic illustration of an embodiment of the methods of the present invention;

(4) FIG. 3 illustrates the ambiguity of distance measurement with only one available reference node resulting in infinite number of candidate positions along the circumference of the circle;

(5) FIG. 4 shows reduction in ambiguity with two available reference nodes with favorable geometry, resulting in two candidate positions at Circle-Circle intersections;

(6) FIG. 5 shows further reduction in ambiguity to a single candidate position when three reference nodes with favorable geometry are available;

(7) FIG. 6 illustrates effects of Precision with two reference nodes;

(8) FIG. 7 shows effects of Precision with three reference nodes;

(9) FIG. 8 shows effects of erroneous measurements and unfavorable geometry resulting in non-intersection of circles;

(10) FIG. 9 shows effects of erroneous measurements and unfavorable geometry with non-intersecting circles when one circle is fully contained in another;

(11) FIG. 10 shows unfavorable geometry with tangent circles;

(12) FIG. 11 shows favorable geometry with three reference nodes;

(13) FIG. 12 is an example of unfavorable geometry with three reference nodes;

(14) FIG. 13 illustrates choosing reference nodes with favorable geometry over those with unfavorable geometry; and

(15) FIG. 14 shows choosing nearby reference nodes over distant ones.

DETAILED DESCRIPTION OF AN EMBODIMENT OF THE INVENTION

(16) FIG. 1 is a diagram of the overall data flow for an illustrative embodiment of the present invention.

(17) In order to determine a node's Position, information is gathered from a variety of sensors. In the context of the illustrated embodiment, the position data of neighboring nodes and the measured distances to such nodes are considered sensor inputs. Other typical sensors include inertial measurement unit (IMU), magnetometer, altimeter and the like.

(18) In the illustrated embodiment, the IMU and altimeter are contained within the hardware portion of a node.

(19) Information about the locations of neighboring nodes is transmitted to the node via a data radio. In some embodiments, a single node will be designated as the Neighborhood Manager responsible for periodically collecting and disseminating this information among the nodes. In other embodiments, this information may be transferred collaboratively peer-to-peer. Many methods for transferring information among a plurality of nodes are known. The details of such methods are outside the scope of the present invention.

(20) Distance measurements between nodes are performed by means of a distance measuring radio. A number of such methods are known. The illustrated embodiment utilizes Decawave DW1000 distance measuring radios, but a number of others are readily available.

(21) Sensors:

(22) Altimeter provides relative altitude. IMU: The Inertial Measurement Unit such as the BNO080 consists of 3 axis accelerometer, gyroscope and magnetometer. These are typically fused (internally in most modern IMUs) to provide virtual sensors such as linear acceleration and step counter along with a rotation vector to transform the linear acceleration with magnetic North. Distance measurement: The Distance measurement module (such as an UWB radio) provides the distances from the current Node to nearby Nodes. Neighborhood: In the context of the illustrated embodiment, Neighborhood is a set of Nodes managed by a Node functioning as the Neighborhood Manager (NHM). Each Node broadcasts its position information via XNet. The NHM maintains a Neighborhood Database (NHDB) of the Nodes in its neighborhood. Each Node in the neighborhood receives the position information for each of its neighbors. This information includes: X, Y, Z coordinates where: X is the West (negative) and East (positive) axis, Y is the North (positive) and South (negative) axis, Z is elevation. Role: Anchor: stationary Node with accurately measured position. Tag: moving or stationary Node whose position is calculated via Positioning. A stationary Tag is referred to as a Breadcrumb. State: Moving or Stationary, Confidence: Key positioning values such as the coordinates and distances have an associated confidence value with a range of 0.0 to 1.0 where 0.0 indicates no confidence and 1.0 indicates absolute confidence. Other numeric scales can be used in different embodiments. Whenever positioning chooses among Node data for calculating a position estimate, it uses the confidence of the data to choose the best (most confident) data.

(23) Only anchors have a pre-assigned and constant confidence, set to 1.0. A tag's confidence is calculated based on the confidence of the data is receives from sensors, distance measurement and neighbor Nodes used to calculate a tag's position.

(24) A tag's Position confidence can decrease over time due to data errors, lack of Precision, and time lag between data updates; confidence decreases with data age. As mentioned above, the confidence never increases on its own, it can only increase if the confidence of incoming new data is higher than currently available data.

(25) Modules:

(26) Positioning consists of the following modules: Inertial Navigation System (INS): Given a starting position, the INS module takes inertial inputs from the IMU and converts them X and Y offsets to compute a predicted Position. Geometry: Given neighbor Node's positions and distances, the Geometry module computes one or more candidate Positions of the local Node. Motion: Given inertial inputs, the Motion module computes whether the local Node is in motion and it computes the direction of travel.

(27) Geometry Module (GEO): GEO uses neighbor Node positions and distances to compute one or more candidate Positions for the local Node. Neighbor positions are provided by the NHM and distances to nearby Nodes are provided by Distance measurement; both are delivered as sensor readings. These are combined into a list of Neighbor Descriptors (ND).

(28) GEO recalculates the estimated position at the same rate as Distance measurement is able to provide distances updates. Each Node implementation may have different Distance measurement technology and thus the update rates vary by Node.

(29) GEO's position calculation is shown in the flow chart shown in FIG. 2 and summarized below:

(30) 1. Remove unsuitable neighbors from the ND List. Desirable neighbors are those with high confidence, recent position and distance data, and are positioned favorably. Some examples of favorable and unfavorable geometry are illustrated in FIG. 8-FIG. 14.

(31) 2. Calculate interpolated position of Tag neighbors.

(32) 3. Sort ND List by confidence.

(33) 4. Select up to three best neighbors based on role (preferring Anchors over non-moving Tags over moving Tags) and Confidence.

(34) 5. If there are only two neighbors, attempt to use Bilateration to calculate two candidate Positions.

(35) 6. If there are three neighbors, attempt to use Trilateration to calculate a single candidate Position. If Trilateration fails, attempt to use to Bilateration on each neighbor pair combination to calculate several pairs of candidate Positions.

(36) 7. Report the list of candidate Positions to the INS module. Each of the above steps may end the calculation due to an insufficient number of usable neighbors. In such cases GEO will report the previously calculated position with a reduced confidence. Thus, as time passes without enough neighbors, GEO's reported position confidence decreases.

(37) Trilateration/Bilateration

(38) Given one Anchor and a Tag and a distance between them of d1 all that is possible is to say that the Tag lies somewhere on the circle with it center at Anchor 1 with radius d1. This information is not sufficient to calculate a finite list of candidate Positions. However, having the predicted Position pP from the INS module, it is possible to adjust the associated Confidence. If pP lies on the circle or near it, its Confidence can be increased. If pP is far from the circle, its confidence can be decreased.

(39) Given two Anchors and a Tag and the distances to the Anchors d1 and d2, we can calculate two possible candidate Positions cP1 and cP2 at the intersections of the two circles around the Anchors as shown in FIG. 4. Having a predicted Position pP from the INS module, the closer candidate Position can be chosen over the other (cP2 in the illustrated case). As in the previous example, the magnitude of the distance between cP2 and pP can be used as the basis for adjusting Confidence.

(40) With three anchors it is possible to calculate a single candidate Position cP of the tag to be at the center of the intersection of all three circles. This scenario is illustrated in FIG. 5. Like in previous examples, the magnitude of the distance between cP and pP, especially as it compares to the inherent Precision of the distance measuring technology being used, can be evaluated to adjust Confidence.

(41) Distance Measurement Errors

(42) Distance measurement radios are not perfect so it may be advantageous to account for the distance measurement errors. In the two-anchor case the candidate Positions will lie somewhere in the areas of intersection A1 and A2 of the four circles around the anchors with radii d1−Error, d1+Error, d2−Error and d2+Error, as shown in FIG. 6. The magnitude of Error is typically proportional to the inherent Precision of the distance measuring technology, however any single distance measurement can have significantly greater Error due to environmental factors such as obstructions, reflections, interference and the like.

(43) With three anchors the location is in the area of the intersection of the six resulting circles as shown in FIG. 7.

(44) Distance measurement errors can also create situations where the resulting circles do not intersect, preventing calculation of position. This would be termed disadvantageous geometry within the context of the illustrated embodiment. Some examples of this are illustrated in FIG. 8.

(45) If the circles are not too far apart then it is possible to create a lower confidence estimate by calculating the center position between the circles. Another alternative is to enlarge the radius by an acceptable error delta to see attempt to create intersection circles. Different embodiments may use one of these approaches or other similar methods.

(46) FIG. 9 and FIG. 10 illustrate other examples of unfavorable geometry as a result of errors in measured distances. In the illustrated embodiment, such measurements would typically be discarded if other measurements that produce desirable circle intersections are available. If no better measurements are available and unfavorable geometry is used, Confidence will be reduced proportionately to the amount of error detected, and, in some embodiments, as it relates to Precision.

(47) Inertial Navigation Module (INS)

(48) INS takes inertial inputs from the IMU and converts them X and Y offsets and adds them to the previous position to compute a new predicted Position and associated Confidence. A large amount of inertial activity will reduce Confidence, while little or no detected inertial activity will, in some embodiments, increase Confidence.

(49) A second function of the INS is to receive a list of geometric candidate Positions from the GEO module, and select among them based on distance from predicted Position and relative Confidence. In some scenarios, the predicted Position may be selected over all candidate Positions. In other scenarios, a new position may be calculated that is a weighted average of the closest candidate Position and predicted Position, with weights being at least in part proportional to respective Confidence.

(50) Motion Module (MOT)

(51) MOT takes inertial inputs from INS and computes whether the local Node is in motion and it computes the direction of travel (bearing). The result of motion computation may be used to adjust Confidence of the node's Position and also may be used to switch node roles between Anchor, Breadcrumb and Tag. Node roles are used by the GEO module as criteria for selecting most favorable sets of distance measurements, with the highest preference being given to Anchors, then Breadcrumbs, then Tags. A Breadcrumb, within the context of the illustrated embodiment, is a Tag that has been stationary for an extended period of time. The primary difference between an Anchor and a Breadcrum, in the illustrated embodiment, is that an Anchor's position is explicitly known and assigned when the Anchor is placed. A Breadcrumb's position was calculated when it was moving as a Tag, but has higher Confidence than that of a moving Tag due to having been stationary and having the benefit of a number of repetitive measurements being considered over time to reduce error.

(52) FIG. 11 shows an example of favorable geometry with three distance measurements producing a well defined intersection. This scenario would result in increased Confidence.

(53) As a Tag moves away from a set of Anchors, accuracy diminishes due to the resultant unfavorable geometry. Such a scenario is illustrated in FIG. 12. In this example, Confidence would be decreased.

(54) The scenario illustrated in FIG. 12 can be mitigated by the introduction of intermediate reference nodes at certain distances and locations along the path of travel. Such nodes can be Anchors, Breadcrumbs or other Tags. As described previously, the node role will have an effect on the Confidence of a Position that is calculated using those references. This is illustrated in FIG. 13, with reference nodes having favorable geometry being chosen as references over those that have unfavorable geometry.

(55) In practice, such additional nodes can be carried by the user and placed at selected locations along the path of travel.

(56) FIG. 14 illustrates a scenario where a Tag has moved out of range of the original set of Anchors and is only using the nearby Breadcrumbs for determining Position.

(57) According to a particular embodiment, the method of determining a new Position for a Node n, said Node having a distance measuring radio, said method comprising the steps of:

(58) a) Starting with a previously determined previous Position P, said previous Position having a previously determined Confidence C, calculating a predicted Position pP, and an associated predicted Confidence pC, said calculations utilizing data from at least a 3-axis Inertial sensor,

(59) b) Electronically measuring at least a first distance Dn1 to at least a first other Node n1, and at least a second distance Dn2 to at least a second other Node n2,

(60) c) Obtaining the current Position Pn1 of at least said first other Node and Position Pn2 of at least said second other Node, said Position comprising at least X, Y, Z coordinates and a Confidence metric Cn1 and Cn2,

(61) d) Geometrically projecting the distances Dn1 and Dn2 onto a common horizontal plane, to determine projected distances pDn1 and pDn2,

(62) e) Calculating Circle-Circle Intersections of said projected distances to determine two candidate Positions cPa and cPb,

(63) f) Determining a candidate Confidence cC as the lesser of Cn1 and Cn2, then adjusting said Confidence cC based on geometric conditions of the calculations in step e),

(64) g) Determining the error distance eDa between pP and cPa, and error distance eDb between pP and cPb,

(65) h) Choosing the candidate Position with the lesser error distance as the candidate Position cP,

(66) i) If confidence cC is greater than pC, choosing candidate position cP as new position P and taking cC as the new confidence C, otherwise choosing predicted position pP as new position P and predicted confidence pC as new confidence C.

(67) According to a particular embodiment, the method of determining a new Position for a Node n, said Node having a distance measuring radio, said method comprising the steps of:

(68) a) Starting with a previously determined previous Position P, said previous Position having a previously determined Confidence C, calculating a predicted Position pP, and an associated predicted Confidence pC, said calculations utilizing data from at least a 3-axis Inertial sensor,

(69) b) Electronically measuring at least a first distance Dn1 to at least a first other Node n1, at least a second distance Dn2 to at least a second other Node n2, and at least a third distance Dn3 to at least a second other Node n3,

(70) c) Obtaining the current Position Pn1 of at said first other Node, Position Pn2 of said second other Node, and Position Pn3 of said third other Node, said Position comprising at least X, Y, Z coordinates and a Confidence metric Cn1, Cn2 and Cn3,

(71) d) Using trilateration with positions Pn1, Pn2 and Pn3 and corresponding measured distances Dn1, Dn2 and Dn3 to determine at least a candidate Position cP,

(72) e) Determining a candidate Confidence cC as the lesser of Cn1, Cn2 and Cn3, then adjusting said Confidence cC based on geometric conditions of the calculations in step d),

(73) f) If confidence cC is greater than pC, choosing candidate position cP as new position P and taking cC as the new confidence C, otherwise choosing predicted position pP as new position P and predicted confidence pC as new confidence C.

(74) According to a particular embodiment, the method of determining a new Position for a Node n, said Node having a distance measuring radio, said method comprising the steps of:

(75) a) Starting with a previously determined previous Position P, said previous Position having a previously determined Confidence C, calculating a predicted Position pP, and an associated predicted Confidence pC, said calculations utilizing data from at least a 3-axis Inertial sensor,

(76) b) Electronically measuring at least a first distance Dn1 to at least a first other Node n1, and at least a second distance Dn2 to at least a second other Node n2,

(77) c) Obtaining the current Position Pn1 of at least said first other Node and Position Pn2 of at least said second other Node, said Position comprising at least X, Y, Z coordinates and a Confidence metric Cn1 and Cn2,

(78) d) Geometrically projecting the distances Dn1 and Dn2 onto a common horizontal plane, to determine projected distances pDn1 and pDn2,

(79) e) Calculating Circle-Circle Intersections of said projected distances to determine two candidate Positions cPa and cPb,

(80) f) Determining a candidate Confidence cC as the lesser of Cn1 and Cn2, then adjusting said Confidence cC based on geometric conditions of the calculations in step e),

(81) g) Determining the error distance eDa between pP and cPa, and error distance eDb between pP and cPb,

(82) h) Choosing the candidate Position with the lesser error distance as the candidate Position cP and the corresponding error distance as the error distance eD,

(83) i) Calculating new position P as a weighted interpolation between at least the predicted position pP and candidate position cP,

(84) j) Calculating new confidence C as a weighted interpolation of predicted confidence pC and candidate confidence cC.

(85) According to a particular embodiment, the method of determining a new Position for a Node n, said Node having a distance measuring radio with an associated Precision, said method comprising the steps of:

(86) a) Starting with a previously determined previous Position P, said previous Position having a previously determined Confidence C, calculating a predicted Position pP, and an associated predicted Confidence pC, said calculations utilizing data from at least a 3-axis Inertial sensor,

(87) b) Electronically measuring at least a first distance Dn1 to at least a first other Node n1, at least a second distance Dn2 to at least a second other Node n2, and at least a third distance Dn3 to at least a second other Node n3,

(88) c) Obtaining the current Position Pn1 of at said first other Node, Position Pn2 of said second other Node, and Position Pn3 of said third other Node, said Position comprising at least X, Y, Z coordinates and a Confidence metric Cn1, Cn2 and Cn3,

(89) d) Using trilateration with positions Pn1, Pn2 and Pn3 and corresponding measured distances Dn1, Dn2 and Dn3 to determine at least a candidate Position cP,

(90) e) Determining a candidate Confidence cC as the lesser of Cn1, Cn2 and Cn3, then adjusting said Confidence cC based on geometric conditions of the calculations in step d),

(91) f) Calculating new position P as a weighted interpolation between at least the predicted position pP and candidate position cP,

(92) g) Calculating new confidence C as a weighted interpolation of predicted confidence pC and candidate confidence cC.

(93) The nodes n1 and n2 and n3 (in the third and fourth embodiments) are chosen from among a plurality of available nodes based at least on corresponding Confidence.

(94) The nodes n1 and n2 and n3 (in the third and fourth embodiments) are chosen from among a plurality of available nodes based at least on the geometry of their corresponding positions relative to previous position P of said node n.

(95) The embodiments disclosed herein are illustrative and not limiting; other embodiments shall be readily apparent to those skilled in the art based upon the disclosures made herein, without departing from the scope of the present invention.