Filtering mechanization method of integrating global positioning system receiver with inertial measurement unit

[0002] This is a regular application of a provisional application having an application No. of 60/223,097 and a filing date of Aug. 3, 2000.

[0003] 1. Field of the Present Invention

[0004] The present invention relates to a filtering mechanization method, and more particularly to a filtering mechanization method for integration of a Global Positioning System (GPS) receiver with an Inertial Measurement Unit (IMU) to produce highly accurate, highly reliable, and mixed GPS/IMU position, velocity, and attitude measurements of a carrier at affordable cost under high dynamic environments, heavy noise environments, and long-term (long-period) operation.

[0005] 2. Description of Related Arts

[0006] The major function of a navigation system is designed to produce continuous position, velocity, and attitude measurements of a carrier, such as a vehicle, under dynamic environments. The attitude measurements usually include pitch, roll, and heading angle of the carrier.

[0007] In the past decade, a low cost inertial navigation system (INS) and GPS (Global Positioning System) receiver were commonly developed to determine the positioning measurements of a vehicle at affordable cost, such as position, velocity, etc.

[0008] An INS relies on three orthogonally mounted inertial angular rate sensors and three orthogonally mounted acceleration sensors to produce three-axis angular rate and acceleration measurements. Based on the carrier acceleration and angular rate measurements, the position, velocity, and attitude measurements of a carrier are obtained by numerically solving Newton's equations of motion. The three orthogonally mounted inertial angular rate sensors and three orthogonally mounted acceleration sensors with additional supporting mechanical structures and electronic devices are conventionally called an Inertial Measurement Unit (IMU). The conventional IMUs may be catalogued into Platform IMU and Strapdown IMU.

[0009] In the platform IMU, angular rate sensors and acceleration sensors are installed on a stabilized platform. Attitude measurements can be directly picked off from the platform structure. But attitude rate measurements cannot be directly obtained from the platform. Moreover, highly accurate feedback control loops must be designed to implement the platform IMU.

[0010] Compared with the platform IMU, in a strapdown IMU, the angular rate sensors and acceleration producers are directly strapped down to the carrier and move with the carrier. The output signals of the angular rate sensors and acceleration sensors are expressed in the carrier body frame. The attitude and attitude rate measurements can be obtained by means of a series of computations.

[0011] The INS, which uses a platform IMU, in general, is catalogued as a gimbaled inertial navigation system. The INS, which uses a strapdown IMU, in general, is catalogued as a strapdown inertial navigation system. In a gimbaled inertial navigation system, the angular rate sensors and acceleration sensors are mounted on a gimbaled platform to isolate the sensors from the rotations of the carrier so that the measurements and navigation calculations can be performed in a stabilized navigation coordinates frame. Generally, the motion of the carrier can be expressed in several navigation frames of reference, such as earth centered inertial (ECI), earth-centered earth-fixed (ECEF), and North-East-Down (NED), etc. In a strapdown inertial navigation system, the inertial sensors are rigidly mounted to the carrier body frame. In order to perform the navigation computation in the stabilized navigation frame, a coordinate frame transformation matrix is used and updated to transform the acceleration measurements from the body frame to the navigation frame.

[0012] In general, the navigation solution from the gimbaled inertial navigation system is more accurate than the one from the strapdown inertial navigation system. But, a gimbaled inertial navigation system is more complex and expensive than a strapdown inertial navigation system. The strapdown inertial navigation systems become the predominant mechanization due to their low cost and reliability.

[0013] An inertial navigation system provides the position, velocity, and attitude information of a carrier through a dead reckoning method. Inertial navigation systems, in principle, perform a self-contained operation and output continuous position, velocity, and attitude data of the carrier after initializing the starting position and initiating an alignment procedure.

[0014] In addition to the self-contained operation, other advantages of an inertial navigation system include the full navigation solution and wide bandwidth.

[0015] However, an inertial navigation system is expensive and is degraded with drift in output (position, velocity, and attitude) over an extended period of time. It means that the position and velocity errors increase with time. This error propagation characteristic is primarily caused by errors, such as, gyro drifts, accelerometer bias, misalignment, gravity disturbance, initial position and velocity errors, and scale factor errors.

[0016] Generally, the ways of improving the accuracy of inertial navigation systems include employing highly accurate inertial sensors and aiding the inertial navigation system using an external sensor. However, highly accurate inertial sensors are very expensive with big size and heavy weight.

[0017] A GPS receiver is a very ideal external source to aid an inertial navigation system. The GPS is a satellite-based, worldwide, all-weather radio positioning and timing system. The GPS system is originally designed to provide precise position, velocity, and timing information on a global common grid system to an unlimited number of adequately equipped users.

[0018] GPS receivers are designed for a user to exploit the advantages of the Global Positioning System. A conventional, single antenna GPS receiver supplies world-wide, highly accurate three dimensional position, velocity, and timing information, but not attitude information, by processing the so-called pseudo range and delta range measurements output from the code tracking loops and the carrier tracking loops in the GPS receiver, respectively. In a benign radio environment, the GPS signal propagation errors and GPS satellite errors, including the Selective Availability, serve as the bounds for positioning errors. However, the GPS signals may be intentionally or unintentionally jammed or spoofed, and the GPS receiver antenna may be obscured during carrier attitude maneuvering, and the performance degrades when the signal-to-noise ratio of the GPS signal is low and the carrier is undergoing highly dynamic maneuvers.

