Abstract: A navigation system for a vehicle comprises onboard sensors including a vision sensor, and an onboard map database of terrain maps. An onboard processer, coupled to the sensors and map database, includes a position PDF filter, which performs a method comprising: receiving image data from the vision sensor corresponding to terrain images captured by the vision sensor of a given area; receiving map data from the map database corresponding to a terrain map of the area; generating a first PDF of image features in the image data; generating a second PDF of map features in the map data; generating a measurement vector PDF by a convolution of the first PDF and second PDF; estimating a position vector PDF using a non-linear filter that receives the measurement vector PDF; and generating statistics from the estimated position vector PDF that include real-time measurement errors of position and angular orientation of the vehicle.

Abstract: A method of integrity monitoring for visual odometry comprises capturing a first image at a first time epoch with stereo vision sensors, capturing a second image at a second time epoch, and extracting features from the images. A temporal feature matching process is performed to match the extracted features, using a feature mismatching limiting discriminator. A range, or depth, recovery process is performed to provide stereo feature matching between two images taken by the stereo vision sensors at the same time epoch, using a range error limiting discriminator. An outlier rejection process is performed using a modified RANSAC technique to limit feature moving events. Feature error magnitude and fault probabilities are characterized using overbounding Gaussian models.

Abstract: A method of assuring integrity of range measurements and position solutions comprises obtaining range measurement statistics between network nodes, performing a snapshot integrity test for the range measurement statistics, and performing a sequential integrity test using the range measurement statistics. The snapshot integrity test comprises using Gram matrices with a current configuration of the nodes; performing a singular value consistency check using the Gram matrices against a user selected threshold; and detecting and excluding range measurement statistics with instantaneous errors that cause the singular value to exceed the threshold.

Abstract: A system for integrity monitoring comprises a processor onboard a vehicle, a terrain map database, vision sensors, and inertial sensors. Each vision sensor has an overlapping FOV with at least one other vision sensor. The processor performs integrity monitoring of kinematic states of the vehicle, and comprises a plurality of first level filter sets, each including multiple position PDF filters. A consistency monitor is coupled to each first level filter set. Second level filter sets, each including multiple position PDF filters and a navigation filter, are coupled to the consistency monitor, the map database, the vision sensors, and the inertial sensors. Consistency check modules are each coupled to a respective second level filter set. A third level fusion filter is coupled to each of the consistency check modules. An integrity monitor is coupled to the fusion filter and assures integrity of a final estimate of the vehicle kinematic state statistics.

Abstract: A multiple faulty global navigation satellite signal detecting system is provided. The system includes at least one pair of spaced antennas, at least one aiding source and processor. The at least one pair of spaced antennas are configured to receive satellite signals from a plurality of satellites. The at least one aiding source is used to generate aiding source position estimate signals. The processor is in communication with each antenna and the at least one aiding source. The processor is configured to determine signals blocks. The signal blocks being a collection of subsets of the determined difference signals and a covariance matrix for the difference signals. The processor further configured to generate a union of good signals from all the good blocks and a complementary set of bad signals.

Abstract: A method for planning a vehicle trajectory is provided. The method comprises obtaining an edge map representation corresponding to one or more terrain images of a given area, and identifying a total number of edge pixels in each of a plurality of sub-regions of the edge map representation. The method further comprises determining a measurement probability density function (PDF) for each of the sub-regions based on the number of edge pixels with information content in each sub-region. The method then computes a trajectory cost for each of the sub-regions by dividing a user-selected scalar by a sum of: a user-selected value and the number of edge pixels with information content in each sub-region. Thereafter, the method selects a trajectory for navigation of a vehicle over the given area based on the trajectory cost for each of the sub-regions.

Abstract: A navigation system for a vehicle comprises onboard sensors including a vision sensor, and an onboard map database of terrain maps. An onboard processer, coupled to the sensors and map database, includes a position PDF filter, which performs a method comprising: receiving image data from the vision sensor corresponding to terrain images captured by the vision sensor of a given area; receiving map data from the map database corresponding to a terrain map of the area; generating a first PDF of image features in the image data; generating a second PDF of map features in the map data; generating a measurement vector PDF by a convolution of the first PDF and second PDF; estimating a position vector PDF using a non-linear filter that receives the measurement vector PDF; and generating statistics from the estimated position vector PDF that include real-time measurement errors of position and angular orientation of the vehicle.

Abstract: Systems and methods are described that illustrate how to determine whether an image includes a travel way, and if so how to determine an angle between a longitudinal axis of a vehicle and a longitudinal axis of a travel way line, and a shortest distance between a reference point on a vehicle and a longitudinal axis of the travel way line.

Abstract: System and methods are described that illustrate how to more accurately determine at least one state variable of a vehicle using imaging of a travel way line. Imaging can be used to determine an angle between a longitudinal axis of the travel way line and a longitudinal axis of the vehicle, a shortest distance between the center axis and a reference point on or in the vehicle, and corresponding errors. The determined angle, the determined distance, and the corresponding errors can be used to more accurately determine the at least one state variable.

