Filtering mechanization method of integrating global positioning system receiver with inertial measurement unit
First Claim
1. A filtering mechanization method for integrating a global positioning system (GPS) receiver carried by a carrier with an inertial measurement unit (IMU) carried by said carrier for producing mixed global positioning system/inertial measurement unit (GPS/IMU) positioning data of said carrier, comprising the steps of:
- (a) receiving angular rate and acceleration measurements of said carrier from said IMU to a navigation equations processor to produce inertial navigation system (INS) data, including position, velocity, and attitude data of said carrier;
(b) sending said INS data from said navigation equations processor and GPS velocity data from a global positioning system (GPS) receiver into said first local filter device;
(c) producing a first local state vector estimate and first local error covariance matrix by said first local filter device;
(d) sending GPS position data from said GPS receiver and said INS data from said navigation equations processor into said second local filter device;
(e) producing a second local state vector estimate and second local error covariance matrix by said second local filter device;
(f) receiving said first local state vector estimate and first local error covariance matrix from said first local filter device and said second local state vector estimate and second local error covariance matrix from said second local filter device by a master filter, so as to mix said first local state vector estimate and second local state vector estimate to form optimal estimates of a global state vector and global error covariance matrix, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, as output of an inertial navigation system (INS) filter; and
(g) compensating an INS position, velocity, and attitude data output of said navigation equations processor to produce a mixed global positioning system/inertial measurement unit (GPS/IMU) position, velocity, and attitude data output.
1 Assignment
0 Petitions
Accused Products
Abstract
A filtering mechanization method is provided for integrating a Global positioning System receiver with an Inertial Measurement Unit to produce highly accurate and highly reliable mixed GPS/IMU position, velocity, and attitude information of a carrier. The GPS filtered position and velocity data are first individually used as measurements of the two local filters to produce estimates of two sets of the local state vector. Then, the estimates of the two sets of local state vectors are mixed by a master filter device to produce global optimal estimates of master state vector including INS (Inertial Navigation System) navigation parameter errors, inertial sensor errors, and GPS correlated position and velocity errors. The estimates of the two sets of local state vector and master state vector are analyzed by a GPS failure detection/isolation logic module to prevent the mixed GPS/IMU position, velocity, and attitude information from becoming contaminated by undetected GPS failures.
142 Citations
24 Claims
-
1. A filtering mechanization method for integrating a global positioning system (GPS) receiver carried by a carrier with an inertial measurement unit (IMU) carried by said carrier for producing mixed global positioning system/inertial measurement unit (GPS/IMU) positioning data of said carrier, comprising the steps of:
-
(a) receiving angular rate and acceleration measurements of said carrier from said IMU to a navigation equations processor to produce inertial navigation system (INS) data, including position, velocity, and attitude data of said carrier;
(b) sending said INS data from said navigation equations processor and GPS velocity data from a global positioning system (GPS) receiver into said first local filter device;
(c) producing a first local state vector estimate and first local error covariance matrix by said first local filter device;
(d) sending GPS position data from said GPS receiver and said INS data from said navigation equations processor into said second local filter device;
(e) producing a second local state vector estimate and second local error covariance matrix by said second local filter device;
(f) receiving said first local state vector estimate and first local error covariance matrix from said first local filter device and said second local state vector estimate and second local error covariance matrix from said second local filter device by a master filter, so as to mix said first local state vector estimate and second local state vector estimate to form optimal estimates of a global state vector and global error covariance matrix, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, as output of an inertial navigation system (INS) filter; and
(g) compensating an INS position, velocity, and attitude data output of said navigation equations processor to produce a mixed global positioning system/inertial measurement unit (GPS/IMU) position, velocity, and attitude data output. - View Dependent Claims (2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16)
(h.1) feeding back said optimal estimates of said global state vector and global error covariance matrix to update common state variable estimates of said first local filter device; and
(h.2) feeding back said optimal estimates of said global state vector and global error covariance matrix to update common state variable estimates of said second local filter device.
-
-
4. The filtering mechanization method, as recited in claim 1, wherein the step (g) comprises the steps of:
-
(gA.1) feeding back said optimal estimates of said global state vector to said navigation equations processor;
(gA.2) compensating an inertial measurement unit (IMU) output with said optimal estimates of said inertial sensor errors from said optimal estimates of said global state vector to improve an accuracy of said IMU output in real-time by said navigation equation processor;
(gA.3) resetting said INS position, velocity, and attitude values used in said navigation equation processor by means of compensating current epoch inertial navigation system (INS) position, velocity, and attitude data with said optimal estimates of said INS parameter errors from said optimal estimates of said global state vector, wherein an inertial navigation system (INS) computation at a next epoch is based on reset INS position, velocity, and attitude values; and
(gA.4) outputting current compensated inertial navigation system (INS) position, velocity, and attitude data by said navigation equations processor as said mixed GPS/IMU position, velocity, and attitude data output.
-
-
5. The filtering mechanization method, as recited in claim 3, after the step (g), further comprising the steps of:
-
(h.1) feeding back said optimal estimates of said global state vector and global error covariance matrix to update common state variable estimates of said first local filter device; and
(h.2) feeding back said optimal estimates of said global state vector and global error covariance matrix to update common state variable estimates of said second local filter device.
-
-
6. The filtering mechanization method, as recited in claim 1, 2, 3, 4, or 5, wherein the step (f) further comprises the steps of:
-
(fA.1) receiving said first local state vector estimate and first local error covariance matrix at a current epoch from said first local filter device, said second local state vector estimate and second error covariance matrix at said current epoch from said second local filter device, and a predicted estimate of said global state vector and global error covariance matrix at said current epoch from said master filter by a global positioning system (GPS) failure detection/isolation logic module;
(fA.2) testing consistency between said first local state vector estimate and first error covariance matrix and said predicted estimate of said global state vector and global error covariance matrix at said current epoch to determine whether said first local state vector estimate is global positioning system (GPS) failure-free;
(fA.3) testing consistency between said second local state vector estimate and second error covariance matrix and said predicted estimate of said global state vector and global error covariance matrix at said current epoch to determine whether said second local state vector estimate is global positioning system (GPS) failure-free; and
(fA.4) updating said predicted estimate of said global state vector and global error covariance matrix at said current epoch.
-
-
7. The filtering mechanization method, as recited in claim 6, wherein the step (fA.4) comprises a step of mixing said first local state vector estimate and first error covariance matrix from said first local filter device and said second state vector estimate and second error covariance matrix from said second local filter device to update a selected predicted estimate of said global state vector and global error covariance at said current epoch to form said optimal estimates of said global state vector and global error covariance, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, as output of said INS filter when both said first and second local state vectors are global positioning system (GPS) failure-free.
-
8. The filtering mechanization method, as recited in claim 6, wherein the step (fA.4) comprises a step of using said first local state vector estimate and first error covariance matrix to update said predicted estimate of said global state vector and global error covariance matrix at said current epoch to form said optimal estimates of said global state vector and global error covariance matrix, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, as output of said INS filter, when said second local state vector estimate is global positioning system (GPS) failure-contaminated, and re-initializing said second local filter device.
-
9. The filtering mechanization method, as recited in claim 6, wherein the step (fA.4) comprises a step of using said second local state vector estimate and second error covariance matrix to update said predicted estimate of said global state vector and global error covariance at said current epoch to form said optimal estimates of said global state vector and global error covariance matrix, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, as output of said INS filter, when said first local state vector estimate is global positioning system (GPS) failure-contaminated, and re-initializing said first local filter device.
-
10. The filtering mechanization method, as recited in claim 6, wherein the step (fA.4) comprises a step of using said predicted estimate of said global state vector and global error covariance at said current epoch as said optimal estimates of said global state vector, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, served as output of said INS filter, when both said first and second local state vector estimates are global positioning system (GPS) failure-contaminated, and re-initializing both said first and second local filter device.
-
11. The filtering mechanization method, as recited in claim 1, 2, 3, 4, or 5, wherein said master filter device comprises a bank of master filters to reduce a probability of misdetecting global positioning system (GPS) failures, wherein a mixing interval of said first and second local state vectors is TM and said bank of master filters contains NM member master filters, so that at every current epoch, only one of said master filters is updated and said other master filters are time-propagated with dynamic equations thereof, therefore predicted N sets of said global state vectors are able to be achieved which are time-propagated from a predetermined number of time points to said current time point, wherein at said current epoch, said predicted N sets of said global state vectors are tested with said first and second local state vector estimates to determine whether a global positioning system (GPS) failure is occurred.
-
12. The filtering mechanization method, as recited in claim 1, 2, 3, 4, or 5, wherein said master filter device comprises two or more master filters to reduce a probability of misdetecting global positioning system (GPS) failures, and the step (f) further comprises the steps of:
-
(fB.1) receiving said first local state vector estimate and first local error covariance matrix at said current epoch from said first local filter device, and said second local state vector estimate and second error covariance matrix at said current epoch from said second local filter device by a global positioning system (GPS) failure detection/isolation logic module;
(fB.2) receiving a selected predicted estimate of said global state vector and global error covariance matrix from one of said master filters selected from said master filter device, when said selected master filter is scheduled to be updated at said current epoch, by said GPS failure detection/isolation logic module;
(fB.3) testing consistency between said first local state vector estimate and first error covariance matrix and said first predicted estimate of said global state vector and global error covariance matrix at said current epoch to determine whether said first local state vector estimate is global positioning system (GPS) failure-free;
(fB.4) testing consistency between said second local state vector estimate and second error covariance matrix and said second predicted estimate of said global state vector and global error covariance matrix at said current epoch to determine whether said second local state vector estimate is global positioning system (GPS) failure-free; and
(fB.5) updating said selected predicted estimate of said global state vector and global error covariance matrix at said current epoch.
-
-
13. The filtering mechanization method, as recited in claim 12, wherein the step (fB.5) comprises a step of mixing said first local state vector estimate and first error covariance matrix from said first local filter device and said second state vector estimate and second error covariance matrix from said second local filter device to update said selected predicted estimate of said global state vector and global error covariance matrix at said current epoch to form said optimal estimates of said global state vector and global error covariance matrix, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, as output of said INS filter when both said first and second local state vectors are global positioning system (GPS) failure-free.
-
14. The filtering mechanization method, as recited in claim 12, wherein the step (fB.5) comprises a step of using said first local state vector estimate and first error covariance matrix to update said selected predicted estimate of said global state vector and global error covariance matrix at said current epoch to form said optimal estimates of said global state vector and global error covariance matrix, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, as output of said INS filter, when said second local state vector estimate is global positioning system (GPS) failure-contaminated, and re-initializing said second local filter device and said another master filter.
-
15. The filtering mechanization method, as recited in claim 12, wherein the step (fB.5) comprises a step of using said second local state vector estimate and second error covariance matrix to update said predicted estimate of said global state vector and global error covariance matrix at said current epoch to form said optimal estimates of said global state vector and global error covariance matrix, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, as output of said INS filter, when said first local state vector estimate is global positioning system (GPS) failure-contaminated, and re-initializing said first local filter device and said another master filter.
-
16. The filtering mechanization method, as recited in claim 12, wherein the step (fB.5) comprises a step of using said predicted estimate of said global state vector and global error covariance matrix at said current epoch as said optimal estimates of said global state vector and global error covariance matrix, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity error estimates, served as output of said INS filter, when both said first and second local state vector estimate are global positioning system (GPS) failure-contaminated, and re-initializing both said first and second local filter device and said another master filter.
-
17. A filtering mechanization method for integrating a global positioning system (GPS) receiver carried by a carrier with an inertial measurement unit (IMU) carried by said carrier for producing mixed global positioning system/inertial measurement unit (GPS/IMU) positioning data of said carrier, comprising the steps of:
-
(a) receiving angular rate and acceleration measurements of said carrier from said IMU to a navigation equations processor to produce inertial navigation system (INS) data for said carrier, including position, velocity, and attitude data, (b) sending INS data from said navigation equations processor and global positioning system (GPS) velocity data from a global positioning system (GPS) receiver into a first local filter device;
(c) producing a first local state vector estimate and first local error covariance matrix by said first local filter device;
(d) sending global positioning system (GPS) position data of said GPS receiver and INS data from said navigation equations processor into a second local filter device;
(e) producing a second local state vector estimate and second local error covariance matrix by said second local filter device; and
(f) compensating an INS position, velocity, and attitude data output of said navigation equations processor to produce a mixed global positioning system/inertial measurement unit (GPS/IMU) position, velocity, and attitude data output. - View Dependent Claims (18, 19)
(f.1) feeding back said first local state vector estimate and said second local state vector estimate to said navigation equations processor; and
(f.2) compensating an inertial measurement unit (IMU) output with said optimal estimates of said inertial sensor errors from said first local state vector estimate and said second local state vector estimate to improve an accuracy of said IMU output in real-time by said navigation equation processor;
(f.3) resetting said INS position, velocity, and attitude values used in said INS navigation equation processor by means of compensating current epoch inertial navigation system (INS) position, velocity, and attitude data with said optimal estimates of said INS parameter errors from said first local state vector estimate and said second local state vector estimate, wherein an inertial navigation system (INS) computation at a next epoch is based on reset inertial navigation system (INS) position velocity, and attitude values; and
(f.4) outputting current compensated inertial navigation system (INS) position, velocity, and attitude data by said navigation equations processor as said mixed GPS/IMU position, velocity, and attitude data output.
-
-
20. A filtering mechanization method for integrating a global positioning system (GPS) receiver carried by a carrier with an inertial measurement unit (IMU) carried by said carrier for producing mixed global positioning system/inertial measurement unit (GPS/IMU) positioning data of said carrier, comprising the steps of:
-
(a) receiving angular rate and acceleration measurements of said carrier from said IMU to a navigation equations processor to produce inertial navigation system (INS) data of said carrier, including position, velocity, and attitude data;
(b) forming a bank of N member filters and a switcher controller;
(c) sending said INS data from said navigation equations processor and GPS position and velocity data from a global positioning system (GPS) receiver into said switch controller in every time interval;
(d) updating one of said member filters to obtain a candidate estimate of a state vector, including inertial navigation system (INS) parameter errors, inertial sensor errors, and global positioning system (GPS) correlated position and velocity errors, and propagating said other member filters to obtain predicted (N−
1) sets of said state vector from N time points to a current point, controlled by said switch controller;
(e) producing a current estimate of said state vector by means of comparing said candidate estimate of said state vector and predicted (N−
1) sets of said state vector to determine whether a global positioning system (GPS) failure is occurred; and
(f) compensating an inertial navigation system (INS) position, velocity, and attitude data output of said navigation equations processor to produce a mixed GPS/IMU position, velocity, and attitude data output. - View Dependent Claims (21, 22, 23, 24)
(f.1) feeding said current estimate of said state vector to said navigation equations processor;
(f.2) compensating an inertial measurement unit (IMU) output with said optimal estimates of said inertial sensor errors from said current estimate of said state vector to improve an accuracy of said IMU output in real-time by said navigation equation processor;
(f.3) resetting inertial navigation system (INS) position, velocity, and attitude values used in said INS navigation equation processor by means of compensating current epoch inertial navigation system (INS) position, velocity, and attitude data with optimal estimates of INS parameter errors from a current estimate of said state vector, wherein an inertial navigation system (INS) computation at a next epoch is based on reset inertial navigation system (INS) position, velocity, and attitude values; and
(f.4) outputting current compensated INS position, velocity, and attitude data said navigation equation processor as said mixed GPS/IMU position, velocity, and attitude data output.
-
-
23. The filtering mechanization method, as recited in claims 20, 21 or 22, wherein the step (d) further comprises a step of outputting said candidate estimate of said state vector as said current estimate of said state vector when there is no global positioning system (GPS) failure detected at all.
-
24. The filtering mechanization method, as recited in claims 20, 21 or 22, wherein the step (d) further comprises a step of outputting predicted estimates of said state vector of said member filters as current estimates of said state vector, and re-initializing said member filters, whether a global positioning system (GPS) failure is detected when said candidate estimate of state vector is compared with said predicted estimates of said state vector of said respective member filter.
Specification