[0019] As both the cost and size of high performance GPS receivers are reduced in the past decade, a multiple-antenna GPS receiver was developed to provide both position and attitude solutions of a vehicle, using interferometric techniques. In principle, the attitude determination technology utilizes measurements of GPS carrier phase differences on the multiple-antenna to obtain highly accurate relative position measurements. Then, the relative position measurements are converted to the attitude solution. The advantages of this approach are long-term stability of the attitude solution and relatively low cost. However, this GPS position and attitude determination system still retains the characterization of low bandwidth which is susceptible to shading and jamming, requires at least 3 antennas configurations for a three-axis attitude solution, and requires antenna separation enough for high attitude resolution.

[0020] Because of the inherent drawbacks of a stand-alone inertial navigation system and a stand-alone GPS receiver, a stand-alone inertial navigation system or a stand-alone GPS receiver can not meet mission requirements under some constraints, such as low cost, long-term high accuracy, highly reliable, continuous high rate output, etc.

[0021] The mutual complementary characteristics of the stand-alone GPS receiver and the stand-alone inertial navigation system suggest that, in many applications, an integrated GPS/IMU system, combining the best positive properties of both systems, will provide superior accurate continuous navigation capability. This navigation capability is unattainable in either one of the two systems standing alone.

[0022] The potential benefits offered by an integrated GPS/IMU system are outlined as follows:

[0023] (1) The aiding of the GPS receiver signal-tracking loop process with inertial data allows the effective bandwidth of the loops to be reduced, resulting in an improved tracking signal in noisy and dynamic environments.

[0024] (2) An inertial navigation system not only provides navigation information when the GPS signal is lost temporarily, but also reduces the search time required to reacquire GPS signals.

[0025] (3) Inertial navigation parameter errors and inertial sensor errors can be calibrated while the GPS signal is available, so that the inertial navigation system can provide more accurate pure inertial position information after the GPS signal is lost.

[0026] (4) The GPS enables and provides on-the-fly alignment of an inertial navigation system by means of maneuvering, eliminating the static self-alignment pre-mission requirements of the stand-alone inertial navigation system.

[0027] Conventionally, a standard Kalman filtering mechanization, which uses centralized processing mechanization, is used to integrate the navigation information from a GPS receiver with that from an INS. However, the conventional method often suffers from some potential problems, such as correlated noise of the GPS receiver output, unpredicted big transient errors of the GPS velocity, level changes of the GPS receiver output errors due to change of the GPS satellite in view, and other undetected GPS failures.

[0028] Moreover, for versatile commercial applications, a low cost, low accuracy IMU is integrated with a GPS chipset. Thus, the long-term position and velocity output of the integrated GPS/IMU system heavily rely on the update of the GPS position and velocity. Therefore, it is very important that the integration process automatically exclude GPS false solutions, so that the integrated GPS/IMU system can exploit the GPS positive features, but remove the GPS negative features. The present invention is disclosed to address these problems.

[0029] A main objective of the present invention is to provide a filtering mechanization method of integrating global positioning system receiver with inertial measurement unit, wherein measurements from a GPS (Global Positioning System) receiver and an IMU (Inertial Measurement Unit) are optimally processed to achieve highly accurate and reliable mixed GPS/IMU position, velocity, and attitude information of a carrier under high dynamic, heavy jamming, and long-term operation environments. The present invention features greater flexibility to incorporate GPS failure detection/isolation requirements of the integrated GPS/IMU systems.

[0030] Another objective of the present invention is to provide a filtering mechanization method of integrating global positioning system receiver with inertial measurement unit, wherein the position and velocity data from the GPS receiver are independently filtered first in parallel with the output from the inertial navigation processing to obtain two sets of local state vector estimates, including INS navigation parameter errors and inertial sensor errors, and then the two sets of local state vector estimates are mixed to obtain one set of global state vector estimate, including the INS navigation parameter errors and inertial sensor errors, with maximal fault toleration.

[0031] Another objective of the present invention is to provide a filtering mechanization method of integrating global positioning system receiver with inertial measurement unit, wherein the local and master filtering mechanization with flexible different update rates is applied to the integration of the GPS receiver with an IMU to overcome the problem of the correlated errors of the position and velocity output from the GPS Kalman filter in conventional GPS/IMU integration approaches.

[0032] Another objective of the present invention is to provide a filtering mechanization method of integrating global positioning system receiver with inertial measurement unit, wherein the local and master filter can be incorporated to minimize the response transients when the GPS satellite constellation in view change occurs.

[0033] Another objective of the present invention is to provide a filtering mechanization method of integrating global positioning system receiver with inertial measurement unit, wherein the local and master filters can be incorporated to autonomously exclude big unpredicted GPS velocity jumps, such as the transients order of a few meters per second without any other indication.

