Patent Description:
Some map based, vision navigation systems can be used for aircraft operating at different altitudes, such as air taxis in urban air mobility applications, as well as high-altitude missiles and munitions. However, a limitation with such navigation systems is that they do not compute real-time errors of the position and angular orientation measurements from the correlated images. These measurement errors are often pre-selected and scheduled depending on the intended operational environment. Such an approach suffers because: position and angular orientation errors vary according to the available features in changing visual environments; the intended vehicle path can be operationally changed; and sensors can be changed in the field prior to vehicle operation. The resulting navigation filter performance is inconsistent because the pre-selected measurement errors do not accurately capture the actual measurement errors. Further, downstream systems and users do not have accurate statistics indicating the true performance of the navigation filter, leading the downstream systems and users to rely on inaccurate vehicle navigation statistics.

Document <CIT> discloses a method for determining a location of a vehicle, the method comprising acquiring a 2D-image using a vehicle-based camera, the image depicting the vehicle surroundings; classifying pixels in the image such that each classified pixel belong to one class of set of classes, thereby forming a classified 2D-image. The method further comprises determining an initial estimated vehicle location and defining possible vehicle locations based on the initial location. Next, the method comprises matching the classified 2D-image with a 3D-map comprising a plurality of geometric objects, each geometric object belonging to one class of the set of classes, by comparing a classification of a geometric object in the 3D-map, for a possible vehicle location, with a classification of a corresponding at least one pixel in the classified 2D-image; determining a matching score for at least one possible vehicle location based on matched pixels of the 2D-image; and determining a vehicle location based on the score of the possible vehicle location.

Document <CIT> discloses a positioning data processing method. The method comprises the steps of obtaining vehicle movement information corresponding to a target vehicle and determining a first movement probability distribution, on a to-be-measured position, of the target vehicle on the basis of the vehicle movement information and a prior probability distribution; matching a measured local pavement reference object with a global pavement reference object in a navigation map and determining a first measurement probability distribution, on the to-be-measured position, of the target vehicle according to the matching result and the prior probability distribution; fusing the first movement probability distribution with the first measurement probability distribution to obtain a posterior probability distribution, on the to-be-measured position, of the target vehicle; and determining optimum target position information corresponding to the target vehicle on the basis of the posterior probability distribution.

A navigation system according to claim <NUM> is provided. The navigation system comprises a plurality of sensors onboard a vehicle, with the sensors including at least one vision sensor, and a data storage unit onboard the vehicle, with the data storage unit including a map database comprising one or more terrain maps. A processer onboard the vehicle is operatively coupled to the sensors and the data storage unit. The processor includes a position probability density function (PDF) filter, which is operative to perform a method comprising: receiving image data from the at least one vision sensor, the image data corresponding to one or more terrain images, captured by the vision sensor, of a given area; receiving map data from the map database, the map data corresponding to a terrain map of the given area; generating a first PDF of one or more image features available in the image data; generating a second PDF of one or more map features available in the map data; generating a measurement vector PDF by a convolution of the first PDF and the second PDF; estimating a position vector PDF using a non-linear filter that receives the measurement vector PDF the position vector PDF being a PDF of the position and angular orientation of vehicle (<NUM>) relative to the one or more terrain maps; generating statistics from the estimated position vector PDF, the statistics including real-time measurement errors of position and angular orientation of the vehicle; extracting the generated statistics; and sending the extracted statistics to a navigation filter to fuse with measurements from other sensors onboard the vehicle, to estimate the position and angular orientation of the vehicle (<NUM>) relative to the one or more terrain maps. In another aspect of the invention, a method of navigating a vehicle according to claim <NUM> is provided.

In the following detailed description, embodiments are described in sufficient detail to enable those skilled in the art to practice the invention. It is to be understood that other embodiments may be utilized without departing from the scope of the invention. The following detailed description is, therefore, not to be taken in a limiting sense.

A statistical approach for determining real-time measurement errors for map based, vision navigation systems is described herein. The present approach determines real-time errors of the measurements computed by correlating features extracted from vision sensor measurements and features from a map by use of a position probability density function (PDF) filter, which is a nonlinear filter.

This statistical approach is particularly advantageous when using map based, vision navigation systems in GNSS denied environments, because such systems can use real-time position and angular orientation measurement errors in a navigation filter, and can address the limitations of pre-selected, gain scheduled measurement errors in navigation filters such as those mentioned previously. The present approach is different from prior methods, in that it estimates the entire position PDF of the vision sensor, and therefore the vehicle, relative to the map and computes real-time measurement errors of a vehicle's position and angular orientation.

The performance of map based, vision navigation systems depends on their ability to use terrain features to estimate a vehicle's position and angular orientation. The typical approach to selecting position and angular orientation measurement errors uses the following process. Several flight tests over multiple types of terrain are conducted that compare the vision system's position and angular orientation measurements to the true position and angular orientation. For example, sensor measurements of the ground are correlated with pre-existing maps of the same ground area to determine position and angular orientation measurements. A statistical analysis is then conducted to identify the largest position and angular orientation measurement errors (standard deviation) in different ground terrains. The largest position and angular orientation measurement errors for different ground terrains are either used directly or inflated to be used in navigation filters. In real-time applications, the measurement errors are pre-selected based on ground terrain, weather conditions, and the sensor performance characteristics.

