CROSS-REFERENCE TO RELATED APPLICATIONS
This application is a divisional of pending U.S. application Ser. No. 12/059,865 entitled “POSITION ESTIMATION FOR NAVIGATION DEVICES,” filed Mar. 31, 2008, which is incorporated herein by reference.
GOVERNMENT INTEREST STATEMENT
The U.S. Government may have certain rights in the present invention under contract No. HDTRA-06-6-C-0058, subcontract No. CHI-06022-001 as awarded by the Defense Threat Reduction Agency.
Reliable navigation systems and devices have always been essential for estimating both distance traveled and position. For example, early navigating was accomplished with “deduced” or “dead” reckoning. In dead-reckoning, a navigator finds a current position by measuring the course and distance the navigator has moved from some known point. Starting from the known point, the navigator measures out a course and distance from that point. Each ending position will be the starting point for the course-and-distance measurement. In order for this method to work, the navigator needs a way to measure a course and a way to measure the distance traveled. The course is measured by a magnetic compass. In pedestrian dead reckoning, the distance is the size of a single step. A position estimate is derived by the integration of distance and direction over a sequence of steps. This type of navigation, however, is highly prone to errors, which when compounded can lead to highly inaccurate position and distance estimates.
In more advanced navigation systems, such as an inertial navigation system (INS), positional errors can accumulate over time. For example, any navigation performed in areas where satellite or radar tracking measurements are inaccessible or restrictive (such as areas where global positioning system, or GPS, measurements are “denied”) is susceptible to the accumulation of similar positional errors. Moreover, in the dead-reckoning methods discussed above, these positional errors accumulate based on the distance traveled. There is a need in the art for improvements in position estimation for navigation devices.
The following specification provides for at least one method of position estimation for navigation devices using sensor data correlation. This summary is made by way of example and not by way of limitation. It is merely provided to aid the reader in understanding some aspects of at least one embodiment described in the following specification.
Particularly, in one embodiment, a method of providing position estimation with a navigation device comprises periodically recording magnetic field strength of an area substantially surrounding a navigation device as a user of the navigation device traverses a select pathway. The method combines the recorded magnetic field strength with measurements from at least a dead reckoning portion of the navigation device to provide position estimates along the select pathway. The method further corrects each of the position estimates from a starting position on the select pathway, where each of the corrected position estimates have an error value below one or more previous position estimates and any intervening positions between each of the one or more previous position estimates and the starting position, with the error value corresponding to an error threshold based on the previous position estimates.
These and other features, aspects, and advantages are better understood with regard to the following description, appended claims, and accompanying drawings where:
FIG. 1 is a block diagram of a navigation device;
FIGS. 2A and 2B are traversing diagrams of navigating in a select pathway;
FIG. 3A is a traversing diagram of navigating in a select pathway prior to position correction;
FIG. 3B is a traversing diagram of navigating in the select pathway of FIG. 3A after position correction;
FIG. 4A is a traversing diagram of a select pathway with at least one marked position before sensor data correlation at a selected position;
FIG. 4B is a traversing diagram of navigating in a select pathway showing one or more positions that correlate with the marked position of FIG. 4A;
FIG. 4C is a traversing diagram of navigating in the select pathway of FIG. 4B after the correlated positions have been corrected;
FIG. 5A is a traversing diagram of navigating in a select pathway using a navigation device indicating the positions which are correlated;
FIGS. 5C is the azimuth data and 5B is the calculated correlation function diagram from the navigation device of FIG. 5A;
FIG. 6A is a traversing diagram of navigating in a select pathway using a navigation device having at least one marked position and one or more correlated positions;
FIGS. 6B and 6C are calculated correlation data diagrams for sensor data channels from the navigation device of FIG. 6A;
FIG. 7 is a diagram in graphical form illustrating product correlation as provided by the correlation data of FIGS. 6B and 6C;
FIG. 8 is a flow diagram of a method for providing position correction in a navigation device;
FIG. 9 is a flow diagram of a method of correlating position measurements in a navigation device;
FIG. 10 is a flow diagram of a method of qualifying navigation data from the navigation device of FIG. 9; and
FIG. 11 is a flow diagram of a method of correlating position measurements in a navigation device.
Like reference characters denote like elements throughout the figures and text of the specification.
Embodiments disclosed herein relate to position estimation for navigation devices using sensor data correlation. For example, at least one navigation device discussed herein substantially reduces positioning errors when the device recognizes that a user of the device is at a previous position. In one implementation, the navigation device discussed herein informs the user that a track crossing event has occurred based on the correlation of accumulated sensor data. Moreover, this event occurrence can be determined manually by the user or automatically detected by the sensors within the navigation device. For example, each track crossing event is detected at recognizable corners, intersections, or bottlenecks found inside a building or along a constrained (that is, a select) pathway. In one embodiment, a laser range-finder or at least one sonar-sounding sensor is coupled to the navigation device to record a characteristic “fingerprint” of each track crossing the user encounters while traversing a select pathway. The device is configurable to automatically and continuously mark likely track crossing points along the select pathway (for example, the device collects tracking data and provides an indication when a current record matches a previously-encountered region). As discussed in further detail below, the device is configurable to adjust and correct positional errors for each of the track crossings detected.
In at least one embodiment, a dead-reckoning (DR) navigation device comprises one or more inertial sensors and one or more magnetic sensors operable to obtain estimates of displacement from a starting point. The track correlation and position recognition discussed herein substantially reduces the accumulated positional errors. For example, as the user travels indoors down halls or corridors, or outdoors along trails and streets, positional corrections can be applied to current and previous position measurements, and continuously “back-propagated” over a history of position estimates between the first and second times the user reaches the same spot. In one implementation, track positions are stored in discrete steps, and the device assigns a correction to each track position by dividing the total error by the number of steps in the interval.
The position estimation methods disclosed herein will not require any additional sensors or communications infrastructure along the select pathways discussed below. For example, existing DR sensors can be used from the DR navigation device. In one embodiment, the DR navigation device uses magnetic sensors for compassing and accelerometers for step counting. Moreover, a processing unit on the DR navigation device is configured to perform the automatic track-crossing recognition and processing based on magnetic field strength measurements of an area substantially surrounding the DR navigation device.
FIG. 1 is an embodiment of a navigation system 100 (for example, a personal navigation system operable in an enclosed environment). The system 100 comprises a navigation device 102 and an output terminal 104 communicatively coupled to the navigation device 102 through an output interface 118. In one embodiment, the output interface 118 further comprises a wireless communications transceiver 119. The navigation device 102 comprises a processing unit 106, a sensor module 108, and a power block 120 that provides electrical power to the navigation device 102 and, in one implementation, the output terminal 104. In the example embodiment of FIG. 1, the navigation device 102 comprises an optional global positioning system (GPS) receiver 114 in operative communication with the processing unit 106 and an optional attachment interface 116 communicatively coupled to the processing unit 106. In one embodiment, the optional attachment interface 116 receives navigation input data (for example, from a manual marking device or the like coupled to the optional attachment interface 116 in order to record a characteristic fingerprint of the track over which a user travels). In the example embodiment of FIG. 1, the sensor module 108 comprises at least a portion of the navigation device 102 operable as a dead reckoning module. In the same embodiment, the output terminal 104 provides position estimation data to a user of the navigation device 100 (for example, the position estimation data processed by the navigation device 102). In this same embodiment, the dead reckoning portion of the navigation device 102 is operable within a sensor measurement range as indicated to the user.
The sensor module 108 comprises one or more accelerometers 109 and one or more magnetometers 110. In one implementation, the sensor module 108 further comprises one or more gyroscopes 111 and a barometric altimeter 112. It is understood that the sensor module 108 is capable of accommodating any appropriate number and types of navigational sensors and sensor blocks operable to receive sensor input data (for example, one or more of the accelerometers 109, the magnetometers 110, the gyroscopes 111, the barometric altimeter 112, and the like) in a single sensor module 108. In the example embodiment of FIG. 1, the processing unit 106 comprises at least one of a microprocessor, a microcontroller, a field-programmable gate array (FPGA), a field-programmable object array (FPOA), a programmable logic device (PLD), or an application-specific integrated circuit (ASIC). In one implementation, the processing unit 106 further comprises a memory block 107. The memory block 107 records at least each of the recognized track crossings measured by the sensor module 108.
In operation, the sensor module 108 provides position estimates based on magnetic field strength and heading data (for example, azimuth data). In one embodiment, the azimuth data is provided by the dead reckoning portion of the sensor module 108. The processing unit 106 records the position estimates along with the magnetic field measured by the sensor module 108 of an area substantially surrounding a user of the navigation device 102. The processing unit 106 measures a track crossing based on a comparison of the magnetic field strength at a current position and one or more previous position estimates. As discussed in further detail below with respect to FIGS. 4A to 4C, for each measured track crossing, the processing unit 106 aligns the current position with the one or more previous position estimates and adjusts an accumulation of measurement error below an error threshold based on the one or more previous position estimates. In the example embodiment of FIG. 1, the processing unit 106 corrects any intervening positions and error estimates between the current position and the one or more previous position estimates, as discussed in further detail below.
In one embodiment, the navigation device 102 comprises at least three magnetometers 110 to provide the heading data within the surrounding magnetic field in at least three orientations. Further, the navigation device 102 comprises at least three accelerometers 109 for step counting and the estimation of vertical in three axes for movements in at least three dimensions. The position estimation performed by the navigation device 102 provides at least one method of correction for position estimates based on at least an error threshold. For example, the sensor module 108 is operable to continually measure the azimuth data from the at least three magnetometers 110 on a sensor channel. Moreover, the sensor module 108 is further operable to provide the position estimates to the processing unit 106. In the example embodiment of FIG. 1, the position estimates are iteratively adjusted based on the error threshold, as discussed in further detail below with respect to FIGS. 5A to 5C. In one implementation, the processing unit 106 automatically recognizes the measured track crossings by correlating the magnetic field strength and the heading data between the current position and the one or more previous position estimates. In this same implementation, the processing unit 106 qualifies the current position based on correlation threshold criteria for the sensor module 108 (discussed in further detail below with respect to FIG. 10).
FIGS. 2A and 2B are traversing diagrams of navigating in a select pathway. In the example embodiments of FIGS. 2A and 2B, a navigation device 202 (representative of the navigation device 102 of FIG. 1) is worn by a user. The user marks potential track crossings using the navigation device 102. In one implementation, the user provides a marking signal from one of a manual marking device, a push button device, or the like. The next time the user crosses through the same position, the user marks the position a second time. For example, in FIG. 2A, the user having the navigation device 202 traverses along three sides of a path 204 that is 25 meters square, starting from the position labeled ‘1’, and proceeding around to a position ‘4’. The track path 204 (labeled as “DT” in FIG. 2A) will be 75 meters. Moreover, a straight line distance 206 (labeled as “DS” in FIG. 2A) between the position 1 and the position 4 is 25 meters.
In one implementation, a measure of the accumulated error between positions 1 and 4 along the select pathway comprises two distances: the first is the straight line distance “DS” between the two positions, and the second is the length of the path traveled “DT” between the two positions ‘1’ and ‘4’. A positional error measure E is provided below in Equation 1, further expressed as a percent in Equation 2 below.
E=DS/DT (Equation 2)
E=100*(DS/DT)% (Equation 2)
In the example embodiment of FIG. 2A, the positional error is 25 m/75 m, or 0.33 (from Equation 1), or 33% (Equation 2). The select pathway depicted in FIG. 2B illustrates the user traversing around a square path 208 back to the starting position 1. As shown in FIG. 2B, positional errors accumulate in a position estimate provided by the navigation device 202, resulting in the navigation device 202 directing the user along a path 210 to the position 1′ instead of 1. For example, if position 1 and position 1′ are three meters apart, then the measure of error is 3 m/100 m, or 0.03 (3%). For each position marking event, all marks that lie outside a error threshold range are disregarded, and marks at or below the error threshold are ranked according to the error measure in Equation 1.
As discussed in further detail below, any accumulated positional errors from the last time the user was at that same position are substantially eliminated using the position estimation methods provided by the navigation device 102. For example, when a position estimate drifts off by 3 meters as illustrated above, the navigation device 102 readjusts a current position back to the previous tracked position by subtracting off the 3 meter drift from a current position. As a result, the position error estimate is reduced to that which was originally associated with that position. Moreover, this correction is not only applicable to the current position, but also back propagated continuously over a history of position estimates between the first and second times the same position was reached. In the example embodiments of FIGS. 2A and 2B, track positions are stored by discrete steps in the navigation device 202. Accordingly, a correction is assigned to each step by dividing the total error by the number of steps in the interval.
FIG. 3A is a traversing diagram of navigating in a select pathway 300 prior to correction. FIG. 3B is a traversing diagram of navigating in the select pathway 300 after position correction. For example, in one implementation, starting position 302 and ending position 304 are manually marked by a user of the navigation device 102 (FIG. 1) as the user traverses the select pathway 300. To further illustrate this example, after the user marks the positions 302 and 304, the dead reckoning portion of the navigation device 102 estimates the position 302 and the position 304 to be about six meters apart after traversing a total of 330 meters. Accordingly, the processing unit 106 in the navigation device 102 calculates the error measure E for this pair of points E=6 m/330 m=0.018. The processing unit 106 compares the error measure E to a predetermined error threshold Et (for example, when Et=0.02, the estimated error is expected to be no more than 2% of the distance traveled). Since the error measure E between positions 302 and 304 passes this criterion (1.8%), the correction is applied. In one embodiment, north and east corrections are applied independently. For example, at each data point between the two marked positions 302 and 304, the correction is applied as illustrated below in Equation 3:
−DSe/N, −DSn/N (Equation 3)
With respect to Equation 3, DS, is the error in the east direction, and DSn is the error in the north direction, and N is the number of data points. In one implementation, a sample point measurement is taken by the navigation device 102 at each step. In the example embodiment of FIG. 3A, there are 415 steps taken from start to finish. Moreover, using sample error components of 2.4 meters east as DSe, and 5.3 meters north as DSn, a linear correction of (−2.4/415) meters in the east direction and (−5.3/415) meters in the north direction is applied to each step using Equation 3. The result is shown in FIG. 3B. The positions 302 and 304 are aligned to provide details of the select pathway 300. It is understood that in alternate implementations, additional methods of error correction based on time (for example, quadratic error growth estimation) or similar variables can be used to provide the position estimation discussed herein. Moreover, additional correlations are possible from the magnetic field strength recordings as discussed in further detail below with respect to FIGS. 4A to 4C.
FIG. 4A is a traversing diagram of a select pathway 400 with at least one marked position. FIG. 4B is a traversing diagram of navigating in the select pathway 400 after a correlation calculation has identified one or more candidate positions that substantially match the marked position estimate of FIG. 4A. FIG. 4C is a traversing diagram of navigating in the select pathway 400 after track corrections have been applied. In the example embodiment of FIG. 4A, a position 412 is marked by a navigation device, similar to the navigation device 102 of FIG. 1, as a user traverses the select pathway 400. As an example of automatic track crossing detection, the track correlation process begins at the most recent position 412 and works backwards over a stored history of position and sensor data. The navigation device 102 has a magnetic fingerprint of the position 412, and the navigation device 102 searches backwards through the select pathway 400 for similarly-matching magnetic fingerprints. In the example embodiment of FIG. 4B, positions 402 to 410 are automatically compared to the marked point 412. The navigation device 102 uses an automatic correlation function to determine if any of the other positions shown in FIG. 4B are a match (for example, any of the positions 402, 404, 406, 408, and 410) with respect to the starting position 412.
In operation, the track correlation process occurs in three interval phases: (1) mark, (2) link, and (3) adjust. In the marking phase, a region of interest is selected as the user passes it. In one implementation, selecting the region of interest is done at discrete times manually by the user. In at least one alternate implementation, selecting the region is accomplished continuously and automatically by the navigation device 202. When the region of interest is selected, the selected position is compared pairwise with any prior position markings When a match is found, the two positions are automatically linked and the positions adjusted as discussed below with respect to FIG. 11. For example, in one implementation, the navigation device 102 determines which of the positions 402 to 412 to link together. For each matching fingerprint identified in near real time, the processing unit 106 evaluates the error between a current position and all the previous matching positions. As discussed in further detail below with respect to FIGS. 9 to 11, the positional estimates are further reduced below a dead-reckoning (DR) error threshold due to the correction of previous position estimates. When the estimate of the position 402 is outside the DR error threshold with respect to the position 412, the cascaded corrections between the positions 404 to 412 bring the estimates of the positions 402 and 412 below the DR error threshold (for example, below the estimated position error as a percentage of the distance traversed along the select pathway 400).
For an iterative adjustment, applying multiple corrections can result in subsequent corrections reversing a portion of the adjustment of previous corrections. In one implementation, the track correlation discussed above is performed automatically based on the correlation of sensor data from at least two different periods of recorded magnetic field strength on a sensor channel, as shown by the repetitive traversal along the select pathway 400. For example, when applying the correction between the positions 402 and 412 linearly to all the points between the positions 402 and 412, at least a portion of the points between the positions 402 and 412 will be separated. In one implementation, adjustments are made from the positions 412 to 410, excluding the data between the positions 408 and 410 and the correction is further applied between the positions 402 and 404. The track correlation discussed here substantially reduces errors in the position estimation, as further discussed below with respect to FIGS. 8 and 9.
FIG. 5A is a traverse diagram of navigating in a select pathway 500 using a navigation device (for example, the navigation device 102 of FIG. 1) having at least one marked position 506 and one or more correlated positions (for example, positions 504 and 502) which have been linked to the at least one marked position 506. In the example embodiment of FIG. 5A, the magnetometers 110 of the navigation device 102 are continuously recording heading data; so as a user traverses the select pathway 500 the heading exhibits a small variation before reaching the marked position 506 and then rises afterwards as the user turns from a southwesterly to a northwesterly direction, for example. A correlation function is calculated for all points prior to the position 506 to determine if this fingerprint is matched by any other point along the select pathway 500 as further discussed below with respect to FIG. 5C. FIG. 5B illustrates the calculated correlation of the heading at 506 with the previous sample points along the select pathway 500. For example, two spikes in the correlation function at positions 504 and 502 are identified as candidates for linking and adjusting. In one embodiment, a range of the correlation function is from −1 to +1, with values near +1 considered a high correlation. Additional objects and magnetic fields substantially surrounding the pathway 500 provide unique signatures based on at least the azimuth data recorded by the navigation device 102 as the user traverses the select pathway 500. The unique signatures are correlated with the magnetic field strength data as discussed below.
In the example embodiment of FIG. 5A, a point at the bottom of the pathway 500 near the first curve is marked with crossing points 3 (point 502), 2 (point 504) and 1 (point 506). Using the correlation tracking discussed above, the navigation device 102 discovers like points using the azimuth signature data within the magnetic field recordings. A sample result is shown in FIG. 5B. For example, point 506 in FIG. 5B represents a position of the most recent section of heading data (that is, point 1) of FIG. 5A. The two peaks 502 and 504 in the correlation graph are the other two linked data points, points 3 and 2. It is further indicated in FIG. 5B that a majority of the correlation plot is flat-line. In the example embodiment of FIG. 5B, data correlation is only performed when the DR constraint is met (for example, when the error threshold calculated in Equation 2 is less than 2 percent). When the DR constraint is not met, then the correlation is set to zero. The source data for each of the linked data points 1, 2, and 3 is shown in FIG. 5C.
FIG. 5C is a signal diagram from the navigation device 102 traversing the select pathway 500. From the source heading data shown in FIG. 5C, the correlation data is calculated from at least one of the sensor channels of the navigation device. For example, after selecting the marked position 506 (data point 1), the correlation function is calculated for the data points 2 and 3 (positions 504 and 502, respectively) representing at least two data sets, x and y, as illustrated below in Equation 4.
rxy=Σ(xi−xm)(yi−ym)/((n−1)sxsy) (Equation 4)
With respect to Equation 4, xm is the mean of the n data points of x, sx is the standard deviation of x, and similar definitions hold for the y data set. In the example embodiment of FIG. 5C, an auto-correlation is determined, meaning that the two sets of data x and y are taken from the same series of points, but separated in time.
In detecting the azimuth signature data discussed here, an acceleration vector is recorded by the navigation device 102 to provide additional navigation information. For example, as shown in Table 1 below, an M-A dot product of the acceleration vector with the magnetic vector is constant and proportional to the dip angle of the Earth\'s magnetic field. The dot product is used to detect ripples in the magnetic field strength recordings. Since the azimuth and magnetic field strength measurements are static, the dot product is dependent on motion of the user. If the dot product correlates at any of the positions identified in FIG. 5A, then not only is the user at the identified position, the user is also traveling with a near similar motion to when the user passes the position on the second and subsequent times. As a result, the example data recordings shown in Table 1 below provide a fingerprinting capability based on the dead reckoning measurement portion of the sensor module 108. With respect to Table 1 below, the columns of data are organized as follows:
Steps: The number of steps taken
East: Displacement from a starting point in the east direction (meters)
North: Displacement from a starting point in the north direction (meters)
Heading: The direction in which the step is taken
Normalized magnetic field: The magnitude of the observed magnetic field divided by the nominal field at the location
M-A Dot product: The cosine of the angle between the magnetic field and the acceleration vector