[0034] Another objective of the present invention is to provide a filtering mechanization method of integrating global positioning system receiver with inertial measurement unit, wherein the local and master filters with a failure detection/isolation logic can detect and isolate GPS failures, especially GPS soft failures. The GPS soft failure means that failure is slowly accumulated, which are very tough to detect traditionally.

[0035] In order to accomplish the above objectives, the present invention provides a filtering mechanization method for integrating a GPS receiver carried by a carrier with an IMU carried by the carrier for producing a GPS/IMU mixed positioning data of the carrier, which comprises the steps of:

[0036] (a) receiving angular rate and acceleration measurements of the carrier from the IMU to a navigation equations processor to produce INS position, velocity, and attitude data of the carrier;

[0037] (b) sending INS velocity data from the navigation equations processor and GPS velocity data from a GPS receiver into the first local filter;

[0038] (c) producing the first local state vector estimate and the first local error covariance matrix by the first local filter;

[0039] (d) sending GPS position data of the GPS receiver and INS position data from the navigation equations processor into the second local filter;

[0040] (e) producing the second local state vector estimate and the second local error covariance matrix by the second local filter;

[0041] (f) receiving the first local state vector estimate and first local error covariance matrix and the second local state vector estimate and second local error covariance matrix by a master filter to mix the first local filter's state vector estimate and the second filter's state vector estimate to form optimal estimates of the global state vector and global error covariance matrix, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, as output of the INS filter; and

[0042] (g) compensating the INS position, velocity, and attitude data output of the navigation equations processor using the optimal estimates of the INS parameter errors of the global state vector by an error remover to form the mixed GPS/IMU position, velocity, and attitude data output.

[0043] FIG. 1 is a block diagram illustrating the filtering mechanization according to a preferred embodiment of the present invention.

[0044] FIG. 2 is a block diagram illustrating the state variables of the global state vector, first local state vector, and the second state vector according to the above preferred embodiment of the present invention.

[0045] FIG. 3 is a block diagram illustrating the GPS failure detection/isolation logic module with relationship of the local filter devices and master filter devices according to the above preferred embodiment of the present invention.

[0046] FIG. 4 is a block diagram illustrating the GPS failure detection/isolation logic module with relationship of the local filter devices and the master filter devices according to the above preferred embodiment of the present invention.

[0047] FIG. 5 is a block diagram illustrating the time propagation and updating of the dual master filters of the master filter device according to the above preferred embodiment of the present invention.

[0048] Referring to FIG. 1, a filtering mechanization method of integrating global positioning system receiver with inertial measurement unit is illustrated, wherein the method is to mixed GPS (Global Positioning System) measurements and IMU (Inertial Measurement Unit) measurements of a carrier to achieve long-term, highly accurate and reliable mixed GPS/IMU position, velocity, and attitude information of a carrier.

[0049] The GPS receiver concept of operation is briefly summarized as follows:

[0050] (1) Tracking the GPS signals from four GPS satellites in view to make the raw GPS pseudorange and delta range measurements of the four GPS satellites by means of the GPS code and carrier tracking loops, wherein the raw GPS pseudorange measurement of a GPS satellite is the sum of the real range from the GPS receiver to the GPS satellite and various errors, and the raw delta range measurements of a GPS satellite is proportional to the Doppler shift on the carrier frequency of the GPS signal from the GPS satellite with various errors; and

[0051] (2) Figuring out the GPS position solution using the raw GPS pseudorange delta range measurements based upon the triangulation principle, and figuring out the GPS velocity solution using the raw GPS delta range measurements.

[0052] Most current GPS receivers commonly have an extended Kalman filter, which is called a GPS Kalman filter, to process the linearized measurements of the raw pseudorange and delta-range measurements obtained from the code and carrier tracking loops residing in the GPS receivers to provide a position, velocity, and time solution. Other methods to convert the raw GPS pseudorange and delta range measurements into the position and velocity of the carrier include single-point solution.

[0053] The GPS/IMU integration can provide clear advantages, including ubiquitous coverage, indoor operation, and non-line-of-sight situations, combining the long-term accuracy of GPS and the short-term stability of an autonomous INS.

[0054] In the conventional cascaded configuration of a GPS/IMU integration, the GPS derived position and velocity are used as measurements of a Kalman filter, which is called the INS Kalman filter (IKF) to produce the optimal estimates of INS parameter errors and inertial sensor errors, which are fed back or fed forward to compensate the INS position, velocity, and attitude solution. However, this method is plagued by the following potential problems.

[0055] (1) With the cascaded configuration of GPS/IMU integration, the position and velocity output from the GPS Kalman filter residing in the GPS receiver is provided as a measurement input of the IKF. The measurement errors must be uncorrelated between observation epochs to meet the statistical conditions required by the IKF. Unfortunately, this requirement cannot be satisfied for the sequences of the filtered position and velocity outputs of the GPS receiver.