Further, the typical approach fuses the position and angular orientation statistics with measurements from other sensors, such as an inertial measurement unit (IMU) or other aiding sensors, using a Kalman filter. The Kalman filter estimates the kinematic states of a vehicle under the assumption that the statistics of the kinematic states can be described using Gaussian probability density functions (PDFs). In effect, the Kalman filter only uses the first moment and second central moment of the sensor measurement statistics to estimate the first moment and second central moment of the statistics of the vehicle's kinematic states. The limitations of this approach are: <NUM>) higher order moments of the position and angular orientation statistics computed from the feature PDF correlation with the map are ignored because only the first two moments of the feature PDF correlation are used by the Kalman filter; and <NUM>) the constant measurement (error) covariance matrices do not reflect changing terrains, weather patterns, changing sensor performance characteristics, and do not reflect current feature availability. As a result, the Kalman filter estimates of the vehicle kinematic state statistics, or Kalman filter performance, are limited.

In the present statistical approach, a position PDF filter estimates the real-time position and angular orientation measurement errors for map-based, vision navigation systems, and the higher order moments of the position and angular orientation measurement statistics. The position PDF filter generally functions as follows.

A first PDF of image features is generated from image data obtained by a vision measurement of an area of the ground, and a second PDF of map features is generated from map data of the same area. The PDFs of the image features and map features refer to the likelihood that a region of the measurement or map contains a feature and statistically outlines the shape of the feature. A position PDF can be selected in a variety of ways, and contains all the moments of the PDF, as no approximations are made. The position PDF is estimated using a non-linear, Bayesian filter, such as a Fokker-Planck filter, which estimates statistics using prediction and update steps. For example, an update step is conducted using a convolution of the PDF measured by the vision sensor and the PDF of the map. The position PDF contains the statistics of the vehicle's position and angular orientation and it is a function of the real-time images. These statistics include the real-time measurement errors of the vehicle's position and angular orientation. These statistics are then fed to a navigation filter for integration with measurements from an IMU and/or additional aiding sensors.

Further details related to the implementation of the position PDF filter to determine real-time measurement errors for a map based, vision navigation system in a vehicle, is described hereafter.

As a vehicle such as an aircraft travels along a path, a vision sensor such as a camera mounted on the vehicle can view the terrain below the vehicle. Comparing an image from the camera to a stored-map image, allows computation of the vehicle's kinematic state statistics, including position and angular orientation with respect to the map, for use in a navigation filter.

A Probability Density Function (PDF) gives the probability of how the camera image needs to be shifted in [East, North], to match the camera image with the map image. In addition, image scale and rotation can also be computed. The PDF typically has multiple peaks since similar landmarks give several possible alignments where the images match. These peaks correspond to multiple possible positions for the vehicle. As time goes on, the camera/map alignment for the true position remains consistent, while other peaks that are not consistent (false positions) need to be filtered out.

The covariance of navigation measurements relates to the error in a vehicle's kinematic states measurements or estimates. In this context, the navigation covariance is equal to the inverse of information. This means that the more information (feature content) there is in an image, the smaller the covariance (or errors); whereas the less amount of information there is in an image, the larger the covariance.

There are various factors that affect the covariance of navigation measurements. For example, the navigation covariance can depend on available information such as landmark features, terrain features, geometry of the features, location of the features, vehicle altitude, edge pixels, and the like. This information can be identified by mean (first moment), covariance (second moment), higher-order moments, or raw edge pixels of an image.

Techniques such as scale invariant feature transform (SIFT) and speeded up robust feature (SURF) can be used to provide scale and rotation invariant features. Sensors such as electro-optical (EO) or infrared (IR) sensors can be used to save terrain features from four seasons of the year. While pixel edges are invariant to the season of the year, camera wavelength sensitivity can be provided by EO/IR sensors. Other information can be density of landmarks, contrast of landmarks, invariance of landmarks to seasons, cloud cover, etc. The variable density of landmarks can result in landmark sparseness in some regions for choosing a route. The terrain type can include forest, rural, urban, desert, ocean, and the like.

Other factors that affect the covariance of navigation measurements can include the flight path of an aircraft, including speed, attitude, and altitude of the aircraft along the flight path, and camera mounting angle, which affect the camera field of view (FOV). Other factors include pixel size on ground, camera contrast, and camera resolution. The camera resolution, contrast, diffraction, EO/IR, can all affect image quality. The camera lens FOV and camera mounting angle affect what the camera views. In addition, other sensors such an IMU need to maintain a navigation solution during times when no landmarks are visible.

<FIG> illustrates a system <NUM> for use in vehicle navigation, according to an exemplary embodiment, which can implement the present statistical approach. The system <NUM> includes a position PDF filter <NUM>, which is implemented in a processor <NUM> onboard a vehicle, such as an aircraft. The processor <NUM> is operatively coupled to at least one vision sensor <NUM>, such as a camera, which is mounted on the vehicle. The processor <NUM> is also operatively coupled to a data storage unit <NUM> onboard the vehicle. The data storage unit <NUM> includes a map database <NUM> that stores various terrain maps.

