Patent Description:
Specifically, the state estimator estimates the kinematic state vector of the vehicle utilizing a two-step process. In the first step, system models are utilized to predict the kinematic state vector forward to the epoch of an available measurement from the sensor set. This estimate is considered as the new predicted state vector. In the second step, the state estimator processes the navigation measurements (e.g., sensor measurements) from the sensor set to update the predicted kinematic state vector. This estimate is considered as the updated (filtered) state vector. The state estimator attempts to reconcile the predicted state vector and measurement vector from the sensor set to obtain updated estimates of the state vector under the assumption that both the predicted state vector and measurement vector are uncertain. However, the system models, or equations, that are utilized to estimate the kinematic, or navigation, state vector are non-linear and, therefore, the navigation system requires an EKF to estimate the statistics of the navigation state vector.

Notably, state estimation algorithms for state vectors governed by nonlinear systems can be divided into the two groups: algorithms utilizing global filters and algorithms utilizing local filters. The global filter-based algorithms can provide consistent estimates of the state vector for almost all types of nonlinearities of the system models without the assumption that the system models are locally linear. Global filters estimate the conditional probability density functions (PDFs) of the state vector that depend on the system models, system uncertainty, and the navigation measurements. These global filter techniques are suitable for estimating the state vector governed by highly nonlinear or non-Gaussian systems, but these state vector estimates are obtained at the cost of substantially high computational demands. Examples of the global filters are the particle filter or the point-mass filter.

In contrast, the local filter-based estimating techniques (e.g., Unscented Kalman Filters (UKFs) and EKFs) can be utilized to extend the capabilities of the Kalman filter so that it can be utilized to estimate the statistics of the state vector with approximations on the nonlinear systems. For example, first-order local filter (e.g., the first-order EKF) techniques can provide computationally efficient estimates of the statistics of the state vector in the form of the mean and covariance matrix of the conditional PDF. However, these local filter-based techniques have limited performance in terms of consistency, stability, and convergence. For example, the local filter estimates are generally inconsistent and unstable, primarily, due to the approximation of local linearity of the system models and the assumption that the statistics of vectors (state, measurement, and uncertainty) are Gaussian. Additionally, among the local filters, there are differences. In general, the high-order local filters (e.g., UKF) produce estimates with better consistency, stability, and convergence than the first order local filters (e.g., EKF).

For the reasons stated above and for other reasons stated below which will become apparent to those skilled in the art upon reading and understanding the specification, there is a need in the art for a statistical technique that can be utilized to monitor the consistency and convergence of the local filter's output and, thereby, enhance the integrity of the navigation system utilizing the measurement vectors provided by the sensor set.

& Orejas, M. & Soták, Miloš & Dunik, Jindrich. Architectures for high integrity multi-constellation solution separation. 27th International Technical Meeting of the Satellite Division of the Institute of Navigation, ION GNSS <NUM>. <NUM>-<NUM>. Relates to the integrity monitoring of general hybrid navigation systems. In particular, the High Integrity Multi-Constellation Solution Separation method is proposed and validated by simulations. The method is capable of providing the integrity of the navigation information of 1e-<NUM>/hr (in terms of probability of hazardous misleading information per hour) or lower if the multi-constellation navigation system is considered.

<CIT> discloses a method for providing integrity for a hybrid navigation system using a Kalman filter. The method includes determining a main navigation solution for one or more of roll, pitch, platform heading, or true heading for the vehicle using signals from a plurality of GNSS satellites and inertial measurements. Solution separation is used to determine a plurality of sub-solutions for the main navigation solution. The method also includes determining a separation between the main navigation solution and each of the sub-solutions, and a discriminator for each of the separations. If a discriminator is greater than the threshold the GNSS satellite at fault can be excluded. The method also includes determining a separation variance between the main navigation solution and each of the sub-solutions, a threshold for detection of a satellite failure based on the separation variances; and a protection limit that bounds error in the main navigation solution based on the threshold.