[0056] (2) A GPS positioning solution (position, velocity, and time) contains a certain amount of errors due to the GPS satellite segment. These errors are unobservable by the GPS Kalman filter of the GPS receiver and cannot be estimated. They change levels when the GPS satellite constellation in view change occurs, at which point they introduce undesirable IKF response transients.

[0057] (3) In most cases, because the GPS velocities provided by the GPS receiver are of very high accuracy, of about 0.2-0.4 m/sec (1σ), the GPS velocity update of the GPS/IMU integration results in the high observability of INS velocity errors, INS heading error, and inertial sensor errors, and can provide the INS with an in-flight startup operation. But in certain cases, transients of the order of a few meters per second dominate the GPS velocity without any other indication.

[0058] (4) Insufficient capability of detection and isolation of other GPS failures.

[0059] To overcome the first problem of the correlated measurement errors into the IKF, the position and velocity update interval applied to the IKF should be larger than their time constants respectively, so that they can be assumed to be uncorrelated in time. The time constant of the GPS position is much larger than that of the GPS velocity, because of the fast response of the GPS filters to the GPS range rate measurements. Thus, there is no constraint on the more accurate velocity update interval. The different requirement for the position and velocity update intervals brings about the inconvenience of implementation of the IKF.

[0060] One way of mitigating the second problem is to increase the measurement noise, so as to reduce the sensitivity to those errors. Another way of handling the above second problem is to employ the Schmidt-Kalman filter, in which the GPS errors due to the GPS space segment are considered but are not estimated.

[0061] As for the third problem, one approach is to formulate the measurement noise statistics dynamically, based on data collected between the filter update epochs (over a moving window). An alternative way to avoid the above third problem is to employ only GPS position information.

[0062] These conventional methods are used at the cost of reducing the accuracy of the integration solution, and have inadequacies in rapid GPS fault detection, isolation, and recovery (FDIR) performance.

[0063] The present invention is to provide a new filtering mechanization for the cascaded GPS/IMU integration, wherein GPS position and velocity updates are treated with separate filters, separate update rate, and multi-level filtering and consistent test.

[0064] Referring to FIG. 1, an INS filter 3 of the present invention, which is used to replace the conventional the IKF, comprises a first local filter device 31, a second local filter device 32, and a master filter device 33. The INS filter 3 is committed to provide optimal estimates of INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates to feed back to an INS unit 1 to reset the navigation equation and compensate the output of an IMU 11 contained in the INS unit 1, or to feed forward to compensate the INS position, velocity, and attitude data. A state vector of the first local filter device 31 is denoted as a first local state vector. A state vector of the second local filter device 32 is denoted as a second local state vector. A state vector of the master filter 33 is denoted as a global state vector.

[0065] Referring to FIG. 1, based on the above described configuration, a filtering mechanization method of integrating a GPS receiver carried by a carrier with the IMU 11 carried by the carrier for producing mixed GPS/IMU positioning data of the carrier is illustrated, which comprises the steps of:

[0066] (1) receiving angular rate and acceleration measurements of the carrier from the IMU 11 to a navigation equations processor 12 to produce INS data which include position, velocity, and attitude data of said carrier;

[0067] (2) sending the INS data from the navigation equations processor 12 and GPS velocity data from a GPS receiver 2 into the first local filter device 31;

[0068] (3) producing a first local state vector estimate and first local error covariance matrix by the first local filter device 31;

[0069] (4) sending GPS position data from the GPS receiver 2 and the INS data from the navigation equations processor 12 into the second local filter device 32;

[0070] (5) producing a second local state vector estimate and second local error covariance matrix by the second local filter device 32;

[0071] (6) receiving the first local state vector estimate and first local error covariance matrix from the first local filter device 31 and the second local state vector estimate and second local error covariance matrix from the second local filter device 32 by a master filter 33, so as to mix the first local state vector estimate and second local state vector estimate to form optimal estimates of a global state vector and a global error covariance matrix, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, as output of the INS filter 3; and

[0072] (7) compensating the INS position, velocity, and attitude data output of the navigation equations processor 12 using the optimal estimates of the INS parameter errors of the global state vector by an error remover 4 to form the mixed GPS/IMU position, velocity, and attitude data output.

[0073] In order to ensure that the linear INS parameter error model of the first local filter device 31, the second local filter device 32, and the master filter device 33 accurately represents the real characteristics of the output data error of the INS unit 1, the above step (7) can be replaced with the steps of:

[0074] (7A.1) feeding back the optimal estimates of the global state vector to the navigation equations processor 12;

[0075] (7A.2) compensating the IMU output with the optimal estimates of the inertial sensor errors from the optimal estimates of the global state vector to improve the accuracy of the output of the IMU 11 in real-time by the navigation equation processor 12;

[0076] (7A.3) resetting the INS position, velocity, and attitude values used in the navigation equations by the navigation equation processor 12 by means of compensating the current epoch INS position, velocity, and attitude data with the optimal estimates of the INS parameter errors from the optimal estimates of the global state vector, so that the INS computation at the next epoch can be based on the reset INS position, velocity, and attitude values; and