The position PDF filter <NUM> is operative to receive image data from vision sensor <NUM> through processor <NUM>. The image data corresponds to terrain images captured by vision sensor <NUM> of a given area over which the vehicle traverses. The position PDF filter <NUM> is also operative to receive map data from map database <NUM> through processor <NUM>, with the map data corresponding a terrain map of the given area.

The position PDF filter <NUM> is operative to generate statistics of the position and angular orientation of the vehicle, based on correlation of the image data and map data. The statistics include a mean and variance of the image/map correlation and the variance comprises real-time measurement errors of the position and angular orientation. The statistics generated by position PDF filter <NUM> are extracted by a processing unit <NUM>. As described further hereafter, the extracted statistics are sent to a navigation filter to fuse with measurements from other sensors onboard the vehicle, to estimate the position and angular orientation of the vehicle relative to the terrain map.

<FIG> is a flow diagram of an operational method <NUM> of a position PDF filter, such as position PDF filter <NUM>, according to an exemplary implementation. In method <NUM>, the position PDF filter receives image data from a vision sensor, such as vision sensor <NUM> (e.g., camera) mounted on a vehicle, with the image data corresponding to one or more terrain images captured by the vision sensor of a given area (block <NUM>). The position PDF filter also receives map data from a map database, such as map database <NUM>, with the map data corresponding to one or more terrain maps of the given area (block <NUM>). The method <NUM> generates a first PDF of one or more image features available in the image data (block <NUM>), and generates a second PDF of one or more map features available in the map data (block <NUM>). The method <NUM> then generates a measurement vector PDF by a convolution operation of the first PDF and the second PDF (block <NUM>). Thereafter, method <NUM> estimates a position vector PDF using a non-linear filter, such as a Fokker-Planck filter, which receives the measurement vector PDF (block <NUM>). The method <NUM> then generates statistics from the estimated position vector PDF, with the statistics including real-time measurement errors of position and angular orientation of the vehicle.

<FIG> illustrates a map based, vision navigation system <NUM>, according to an exemplary embodiment, which can implement the present statistical approach. The navigation system <NUM> includes a position PDF filter <NUM>, which is implemented in a processor <NUM> onboard a vehicle <NUM>, such as an aircraft. At least one vision sensor <NUM>, such as a camera, is mounted on vehicle <NUM>. The vision sensor <NUM> is operative to capture terrain images of a given area over which vehicle <NUM> traverses. A data storage unit <NUM> onboard vehicle <NUM> includes a map database <NUM> that stores various terrain maps.

The position PDF filter <NUM> is operative to receive image data from vision sensor <NUM> through processor <NUM>. The image data corresponds to the terrain images captured by vision sensor <NUM> of the given area. The position PDF filter <NUM> is also operative to receive map data from map database <NUM> through processor <NUM>, with the map data corresponding to a terrain map of the given area.

The position PDF filter <NUM> is operative to generate statistics of the position and angular orientation of vehicle <NUM>, based on correlation of the image data and map data. The statistics include a mean and variance of the image/map correlation and the variance comprises real-time measurement errors of the position and angular orientation. For example, position PDF filter <NUM> can perform a correlation between features extracted from a terrain image of a given area, and features in a terrain map of the given area, to estimate the PDF of the position and angular orientation of vehicle <NUM> relative to the map.

A non-linear filter such as the Fokker-Planck filter can be used to estimate a position PDF from a convolution of a PDF of the image features and a PDF of the map features. Multiple correlation peaks are possible following this convolution given the environment and the number of features. The Fokker-Planck filter can easily handle multiple correlation peaks, and no assumptions are made about the statistics of the PDF. The Fokker-Planck filter is a generalization of a Bayesian filter, which is a predict/correct filter. A Kalman filter is an example of a Bayesian filter under certain assumptions on the state vector and measurement vector statistics. Given a sequence of measurements, the Fokker-Planck filter will usually converge to a single dominant peak.

From the estimated position PDF, the mean and variance of the camera position relative to the map can be extracted. The mean is the measurement and the variance are the measurements error statistics. Further details and examples of how the position PDF filter operates are described hereafter.

The statistics generated by position PDF filter <NUM> are extracted by a processing unit <NUM>, and sent to a navigation filter <NUM>, such as a Bayesian filter. The navigation filter <NUM> fuses the extracted statistics from position PDF filter <NUM> with measurement statistics from other sensors, to estimate the vehicle's position and angular orientation relative to the terrain map. These other sensors include an IMU <NUM> and one or more aiding sensors <NUM>, which are mounted on vehicle <NUM>. Examples of aiding sensors <NUM> include a magnetometer, a GNSS receiver such as a Global Positioning System (GPS) receiver, a barometer, or the like. The IMU <NUM> is operative to produce inertial measurements that are sent to an inertial navigation system (INS) <NUM>, such as a strapdown INS, which is operatively coupled to navigation filter <NUM>.

The INS <NUM> is configured to generate estimated vehicle kinematic state statistics (including position, velocity, and angular orientation) based on integration of the inertial measurements from IMU <NUM>. However, errors can occur when the inertial measurements are integrated by INS <NUM>, as the IMU <NUM> can have measurement errors that, when integrated, grow over time. The statistics generated by position PDF filter <NUM> are used by navigation filter <NUM> to aid in correction of errors in the estimated vehicle kinematic state statistics.