<CIT> discloses a method of advanced receiver autonomous integrity monitoring of a navigation system and two modifications facilitating its implementation in a hybrid navigation system. In the first approach, relations describing the effect of unmodeled biases in pseudo-measurement on the Kalman filter state estimate are analytically derived and their incorporation into the integrity monitoring algorithm is described. The method comprises receiving a plurality of signals transmitted from spaced-based satellites, determining a position full-solution and sub-solutions, specifying a pseudorange bias, computing a transformation matrix for the full-solution and all sub-solutions using a Kalman filter, computing a bias effect on an error of filtered state vectors of all sub-solutions, and adding the effect to computed vertical and horizontal protection levels. In the second approach, a modification for computationally effective calculation of the protection levels of hybrid navigation systems based on both integrity and non-integrity assured pseudorange error descriptions is disclosed.

discloses developments in an improved particle filtering algorithm for nonlinear states estimation. The proposed algorithm consists of a particle filter based on minimizing the Kullback-Leibler divergence distance to generate the optimal importance proposal distribution. The proposed algorithm allows the particle filter to incorporate the latest observations into a prior updating scheme using the estimator of the posterior distribution that matches the true posterior more closely.

The present invention in its various aspects is as set out in the appended claims. The present invention provides a statistical technique that can be utilized to monitor the consistency, stability, and convergence of the local filter's output, and enhance the integrity of the output of a navigation system utilizing a plurality of filters that operate on measurements from a sensor set.

Embodiments of the present disclosure can be more easily understood and further advantages and uses thereof more readily apparent, when considered in view of the description of the preferred embodiments and the following figures in which:.

In accordance with common practice, the various described features are not drawn to scale but are drawn to emphasize features relevant to the present disclosure. Reference characters denote like elements throughout the figures and text.

In the following detailed description, reference is made to the accompanying drawings that form a part hereof, and in which is shown by way of specific illustrative embodiments in which the embodiments may be practiced. These embodiments are described in sufficient detail to enable those skilled in the art to practice the embodiments, and it is to be understood that other embodiments may be utilized and that logical, mechanical, and electrical changes may be made without departing from the scope of the present invention as defined by the appended claims. The following detailed description is, therefore, not to be taken in a limiting sense.

The present invention provides a technological improvement over existing navigation system techniques for monitoring the output performance of state estimators. As such, the present invention achieves an improved technological result in the existing navigation system practice of monitoring the integrity of the navigation measurement information received, as described in more detail below.

<FIG> is a block diagram illustrating a statistical technique <NUM> that can be utilized to monitor the output performance of a local filter-based state estimator, in accordance with one example embodiment of the present invention. For example, in one embodiment, the local low-order filter-based state estimator can be implemented with a first-order EKF. In a second embodiment, the state estimator can be implemented with any suitable high-order local filter (e.g., UKF, second-order EKF) depending on the required accuracy of the estimated state vector statistics and/or the computational complexity supported by the navigation system. In some embodiments, the statistical technique <NUM> can be utilized for estimation (or prediction) of measurement information received from navigation systems, such as, GPS measurement information, hybrid GPS/INS measurement information, Attitude and Heading Reference System (AHRS) measurement information, GPAHRS measurement information, and the like.

Note that, for some embodiments, the statistical technique <NUM> can be deemed more suitable, for example, if the regions of linear validity for the EKF-based "full-solution" and each of the EKF-based "sub-solutions" are substantially the same. If so, then as indicated by the exemplary embodiment illustrated in <FIG>, the set of EKFs being utilized is extended with a global or high-order local filter (e.g., G/HF), which performs the same or similar estimation tasks as the EKF-based "full-solution" filter and thus enhances the ability of the state estimator to provide consistent, stable, and converging state vector estimates at its output. If the statistical estimates of EKF and G/HF based "full-solution" filters are substantially equivalent, then the assumptions of local linearity of the EKF system model are deemed valid.