Abstract: A method of assuring integrity of range measurements and position solutions comprises obtaining range measurement statistics between nodes in a network, performing a snapshot integrity test for the range measurement statistics, and performing a sequential integrity test using the range measurement statistics. The snapshot integrity test comprises using Gram matrices along with a current configuration of the nodes; performing a singular value consistency check using the Gram matrices against a user selected threshold; and detecting/excluding range measurement statistics with instantaneous errors that cause the singular value to exceed the threshold.

Abstract: A method of determining three-dimensional attitude is provided. The method includes measuring a carrier phase of each satellite signal received at plurality of spaced antenna. A carrier phase difference between the measured carrier phase for each satellite signal from each satellite received at each antenna is determined. The integrity of the integer ambiguity resolution relating to the carrier phase difference is assured by applying a least-square-error solution using differential carrier phase measurements with applied integer ambiguities between at least two of the plurality of antennas and observing measurement residuals after the least-square-error solution is computed and applying an instantaneous test, an interval test and a solution separation function. Three-dimensional attitude is determined from the carrier phase differences upon completion of the integer ambiguity resolution and the assurance of integrity of the integer ambiguity resolution.

Abstract: In one embodiment, a method of tracking multiple objects with a probabilistic hypothesis density filter is provided. The method includes obtaining measurements corresponding to a first object with at least one sensor, the at least one sensor providing one or more first track IDs for the measurements. A Tk+1 first predicted intensity is generated for the first object based on a Tk first track intensity. A Tk+1 measurement from a first sensor of the at least one sensors is obtained, the first sensor providing a second track ID for the Tk+1 measurement. The second track ID is compared to the one or more first track IDs, and the Tk+1 first predicted intensity is selectively updated with the Tk+1 measurement based on whether the second track ID matches any of the one or more first track IDs to generate a Tk+1 first measurement-to-track intensity for the first object.

Abstract: A multiple faulty global navigation satellite signal detecting system is provided. The system includes at least one pair of spaced antennas, at least one aiding source and processor. The at least one pair of spaced antennas are configured to receive satellite signals from a plurality of satellites. The at least one aiding source is used to generate aiding source position estimate signals. The processor is in communication with each antenna and the at least one aiding source. The processor is configured to determine signals blocks. The signal blocks being a collection of subsets of the determined difference signals and a covariance matrix for the difference signals. The processor further configured to generate a union of good signals from all the good blocks and a complementary set of bad signals.

Abstract: Systems and methods for integrity monitoring of odometry measurements within a navigation system are provided herein. In certain embodiments, a system includes imaging sensors that generate image frames from optical inputs. The system also includes a GNSS receiver that provides pseudorange measurements; and computational devices that receive the image frames and the pseudorange measurements. Further, the computational devices compute odometry information from image frames acquired at different times; and calculate a full solution for the system based on the pseudorange measurements and the odometry information. Additionally, the computational devices calculate sub-solutions for the system based on subsets of the pseudorange measurements and the odometry information, wherein one of the sub-solutions is not based on the odometry information.

Abstract: A system and methods for monitoring the integrity of navigation measurement information are disclosed. One method includes receiving a plurality of navigation measurement values, computing a first set and second set of estimates of the navigation measurement values, comparing the first set to the second set, and if the second set is statistically consistent with the first set, computing a plurality of sub-sets of the second set of estimates, computing a sub-solution for each sub-set of the second set of estimates, and computing an integrity value for each sub-solution.

Abstract: In one embodiment, a method of tracking multiple objects with a probabilistic hypothesis density filter is provided. The method includes providing a plurality of intensities. The plurality of intensities are pruned based on their respective weight to remove lower weighted intensities and produce a first set of intensities. Intensities in the first set of intensities that are identified as corresponding to the same object are then merged to produce a second set of intensities. The second set of intensities is then pruned again to produce a final set of intensities, wherein pruning the second set of intensities includes in the final set of intensities, up to a threshold number of intensities having the largest weights in the second set of intensities and excludes from the final set of intensities any remaining intensities in the second set of intensities.

Abstract: A system and methods for monitoring the integrity of navigation measurement information are disclosed. One method includes receiving a plurality of navigation measurement values, computing a first set and second set of estimates of the navigation measurement values, comparing the first set to the second set, and if the second set is statistically consistent with the first set, computing a plurality of sub-sets of the second set of estimates, computing a sub-solution for each sub-set of the second set of estimates, and computing an integrity value for each sub-solution.

Abstract: A method of mitigating filter inconsistency of a heading estimate in an inertial navigation system is provided.

Abstract: In one embodiment, a method of tracking multiple objects with a probabilistic hypothesis density filter is provided. The method includes generating a first intensity by combining a first one or more measurements, wherein a first set of track IDs associated with the first intensity includes track IDs corresponding to respective measurements in the first one or more measurements. A second intensity is generated by combining a second one or more measurements, wherein a second set of track IDs associated with the second intensity includes track IDs corresponding to respective measurements in the second one or more measurements. The first set of track IDs is compared to the second set of track IDs, and the first intensity is selectively merged with the second intensity based on whether any track IDs in the first set of track IDs match any track IDs in the second set of track IDs.

Abstract: A method of mitigating filter inconsistency of a heading estimate in an inertial navigation system is provided.