The navigation filter <NUM> includes a prediction module <NUM>, and a correction module <NUM> configured to receive an output from prediction module <NUM>. The prediction module <NUM> is configured to receive the estimated vehicle kinematic state statistics produced by INS <NUM>. The prediction module <NUM> is operative to predict a state vector PDF from the estimated vehicle kinematic state statistics. The correction module <NUM> receives the predicted state vector PDF from prediction module <NUM>. The correction module <NUM> also receives the extracted statistics from processing unit <NUM>, and sensor measurement statistics from aiding sensors <NUM>. The correction module <NUM> is configured to generate an error correction signal based on the various received inputs.

The error correction signal is sent from correction module <NUM> to an input of a subtractor <NUM>, which is also configured to receive an output signal from INS <NUM>. The differential between the signals from correction module <NUM> and INS <NUM> is output from subtractor <NUM> as updated estimated vehicle kinematic state statistics. These updated statistics are sent to other vehicle systems for use in vehicle <NUM> operations, and also fed back to navigation filter <NUM> for further processing.

The measurement PDF, which is the actual position of the camera/vehicle relative to a map, is estimated by a correlation of the image PDF and the map PDF. The measurement PDF often has multiple measurement peaks representing possible positions of the vehicle. The position PDF filter gradually converges to a true measurement peak since the other peaks fluctuate over time. In addition, the correlation of images produces a shift. Thus, correlating the relative position shift and rotation of consecutive images can be done using convolution and the Fast Fourier Transform (FFT). Such a technique is described in Reddy et al. , <NPL>), the disclosure of which is incorporated by reference herein.

A nonlinear filter such as a Fokker-Planck filter can be employed over time to predict and correct a PDF that describes the vehicle position relative to the map. The measurement PDF is fed into the Fokker-Planck filter, which estimates the position PDF over time. The Fokker-Planck filter can be used to incorporate process noise and image translation, as part of the position PDF filter. Bayesian formulas can be used to update the PDF, based on new image measurements. A technique for using the Fokker-Planck filter and a Bayes measurement update is described in <NPL>), the disclosure of which is incorporated by reference herein.

As described in Challa et al. , a continuous time stochastic dynamic system (SDS) can be described by the following expression: <MAT> where Xt = [x<NUM>, x<NUM>,. , xn]'t represents the state vector of the system at any time t, F(Xt, t) is a linear/nonlinear vector valued function with real components, G(Xt, t) is an n × m real matrix, and ηt is a zero-mean, white Gaussian noise process vector having autocorrelation E[ηtnt, ] = Qtδ(t - t'). Observations Ytk of this system are taken at discrete time instants tk and the relationship between the state vector of the system of the observations can be described by the following expression: <MAT> where H(Xtk, tk) is a linear/nonlinear function of the state vector of the SDS and vtk is a white Gaussian measurement noise vector with covariance matrix Rtk. It is also assumed that the initial state vector, the process noise vector, and the measurement noise vector are independent. A filter is used to estimate the state vector statistics conditioned on the measurement vector Ytk in a minimum mean square error (MMSE) sense.

Under the assumption that the prior density for the system exists and is once continuously differentiable with respect to t and twice continuously differentiable with respect to Xt, it can be shown that, between observations, the conditional density p〈X, t|Ytk-<NUM>〉 satisfies the Fokker-Planck-Kolmogorov equation (FPKE): <MAT> where p〈X, t|Ytk-<NUM>〉, Fi(Xt, t), G(X, t), Qt are replaced by p, Fi, G, and Q, respectively, for the sake of simplicity.

Following an observation at tk the conditional density satisfies Bayes' formula: <MAT> where <MAT>. Equation (<NUM>) applies to general PDFs p(Ytk|X) that may have multiple peaks. An example with Gaussian PDF, which has a single peak, would result in p(Ytk|X) being of the form: <MAT> Equation (<NUM>) can be used to define image motion due to vehicle motion, and governs the time evolution of the position PDF. Equation <NUM> can be used to define the relationship between the measurement PDF and the predicted PDF. Equations (<NUM>) and (<NUM>) above are the predictor and corrector equations of the probability density function evolution method, respectively. The mean of the conditional PDF obtained from equation (<NUM>) gives the optimal estimated state vector statistics.

<FIG> is a block diagram of a nonlinear filter <NUM> using the solution methodology presented above with respect to equations (<NUM>) to (<NUM>), which can be implemented in a position PDF filter of the present navigation system. The nonlinear filter <NUM> includes a FPKE solver <NUM> (predictor), which is used in conjunction with a Bayes' formula solver <NUM> (corrector) to provide optimal estimates for continuous or discrete nonlinear filtering. The FPKE solver <NUM> is configured to receive system dynamics information, such as a dynamic model that is used to predict the time evolution of the position PDF. The FPKE solver <NUM> is operative to generate and output a predicted PDF based on the system dynamics information. The Bayes' formula solver <NUM> is configured to receive the measurement PDF generated by the convolution of the image feature PDF and the map feature PDF. The Bayes' formula solver <NUM> is also configured to receive the predicted PDF from the FPKE solver <NUM>. The Bayes' formula solver <NUM> is operative to generate and output a corrected (updated) predicted PDF. The corrected (updated) predicted PDF is also sent to the FPKE solver <NUM> in a feedback loop. As the FPKE solver <NUM> is implemented with a partial differential equation, it can be solved using one of the various techniques available for solving such equations.