[0077] (7A.4) outputting the current compensated INS position, velocity, and attitude data by the navigation equations processor 12 as the mixed GPS/IMU position, velocity, and attitude data output.

[0078] Referred to FIG. 2, the preferred global filter vector uses a high-order error model, including the INS parameter errors, inertial sensor errors, and GPS correlated position and velocity errors. The INS errors comprise three position errors, three velocity errors, and three attitude errors, which form a linear INS error model. The inertial sensor errors refer to measurement errors of the angular rate sensor and acceleration sensors of the IMU 11. The number of the inertial sensor errors is based on the dimension of the inertial error models. In a preferred configuration, the global state vector uses the high-order IMU error model and local filters use the reduced-order IMU error model, because the master filter can operate at reduced update rate. The number of the GPS correlated position and velocity errors is based on the dimension of the GPS correlated position and velocity error model. In a preferred configuration, the global state vector uses the high-order GPS correlated position and velocity error model and local filters use the reduced-order GPS correlated position and velocity error model, because the master filter can operate at reduced update rate.

[0079] The common state vector variables between the local filters and master filter are the INS parameter errors. However, if the local filters and master filter use the same IMU error model, the common state vector variables should include the inertial sensor errors.

[0080] The update intervals of the state vector estimate of the first local filter, second local filter, and master filter can be the same or different. The preferred update intervals of the first local filter, second local filter, and master filter are configured as follows:

[0081] (a) The first local filter device 31 can use the shortest update interval, such as one second.

[0082] (b) The second local filter device 32 can use the middle update interval, such as 25 seconds. The update interval of the second local filter should be more than the time constant of the GPS position to meet the requirement of the measurements of the second local filter device 32.

[0083] (c) The master filter employs the same as the update interval of the first local filter device 31 or second local filter device 32, or other alternatives.

[0084] Referring to FIG. 1, the optimal estimates of the global state vector and global error covariance matrix can be fed back to the first and second local filter devices 31, 32 to update the common state variable estimates to improve system performance. Therefore, after the step (7), the present invention further comprises the additional steps of:

[0085] (7B.1) feeding back the optimal estimates of the global state vector and global error covariance matrix to update the common state variable estimates of the first local filter device 31; and

[0086] (7B-2) feeding back the optimal estimates of the global state vector and global error covariance matrix to update the common state variable estimates of the second local filter device 32.

[0087] Because the IMU 11 is a self-contained device, which implies that the output generation of the IMU 11 doesn't rely on any external signals at all. So, the reliability of the IMU is far better than that of GPS receivers. As availability of low cost, miniaturized MEMS (MicroElectroMechanical Systems) inertial sensors increase, the redundant IMU, in which more than three angular rate sensors and acceleration sensors are mounted in skew, has high degree of reliability. Therefore, it becomes critical to detect and isolate any GPS deterioration and failure for system reliability.

[0088] Referring to FIG. 3, in order to achieve high degree of reliability of the mixed GPS/IMU positioning solution, the INS filer 3 further comprises a GPS failure detection/isolation logic module 34 to detect a GPS solution's undesired transient, big velocity jump, and other unpredicted failures. Accordingly, the step (6) further comprises the following steps: (6A.1) receiving the first local state vector estimate and the first local error covariance matrix at a current epoch from the first local filter device 31, the second local state vector estimate and second error covariance matrix at the current epoch from the second local filter device 32, and a predicted estimate of the global state vector and global error covariance matrix at the current epoch from the master filter 33 by the GPS failure detection/isolation logic module 34;

[0089] (6A.2) testing consistency between the first local state vector estimate and first error covariance matrix and the predicted estimate of the global state vector and global error covariance matrix at the current epoch to determine whether the first local state vector estimate is GPS failure-free;

[0090] (6A.3) testing consistency between the second local state vector estimate and second error covariance matrix and the predicted estimate of the global state vector and global error covariance matrix at the current epoch to determine whether the second local state vector estimate is GPS failure-free; and

[0091] (6A.4) updating the predicted estimate of the global state vector and global error covariance matrix at the current epoch by means of one of the following four steps:

[0092] (6A.4A) mixing the first local state vector estimate and first error covariance matrix from the first local filter device 31 and the second state vector estimate and second error covariance matrix from the second local filter device 32 to update the selected predicted estimate of the global state vector and global error covariance at the current epoch to form the optimal estimates of the global state vector and global error covariance, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, as output of the INS filter 3 when both the first and second local state vectors are GPS failure-free;

[0093] (6A.4B) using the first local state vector estimate and first error covariance matrix to update the predicted estimate of the global state vector and global error covariance matrix at the current epoch to form optimal estimates of the global state vector and global error covariance matrix, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, as output of the INS filter 3, when the second local state vector estimate is GPS failure-contaminated, and re-initializing the second local filter device 32;

[0094] (6A.4C) using the second local state vector estimate and second error covariance matrix to update the predicted estimate of the global state vector and global error covariance at the current epoch to form the optimal estimates of the global state vector and global error covariance matrix, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, as output of the INS filter 3, when the first local state vector estimate is GPS failure-contaminated, and re-initializing the first local filter device 31; or