Referring now to the exemplary embodiment for the statistical technique <NUM> illustrated in <FIG>, a suitable (e.g., navigation) system model and a plurality of (e.g., navigation) measurements <NUM> are provided as inputs to a "full-solution" G/HF <NUM> and a "full-solution" EKF <NUM>. Also, the system model and a plurality of sub-sets of the navigation measurement information <NUM> are provided as inputs to a plurality of "sub-solution" EKFs <NUM>(<NUM>)-<NUM>(N). The outputs of the "full-solution" G/HF <NUM> and the "full-solution" EKF <NUM> are statistically compared to determine the consistency of the output <NUM> of the "full-solution" EKF, and the EKF-based "full-solution" <NUM> is statistically compared with the EKF-based "sub-solution" EKFs <NUM>(<NUM>)-<NUM>(N) utilizing, for example, a known solution separation technique in order to monitor the integrity of the navigation measurement information measured by the sensor set <NUM>.

<FIG> is a flow diagram illustrating a method <NUM>, which can be utilized to implement one example embodiment of the present invention. For example, the method <NUM> can be utilized to implement the Statistical Technique <NUM> illustrated in <FIG>. As such, referring to the exemplary embodiment illustrated in <FIG>, the method <NUM> begins (e.g., at a time epoch denoted as "k") by computing an estimate of the received navigation information with the G/HF "Full-Solution" utilizing the available system model and the received navigation measurement information (<NUM>). For this embodiment, this computed estimate can be described in the form of a PDF denoted as pG/HF,full(x). Next, the method <NUM> computes an estimate of the received navigation information with the EKF "Full-Solution" utilizing the available system model and measurement information (<NUM>). For this embodiment, this computed estimate also can be described in the form of a PDF denoted as pEKF,full(x). The method then statistically compares the two computed estimates, pG/HF,full(x) and pEKF,full(x) (<NUM>), and determines if the estimates are substantially consistent (<NUM>). If the statistical comparison indicates that the two estimates, pG/HF,full(x) and pEKF,full(x) are substantially consistent (e.g., the EKF full-solution is deemed "healthy" and thus substantially consistent with the G/HF full-solution), then the method computes the measurement integrity-related parameters utilizing the EKF-based "Sub-Solutions" (<NUM>). Specifically, utilizing the system model, EKF, and sub-sets of the received navigation measurements, the method computes the EKF-based "Sub-Solutions", pEKF,sub,n(x), wherein n = <NUM>,.

However, returning to (<NUM>), if the statistical comparison indicates that the two estimates, pG/HF,full(x) and pEKF,full(x) are not substantially consistent, then the method <NUM> is terminated. Notably, only one G/HF is utilized for this example embodiment. Therefore, this solution is computationally feasible. However, more importantly, this solution enables the state estimator to monitor for faults in the received navigation measurement information as well as for possible faults in the EKF algorithm.

<FIG> is a flow diagram illustrating a method <NUM>, which can be utilized to perform a statistical comparison of a plurality of PDFs, in accordance with one example embodiment of the present invention. For example, the method <NUM> can be utilized to compare the EKF PDF with the G/HF PDF illustrated in <FIG>. Referring to <FIG>, the method <NUM> begins by computing a distance between the PDFs pG/HF,full(x) and pEKF,full(x) (<NUM>). For example, the "distance" can be computed utilizing a suitable statistical divergence technique, such as, the Kullback-Liebler's divergence technique, Rényi's divergence technique, and the like. However, for this example embodiment, the statistical distance between the G/HF PDF and the EKF PDF is computed utilizing an integral distance measuring (divergence) technique, which can be expressed in equation form as: <MAT> where the term ||. || can be an arbitrary norm. The output of this integral "distance" measuring technique is typically a scalar variable. Note that in Equation (<NUM>), the pEKF,full(x) term provides a statistical estimate consisting of the first moment and second central moment of the state vector. In other words, the pEKF,full(x) term in Equation (<NUM>) provides an estimate in the form of the state mean vector and state covariance matrix. These moments can be assumed to form a Gaussian distribution determined by the moments. Also note that, in some embodiments, several EKF and G/HF PDF estimate comparisons can be made utilizing the above-described "distance" measurement criteria.