In the measurement stage, a measurement PDF (PDF_meas) is equal to the convolution (camera and map PDFs), and may have multiple peaks. Although the measurement PDF can have multiple peaks, this is acceptable since the Fokker-Planck filter updates the entire PDF. In the measurement update stage, the position PDF filter update multiplies the state and measurement PDFs.

The convolution could be implemented by sliding a camera image over a map image, and counting the number of overlapping pixels at each shift value. Thus, the pixels between the camera image and the map image will either match or not when the camera image is slid around the map image. The method of sliding the image, one pixel at a time, and comparing pixels at each slide position, is too slow. The same result can be obtained much more efficiently, by noting that multiplication in the frequency domain, is equivalent to convolution in the spatial domain. So, taking the FFT of each image, multiplying them, then taking an inverse FFT, gives the convolution result in one step. An example of a convolution to generate a measurement PDF is shown in <FIG>.

<FIG> is a modeled camera image <NUM> having <NUM> by <NUM> pixels, which shows a ground region <NUM>. The camera image <NUM> shows the outline edges of a first house <NUM> by a road <NUM> (in a binary distribution). <FIG> shows a modeled map image <NUM> having <NUM> by <NUM> pixels, which shows the same ground region <NUM>. However, the map image <NUM> shows the outline edges of the first house <NUM> and a second house <NUM> by the road <NUM> (in a binary distribution). The map image was made at a different time than the camera image, so a different number of houses exist at those two times.

<FIG> graphically depicts a measurement PDF <NUM> based on a convolution of a PDF of the camera image <NUM> and a PDF of the map image <NUM> ((PDF meas) = convolution(camera image, map image)). As shown in <FIG>, the measurement PDF <NUM> has multiple peaks, and the center peak has <NUM> shift [East, North] from the origin of the reference frame. Other peaks occur where the edges of the first house <NUM> shift to the edges of the second house <NUM> or the road <NUM>. The highest correlation peak gives the correct shift of the camera image <NUM> with respect to the map image <NUM>.

A vision sensor such as a camera on a vehicle observes the underlying terrain to produce a camera image, and convolution with map features of the terrain, gives a position measurement. If the terrain has localizable features, then the convolution(camera image, map image) provides the position of the camera, and vehicle, relative to the map. The covariance of the convolution then becomes the covariance matrix, R, of the position measurement. The covariance matrix R along a path through terrain that contains features is time-varying along that path. Some paths will have lower covariance than other paths through the same terrain.

Thus, a covariance matrix R(path(t)) will have a smaller norm, ∑iσi (R(path(ti))), for paths that pass over more features. For example, P(path(t)) ellipses, as well as Rj(path(t)) ellipses, can be compared for different paths though one map. Any PDF with the same covariance matrix R, will result in the same ellipses with differences in the higher moments of the PDF. In the present approach, the nonlinear filter (e.g., Fokker-Planck filter) allows for making use of the whole PDF, instead of just its mean and covariance.

In an example, a [Z(k), R(k)] ellipse, with center of the ellipse Z and covariance matrix R giving the principle axes of the ellipse, approximates the convolution(camera image, map image). Portions of the SDS (F, B, Q, and H) can be constant, but an R(k) matrix is generated from the convolution(camera image, map image) and varies as the camera travels over the terrain. The dynamic model is x(k) = F*x(k-<NUM>) + B*u(k) + G*w(k) with a four dimensional state vector, with East and North position (E and N) and East and North velocity (E_dot and N_dot) x' = [E N; E_dot N_dot] with dynamic matrix F = [I, I*dt; <NUM>, I], and process noise covariance matrix = Q. The measurement model is y(k) = H*x(k) + v(k) with measurement noise covariance matrix R(k) which forms 2D edge-pixel ellipses and H = [I, <NUM>].

In a predict step: <MAT> where F moves the ellipse-center of the state covariance matrix P in the velocity direction; and <MAT> where Q expands the P ellipse.

In an update step: <MAT> and if this error is small, then the correction to x will be small; <MAT> with R(k) being large if there are no features, and the semi-major-axis >> the semi-minor-axis near line features; <MAT> so inv(S(k)) and K(k) are very small when there are no features; <MAT> is nearly x(k,k-<NUM>) when there are no features; <MAT> so P(k,k) is nearly P(k,k-<NUM>) when there are no features.

An example resulting from the computation of a [z(k), R(k)] ellipse is shown in <FIG> is a camera image <NUM> with a <NUM> by <NUM> pixel resolution, showing a rural area <NUM>. <FIG> is a map image <NUM> of the same rural area <NUM>, showing vehicle path points. The map image <NUM> shows vehicle position measurements computed from correlated images in the form of ellipses, where the center of each ellipse is the measurement mean vector and the ellipse shape and size represent measurement uncertainty.

A depicted in <FIG>, a set of R(k) ellipses <NUM> are plotted, and a series of P(k) ellipses <NUM> are plotted, along the same vehicle path on map image <NUM>. The R(k) ellipses <NUM> approximate the shape of image pixels near the vehicle, and the P(k) ellipses <NUM> represent the shapes of the region that contain the estimates of the vehicle's position. The center of each P(k) ellipse <NUM> is the current best estimate of the vehicle's position at the corresponding time, while the shape and size of each P(k) ellipse <NUM> corresponds to the level of uncertainty in the vehicle's position.