[0095] (6A.4D) using the predicted estimate of the global state vector and global error covariance at the current epoch as the optimal estimates of the global state vector, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, served as output of the INS filter 3, when both the first and second local state vector estimates are GPS failure-contaminated, and re-initializing both the first and second local filter device 31, 32.

[0096] Referring to FIG. 4, in order to further improve reliability of the system, the master filter device 33 further comprise a bank of master filters to reduce the probability of misdetecting GPS failures. Assuming the mixing interval of the two local state vectors is T_Mand the bank of master filters contains N_Mmember master filters. At every current epoch, only one master filter is updated and the other master filters are just time-propagated with their dynamic equations. Therefore, we can get predicted N sets of the global state vectors, which are time-propagated from T_M, 2T_M, 3T_M, . . . N_MT_Mtime points to the current time point. At the current epoch, the predicted N sets of the global state vectors can be tested with the two sets of local state vector estimates to determine whether any GPS failure is occurred. The preferred embodiment of the present invention also can determine when the GPS failure happened.

[0097] Referring to FIG. 5, the scenario of the master filer device 33 using a pair of master filters, the first master filter 331 and the second master filter 332 are disclosed as follows. The first and second master filters 331, 332 alternatively mix the first and second local state vector estimates from one epoch to another epoch.

[0098] Referring to FIGS. 4 and 5, the advantage of a pair of first and second master filters 331, 332 used for enhancing GPS failure detection/isolation is that we can get a predicted set of global state vectors at each epoch of mixing the two local state vector estimates, which is propagated independently without GPS information for two mixture intervals of the two sets of the local state vector estimates to facilitate detection of GPS soft failures. Meanwhile we can mix the two sets of the local state vector estimates in a short interval to form the optimal estimates of the global state vector. The GPS soft failure means that the GPS failure is slowly accumulated, which are very tough to detect traditionally.

[0099] Accordingly, the step 6 further comprises the steps of:

[0100] (6B.1) receiving the first local state vector estimate and the first local error covariance matrix at the current epoch from the first local filter device 31, and the second local state vector estimate and second error covariance matrix at the current epoch from the second local filter device 32 by the GPS failure detection/isolation logic module 34;

[0101] (6B.2) receiving the selected predicted estimate of the global state vector and global error covariance matrix from a selected master filter, which may be the first predicted estimate of the global state vector and global error covariance matrix from the first master filter 331 if the first master filter 331 is scheduled to be updated at the current epoch, or the second predicted estimate of the global state vector and global error covariance matrix from the second master filter 332 if the first master filter 332 is scheduled to be updated at the current epoch, by the GPS failure detection/isolation logic module 34;

[0102] (6B.3) testing consistency between the first local state vector estimate and first error covariance matrix and the selected predicted estimate of the global state vector and global error covariance matrix at the current epoch to determine whether the first local state vector estimate is GPS failure-free;

[0103] (6B.4) testing consistency between the second local state vector estimate and second error covariance matrix and the selected predicted estimate of the global state vector and global error covariance matrix at the current epoch to determine whether the second local state vector estimate is GPS failure-free; and

[0104] (6B.5) updating the selected predicted estimate of the global state vector and global error covariance matrix at the current epoch by means of one of the following four steps:

[0105] (6B.5A) mixing the first local state vector estimate and first error covariance matrix from the first local filter device 31 and the second state vector estimate and second error covariance matrix from the second local filter device 32 to update the selected predicted estimate of the global state vector and global error covariance matrix at the current epoch to form the optimal estimates of the global state vector and global error covariance matrix, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, as output of the INS filter 3 when both the first and second local state vectors are GPS failure-free;

[0106] (6B.5B) using the first local state vector estimate and first error covariance matrix to update the selected predicted estimate of the global state vector and global error covariance matrix at the current epoch to form the optimal estimates of the global state vector and global error covariance matrix, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, as output of the INS filter 3, when the second local state vector estimate is GPS failure-contaminated, and re-initializing the second local filter device 32 and the other master filter;

[0107] (6B.5C) using the second local state vector estimate and second error covariance matrix to update the predicted estimate of the global state vector and global error covariance matrix at the current epoch to form the optimal estimates of the global state vector and global error covariance matrix, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, as output of the INS filter 3, when the first local state vector estimate is GPS failure-contaminated, and re-initializing the first local filter device 31 and the other master filter; or

[0108] (6B.5D) using the predicted estimate of the global state vector and global error covariance matrix at the current epoch as the optimal estimates of the global state vector and global error covariance matrix, including INS parameter errors, inertial sensor errors, and GPS correlated position and velocity error estimates, served as output of the INS filter 3, when both the first and second local state vector estimate are GPS failure-contaminated, and re-initializing both the first and second local filter device 31, 32 and the other master filter.

[0109] The preferred embodiment of the present invention is further disclosed as follows. The first local filter device corresponding to the GPS velocity update is taken as a local filter #1 (LF1). The second local filter device dedicated to the GPS position update is taken as local filter #2 (LF2), while a master filter (MF) fuses the estimates of the common state variables of the LF1 and LF2 local state vectors to obtain a optimal estimates of the global state vector. The information sharing principle is implemented only at the initialization of LF1 and LF2. In this configuration, the function of the conventional INS Kalman filter is provided by co-operation of the LF1, LF2, and MF.