Next, the method <NUM> defines a user specified threshold "distance" (<NUM>), and compares this threshold "distance" with the computed distance, D (<NUM>). The method <NUM> then determines if the computed distance, D, is below the user specified threshold distance (<NUM>). If (at <NUM>), the computed distance, D, is determined to be below the user specified threshold distance, then the EKF estimate is deemed to be consistent with the G/HF estimate and thus considered to be "healthy" (<NUM>). The method <NUM> is then terminated. However, if (at <NUM>) the computed distance, D, is not below the user specified threshold, then the EKF estimate is inconsistent with the G/HF estimate and the two estimated PDFs are potentially divergent (<NUM>). The method <NUM> is then terminated (and a user is informed).

<FIG> is a flow diagram illustrating a method <NUM> that can be utilized to statistically compare an EKF PDF and G/HF PDF, in accordance with a second example embodiment of the present invention. Note that instead of computing the integral distance, D, on the basis of the entire PDFs, the distance, D, can be alternatively computed merely with a set of moments of the estimated PDFs pG/HF(x) and pEKF(x). Utilizing selected moments of the PDFs is advantageous because it limits the computational complexity necessary to compare the two PDFs. Referring to <FIG>, the method <NUM> begins by computing the mean and covariance matrix for each PDF (<NUM>). The mean for the estimated PDF pG/HF,full(x) can be expressed as follows: <MAT> and the covariance matrix for the estimated PDF pG/HF,full(x) can be expressed as follows: <MAT>.

Also, the mean for the estimated PDF pEKF,full(x) can be expressed as follows: <MAT> the covariance matrix for the estimated PDF pEKF,full(x) can be expressed as follows: <MAT> and the cross-covariance matrix for the estimated PDF pEKF,full(x) and the estimated PDF pGH/F,full(x) can be expressed as follows: <MAT>.

Note that the actual form of the cross-covariance matrix in Equation (<NUM>) is determined by the specific global or high-order filter (G/HF) utilized. Next, the method <NUM> computes a combined point state estimate (<NUM>), which can be expressed as follows: <MAT> where the diagonal matrices CG/HF, CEKF are defined by the user and have diagonal elements across both matrices whose sum equals to one. In some embodiments, the diagonal elements of the matrices CG/HF, CEKF can be constant for all time epochs and all elements of the state vector involved. In other embodiments, the diagonal elements of the matrices CG/HF, CEKF can vary with respect to time and be different for the particular state vector elements involved. Next, the method computes separation statistics for the moments involved (<NUM>). Specifically, for this embodiment, the term x̂G/HF for the estimated PDF pG/HF,full(x) can be statistically separated into two parts: <MAT>.

Similarly, for this embodiment, the term x̂EKF for the estimated PDF pEKF,full(x) can be statistically separated into two parts: <MAT>.

Next, the method computes distance (divergence) thresholds dG/HF and dEKF based on the user-defined probability of a false alert, PFA and the covariance matrices Px̃,G/HF and Px̃,EKF provided by the G/HF and the EKF (<NUM>). The method then determines if the moments for the G/HF and EKF PDF are less than the respective distance thresholds (<NUM>). In other words, the method determines if the magnitude |x̃G/HF| is less than the computed distance, dG/HF, and the magnitude |x̃EKF| is less than the computed distance, dEKF. If so, then the EKF estimate is considered to be statistically consistent or "healthy" (<NUM>), and the input state measurements may be utilized with confidence by the navigation system involved. Returning to (<NUM>), if the method determines that the magnitude |x̃G/HF| is not less than the computed distance, dG/HF, or the magnitude |x̃EKF| is not less than the computed distance, dEKF, then the EKF estimate is assumed to be potentially divergent (<NUM>). The method is then terminated.

<FIG> is a block diagram illustrating a statistical technique <NUM> that can be utilized to monitor the output performance of a local filter-based state estimator, in accordance with a second example embodiment of the present invention. For example, in this embodiment, the low-order local filter-based state estimator is implemented utilizing an EKF. In a second embodiment, the state estimator can be implemented with any suitable local filter (e.g., UKF, first-order local filter, higher-order local filter, and the like) depending on the level of estimation accuracy and/or computational complexity desired. In some embodiments, the statistical technique <NUM> can be utilized for state estimation of measurement information received from navigation systems, such as, GPS measurement information, hybrid GPS/INS measurement information, AHRS measurement information, GPAHRS measurement information, and the like.