The [z(k), R(k)] ellipse is computed from the convolution(camera image, map image) in a <NUM>-by-<NUM> square near the vehicle location at time k, where R is the covariance matrix of the convolution(camera image, map image) in the n-by-n square near the vehicle. If the n-by-n square is empty for some time k, then there is no measurement, so the matrix R(k) is large, inv(S(k)) = <NUM>, K(k) = <NUM>, and the filter continues to predict the state vector statistics without a measurement update. The paths are made by plotting the covariance matrix R(k) from the convolution(camera image, map image) at the path location at time k. When the n-by-n square is empty, the covariance matrix R(k) is not plotted.

As mentioned above, the R(k) ellipses <NUM> and the P(k) ellipses <NUM> are plotted along the same path on map image <NUM> in <FIG>. Thus, the measurement PDF [z(k),R(k)] = convolution(camera image, map image). The posterior state PDF [x(k,k),P(k,k)] combines the measurement vector PDF [z(k),R(k)] with the prior state vector PDF [x(k,k-<NUM>),P(k,k-<NUM>)] using the SDS modeled using the matrices [F, G, Q, H].

This example uses <NUM> x <NUM> segments of the map image <NUM> for portions of the map under the vehicle's flight trajectory and its <NUM> x <NUM> pixel camera. The moments [z(k),R(k)] give the ellipse that is best fit to the measurement PDF. If the camera image <NUM> is equal to the shifted map image <NUM>, then the measurement PDF is a single point at that [East, North] shift value. If the camera image <NUM> and the map image <NUM> both contain a single line segment (e.g., a road), then the measurement PDF is a line segment. The Kalman filter updates the prior state vector PDF [x(k,k-<NUM>),P(k,k-<NUM>)] using the measurement vector PDF = [z(k),R(k)] and the measurement matrix H. Between measurements, the state vector PDF statistics [x(k),P(k)] are predicted forward in time using the SDS model matrices [F,G,Q] so that the mean vector moves and the state covariance matrix grows.

The state vector PDF gives the location and shape of the region of the vehicle's most likely location. The measurement vector PDF is the convolution(camera image, map image). The measurement vector PDF is a single shift point if the camera image is equal to the shifted map image. The measurement vector PDF is an ellipse if the camera image is a blurry approximation of the map image. In this case, the measurement vector PDF has wide peaks, so the corresponding covariance matrix is large. The measurement vector PDF is a line segment if the camera image and map image both have a single line. The measurement vector PDF has m peaks if the camera and map images have m parallel lines (e.g., from roads or buildings). The measurement vector PDF is undefined, if the camera image is empty (e.g., over water with no features).

When the convolution is done in the frequency domain, there is a runtime of O(N*log(N)) for N pixels in each image.

Bayesian filters can be used to predict the state vector PDF, using model matrices and process noise vector statistics. The Bayesian filters update the predicted, or prior, state vector PDF, by multiplying the current state PDF and measurement PDF, and then normalizing. A Gaussian example is given as follows.

The state vector PDF is represented by the expression: <MAT> where x<NUM> is the mean vector of the state PDF. The measurement vector PDF is represented by the expression: <MAT>.

The state vector PDF is updated by multiplying the state vector PDF and the measurement vector PDF, then normalizing, as follows: <MAT> where <MAT> Consider a generalized form of <MAT>. By comparing the two equations for pnew(x) and equating coefficients, <MAT> HT(R(k))-<NUM>H. Since information is the inverse of covariance, this expression shows that the new information is the sum of the predicted, or prior, information and the measurement information. The notation can be updated to x(k,k), x(k,k-<NUM>), P(k,k-<NUM>), P(k,k), etc..

<FIG> are example plots of updated state vector PDFs, which are described further as follows.

<FIG> is an example plot <NUM> with Gaussian PDFs, in which the updated state vector PDF is equal to the predicted state vector PDF multiplied by the measurement vector PDF, and then normalized. In a linear model: <MAT> The state vector PDF is represented by the expression: <MAT> The measurement vector PDF is represented by the expression: <MAT>.

In <FIG>, the plot <NUM> shows the predicted, or prior, state vector PDF, the measurement vector PDF, and the updated, or posterior, state vector PDF. The measurement vector PDF uses the [z, R] ellipses as the first two moments of pmeas. <FIG> shows that either the Kalman filter equations or the direct Bayesian filter equations can be used to update the state vector PDF. In the case of Gaussian distributions, the Kalman filter equations are more efficient than multiplying PDFs in the direct Bayesian filter equations. But for general non-Gaussian PDFs, the direct Bayesian filter equations have to be used to update the predicted state vector PDF.

<FIG> is an example plot <NUM> with general (non-Gaussian) PDFs and the direct Bayesian filter equations, in which the updated state vector PDF is computed by multiplying the predicted state vector PDF by the measurement vector PDF, and then normalizing. In a nonlinear model: <MAT>.

To predict the state vector PDF: <MAT>. To update the state vector PDF: p(x) = p(x) * pmeas(x), which is then normalized.

In <FIG>, the plot <NUM> depicts a general PDF for pmeas. The plot <NUM> shows that for arbitrary PDFs, the updated state vector PDF can be computed by multiplying the predicted state vector PDF and the measurement vector PDF.