[0110] The preferred first local state vector for LF1 is selected as

[0111] X_LF1=X_INS

[0112] where, X_INS(15×1) includes: 3 INS velocity errors, 3 INS attitude errors, 3 INS position errors, 3 INS gyro drifts, 3 INS accelerometer biases. The measurement equation for LF1 is

[0113] Z_LF1=H_LF1X_LF1+V_LF1

[0114] where, $H_{\mathrm{LF1}}=\left[\begin{array}{cccc}1& 0& 0& \\ 0& 1& 0& 0\left(3\times 12\right)\\ 0& 0& 1& \end{array}\right]$

[0115] V_LF1is the measurement white noise of the velocity output of the GPS receiver.

[0116] The second local state vector for LF2 is chosen as

[0117] X_LF2=[X_INS, δφ_GPS, δλ_GPS, δh_GPS]

[0118] where, the δφ_GPS, δλ_GPS, and δh_GPSare the correlated position errors of the position output of the GPS receiver output. The first order Gauss-Markov models are used to model their dynamics. Even though these correlated position errors are unobservable, augmenting LF2 with these errors can affect the covariance matrix of the other states and enhance the local filter robustness. The measurement equation for LF2 is

[0119] Z_LF2=H_LF2X_LF2+V_LF2

[0120] where $H=\left[\begin{array}{ccccc}& 1& 0& 0& \\ 0\left(3\times 6\right)& 0& 1& 0& 0\left(3\times 12\right)\\ & 0& 0& 1& \end{array}\right]$

[0121] V_LF2is the measurement white noise of the position output of the GPS receiver. The standard Kalman filter prediction and update equations can be directly applied to LF1 and LF2. The optimal estimates of LF1 and LF2 local state vectors are referred to as {circumflex over (X)}_LF1Kand {circumflex over (X)}_LF2K, with the error covariance matrices P_LF1Kand P_LF2K, respectively. The dynamics of the system and measurement equations of LF1 and LF2 can be described as the following standard state and measurement equations

[0122] X_k=Φ_k,k−1X_k−1+W_k−1

[0123] Z_k=H_kX_k+V_K

[0124] where:

[0125] X: n-vector, state,

[0126] Z: m-vector, measurement

[0127] Φ: n×n matrix, state transition,

[0128] H: n×m matrix, measurement dynamics,

[0129] W: n×n matrix, system noise with covariance matrix, Q,

[0130] V: n×m matrix, measurement noise with covariance matrix, R,

[0131] k: time step.

[0132] The state estimation equations for LF1 and LF2 are:

[0133] {circumflex over (X)}_k,k−1=Φ_k,k−1{circumflex over (X)}_k−1

[0134] P_k,k−1=Φ_k,k−1P_k−1Φ_k,k−1^T+Q_k−1

[0135] K_k=P_k,k−1H_K^T(H_kP_k,k−1H_k^T+R_k)⁻¹

[0136] {circumflex over (X)}_K={circumflex over (X)}_k,k−1+K_k(Z_k−H_k{circumflex over (X)}_k,k−1)

[0137] P_k=P_k,k−1K_kH_kP_k,k−1

[0138] where:

[0139] {circumflex over (X)}_k: n-vector, estimated state vector,

[0140] P_k: n×n matrix, covariance matrix,

[0141] K_k: n×m matrix, gain matrix.

[0142] According to the preferred embodiment of present invention, the system information is fully assigned to LF1 and LF2. The information sharing requirement is only performed initially, one time, by setting the initial values of the covariance matrix of the local state vectors and the covariance matrix of the process noise of LF1 and LF1 to 2 times the values they have in a traditional INS Kalman filter, as follows:

[0143] {circumflex over (X)}_LF10={circumflex over (X)}₀

[0144] {circumflex over (X)}_LF20={circumflex over (X)}₀

[0145] P_LF10=2P₀

[0146] P_LF20=2P₀

[0147] Q_LF10=2P₀

[0148] Q_LF20=2P₀

[0149] where, {circumflex over (X)}_LF10and {circumflex over (X)}_LF20are the initial local state estimates of LF1 and LF2 respectively. P_LF10and P_LF20are the error covariance matrices of the initial state estimate errors of LF1 and LF2, respectively. Q_LF10and Q_LF20are the covariance matrices of the process error of LF1 and LF2, respectively. {circumflex over (X)}₀, P₀, And Q₀are the initial global state estimate, the initial covariance matrix of the global state vector, and initial covariance matrix of the system process error of the traditional INS Kalman filter.

[0150] When the optimal local state estimates and their error covariance matrix from LF1 and LF2 are passed to the MF, the MF produces the optimal global state estimates and the error covariance matrix. The equations for the optimal fusion for producing the global state vector using the optimal local state vector estimates from LF1 and LF2 in the MF are:

[0151] P_K=[P_LF1K⁻¹+P_LF2K⁻¹]⁻¹

[0152] {circumflex over (X)}_K=P_K[P_LF1K⁻¹{circumflex over (X)}_LF1K+P_LF2K⁻¹{circumflex over (X)}_LF2K]

[0153] In some applications, an alternative mode of the present invention can be embodied as follows:

[0154] (1C) Receiving angular rate and acceleration measurements of the carrier from the IMU 11 to a navigation equations processor 12 to produce INS data of the carrier, including position, velocity, and attitude data;

[0155] (2C) Sending INS data from the navigation equations processor 12 and GPS velocity data from a GPS receiver 2 into the first local filter device 31;

[0156] (3C) Producing the first local state vector estimate and first local error covariance matrix by the first local filter device 31;

[0157] (4C) Sending GPS position data of the GPS receiver 2 and INS data from the navigation equations processor 12 into the second local filter device 32;

[0158] (5C) Producing the second local state vector estimate and second local error covariance matrix by the second local filter device 32; and (6C) Compensating the INS position, velocity, and attitude data output of the navigation equations processor 12 using the first local state vector estimate and the second local state vector estimate by an error remover 4 to form the mixed GPS/IMU position, velocity, and attitude data output.

[0159] In order to ensure that the linear INS parameter error model used by the first local filter and second local filter accurately represents the real characteristics of the output data error of the INS unit 1, the above step (6C) can be replaced with the following steps:

[0160] (6C.1) feeding back the first local state vector estimate and the second local state vector estimate to the navigation equations processor 12; and

[0161] (6C.2) compensating the IMU output with the optimal estimates of the inertial sensor errors from the first local state vector estimate and the second local state vector estimate to improve the accuracy of the output of the IMU 11 in real-time by the navigation equation processor 12;

[0162] (6C.3) resetting the INS position, velocity, and attitude values used in the navigation equations by the INS navigation equation processor 12 by means of compensating the current epoch INS position, velocity, and attitude data with the optimal estimates of the INS parameter errors from the first local state vector estimate and the second local state vector estimate, so that the INS computation at the next epoch can be based on the reset INS position, velocity, and attitude values;

[0163] (6C.4) outputting the current compensated INS position, velocity, and attitude data by the navigation equations processor 12 as the mixed GPS/IMU position, velocity, and attitude data output.

[0164] In some applications, another alternative mode of the present invention can be embodied as follows:

[0165] (1D) Receiving angular rate and acceleration measurements of the carrier from the IMU 11 to a navigation equations processor 12 to produce INS data of the carrier, including position, velocity, and attitude data;

[0166] (2D) Forming a bank of filters and switcher controller, including 1, 2, 3, . . . N member filter, starting from earliest, to constructing a filter sequences;

[0167] (3D) Sending INS data from the navigation equations processor 12 and GPS position and velocity data from a GPS receiver 2 into the switch controller in every T interval;

[0168] (3D) Updating one of the N member filter to obtain a candidate estimate of the state vector, including the INS parameter errors, inertial sensor errors, and GPS correlated position and velocity errors, and propagating the others (N−1) individual filter to obtain predicted (N−1) sets of state vector from T, 1T, 2T . . . NT time points to the current point, controlled by the switch controller;

[0169] (4D) Producing the current estimate of the state vector by means of comparing the candidate estimate of the state vector and predicted (N−1) sets of the state vector to determine if any GPS failure occurred;

[0170] (5D) Compensating the INS position, velocity, and attitude data output of the navigation equations processor 12 using the current estimate of the state vector by an error remover 4 to form the mixed GPS/IMU position, velocity, and attitude data output.

[0171] In order to ensure that the linear INS parameter error model used by the bank of filters accurately represents the real characteristics of the output data error of the INS unit 1, the above step 5D can be replaced with the following steps:

[0172] (5D.1) feeding the current estimate of the state vector to the navigation equations processor 12;

[0173] (5D.2) compensating the IMU output with the optimal estimates of the inertial sensor errors from the current estimate of the state vector to improve the accuracy of the output of the IMU 11 in real-time by the navigation equation processor 12;

[0174] (5D.3) resetting the INS position, velocity, and attitude values used in the navigation equations by the INS navigation equation processor 12 by means of compensating the current epoch INS position, velocity, and attitude data with the optimal estimates of the INS parameter errors from the current estimate of the state vector, so that the INS computation at the next epoch can be based on the reset INS position, velocity, and attitude values; and

[0175] (5D.4) outputting the current compensated INS position, velocity, and attitude data by the navigation equation processor 12 as the mixed GPS/IMU position, velocity, and attitude data output.

[0176] The step (4D) further comprises one of the following steps:

[0177] (4D.1) outputting the candidate estimate of the state vector as the current estimate of the state vector whether any GPS failure is not detected at all; or

[0178] (4D.2) outputting the predicted estimates of the state vector of the (i−1) individual filter as current estimate of the state vector, and re-initializing the (i)-(N) member filters, whether any GPS failure is detected when the candidate estimate of state vector is compared with the predicted estimates of the state vector of the (i) member filter.