Note that, for some embodiments, the statistical technique <NUM> can be deemed more suitable, for example, when the system's statistical observability is based on one (or combination of multiple) measurement(s). It shall be noted, that the total number of measurements is denoted as N and the number of measurements needed for system statistical observability is denoted as M, where M < N. For the example embodiment illustrated in <FIG>, the statistical technique <NUM> extends the set of EKFs with additional G/HFs that are configured to perform substantially the same estimation tasks as the EKFs. Also, the statistical technique <NUM> substitutes a plurality of G/HF-based sub-solutions for a plurality of the EKF-based sub-solutions illustrated, for example, in <FIG>. As such, referring to <FIG>, for this example embodiment, a suitable (e.g., navigation) system model and a plurality of (e.g., navigation) measurements <NUM> are provided as inputs to a "full-solution" G/HF <NUM> and a "full-solution" EKF <NUM>. Also, the system model and a plurality of sub-sets of the navigation measurement information <NUM> are provided as inputs to a plurality of "sub-solution" EKFs <NUM>(M+<NUM>)-<NUM>(N) and "sub-solution" G/HFs <NUM>(<NUM>)-<NUM>(M). Note that the G/HF sub-solutions are derived from the measurement plurality sub-set(s), which are driving the system's statistical observability. The outputs of the "full-solution" G/HF <NUM> and the "full-solution" EKF <NUM> are statistically compared to determine the consistency of the output <NUM> of the "full-solution" EKF <NUM>. The EKF-based "full-solution" <NUM> is statistically compared with the EKF-based "sub-solution" EKFs <NUM>(M+<NUM>)-<NUM>(N) utilizing, for example, a solution separation technique in order to monitor the integrity of the navigation measurement information being received <NUM>. Similarly, the G/HF-based "full-solution" <NUM> is statistically compared with the G/HF-based "sub-solution" G/HFs <NUM>(<NUM>)-<NUM>(M) utilizing, for example, the solution separation technique described above, in order to monitor the integrity of the navigation measurement information being received <NUM>.

<FIG> is a flow diagram illustrating a method <NUM>, which can be utilized to implement one example embodiment of the present invention. For example, the method <NUM> can be utilized to implement the statistical technique <NUM> illustrated in <FIG>. As such, referring to the exemplary embodiment illustrated in <FIG>, the method <NUM> begins by computing an estimate of the received navigation information with the G/HF "full-solution" utilizing the available system model and the received navigation measurement information (<NUM>). For example, the method <NUM> described above at (<NUM>) can be utilized to make this computation. As such, for this embodiment, the computed estimate can be described in the form of a PDF denoted as pG/HF ,full(x). Next, the method <NUM> computes an estimate of the received navigation information with the EKF "full-solution" utilizing the available system model and measurement information (<NUM>). For example, the method <NUM> described above at (<NUM>) can be utilized to make this computation. As such, for this embodiment, the computed estimate also can be described in the form of a PDF denoted as pEKF,full(x). The method then statistically compares the two computed estimates, pG/HF ,full(x) and pEKF ,full(x) (<NUM>), and determines if the estimates are consistent (<NUM>). For example, the method <NUM> described above at (<NUM>) and (<NUM>) can be utilized to make these computations. As such, if (at <NUM>) the statistical comparison indicates that the two estimates, pG/HF ,full(x) and pEKF,full(x) are substantially consistent (e.g., the EKF full-solution is deemed "healthy" and thus substantially consistent with the G/HF full-solution), then the method computes the G/HF-based sub-solutions for the sub-sets of navigation measurements, pG/HF,sub,n(x), where n = <NUM>,. ,M (<NUM>). The method then computes integrity-related quantities for the navigation measurements utilizing solution separation and the G/HF-based full-solution (<NUM>). Next, the method computes the EKF-based sub-solutions for the sub-sets of navigation measurements, pEKF ,sub,n(x), where n = (M+<NUM>),. ,N (<NUM>). The method then computes integrity-related quantities for the navigation measurements utilizing solution separation and the EKF-based full-solution (<NUM>). The method then combines the integrity quantities from the G/HF-based and the EKF-based solution separation computations (<NUM>).