The Fokker-Planck equation predicts and updates the general state vector PDF, using nonlinear models, and a general measurement vector PDF. In a nonlinear dynamic model: <MAT> where x ∈ Rn and w is a Gaussian noise vector with covariance Q. The state vector PDF is p(x, t), which is predicted forward in time using the expression: <MAT> where -∇T(p * f) is drift, and <MAT> is diffusion. The measurement vector PDF is pmeas(x, t) where pmeas = convolution(camera image, map image). The update of the predicted state vector PDF is computed using the expression: <MAT> where p(x, t| y(k - <NUM>)) is the predicted state vector PDF and pmeas(t, y(k - <NUM>)|x) is the measurement vector PDF.

The Fokker-Planck filter can be implemented along any path in a map image, since the Fokker-Planck filter can use any measurement vector PDF including a PDF that has multiple peaks. The state vector PDF is any positive function that sums to <NUM>. In this case, the state vector PDF is an NxN image, which is predicted and corrected by the Fokker-Planck equations. The measurement vector PDF is any positive function that sums to <NUM>. The Fokker-Planck filter provides the general measurement PDF, by the convolution of the camera image with the map image: pmeas(x) = convolution(camimage, mapimage) where pmeas(x) is an NxN image with multiple peaks.

<FIG> and <FIG> show example simulations of the operation of a position PDF filter for a vehicle traveling along a path, which uses a Fokker-Planck filter to process camera and map images. These simulations used an evolving <NUM> x <NUM> pixel state vector PDF for each step along the path, where <MAT> was used to predict the vehicle state vector PDF forward in time. Fourier transforms of the camera image and map image were then performed to efficiently compute the convolution of the camera and map images and to compute the measurement vector PDF.

In these examples, a vehicle follows a two-dimensional path: x(tk) = [East(tk); North(tk)]. Initially, the state vector PDF starts with multiple peaks representing different possible vehicle positions. The prediction step uses f, g, Q from the nonlinear model. The update step uses the measurement vector PDF computed from the convolution of camera and map images to update the predicted state vector PDF and compute the updated state vector PDF. The update step of the Fokker-Planck filter reduces the secondary peaks in the state vector PDF over time.

The simulation of <FIG> used a camera image <NUM> of a simple terrain, and a map image <NUM> of the simple terrain, where k = measurement times (e.g., <NUM> second for frame rate of camera) for each step along the path. In particular, for k = <NUM> to k = <NUM>, the first (top) row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> show features extracted from camera image <NUM>, and the second row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> show features extracted from map image <NUM>. The third row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> is the measurement PDF, which is determined by the convolution of the camera image (first row) and map image (second row) at a given time (k = <NUM>, k = <NUM>, etc.). The plots of the measurement PDF have lines if the camera image and map image have lines. The fourth (bottom) row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> is the updated state vector PDF computed by the Fokker-Planck filter. The plots of the updated state vector PDF start with multiple peaks (plot <NUM>), but converge to a single-peak updated state vector PDF (plot <NUM>). The mean vector and state covariance matrix are extracted as statistics from the fourth row of plots - the updated state vector PDF, and would be fed to the navigation filter in the present system. The mean vector corresponds to the camera measurement, and the state covariance matrix is the error of the vehicle position relative to the map.

For example, plot <NUM> shows the convolution of plot <NUM> and plot <NUM> at k = <NUM>. The large number of white dots in plot <NUM> means that the measurement vector has many peaks, such that there is a lot of ways to match features between the camera image and map image when shifted left or right. The plot <NUM> (output of Fokker-Planck filter) is the updated position vector PDF based on the measurement vector PDF from plot <NUM>. This process repeats itself for each measurement time (k = <NUM>, k = <NUM>, etc.) to provide an updated position vector PDF at each time. As the process proceeds, more information is obtained at each measurement time, so many of the earlier correlation peaks are reduced.

The simulation of <FIG> used a camera image <NUM> of a rural area, and a map image <NUM> of the rural area, where k = measurement times (e.g., <NUM> second for frame rate of camera) for each step along the path. In particular, for k = <NUM> to k = <NUM>, the first row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> show features extracted from camera image <NUM>, and the second row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> show features extracted from map image <NUM>. The third row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> is the measurement PDF, which is determined by the convolution of the camera image (first row) and map image (second row) at a given time (k = <NUM>, k = <NUM>, etc.). The fourth row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> is the updated state vector PDF computed by the Fokker-Planck filter. The plots of the updated state vector PDF start with multiple peaks (plot <NUM>), but converge to a single-peak updated state vector PDF (plot <NUM>). The mean vector and state covariance matrix are extracted as statistics from the fourth row of plots - the updated state vector PDF, and would be fed to the navigation filter in the present system.

For example, plot <NUM> shows the convolution of plot <NUM> and plot <NUM> at k = <NUM>. The plot <NUM> (output of Fokker-Planck filter) is the updated position vector PDF based on the measurement vector PDF from plot <NUM>. This process repeats itself for each measurement time (k = <NUM>, k = <NUM>, etc.) to provide an updated position vector PDF at each time. As the process proceeds, more information is obtained at each measurement time, so many of the earlier correlation peaks are reduced.