However, returning to (<NUM>), if the statistical comparison indicates that the two estimates, pG/HF ,full(x) and pEKF,full(x) are not substantially consistent, then the EKF full-solution is deemed potentially divergent (<NUM>), and the method <NUM> is terminated. Note that this solution is computationally feasible if M is small, because (M+<NUM>) G/HFs are utilized. In any event, this technique enables the state estimator to monitor for faults in the received navigation measurement information as well as for a possible failure of the EKF algorithm.

<FIG> is a block diagram illustrating a system <NUM>, which can be utilized to implement one example embodiment of the present invention. For example, in some embodiments, the system <NUM> can be utilized to implement enhanced-integrity monitoring of measurement values provided by navigation systems such as, for example, a GPS, INS, hybrid GPS/INS, AHRS or GPAHRS navigation system. In one embodiment, the system <NUM> can be utilized to implement a statistical technique that can monitor the consistency and convergence of a local filter's output and, thereby, enhance the integrity of the navigation system utilizing measurement vectors provided by a sensor set coupled to the navigation system.

Referring to the exemplary embodiment illustrated in <FIG>, the system <NUM> includes a navigation system <NUM>. For example, the navigation system <NUM> can be a GPS, INS, hybrid GPS/INS, AHRS or GPAHRS navigation system. The navigation system <NUM> includes an inertial measurement unit (IMU) <NUM>. For example, in one embodiment, the IMU <NUM> provides measurement information or values <NUM> that can be utilized to calculate a vehicle's attitude, angular rate of turn, linear velocity, heading, inclination, acceleration, position and the like. In any event, the IMU <NUM> is coupled to a state estimator <NUM> for operable communications therebetween. As such, in one example embodiment, the state estimator <NUM> receives the navigation measurement values <NUM> from the IMU <NUM> and, implementing the statistical technique illustrated in <FIG>, computes a set of estimates of the navigation measurement values with a global or local filter and a system model, and computes a second set of estimates of the navigation measurement values with a local filter and the system model. The state estimator <NUM> then compares the first set of estimates with the second set of estimates. If the second set of estimates is statistically consistent with the first set of estimates, the state estimator <NUM> computes a plurality of sub-sets of the second set of estimates, a sub-solution for each sub-set of the second set of estimates, and then computes an integrity value for each sub-solution. As such, the state estimator <NUM> monitors the integrity of the navigation measurement values <NUM> provided by the IMU <NUM>. The monitored, enhanced-integrity navigation measurement values <NUM> output from the state estimator <NUM> are then coupled to a vehicle (e.g., airborne, land-based, sea-based vehicle) <NUM>. In accordance with the above-described teachings of the present disclosure, the state estimator <NUM> can effectively monitor and thereby enhance the integrity of the navigation measurement values provided by the IMU <NUM>.

Claim 1:
A method (<NUM>) for monitoring the integrity of navigation measurement information, comprising:
receiving a plurality of navigation measurement values from an inertial measurement unit, IMU, for a vehicle in transit;
computing a first set of estimates of the plurality of navigation measurement values utilizing a global filter or a local filter having an order O and a system model (<NUM>);
computing a second set of estimates of the plurality of navigation measurement values utilizing a local filter having an order lower than O and the system model (<NUM>);
comparing the first set of estimates to the second set of estimates (<NUM>, <NUM>);
determining if the second set of estimates is statistically consistent with the first set of estimates (<NUM>, <NUM>,); and
if the second set of estimates is statistically consistent with the first set of estimates, inputting the system model and a plurality of sub-sets of the navigation measurement values respectively into a plurality of local sub-solution filters, computing a sub-solution for each sub-set of the navigation measurement values in a respective one of the local sub-solution filters, and computing an integrity value for each sub-solution (<NUM>);
wherein computing the second set of estimates comprises computing the second set of estimates utilizing a full-solution extended Kalman filter, EKF (<NUM>) and wherein the plurality of local sub-solution filters are sub-solution extended Kalman filters.