<FIG> relate to an actual flight path example of the present approach, in which an aircraft was flown over a suburban area (Sun Lakes, AZ). <FIG> is a map <NUM> (from Google Earth) of the suburban area and shows a flight path <NUM> for the aircraft, which had a flight direction of North by North-West. As the aircraft was flying, a camera on the aircraft captured images of the underlying area and an onboard computer processed the images at about one frame per second. The position PDF filter operated on the post-processed data, and provided updates based on the image capture rate and the computer computation speed.

<FIG> show edge images that were obtained using an edge detector, based on actual images captured by the camera on the aircraft along flight path <NUM>. In particular, a first row of camera edge images <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> show edge features that were extracted from the camera images, and a second row of map edge images <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> show features that were extracted from the corresponding map images. A third row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> are the measurement vector PDFs, which were determined by the convolution of the camera edge images (first row) and the map edge images (second row) at a given time (k = <NUM>, k = <NUM>, etc.). A fourth row of plots <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM>, <NUM> are the updated position vector PDFs that were output by a Fokker-Planck filter. The plots of the position vector PDF start with multiple peaks (plot <NUM>, <FIG>), but converge to a single-peak estimated-position solution (plot <NUM>, <FIG>). The mean vector and state covariance matrix were extracted as statistics from the fourth row of plots in <FIG>, and were output as real-time measurement mean vectors and their errors to the navigation filter in the aircraft.

<FIG> respectively show enlarged views of the measurement vector PDF and the updated position vector PDF at the last time step (from plots <NUM> and <NUM> in <FIG>). The standard deviation of the measurement vector PDF was <NUM> times greater than the standard deviation of the updated position vector PDF.

<FIG> respectively show graphical representations of the center part of the measurement vector PDF (PDFmeas) and the updated position vector PDF (PDFk+<NUM>), from <FIG>, at the last time step.

The processing units and/or other computational devices used in the method and system described herein may be implemented using software, firmware, hardware, or appropriate combinations thereof. The processing unit and/or other computational devices may be supplemented by, or incorporated in, specially-designed application-specific integrated circuits (ASICs) or field programmable gate arrays (FPGAs). In some implementations, the processing unit and/or other computational devices may communicate through an additional transceiver with other computing devices outside of the navigation system, such as those associated with a management system or computing devices associated with other subsystems controlled by the management system. The processing unit and/or other computational devices can also include or function with software programs, firmware, or other computer readable instructions for carrying out various process tasks, calculations, and control functions used in the methods and systems described herein.

The methods described herein may be implemented by computer executable instructions, such as program modules or components, which are executed by at least one processor or processing unit. Generally, program modules include routines, programs, objects, data components, data structures, algorithms, and the like, which perform particular tasks or implement particular abstract data types.

Instructions for carrying out the various process tasks, calculations, and generation of other data used in the operation of the methods described herein can be implemented in software, firmware, or other computer readable instructions. These instructions are typically stored on appropriate computer program products that include computer readable media used for storage of computer readable instructions or data structures. Such a computer readable medium may be available media that can be accessed by a general purpose or special purpose computer or processor, or any programmable logic device.

Suitable computer readable storage media may include, for example, non-volatile memory devices including semi-conductor memory devices such as Random Access Memory (RAM), Read Only Memory (ROM), Electrically Erasable Programmable ROM (EEPROM), or flash memory devices; magnetic disks such as internal hard disks or removable disks; optical storage devices such as compact discs (CDs), digital versatile discs (DVDs), Blu-ray discs; or any other media that can be used to carry or store desired program code in the form of computer executable instructions or data structures.

Claim 1:
A navigation system (<NUM>, <NUM>), comprising:
a plurality of sensors onboard a vehicle (<NUM>), the sensors including at least one vision sensor (<NUM>, <NUM>);
a data storage unit (<NUM>, <NUM>) onboard the vehicle, the data storage unit including a map database (<NUM>, <NUM>) comprising one or more terrain maps; and
a processor (<NUM>, <NUM>) onboard the vehicle, the processor operatively coupled to the sensors and the data storage unit, the processor including a position probability density function, PDF, filter, wherein the position PDF filter (<NUM>, <NUM>) is operative to perform a method (<NUM>) comprising:
receiving image data from the at least one vision sensor, the image data corresponding to one or more terrain images, captured by the vision sensor, of a given area;
receiving map data from the map database, the map data corresponding to a terrain map of the given area;
generating a first PDF of one or more image features available in the image data;
generating a second PDF of one or more map features available in the map data;
the navigation system being characterised in that the position PDF filter is further configured to perform the steps of:
generating a measurement vector PDF by a convolution of the first PDF and the second PDF;
estimating a position vector PDF using a non-linear filter that receives the measurement vector PDF,
the position vector PDF being a PDF of the position and angular orientation of vehicle relative to the one or more terrain maps obtained by correlation between features extracted from a terrain image of the given area and the terrain map of the given area;
generating statistics from the estimated position vector PDF, the statistics including real-time measurement errors of position and angular orientation of the vehicle,
extracting the generated statistics; and
sending the extracted statistics to a navigation filter (<NUM>) to fuse with measurements from other sensors among the plurality of sensors onboard the vehicle, to estimate the position and angular orientation of the vehicle relative to the one or more terrain maps.