INTEGRATED INERTIAL/GPS NAVIGATION SYSTEM
The invention is a method and apparatus for adjusting the phase and frequency of a received GPS signal in an inertial-GPS navigation system by inputting a delta-phase at delta-time intervals into the received satellite signal. The method comprises the steps of determining two delta-phase components: (a) a Kalman delta-phase component derived from a plurality of candidate Kalman delta-phase components obtained by performing a first set of more than one Kalman filter processes and (b) an IMU delta-phase component derived from the IMU outputs and a direction-cosine matrix that translates the body coordinates of the IMU into the navigation coordinates of the inertial-GPS navigation system.
[0001] This application claims the benefit of U.S. Provisional Application No. 60/199,897, filed Apr. 26, 2000.
STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH AND DEVELOPMENT[0002] (Not applicable)
BACKGROUND OF THE INVENTION[0003] This invention relates generally to integrated inertial/GPS navigation systems and more specifically to integrated inertial/GPS navigation systems that achieve improved operational reliability through the use of a plurality of Kalman filters.
[0004] The Autonomous Integrity Monitored Extrapolation (AIME™) algorithm is an improved algorithm for integrating the Global Positioning System (GPS) with an inertial navigation system (INS) in a way that the navigation solution has a minimum susceptibility to equipment failures, interference, and jamming. The principal problem with using Kalman filters to integrate GPS with INS is that a slow GPS failure due to interference or spoofing can contaminate the integrated solution before it is detected. The AIME™ algorithm solves this problem by analyzing the residuals of the filter over long time intervals before the corrections are added externally to the INS solution. Because the solution is based on the continuous INS output, it is extremely reliable, and continuous in case of complete loss of the GPS. By using parallel solutions which are not sensitive to the failure, it rejects the contamination and continues using an uncontaminated, reliable precision navigation solution.
[0005] This principle is applied in the present invention to carrier tracking. The basis of the invention is the Anti-Jam AIME™ (AJ-AIME™) algorithm. This algorithm achieves at least 20 dB J/S improvement, and possibly 30-40 dB J/S improvement. This is in addition to the J/S improvements from other techniques. Previous solutions to improve J/S performance are based on deep coupling using a vector solution combining both carrier and code measurements and INS errors. These errors are estimated in a large, complex maximum likelihood estimator. One problem with these approaches is that the signals are no longer independent so that receiver autonomous integrity monitoring (RAIM) cannot be used to determine integrity.
[0006] The AJ-AIME approach avoids the loss of integrity and possible contamination inherent in such vector approaches. Even if jamming is successful, the AIME™ solution provides maximum accuracy for coasting with the calibrated INS solution, since this solution is uncontaminated from the period when jamming or interference first began.
[0007] For manned aircraft or long range missiles already equipped with a high-quality INS, the AIME™ algorithm can be implemented in a separate, very inexpensive small box, which adds corrections to the INS solution. For inexpensive short range missiles, or munitions, without INS, the AIME™ algorithm is combined with MEMS technology. This solution uses 1 deg/hr silicon gyros, 20 micro-g accelerometers, and a GPS receiver, all integrated in an inexpensive, small box.
[0008] Without WAAS, the failure detection and exclusion (FDE) availability of RAIM for non-precision approach (NPA) using 24-satellite constellations with up to three outages is less than 50%. Using WAAS for Precision Approach CAT I (DH=200 feet), the FDE availability of RAIM is less than 2%. One objective of this invention is to improve FDE availability by up to five orders of magnitude. RAIM is basically a “snapshot” approach using instantaneous noisy position data. Instead of RAIM, this invention uses an integrated systems approach. With today's system technology, including micro-electro-mechanical system (MEMS) technology, and using Kalman filter algorithms, it is possible with this approach to use feedback from rate, acceleration, acceleration rate, etc., up to at least the fourth derivative of position, in the Kalman filter error model.
[0009] In order to reject errors caused by satellite clock failures, multipath, excessive atmospheric errors, or subsystem failures, these errors and all other errors are modeled in a bank of Kalman filters in the AIME™ algorithm. Each of these filters is tuned for excessive errors in one channel, or in one group of channels, or in a subsystem. The filter which models the errors correctly will have uncorrelated residuals, because of the “innovations property” of Kalman filters. By estimating the mean of the residuals over many Kalman filter cycles, a test statistic is obtained for each filter. This statistic is the mean-square estimated residual for that filter. The filter with the correct model will have the lowest test statistic and the output of that filter will be used. The other filters have large mean-square estimated residuals, since they do not correctly model the excessive error.
[0010] The word “failure” is used herein to represent any excessive error which can contaminate the solution, whether due to an actual subsystem soft failure or due to a temporary excessive instrument error or signal error.
[0011] Recursive algorithms are used in order that the estimate can use very large numbers of Kalman filter cycles to detect slow failures. In present civil applications, these estimates use residuals over the past two hour period to detect and exclude errors due to slow satellite clock drifts. This algorithm has been certified for Primary Means and has been flown on commercial airliners for many years. In addition, many years of data have been collected and analyzed from flights in normal airline operation.
[0012] To improve anti-jam performance, other approaches have claimed that 30-40 dB J/S improvement can be achieved by a vector processing algorithm using deep coupling, where the INS is integrated with the measurements from both the carrier and code discriminators. This would be done in a large, complex, maximum-likelihood estimator which attempts to estimate all error sources including INS errors. This approach creates an integrity problem since the different satellite measurements are no longer independent, and RAIM-type integrity checking is no longer valid.
[0013] There are three additional difficulties with this approach: (1) it does not consider the fundamental problem that arises when integrating inertial information with carrier phase information, (2) it does not properly consider the unique characteristics of inertial system errors, and (3) it does not consider the very different characteristics of carrier phase errors vs. code errors.
[0014] The fundamental problem referred to in (1) above is that the wavelength of the carrier is only 19 centimeters. During turbulence or high dynamics, the separation between the INS and the GPS antenna makes it difficult to process changing inertial attitude information fast enough to aid the carrier tracking at the antenna to centimeter accuracy at 1000 Hz without excessive time lags.
[0015] The unique characteristics of inertial systems referred to in (2) above arise because inertial errors vary slowly. Gyro and accelerometer errors change at only a fraction of a micro-g per second. This causes phase errors of only a few micrometers per second. This is too small to estimate with short term phase measurements, which are noisy. It is much better to estimate these errors over long periods of time using PR measurements, which have bounded position errors over these long periods.
[0016] The difficulty identified in (3) above arises because pseudorange measurements are made at 1 Hz with an accuracy of meters while the carrier phase is measured at 1000 Hz with an accuracy of a centimeter or less. It is not practical to combine these measurements in the same Kalman filter. If they are combined, it is almost impossible to prevent contamination from individual signals.
[0017] The solution to (1) above (i.e. aiding the carrier discriminators in turbulence and dynamics) is to use strapdown measurements in body axes at 400 to 1600 Hz to compute the antenna position relative to the INS. These measurements are used to estimate antenna position a short time in the future at 1000 Hz. The short prediction time is adjusted to cancel time lags in the system. This effectively eliminates the dynamic stress that the carrier loop has to track.
[0018] The solution to (2) above (i.e. inertial characteristics) is to use only pseudorange information in a navigation Kalman filter with a large step size, such as 20 seconds or more, to estimate INS errors. These estimated corrections are added to the INS solution to obtain the 1000 Hz extrapolator aiding information for the carrier discriminators.
[0019] The solution to (3) above (i.e. carrier vs. code characteristics) is to use a separate AJ-AIME™ Kalman filter for the carrier discriminator information. This Kalman filter has a much smaller update frequency of 1 Hz., to estimate slow corrections to the 1000-Hz phase discriminator data. The inertial extrapolator has already eliminated the dynamics from the measurements, and the navigation Kalman filter has eliminated slowly varying errors, such as user clock errors, atmospheric errors, and IMU instrument errors. The 1-Hz AJ-AIME™ Kalman filter will only have to correct small, residual medium-frequency errors in the carrier tracking loop. This 1-Hz carrier loop filter will also use parallel banks of similar Kalman filters with residuals averaged over short and long periods to detect and reject phase error contamination.
[0020] Measurements for different satellites are no longer independent at the same update cycle, but this was never a requirement for AIME™ tegrity as it is for RAIM integrity. In fact, the AIME™ residuals are highly correlated at each Kalman measurement cycle, but they are uncorrelated from one cycle to the next, provided there is no failure, and the model is correct.
[0021] In military applications, the AIME™ navigation Kalman filter cycle time can be reduced from 60 seconds, as used in civil applications, to 20 seconds. This navigation Kalman filter is then used to aid a second ANTI-JAM AIME™ (AJ-AIME™) Kalman filter with cycle time of only one second. This filter in turn aids an inertial extrapolator, operating at 1000 Hz., which is used to eliminate the high frequency dynamics from the carrier tracking loop. This provides J/S improvement of 30-40 dB. The objective is to reject multipath, unintentional anomalies or interference, jamming, and spoofing, and to continue carrier tracking for more than a minute with complete loss of the GPS signal, as caused by masking due to aircraft or missile motion.
[0022] Definitions of symbols used herein are as follows.
[0023] Ax, Ay, Az Non-gravitational accelerations along navigation axes
[0024] ADIRS Air Data Inertial Reference System
[0025] AHRS Attitude and heading reference system
[0026] AIME™ Autonomous integrity monitored extrapolation
[0027] AJ-AIME™ Anti-Jam autonomous integrity monitored extrapolation
[0028] [CRB] Direction cosine matrix from Body to navigation Reference axes
[0029] CAT I Category of precision approach with 200 foot decision height
[0030] DGPS Differential Global Positioning System
[0031] DH Decision height for approach to landing
[0032] FDE Failure detection and exclusion
[0033] FOM Figure of Merit
[0034] HDOP Horizontal Dilution of Precision
[0035] HPL Horizontal integrity level
[0036] HPL Horizontal protection level
[0037] IMU Inertial measurement unit
[0038] INS Inertial navigation system
[0039] IRS Inertial reference system
[0040] J/S Ratio of jamming power to signal power, usually measured in dB
[0041] MEMS Micro-electro-mechanical system
[0042] NPA Non precision approach
[0043] OCS Operational Control Segment
[0044] PR Pseudo Range measurement from GPS
[0045] R Earth radius
[0046] RLR Lever arm from IMU to GPS antenna in body axes
[0047] RAIM Receiver autonomous integrity monitoring
[0048] SA Selective Availability
[0049] VDLL Vector delay lock loop
[0050] VPL Vertical protection level
[0051] WAAS Wide Area Augmentation System
[0052] deg/hr angular rate unit of degrees per hour
[0053] g unit of acceleration equal to 32.2 feet/second2
[0054] {overscore (&ohgr;)} Total spatial angular rate of navigation coordinates (earth rate +craft rate)
[0055] &tgr; Correlation time of first order Markov process
[0056] Definitions of Kalman filter symbols used herein are as follows.
[0057] KF Kalman filter
[0058] k Index of Kalman filter cycle, or navigation solution cycle
[0059] M, N Dimension of measurement vector, error state vector, resp.
[0060] rk Residual vector of Kalman filter, dimension (M×1)
[0061] Vk Covariance matrix of residuals, dimension (M×M)
[0062] rest Estimated mean residual vector of Kalman filter, dimension (M×1)
[0063] Vest Covariance matrix of estimated mean residual vector, dimension (M x M)
[0064] sest2 Test statistic
[0065] zk Measurement vector of Kalman filter
[0066] Hk, F Observation matrix (M×N), dynamics matrix N×N), resp.
[0067] xk−, xk+ Error state vector before, and after, update, resp., dimension (N×1)
[0068] Rk, Qk Measurement noise (M×M), Process noise (N×N), resp.
[0069] Pk−, Pk+ Error state Covariance matrix before, and after update, dimension (N×N)
[0070] Kk Kalman gain matrix, dimension (N×M)
[0071] T Superscript indicating transpose of matrix
[0072] −1 Superscript indicating inverse of matrix
[0073] zi(t) Measurement from ith satellite at time t
[0074] xz(t) Temporary error state used to make measurement at time t
[0075] PRic(xz(t)) Pseudorange computed from error state xz(t)
[0076] PRim(t) Pseudorange measured at time t
[0077] &PHgr;(t−tk) Transition matrix from time tk to time t
[0078] e&phgr;ic(xz(t)) Computed phase errors at time t
[0079] e&phgr;im(t) Measured phase errors at time t
BRIEF SUMMARY OF THE INVENTION[0080] The invention is a method and apparatus for adjusting the phase and frequency of a received GPS signal in an inertial-GPS navigation system comprising an inertial measurement unit (IMU) and a GPS receiver where the GPS receiver is postulated as having an input port for inputting a delta-phase at delta-time intervals into a received satellite signal and an output port for outputting a carrier phase error defined as the difference between the actual phase of the received GPS signal and a reference value. The GPS receiver is also postulated as having an output port for outputting a pseudorange associated with the received GPS signal. The IMU is postulated as having an output port for outputting the acceleration and angular velocity of the The method comprises the steps of determining two delta-phase components: (a) a Kalman delta-phase component and (b) an IMU delta-phase component. The Kalman delta-phase component is derived from a plurality of candidate Kalman delta-phase components obtained by performing a first set of more than one Kalman filter processes. The inputs to each first-set Kalman filter process include the carrier phase error and a position vector associated with the JMU. A candidate Kalman delta-phase component is produced as an output of each first-set Kalman filter process. The Kalman delta-phase component is determined from a candidate Kalman delta-phase components selected from the plurality of candidate Kalman delta-phase components by applying a predetermined Kalman delta-phase component selection rule.
[0081] The IMU delta-phase component is derived from the IMU outputs and a direction-cosine matrix that translates the body coordinates of the IMU into the navigation coordinates of the inertial-GPS navigation system.
BRIEF DESCRIPTION OF DRAWINGS[0082] FIG. 1 shows an embodiment of the invention in the context of a GPS receiver and an
[0083] FIG. 2 shows an embodiment of that portion of the invention that transforms present and past IMU acceleration and angular velocity into estimates of future values of IMU acceleration and angular velocity.
DETAILED DESCRIPTION OF THE INVENTION[0084] The AIME™ invention described herein applies to both the AIME™ navigation Kalman filter (KF), and to the AJ-AIME™ carrier-loop Kalman filter (KF). The AIME™ navigation KF has been used by itself in the past for navigation of commercial airliners. Details of this filter can be found in U.S. Pat. No. 5,583,774 which is incorporated herein by reference.
[0085] In a typical application to prevent jamming, the AIME™ navigation KF is used both for navigation and to aid the AJ-AIME™ carrier-loop KF. The AIME™ navigation KF typically operates at Kalman update rates of 1 cycle per minute when using 0.01 deg/hr inertial grade gyros and 3 cycles per minute or higher when using 0.1 deg/hr to 1 deg/hr attitude and heading reference system (AHRS) grade gyros. The AJ-AIME™ carrier-loop KF typically operates at a Kalman update rate of 1 Hz. The AIME™ navigation KF estimates low-frequency error states with correlation times of more than 5 minutes. The AJ-AIME™ carrier-loop KF estimates medium-frequency errors with correlation times of 5 minutes or less. Much-higher-frequency errors are caused by aircraft dynamics. These dynamic errors are corrected by an inertial extrapolator which operates at 1000 Hz. The extrapolator uses IMU measurements at 200 Hz to 1600 Hz.
[0086] The AIME™ algorithm applies to the AIME™ navigation KF and the AJ-AIME™ carrier-loop KF separately. In each KF, the algorithm is used to detect and exclude excessive errors in the frequency range of its model and to exclude contamination or spoofmg. This makes carrier tracking less vulnerable to interference, jamming, or spoofmg, and less vulnerable to temporary loss of the GPS signal caused by masking due to aircraft angular motion.
[0087] The equations for the test statistic used in the AIME™ algorithm are based on the optimal estimation of a hypothesized true residual, based on observations of the measured residual over many Kalman filter cycles. This is the essence of the invention.
[0088] It is assumed that the individual Kalman filter residuals are independent measurements of the true residual. They differ from the true residual because of measurement noise, which is the innovation in the Kalman filter caused by process noise and GPS measurement noise. The sequence of these measurement residuals over a long time period is used to estimate the true residual. This sequence is the innovations sequence of the Kalman filter.
[0089] AIME™ detects and identifies failures by estimating the mean of the residuals in the Kalman filter over various time intervals. If the Kalman filter model is correct (no failures), the residuals are uncorrelated over time (innovations principle) and the mean of the residuals is zero. The estimate of the mean approaches zero as longer time intervals, represented by more Kalman filter cycles, are used in the estimate.
[0090] Large failures can be detected quickly by estimating the mean residual over a few cycles. Slow failures are the most difficult to detect. The more Kalman filter cycles used in estimating the mean residual, the smaller the failure that can be detected, without excessive false alarms. This invention includes a recursive algorithm which makes it possible to use an extremely large number of Kalman filter cycles in the estimate of the mean residual without excessive use of computer memory or processing capacity. In the preferred embodiment described below, 120 Kalman filter cycles are used. However, 256, 1024, or an even greater number of cycles could be used, thereby making it practically impossible for an adversary or terrorist to “spoof” the system.
[0091] In addition to using the increased number of cycles to increase the averaging time used in the estimate, it is used to reduce the Kalman filter update interval. A smaller update interval is desirable, in order to improve the performance when IMU instrument errors are larger than normal.
[0092] The recursive algorithm computes the estimated residual over various time intervals by adding single terms to running sums. The individual terms, indexed by subscript “k”, are separated into sub-groups of several Kalman filter cycles each. Because of the associative law of addition, the sums for each sub-group can be determined first, and then the sums for the sub-groups can be summed to determine the sum for the total. The total sum is formed by starting with the first sub-group sum, and adding one new term at a time to a running sum of sub-group sums, as explained in the preferred embodiment.
[0093] If a failure occurs, the Kalman filter model is incorrect, and the residuals of the Kalman filter are no longer uncorrelated from one cycle to the next. In this case, the failure detection statistic is non-central Chi-square distributed, and it exceeds a threshold, which can be computed for a maximum missed-detection probability.
[0094] Isolation is accomplished by using a bank of parallel hypothesis test Kalman filters, each tuned for a different failure. There is a separate filter for each satellite failure or group of satellite failures, for satellite clock failures, for IMU instrument failures, for GPS user clock failures, for altimeter failures, or for the onset of jamming. Each of these filters saves its residuals over long time intervals.
[0095] Navigation can continue despite IMU or altimeter soft failures, by using a solution re-tuned for large IMU errors, or large altimeter errors, respectively. The same is true for large GPS user clock errors. If cost permits, redundant systems can also be used even in the event of hard failures. Redundant IMUs, altimeters, and GPS receivers are typically used in commercial airliner applications.
[0096] When there is a failure, as detected by the single Kalman filter which is modeled for no failures, the corresponding long term residuals of the different failure hypothesis test filters are analyzed. The test filter which is modeled for the particular failure will still have small, uncorrelated residuals over the entire interval, and a small test statistic. All the other test filters will have correlated residuals and large test statistics.
[0097] The squared estimated mean residual is transformed and normalized. This is done by using the inverse of the covariance matrix Vest of the estimated mean residual rest to form a statistic sest2. If there are no failures, the mean of the residuals is zero, and the statistic sest2 is central Chi-square distributed. If there are failures, the mean of the residuals is non-zero, and the statistic sest2 is non-central Chi-square distributed.
[0098] In both civil and military applications, invulnerability to intentional jamming is highly desirable. The objective is 30-40 dB J/S improvement. The AJ-AIME™ algorithm differs from a vector delay lock loop (VDLL) approach or other deep integration approaches using all measurements to aid each tracking loop in coupled signal-tracking channels. These approaches use large, complex, maximum-likelihood estimation techniques to calibrate carrier, code, and IMU errors all in the same filter. As a result, the signals are no longer independent, and integrity algorithms such as RAIM cannot be used to determine interference or spoofing. In addition, individual signal lock is difficult to detect. Because of the dynamic stress in high performance weapons systems, effective bandwidths are still too wide to provide significant J/S improvement.
[0099] The AJ-AIME™ approach solves this last problem by using in-flight calibrated delta-theta, delta-V IMU measurements in body axes to predict both body linear and angular position at a high rate. This makes it possible to extrapolate antenna position ahead of real time at 1000 Hz in navigation axes. The position is then resolved along line-of-sight to each satellite to obtain carrier phase and frequency for generating phase rotation for Doppler removal in each channel ahead of real time. It will be shown that this makes the system relatively unaffected by extremely high rates of change of linear or angular acceleration, commonly called “jerk”. In effect, the high-frequency dynamics called “stress” is removed before the tracking loop is closed.
[0100] Since the bandwidth of the carrier tracking loops is normally determined by the high frequency dynamics, the bandwidth is now determined only by additional residual errors. These residual errors are changes in user clock frequency, changes in atmospheric delays, multipath changes, and gravity anomaly changes. Additional slow corrections for these changes are estimated by the outputs from one of a bank of high frequency (1 Hz.) Kalman filters. The filter output used is the filter which most closely models the errors. The measurements to these filters are the average carrier phase error, after the high frequency dynamics, user clock errors, multipath errors, and atmospheric errors have already been removed or estimated.
[0101] Instead of using the Costas filter, the loop is closed by the extrapolator, together with these small Kalman filter corrections. Each of these parallel Kalman filters is tuned for a different excessive contamination effect. The particular output used is the one with the minimum AIME™ test statistic. The Kalman filter with the best model has uncorrelated residuals. This is the “innovations property” of Kalman filters. The uncorrelated residuals result in the minimum test statistic. In effect, the AIME™ algorithm uses adaptive Kalman filter principles, but with no time lag. This not only solves the integrity problem, but makes it almost impossible to spoof. The spoofer would have to modify each signal to correspond to the effect on every channel of a small change in user clock frequency, an IMU sensor error, a small multipath error, or an atmospheric error. Each simulated error would be too small to drag the solution off.
[0102] These high-frequency (1 Hz) AJ-AIME™ carrier-loop Kalman filters are aided by one of a bank of low frequency (I to 3 cycles per minute) AImE navigation Kalman filters. These AIME™ navigation filters use pre-filtered 1-Hz GPS PR measurements to estimate the low frequency or slow “DC” errors, such as navigation axis misalignments, gyro and accelerometer bias errors, low-frequency user clock and frequency errors, and slow atmospheric offsets for each satellite. IMU instrument misalignments are stable, and they can be calibrated at the factory to an accuracy of 1 arc second, so that they do not have to be re-estimated by the navigation filter.
[0103] Because of the IMU, the response to real dynamic motion is almost instantaneous. However, the Kalman filters act as low pass filters to GPS signal errors. The time constants of these AIME™ Kalman filters is at least 1 minute using 1 deg/hr gyros, and at least 5 minutes, using 0.1 deg/hr gyros. The time constants of conventional carrier-tracking loops is 8 milliseconds for a low-dynamics civil-aviation 20-Hz loop, and approximately 3 milliseconds for a military-aviation 50-Hz loop. It is concluded that J/S improvement is 30 to 40 dB, compared to a 20-Hz conventional carrier-tracking loop when AIME™ uses 0.1 deg/hr gyros or compared to a 50-Hz tracking loop, when AIME™ uses 1 deg/hr gyros.
[0104] The equations for the estimated residual rest, its covariance Vest, and its normalized sum-squared statistic sest2 are given below.
[0105] The measurement residual rk and its covariance Vk at each Kalman filter cycle k is given by
rk=zk−Hkxk− (1.1)
[0106] and
Vk=HkPk−HkT+Rk (1.2)
[0107] The inverse of the covariance matrix (information matrix) of estimated mean is given by
Vest−1=&Sgr;kVk−1 (summing over cycles k=1 to kmax) (1.3)
[0108] The sum of weighted residuals is given by
(Vest−1rest)=&Sgr;kVk−1rk (summing over cycles k=1 to kmax) (1.4)
[0109] The estimated mean and normalized sum-squared test statistic are given by
rest=(Vest−1)−1(Vest−1rest) (1.5)
sest2=restTVest−1rest=restT(Vest−1rest) (1.6)
[0110] The equations are performed in single precision in order to reduce the computer duty cycle requirements and memory requirements. The duty cycle requirements are a result of the many inversions and re-inversions of the covariance matrices of the measurement residuals, which are required whenever the satellites in use are changed, as explained in the next section. The memory requirements result from the many matrix inverses which must be saved for the multiple averaging which is explained in the next sections.
[0111] The Kalman filter equations are given below. These equations are similar to those in Gelb, Applied Optimal Estimation, TASC, MIT Press, 1974, Table 4.2-1, page 110. The equations are performed in double precision in order that the error state covariance matrix remain positive definite.
[0112] The measured difference between computed and measured PR at 1 Hz, for satellite i is given by
zi(t)=PRic(xz(t))−PRim(t) where xz(t)=&PHgr;(t−tk)xz(tk) (2.1 C)
[0113] The measured difference between computed and measured carrier phase is given by
zi(t)=e&phgr;ic(xz(t))−e&phgr;im(t) where xz(t)=&PHgr;(t−tk)xz(tk) (2.1N)
[0114] The averaged or “pre-filtered” measurement at k is given by
zk=(1/T)&Sgr;(t)zi(t) where t=tk, . . . , t=tk+1, T=tk+1−tk (2.2)
[0115] The Kalman filter update is accomplished using the Kalman filter gain Kk, the state estimate update xk+, and the error covariance update Pk+.
Kk=Pk−.HkT[HkPk−HkT+Rk]−1 (2.3)
xk+=xk−+Kk[zk−Hkdxk−] where dxk−=xk−−xz(tk) (2.4)
Pk+=[I−KkHk]Pk− (2.5)
[0116] Kalman filter propagation is accomplished using the state estimate extrapolation xk+, and the error covariance extrapolation Pk+1.
xk+1−=&PHgr;(k)xk+ (2.6)
Pk+1=&PHgr;kPk−&PHgr;kT+Qk (2.7)
[0117] Unlike Gelb, the propagation is done after the update. This is necessary because the updates are done on the error state xk− after the measurements zk are averaged or pre-filtered at 1 Hz between k and k+1, as indicated in Equation (2.2). The Kalman filter updates of xk− are actually performed in the background during the interval from k+1 to k+2. hi order to begin collecting measurements during this interval, before xk+1− has been computed, a temporary estimate xz(tk+1) is used to compute the zi(t) during this interval, as indicated in Equation (2.1). The measurements z(k+1) are then adjusted for the difference
dxk+1−=xk+1−−xz(tk+1) (2.8)
[0118] as indicated in Equation (2.4) with k replacing k+1 at the next cycle.
[0119] Modem receivers can accommodate up to 12 satellites in view. With the present constellation of 24 satellites, there are typically an average of 8 of the original 24 satellites in view, and sometimes as few as 6. Spare satellites are of little use in improving geometry, since they are spaced close to one of the original satellites most likely to fail. It is unnecessary to use more than 8 satellites when the geometry is good.
[0120] As an extreme example of bad geometry, the line-of-sights to seven of the satellites could all lie in a plane, and the line-of-sight to the eighth satellite could be perpendicular to that plane. The failure of the eighth satellite would be undetectable using only the original eight satellites. If one of the more than eight satellites used was an additional satellite also perpendicular to the plane, the satellite failure would be detectable by using this additional satellite.
[0121] When there are more than 8 in view, only 8 satellites with good geometry are used. These 8 satellites are selected by computing the RAIM protection level for the set of 8 satellites. The horizontal protection level (HPL) would be used to select for horizontal accuracy, while the vertical protection level (VPL) would be used for applications like precision approach, where vertical accuracy is critical.
[0122] Initially, the first 8 satellites in view are used in the table. As explained previously, the RAIM protection level is used to determine bad geometry. If the 8 satellites used have a computed protection level exceeding the average level with 8 satellites, one of the satellites is replaced by one of the satellites not used, and the RAIM protection level is re-computed with the new set. If the new set of 8 satellites has larger protection level than the previous set of 8, the newer set is used. If there were more than 9 satellites in view, this process is repeated until the specified average protection level is achieved.
[0123] The decision must be made on which satellites to replace, when the protection level is too large with the presently selected 8 satellites, and more than 8 satellites are in view. The RAIM protection level is determined by computing the “slope” for each of the satellites. This slope is determined for each of the satellites used, by computing the least squares position error solution. Then the position error and test statistic corresponding to a fixed bias error in that satellite is determined. The slope for that satellite bias error is the ratio of the position error resulting from the fixed bias error to the test statistic resulting from the fixed bias error.
[0124] The RAIM protection level is determined from the satellite error which results in the maximum slope, since that is the most difficult-to-detect satellite. This means that when this satellite has a large bias error, it also has the largest position error when the test statistic exceeds the threshold. This implicitly means that it has the least redundancy checking from other satellites. The satellite to be replaced is the satellite with the minimum slope, since this is the most-easy-to-detect satellite. This means it has the most redundancy from other satellites, and can be rejected with little loss of redundancy for integrity checking.
[0125] The satellite information is kept in a table, with 8 rows, corresponding to a maximum of 8 satellites used. The rows are filled, one row at a time, as new satellites first come into view. The first column of the table shows the ID Number of the satellite. This column vector is referred to as the “current map”. When a satellite goes out of view, the corresponding row is cleared, and a “−1” is used to replace the satellite ID number. If a new satellite replaces the old in this row, the new satellite ID number replaces the old. The other columns in the table contain information on the status of the satellite, such as how long it has been in view.
[0126] The two sums &Sgr;kVk−1 and &Sgr;kVk−1rk used in the estimated mean in Equations (1.3) and (1.4) respectively are taken over many Kalman filter cycles as explained above. These sums are performed as running sums by starting with the first term, and adding one new term to the running sum at each cycle.
[0127] Both the covariance matrix Vk from Equation (1.2) and its inverse Vk−1 as used in Equation (1.3) are stored in full 8×8 matrices even though there are missing satellites. The corresponding row and column for a missing satellite, as indicated by the “−1” in the map, are filled with zeroes.
[0128] The residual vector rk in Equation (1.1), the weighted residual vector Vk−1rk, the running sum of weighted residuals &Sgr;kVk−1rk in Equation (1.4), and the estimated mean residual rest in Equation (1.5) are each stored in a full 8×1 column matrix, even though the number of satellites used is less than 8. The missing satellites in this matrix are replaced by zeroes corresponding to the “−1” flags from the current map which is also 8×1.
[0129] The individual terms, indexed by subscript “k”, can be separated into sub-groups of several Kalman filter cycles each. Because of the associative law of addition, the sums for each sub-group can be determined first and then the sums for each sub-group can be summed to determine the sum for the total. The total sum is formed by starting with the first sub-group sum, and adding one new term at a time to a running sum of sub-group sums, as explained in the next section.
[0130] The satellites used in a cycle or group are represented by a reference map for that cycle or group. The map for the current Kalman filter measurement is simply the first column of the current satellite table, showing the ID numbers of the satellites used or the “−1” indicating no satellite in that position of the table. The reference map for a group of cycles is adjusted starting with the reference map for the first cycle or sub-group, as explained below
[0131] When adding a current new term to a running sum, a special procedure, described below, is required when the current term uses a different set of satellites (represented by the current map for that term), from the set of satellites used by the running sum up to that term (represented by the reference map for that running sum).
[0132] This map difference could be caused by a satellite going out of view, as determined by a count of the number of consecutive Kalman filter cycles for which the measurement for that satellite is missing. The satellite is assumed to be out of view if this count equals six or more. If less than six, as due to masking during a turn, it is assumed that the satellite is still in use, although its measurement is missing temporarily. In this case, the reference map for the running sum is unchanged in the procedure below. The map difference also could be caused because of a previously used satellite being replaced by a new satellite, or because a new satellite comes into view in an unoccupied position in the table.
[0133] These differences may also cause the reference maps for sub-groups to differ. When summing the sums for each subgroup, a similar procedure is required when the new sub-group sum uses a different set of satellites (represented by the current map of that sub-group sum), than that used by the running sum of sub-group sums up to that point (represented by the reference map of that running sum of sub-group sums).
[0134] When the maps differ, the running sums must be modified to correspond to the satellites used in the new term before adding the new term. Since the sums in Equations (1.3) and (1.4) represent information matrices, and weighted residuals, with zero rows and columns for missing satellites, it is not clear how to modify the running sums before adding the new term.
[0135] The answer is to first compress the 8×8 information matrix and the running sum of weighted residuals to compact matrices with no zeroed rows or columns. The dimensions are determined by the number of measurements used in the running sums at this point before adding the new term. Since there are no zeroed rows and columns in the compressed information matrix, it can be inverted to obtain the corresponding covariance matrix called Vavg. This is the covariance matrix of the estimated residual ravg based on the measurements up to that point in the running sum.
[0136] The estimated residual ravg up to that point can also be computed as rest from Equation (1.5) since
(Vest−1)−1=Vavg
rest=(Vest−1)−1(Vest−1rest)
[0137] and since the weighted sum of residuals is available as the running sum from Equation (1.3). The row and column for the satellite not used in the new term, represented by the current map, are then deleted from this covariance matrix and residual, to reduce the dimension by one. As explained, the reference map for the running sums is modified to replace the deleted satellite ID's by “−1”, if the satellite has been missing for three or more Kalman cycles at 1 cycle per minute.
[0138] The covariance matrix is then inverted back to the information matrix with the satellites deleted. It is then uncompressed to an 8×8 by adding back in the rows and columns of zeroes. The running sum of weighted residuals with the satellites deleted can then be re-computed using Equation (1.4) to compute the right side from the left side:
&Sgr;kVk−1rk=(Vest−1rest)
[0139] The new terms can now be added to the running sums. If the new terms contain rows and columns for new satellites, these are added to the rows and columns of zeroes in those positions. The map with the new satellites replaces the reference map for the running sums.
[0140] As explained in the previous section, the terms in the sums in Equations (1.3) and (1.4) can be computed by separating the total group of terms into sub-groups of several cycles each. The sums for each sub-group are computed first, and then the sums for each sub-group can be summed to determine the sum for the total group. There are two ways of doing this. The first is called “buffered averaging”. The second is called “sampled averaging”. These methods are described below.
[0141] In buffered averaging, the terms for each Kalman filter cycle are added to a running sum until a maximum number of cycles is reached. This maximum number of cycles is different for the navigation AIME™ Kalman filter than it is for the carrier loop AJ-AIME™ Kalman filter. For the navigation filter there are also different maximum numbers depending on the cycle rate, which in turn depends on the gyro grade used.
[0142] For civilian navigation grade gyros of 0.01 deg/hr, with Kalman update cycle rate of 1 cycle per minute, the maximum number accumulated is 10 representing ten minutes of averaging per buffer cycle. For the military 0.1 to 1.0 deg/hr grade gyro, the Kalman update cycle rate is 3 cycles/minute. The maximum number per buffer cycle is 6, representing two minutes of averaging. For the carrier loop AJ-AIME™ filter, whether civilian or military, the maximum number per buffer cycle is 10, representing ten seconds of averaging.
[0143] In any of the three cases, the accumulated totals are then stored in one position of a circular buffer with 12 buffer cycle positions. When 12 sub-group sums have been accumulated, the next sub-group sum replaces the first sub-group sum stored. Similarly, the next sub-group sum replaces the original second sub-group sum stored previously. In this way, the buffer stores the sub-group sums until 12 sums are accumulated and thereafter stores only the last 12 sub-group sums.
[0144] This process is repeated, with a pointer keeping track of the most recent position stored. This pointer counts from 1 to 12, when it is reset to 0, since 12=0 (MOD 12). In this way the buffer always stores the most recent 12 buffer cycles. When each new buffer sub-group sum is stored, the last 12 sub-group sums are summed over the past 12 buffer cycles according to Equations (1.3), and (1.4) where the k subscript now corresponds to each buffer cycle sub-group sum of 10 Kalman filter cycles at 1 cycle per minute or 6 cycles at 3 cycles per minute or 10 cycles at 1 cycle per second.
[0145] The estimated mean residuals and corresponding test statistic are computed from Equations (1.5) and (1.6) for the accumulated sub-group sums until the buffer is full and thereafter the estimated mean residuals and corresponding test statistic are computed for the past 12 buffer cycles. Similarly, the estimated mean residuals and corresponding test statistic are also computed until 4 buffer cycles are accumulated and thereafter for the last 4 buffer cycles.
[0146] For the 0.01 deg/hr grade gyro navigation filter, when the buffer is full, it corresponds to 12×10=120 Kalman filter cycles. At one minute per cycle, this is a total averaging time of 120 minutes (two hours). The 4 cycle buffer average corresponds to a total averaging time of 40 minutes. The four cycle buffer average is intended to more quickly detect drifts which are not quite as slow as those detected by the 12 cycle buffer.
[0147] As an example, a drift of 0.02 m/s will cause a range error in that satellite of 144 meters after 120 minutes (7200 seconds). Such a drift is extremely rare, and is normally corrected by the Operational Control Segment (OCS). Even with selective availability (SA) on, this 144 meters is a 6-sigma error, assuming a 24-meter one-sigma error from SA, after pre-filtering for 1 minute.
[0148] This 6-sigma error would be detected with very high probability. The position error is less than HDOP×144 meters. Assuming a bad HDOP of 3, the maximum position error is only 432 meters, which is less than the 556 meter (0.3 nm) alert limit for non-precision approach (NPA). With the low-pass filtering due to the long Kalman filter time constant, the position error will be reduced even further.
[0149] The test statistic for this buffer is only computed every ten minutes, assuming a navigation Kalman filter interval of 1 minute. Therefore, this test statistic is used only to detect the slowest satellite drifts of 0.3 m/s or less. A drift of 0.3 m/s causes a range error in the satellite of 180 meters in one buffer cycle sub-group of ten Kalman filter cycles (600 seconds). Jo This drift will be detected in less than ten minutes since it is a 7.5 sigma error. Even with a bad HDOP of 3, this drift will be detected long before the position error approaches 540 meters (3×180 meters). The low-pass filter effect will reduce this maximum error to a fraction of 540 meters.
[0150] Faster drifts of 0.3 m/s or higher are detected by sampled averaging, which will now be described. In sampled averaging, there will be three levels of averaging.
[0151] The first-level quantities V1, Vr1, ZV1, and &Sgr;Vr1 are defined as follows:
V1=Vk−1; Vr1=Vk−1rk
&Sgr;V1=&Sgr;kVk−1; &Sgr;Vr1=&Sgr;kVk−1rk
MapV1=mapV−1(k) (current map)
[0152] (As explained in previous section on modifications required when satellites are changed: If MapV1=Refmap1, due to a new satellite which is new, changed, or missing—compress and decompress &Sgr;V1, &Sgr;Vr1, to omit old satellite from slot for new satellites, and change Refmap1=MapV1 if satellite is missing for three or more cycles.)
[0153] The first-level quantities are initialized as follows:
k1=0, &Sgr;V1=0, &Sgr;Vr1=0, Refmap1=MapV1
[0154] The index k1 is incremented and the quantities V1 and Vr1 are incrementally summed at each Kalman filter cycle until k1 reaches a maximum count of k1max:
k1=k1+1
&Sgr;V1=V1+&Sgr;V1
&Sgr;Vr1=Vr1+&Sgr;Vr1
[0155] When k1=k1max, the following operations are performed:
V2=&Sgr;V1, Vr2=&Sgr;Vr1, and MapV2=Refmap1
k1=0, &Sgr;V1=0, and &Sgr;Vr1=0.
[0156] The second-level quantities are initialized as follows:
k2=0, &Sgr;V2=0, &Sgr;Vr2=0, Refmap2=MapV2
[0157] (As explained in the previous section on modifications required when satellites are changed: If MapV2!=Refmap2, due to a new satellite which is new, changed, or missing—compress and decompress &Sgr;V2, &Sgr;Vr2, to omit old satellite from slot for new satellites, and change Refmap2=MapV2.)
[0158] The index k2 is incremented and the quantities V2 and Vr2 are incrementally summed at each Kalman filter cycle until k2 reaches a maximum count of k2max:
k2=k2+1
&Sgr;V2=V2+&Sgr;V2
&Sgr;Vr2=Vr2+&Sgr;Vr2
[0159] When k2=k2max, the following operations are performed:
V3=&Sgr;V2, Vr3=&Sgr;Vr2, and MapV3=Refmap2.
k2=0, &Sgr;V2=0, and &Sgr;Vr2=0.
[0160] The third-level quantities are initialized as follows:
k3=0, &Sgr;V3=0, &Sgr;Vr3=0, Refmap3=MapV3
[0161] (If MapV3!=Refinap3, due to a new satellite which is new, changed, or missing, compress and decompress &Sgr;V3, &Sgr;Vr3, to omit old satellite from slot for new satellites, and change Refmap3=MapV3.)
[0162] The index k3 is incremented and the quantities V3 and Vr3 are incrementally summed at each Kalman filter cycle until k3 reaches a maximum count of k3max:
k3=k3+1
&Sgr;V3=V3+&Sgr;V3
&Sgr;Vr3=Vr3+&Sgr;Vr3
[0163] When k3=k3max, the following operations are performed:
V4=&Sgr;V3, Vr4=&Sgr;Vr3, and MapV4=Refmap3.
k3=0, &Sgr;V3=0, and &Sgr;Vr3=0
[0164] As indicated below, the test statistic is computed as each new term is added after the first term in Equations (1.3) and (1.4). This is done to increase the likelihood of detection before the maximum number of cycles k1max is accumulated. To indicate that the sums and test statistic for the next higher level are being accumulated, a subscript index “n” for the next higher level, followed by subscript “e” is used to indicate extrapolation.
[0165] The first level s-square equations are:
V2e=V1+&Sgr;V1
Vr2e=Vr1+&Sgr;Vr1
r2e=V2e−1Vr2e
s2e2=r2eTVr2e
[0166] The second level s-square equations are:
V3e=V2+&Sgr;V2
Vr3e=Vr2+&Sgr;Vr2
r3e=V3e−1Vr3e
s3e2=r3eTVr3e
[0167] The third level s-square equations are:
V4e=V3+&Sgr;V3
Vr4e=Vr3+&Sgr;Vr3
r4e=V4e−1Vr4e
s4e2=r4eTVr4e
[0168] When the number of cycles reaches k1max, the counter k1 is reset to zero, and the accumulated sums are stored as the first sub-group sum of the second level of averaging. The sums at the first level of averaging are then cleared to zero, and the first level averaging process is repeated.
[0169] The terms of sub-group sums are summed at each second level cycle whenever k1 reaches k1max. The second level counter k2 is then incremented at each second level cycle until it reaches a count of k2max. The test statistic is computed as each new sub-group sum is added, after the first term in Equations (1.3) and (1.4). This is done to increase the likelihood of detection before the maximum number of cycles k2max is accumulated. When the number of cycles reaches k2max, the counter k2 is reset to zero, and the accumulated second level sums are stored as the first sub-group sum of the third level of averaging. The sums at the second level of averaging are then cleared to zero, and the second level averaging process is repeated.
[0170] The terms of second level sub-group sums are summed at each third level cycle whenever k2 reaches k2max. The third level counter k3 is then incremented at each third level cycle until it reaches a count of k3max. The test statistic is computed as each new third level sub-group sum after the first term is added in Eqations (1.3) and (1.4). This is done to increase the likelihood of detection before the maximum number of cycles k3max is accumulated. When the number of cycles reaches k3max, the counter k3 is reset to zero, and the accumulated sums are stored as the final sub-group sum of the third level of averaging. The sums at the third level of averaging are then cleared to zero, and the third level averaging process is repeated In the preferred embodiment, the maximum counts at each level are:
k1max=2,
k2max=2,
k3max=2
[0171] With these maximum counts, the test statistic is only computed once as the second new term is added after the first term in Equations (1.3) and (1.4). This means the first level will accumulate Kalman filter cycles for two minutes, the second level will accumulate two minute sub-groups sums for four minutes, and the third level will accumulate four minute sub-group sums for 8 minutes.
[0172] The residuals are used to determine test statistics at each Kalman cycle, at each first level average every two cycles, at each second level average every 4 cycles, and at each third level average every 8 cycles. Since these averages are not computed in circular buffers, they are called sampled averages.
[0173] As an example for the navigation grade gyros, assuming SA, a satellite drift of 0.3 m/s will cause a range error in that satellite of 144 meters after 8 minutes (480 seconds). Since 144 meters is a 6-sigma error, this drift will be detected in less than 8 minutes. The position error will be a very small fraction of 432 meters (3×144 meters) for HDOP of 3, because of the long Kalman filter time constant low-pass filter effect. Similarly, larger drifts will be detected sooner, with even smaller position errors.
[0174] Failures are detected by comparing the s2 statistics with thresholds. When there are no failures in the Kalman filter from which s2 was computed, the statistic s2 is central Chi-square distributed. The detection threshold sD2 is selected at the tail of the distribution, to result in a low specified false alarm rate when there are no failures.
[0175] The statistics computed at 1 Kalman filter cycle, at the sampled averaging sample times, and at the buffered averaging cycle lengths, are shown below. 1 Kalman Filter Cycle Rate Test 1/mm 3/mm 1 Hz Stat. Averaging time Description S12 1 minute 1/3 min 1 sec Computed each Kalman filter cycle S22 2 minute 2/3 min 2 sec Second sampled average, level one (S2e2) S42 4 minute 4/3 min 4 sec Second sampled average, level two (S3e2) S82 8 minute 8/3 min 8 sec Second sampled average, level three (S4e2) SB12 10 minute 2 min 10 sec Current one buffer cycle average SB42 40 minute 8 min 40 sec Most recent four buffer cycle average SB122 120 minute 24 min 120 sec Most recent twelve buffer cycle average
[0176] In general, fast failures, such as large step changes or large ramp errors in the satellites, are first detected by the statistics with shortest averaging time. Slow failures are detected by the statistics with the longest averaging times.
[0177] In addition to the Kalman filter fo, which is modeled for normal errors with no failures, there is a bank of ten Kalman filters running in parallel, each modeled for a different subsystem failure. These Kalman filters are listed below. 2 Filter Hypothesis Description of Kalman filter Model f0 No failures P's, Q's, and R's, from normal error specs f1 sat 1 failure P's, Q's, and R's for sat 1 increased f2 sat 2 failure P's, Q's, and R's for sat 2 increased f3 sat 3 failure P's, Q's, and R's for sat 3 increased f4 sat 4 failure P's, Q's, and R's for sat 4 increased f5 sat 5 failure P's, Q's, and R's for sat 5 increased f6 sat 6 failure P's, Q's, and R's for sat 6 increased f7 sat 7 failure P's, Q's, and R's for sat 7 increased f8 sat 8 failure P's, Q's, and R's for sat 8 increased f9 ADIRS failure P's, Q's, for IRS and Baro increased f10 GPS failure P's, Q's, and R's for all GPS sats increased (e.g. jamming)
[0178] The first filter, f0 is used to detect, but not isolate, a failure condition in any one of the subsystems. The other filters are used to isolate the failure to a specific subsystem, once a failure auto condition is detected. In addition to computing the position outputs, each test filter will also compute the two-sigma accuracy figure of merit (FOM), and the horizontal integrity level (HL), based on its own Kalman filter tuning. When the failure is isolated to a particular subsystem, it is safe to use the outputs from the corresponding test filter, since it is insensitive to that subsystem failure. The ouputs from this filter are therefore selected by index fx.
[0179] The isolation is achieved by computing statistics as above for each of the Kalman filters in the above table. When a failure occurs, only one of the test filters will continue to have small, uncorrelated residuals, and correspondingly small test statistics. This is the filter which is tuned for that specific subsystem failure. The failure is assumed to have occurred in the subsystem with the smallest test filter statistics at the maximum averaging times where the corresponding detection threshold is exceeded. Since this filter is insensitive to failures in that subsystem, it is safe to use the position solution from this test filter.
[0180] The P's in the description column of the above table are the covariances that the Kalman fiter is initialized with. These are generally diagonal terms corresponding to the manufacturer's specifications on the error variance. The only off-diagonal terms are those due to correlations of some of the inertial error states at the end of a short alignment since the Kalman filter is initialized at the end of a short alignment.
[0181] For the satellites, the diagonal element corresponding to that satellite range bias error state is initialized or re-initialized when the satellite first comes into view. All other elements in the corresponding row and column are initialized with zeroes, since the satellite error is initially uncorrelated with any of the other error states.
[0182] For satellite failure test filters f1 through f8, the principal change in the Kalman filter model is to increase the process noise Q for that satellite range bias error state, or carrier phase error, until that satellite measurement is effectively ignored in the updates for that test filter.
[0183] For the IMU-Baro failure test filter f9, the process noises Q for all gyro, accelerometer, and baro altitude error states is increased, but not so much that the ADIRS is assumed to be completely unusable. Instead, the Kalman filter f9 gives more weight than normal to the GPS measurements, so that it relies less on IMU-Baro coasting capability. It therefore only models soft failures, since hard failures will shut the system down completely.
[0184] When there is a soft IMU-Baro failure, such as a large change in gyro bias error, or a rapid baro-inertial altitude bias change due to a weather front, the residuals and test statistics from this filter will remain small. The test statistics from all of the other filters will be large. A soft failure is therefore assumed to have occurred in the IMU-Baro system. In this case, it is safe to use the position output from this test filter, since it is relatively insensitive to such changes. This is accomplished by using the index, fx, which specifies which filter outputs to use.
[0185] When there are large errors in more than one satellite, such as due to the onset of jamming or spoofing, or due to GPS receiver problems, the residuals and test statistics from the GPS failure test filter will remain small. The test statistics from all of the other filters will be large. In this case, it is safe to use the position output from this test filter, since it relies mostly on IRS coasting with only gradual updating from the GPS. The index fx then specified this filter for the outputs. The outputs from this filter can also be used when there is more than one satellite failure at a time or when the statistics from more than one satellite failure test filter are small due to unusual geometry, making it difficult to isolate the bad satellite.
[0186] When there is a failure, and it is isolated to a bad subsystem, the failure condition is re-modeled in the other test filters, but not to the no-failure test filter f0. This is done so that these other filters will eventually recover from the failure, even though they are contaminated at the time of detection and isolation, as evidenced by their residuals and test statistics being large.
[0187] For satellite failures, the failure condition is modeled in all of the other test filters by deleting the failed satellite from the measurements. If the original satellite was excluded incorrectly, the test statistic of the no-failure filter may again give small residuals. If its statistic is less than one half the threshold, the output will again be taken from the no-failure filter, and the previously excluded satellite will again be used by all filters.
[0188] Another possibility is that the test statistic for one of the other test filters will become smaller than that for the original test filter, which incorrectly excluded the wrong satellite. This could also occur if two satellites fail at nearly the same time, although this is extremely unlikely. If this occurs, the second satellite is also excluded from all of the other test filters, and the outputs from the second satellite test filter are safe to use.
[0189] If the first satellite was incorrectly excluded, its test statistic will again exceed the threshold, and it will again be accepted as a new satellite by the other test filters, which have been excluding both satellites. If the no-failure filter statistic becomes less than one-half the detection threshold, all excluded satellites will be used by all of he test filters.
[0190] The failure condition for failure of the IMU-Baro is re-modeled in the other filters by increasing the Q for one of the instruments. The failed instrument is selected by checking which instrument, or instruments, have estimated error state variables which exceed a pre-determined three-sigma limit in the ADIRS failure test filter (or no-failure filter). The state variables checked are the baro-offset, each gyro, and each accelerometer.
[0191] The Q selected for increase is thereafter decreased exponentially with a 20 minute time constant in all filters, except in the IMU-Baro failure test filter where all Q's remain large. If the no-failure residuals and test statistic of the no-failure filter eventually become smaller than a two sigma threshold, the outputs are again taken from the no-failure filter rather than from the IMU-Baro failure test filter. This is done because the no-failure filter has only one large Q, which is decaying exponentially, while all the gyro, accelerometer, and baro Q's of the IMU-Baro failure test filter remain large.
[0192] FIG. 1 shows how AJ-AIME closes the carrier and code tracking loops. The Carrier NCO and CODE NCOi boxes and the boxes above these boxes in FIG. 1 show the conventional mechanization of a carrier Costas loop and a code delay lock loop except for the conventional filters which close the loop.
[0193] The boxes below the Carrier NCO and CODE NCO, boxes show a Doppler phase rotation increment command &phgr;(t+tp) entering the Carrier NCO box. In a conventional Costas loop, this command is the output of a first, second, or third order filter. In FIG. 1 it is the 1000-Hz output of the Extrapolator with small 1000-Hz aiding corrections from the AJ-AIME KF (Kalman Filter) and State Error Extrapolator.
[0194] The Carrier Phase Error (IpQp=e&phgr;) coming from the discriminator, which would go into the Costas loop filter in a conventional mechanization, enters the AJ-AIME KF box. In effect, the Extrapolator and the AJ-AIME KF and State Error Extrapolator replace the Costas loop filter that conventionally closes the carrier tracking loop.
[0195] The AJ-AIME mechanization is really a sequence of three filters in series, each aiding the next. The AIME KF (Kalman Filter) has the slowest update rate. The rate is 1 cycle per minute, for 0.01 deg/hr navigation grade gyros, and the rate is 3 cycles per minute for missile applications with lower grade gyros of 0.1 deg/hr to 1.0 deg/hr. The measurements to this filter are the PR measurements at 1 Hz, averaged or pre-filtered over the update interval of one to three cycles per minute. Further details concerning the AIE KF can be found in U.S. Pat. No. 5,583,774 which is incorporated by reference.
[0196] The AIME KF uses PR measurements rather than phase measurements, since IMU errors such as gyro and accelerometer bias errors or reference navigation axis alignment errors, take many minutes to estimate. The error states and correlation times are shown below for both navigation grade gyros, and for lower grade gyros which might be used in missiles or munitions.
[0197] The correlation times of these navigation error sources are from ten minutes to an hour. It is best to use a measurement such as PR, which has a bounded position error over these long correlation periods. Without DGPS, the integrated carrier Doppler phase error becomes unbounded over long periods of time.
[0198] Update Rate: 1/min., navigation grade gyros; 3/min, AHRS grade gyros. Process Noise: Qn=2&sgr;2n(1/&tgr;n) 3 Group N Symbol Definition &sgr;n 1/&tgr;n XN: 1 dPx 10 meters 0 (Nav) 2 dPy 10 meters 0 7 3 dVx 0.1 m/s 0 4 dVy 0.1 m/s 0 5 d&phgr;x x - nav axis misalignment 0.001 degree* 1/hour 6 d&phgr;y y - nav axis misalignment 0.001 degree* 1/hour 7 d&phgr;z z - nav axis misalignment 0.001 degree* 1/hour X1 8 dGBx x - gyro bias 0.01-1.0 deg/hr** 1/600s (Instr.) 9 dGBy y - gyro bias 0.01-1.0 deg/hr** 1/600s 5 10 dGBz z - gyro bias 0.01-1.0 deg/hr** 1/600s 11 dABx x - accelerometer 20 micro-g 1/600s 12 dABy y - accelerometer 20 micro-g 1/600s XG 13 DB User clock bias 100 feet 1/3600s (GPS) 14 dBr User clock bias rate 1 foot/sec 1/600s 4 15 dhB Baro offset 250 feet 1/3600s 16 dhBh Normalized baro offset (1000 ft/30 K ft) · h 1/3600s scale factor XS 17 dRB1 Range bias for sat. 1 10 meters 1/3600s (Sat.) 18 dRB2 Range bias for sat. 2 10 meters 1/3600s 8 19 dRB3 Range bias for sat. 3 10 meters 1/3600s 20 dRB4 Range bias for sat. 4 10 meters 1/3600s 21 dRB5 Range bias for sat. 5 10 meters 1/3600s 22 dRB6 Range bias for sat. 6 10 meters 1/3600s 23 dRB7 Range bias for sat. 7 10 meters 1/3600s 24 dRB8 Range bias for sat. 8 10 meters 1/3600s *Gyro Random Walk: 0.001 deg/{square root}hr **1 deg/hr - munitions, missiles (MEMS); 0.1 deg/hr - missiles, aircraft (FOG); 0.01 deg/hr - aircraft (RLG, ZLG)
[0199] The corresponding dynamics matrix is shown below. 4 xN xI xG xS 24×24 {dot over (x)}N 7×7 7×5 0 [F] FNN FNI {dot over (x)}I 0 5×5 0 FII {dot over (x)}G 0 0 4×4 FGG {dot over (x)}S 8×8 FSS dPx dPy dVx dVy d&phgr;x d&phgr;y d&phgr;z 7×7 d{dot over (P)}x 1 Body(B)-to-Ref(R) [FNN] d{dot over (P)}y 1 Direction Cosine d{dot over (V)}x 0 −Az +Ay (Dc)Matrix: d{dot over (V)}y +Az 0 −Ax 3×3 Cxx Cxy Cxz d{dot over (&phgr;)}x −R&ohgr;z −1/R 0 &ohgr;z −&ohgr;y [CRB] Cyx Cyy Cyz d{dot over (&phgr;)}y −R&ohgr;z +1/R −&ohgr;z 0 +&ohgr;x Czx Czy Czz d{dot over (&phgr;)}z +R&ohgr;x −R&ohgr;y +&ohgr;y −&ohgr;x 0 dGBx dGBy dGBz DABx DABy dGBx dGBy dGBz dABx dABy 7×5 d{dot over (P)}x 5×5 d{dot over (G)}Bx −1/&tgr;G [FNI] d{dot over (P)}y [FII] d{dot over (G)}By −1/&tgr;G d{dot over (V)}x Cxx Cxy d{dot over (G)}Bz −1/&tgr;G d{dot over (V)}y Cyx Cyy d{dot over (A)}Bx −1/&tgr;A d{dot over (&phgr;)}x Cxx Cxy Cxz d{dot over (A)}By −1/&tgr;A d{dot over (&phgr;)}y Cyx Cyy Cyz d{dot over (&phgr;)}t Czx Czy Czz dB dBr DhB dhBh 4×4 {dot over (d)}B −1/&tgr;B 1 0 0 [FGG] {dot over (d)}Br 0 −1/&tgr;Br 0 0 d{dot over (h)}B 0 0 −1/&tgr;hB 0 d{dot over (h)}Bh 0 0 0 −1/&tgr;hBh dRB1 dRB2 dRB3 dRB4 dRB5 dRB6 dRB7 DRB8 8×8 d{dot over (R)}B1 −1/&tgr;RB [FSS] d{dot over (R)}B2 −1/&tgr;RB d{dot over (R)}B3 −1/&tgr;RB d{dot over (R)}B4 −1/&tgr;RB d{dot over (R)}B5 −1/&tgr;RB d{dot over (R)}B6 −1/&tgr;RB d{dot over (R)}B7 −1/&tgr;RB d{dot over (R)}B8 −1/&tgr;RB
[0200] The measurements and observation matrices are shown below.
[0201] Enroute through non-precision approx. (NPA): dtk=1 minute (60 s)
[0202] Precision Approach (PA) or missiles: dtk=⅓ minute (20 s) 1 PR i ( i ) = 1 / T ∑ t = 1 T PR i ( t ) , dt = 1 sec , T = dt k PR 1 ( k ) 8 × 1 PR 2 ( k ) 8 × 1 PR 3 ( k ) [ Z ' ( k ) ] = PR 4 ( k ) PR 5 ( k ) PR 6 ( k ) PR 7 ( k ) PR 8 ( k ) ] ; 5 dPx dPy . . . dB . . . dhB . . . dRB1 dRB2 dRB3 dRB4 dRB5 dRB6 dRB7 dRB8 8×24 e1x e1y −1 e1z 1 [H(k)] = e2x e2y −1 e2z 1 e3x e3y −1 e3z 1 e4x e4y −1 e4z 1 e5x e5y −1 e5z 1 e6x e6y −1 e6z 1 e7x e7y −1 e7z 1 e8x e8y −1 e8z 1
[0203] The error state corrections x(t) are sent to the NAV computer in FIG. 1. This is done at a rate of 1 Hz, by using transition matrix extrapolation between update intervals of {fraction (1/3)} to 1 minute. The NAV computer adds these slowly-varying 1-Hz corrections to the uncalibrated navigation solution based on uncalibrated IMU inputs. This is done at the navigation solution rate of 200 Hz to obtain the calibrated position output P(tk) which is sent to the AJ-AIME KF and State Error Extrapolator.
[0204] The inertial errors, low-frequency user clock errors, and low-frequency atmospheric errors have already been removed by the AIME KF. The AJ-AIME KF and State Error Extrapolator only has to estimate small, residual, high frequency errors in the signal to each satellite. These errors are the high frequency clock errors, the high frequency atmospheric anomalies and multipath errors, and the gravity anomalies. These errors each have correlation times of less than 5 minutes as shown below. As shown, the correlation time of the gravity anomaly errors is proportional to ground speed (VGS), since the gravity anomalies vary with distance. The correlation distance is taken here to be 10 mn, rather than the usual 20 nm, since a first-order Markov model is used, rather than the second-order model of Gelb.
[0205] Update Rate: 1 Hz Process Noise: Qn=2&sgr;2n(1/&tgr;n) 6 Group n Symbol Definition &sgr;n 1/&tgr;n N 1 &Dgr;Px Delta position x 1 cm 1/300s (Nav) 2 &Dgr;Py Delta position y 1 cm 1/300s 6 3 &Dgr;Pz Delta position z 1 cm 1/300s 4 &Dgr;Vx Delta velocity x 0.1 cm/sec 1/300s 5 &Dgr;Vy Delta velocity y 0.1 cm/sec 1/300s 6 &Dgr;Vz Delta velocity z 0.1 cm/sec 1/300s G 7 &Dgr;Ax Gravity anomaly x 5 micro-g VGS (G.A., (knots)/ (GPS) 10 nm 5 8 &Dgr;Ay Gravity anomaly y 5 micro-g VGS (knots)/ 10 nm 9 &Dgr;Az Gravity anomaly z 5 micro-g VGS (knots)/ 10 nm 10 &Dgr;B Delta user clock 10 nano-sec 1/300s 11 &Dgr;Br Delta user clock rate 1 nano-sec/sec 1/300s S 12 &Dgr;RB1 Delta range bias, sat. 1 1 meter 1/60s (Sat.) 13 &Dgr;RB2 Delta range bias, sat. 2 1 meter 1/60s 8 14 &Dgr;RB3 Delta range bias, sat. 3 1 meter 1/60s 15 &Dgr;RB4 Delta range bias, sat. 4 1 meter 1/60s 16 &Dgr;RB5 Delta range bias, sat. 5 1 meter 1/60s 17 &Dgr;RB6 Delta range bias, sat. 6 1 meter 1/60s 18 &Dgr;RB7 Delta range bias, sat. 7 1 meter 1/60s 19 &Dgr;RB8 Delta range bias, sat. 8 1 meter 1/60s
[0206] The corresponding dynamics matix is shown below. 7 xN xG xS 19×19 {dot over (x)}N 6×6 6×5 [F] FNN FNG {dot over (x)}G 5×5 FGG {dot over (x)}S 8×8 FSS &Dgr;Px &Dgr;Py &Dgr;Pz &Dgr;Vx &Dgr;Vy &Dgr;Vz 6×6 &Dgr;{dot over (P)}x 1 0 0 [FNN] &Dgr;{dot over (P)}y 0 1 0 &Dgr;{dot over (P)}z 0 0 1 &Dgr;{dot over (V)}x &Dgr;{dot over (V)}y &Dgr;{dot over (V)}z &Dgr;Ax &Dgr;Ay &Dgr;Az &Dgr;B &Dgr;Br &Dgr;Ax &Dgr;Ay &Dgr;Az &Dgr;B &Dgr;Br 6×5 &Dgr;{dot over (P)}x 5×5 &Dgr;{dot over (A)}x −1/&tgr;GA [FNG] &Dgr;{dot over (P)}y [FGG] &Dgr;{dot over (A)}y −1/&tgr;GA &Dgr;{dot over (P)}z &Dgr;{dot over (A)}z −1/&tgr;GA &Dgr;{dot over (V)}x 1 0 0 &Dgr;{dot over (B)} −1/&tgr;&Dgr;B 1 &Dgr;{dot over (V)}y 0 1 0 &Dgr;{dot over (B)}r 0 −1/&tgr;&Dgr;B &Dgr;{dot over (V)}z 0 0 1 &Dgr;RB1 &Dgr;RB2 &Dgr;RB3 &Dgr;RB4 &Dgr;RB5 &Dgr;RB6 &Dgr;RB7 &Dgr;RB8 4×4 &Dgr;RB1 −1/&tgr;&Dgr;RB [FGG] &Dgr;RB2 −1/&tgr;&Dgr;RB &Dgr;RB3 −1/&tgr;&Dgr;RB &Dgr;RB4 −1/&tgr;&Dgr;RB &Dgr;RB5 −1/&tgr;&Dgr;RB &Dgr;RB6 −1/&tgr;&Dgr;RB &Dgr;RB7 −1/&tgr;&Dgr;RB &Dgr;RB8 −1/&tgr;&Dgr;RB
[0207] The corresponding measurement and observation matrices are shown below.
[0208] Update interval dtk=1 second; dtm=0.020 s (20 ms)
[0209] Transition matrix from tk to tm: &PHgr;(tm−tk) 2 e φ i ( k ) = 1 / 50 ∑ t m = 1 50 e φ i ( t m ) Φ ( t m - t k ) ,
[0210] e&phgr;i(tm)=Average phase error measurement over data bit interval. 3 e φ 1 ( k ) e φ 2 ( k ) 8 × 1 e φ 3 ( k ) [ Z ( k ) ] = e φ 4 ( k ) e φ 5 ( k ) e φ 6 ( k ) e φ 7 ( k ) e φ 8 ( k ) ] ; 8 &Dgr;Px &Dgr;Py &Dgr;Pz . . . &Dgr;B . . . &Dgr;RB1 &Dgr;RB2 &Dgr;RB3 &Dgr;RB4 &Dgr;RB5 &Dgr;RB6 &Dgr;RB7 &Dgr;RB8 8×24 e1x e1y e1z −1 1 [H(k)] = e2x e2y e2z −1 1 e3x e3y e3z −1 1 e4x e4y e4z −1 1 e5x e5y e5z −1 1 e6x e6y e6z −1 1 e7x e7y e7z −1 1 e8x e8y e8z −1 1
[0211] It is assumed here that the body mounted IMU axes lie along the principal (eigenvector) axes of the moment of inertia tensor of the aircraft or missile, and that the autopilot controls the angular rates, &ohgr;x, &ohgr;y, &ohgr;z about these axes. The angular rates and angular accelerations can then be predicted a short time tp into the future at 1000 Hz by the body axis extrapolator shown in FIG. 2. This prediction time is necessary because of the time lag in rotating the carrier phase in the Carrier NCO shown in FIG. 1. If the phase is rotated at constant rate every 5 milliseconds, the prediction time must be at least 5 milliseconds. If the phase is rotated at a changing rate every millisecond, the prediction time must be at least 1 millisecond. The sampling switch at interval Tk shown in FIG. 2 is also necessary when EMU measurements are asynchronous with the Carrier NCO.
[0212] It is also assumed that the autopilot controls the acceleration along body axes, by controlling lift and thrust forces. The linear accelerations are also predicted as shown by the dV Predictor in FIG. 2.
[0213] The rate of change of angular acceleration, called angular “jerk”, is not estimated by the body axis extrapolator of FIG. 2. Using as an example a 1-foot lever arm from the center of mass to the GPS antenna, a maximum antenna jerk of 10 g's/sec would result from an angular jerk of 320 radian/sec2 per second. This would be typical of a low performance aircraft or missile. A maximum jerk of 100 g's per second, resulting from 3200 rad/sec2 per sec, would be typical for high performance aircraft or missiles. A jerk of 1000 g's per second, resulting from 32,000 radians/sec2 per second, would be typical for many munitions.
[0214] With 1-foot lever arm, the 1000 g/sec jerk resulting from 32,000 rad/sec3 is the same as 32,000 feet/sec3. This is approximately 10,000 meters/sec3, which is 0.010 millimeter/millisecond3. The maximum phase displacement with prediction time 5 milliseconds is 0.2083333 mm, obtained from (0.010 mm/ms3)×(5 ms)3/6.
[0215] Similar results apply to the linear acceleration predictor in FIG. 2. Although 1000 g's/sec seems quite large, it corresponds to changing the acceleration by 10 g's in 0.01 seconds, or by 100 g's in 0.1 second. Angular acceleration jerks about the roll axis are sometimes larger than linear acceleration jerks. Larger jerks can be accommodated by increasing the angular measurement rate from the IMU from 400 Hz to 1600 Hz, and by reducing the constant phase rate rotation interval in the Carrier NCO to 1 millisecond.
[0216] Since the phase angle and velocity increments in FIG. 2 are in body axes, they must be converted to position along the line-of-sight (LOS) in order to rotate the phase of the Carrier NCO. This is done by the algorithm shown below. Define: 4 3 × 1 3 × 1 Δθ B ] = ∑ t = t k t + t p ⅆ θ B ( t ) ;
[0217] dtk=5 ms (200 Hz)
[0218] Body-to-Reference Direction Cosine (DC) Matrix 5 3 × 3 3 × 3 [ C R B ( t + t p ) ] = [ 1 - Δθ z + Δθ y Δθ z 1 - Δθ x - Δθ y + Δθ x 1 ] · [ C R B ( t k ) ]
[0219] Body Axes: 6 3 × 1 3 × 1 3 × 1 dV B ( t + t p ) ] = ∑ t = t k t + t p dV B ( t + t p ) ] - ∑ t = t k t + t p - 1 dV B ( t + t p ) ]
[0220] Reference (NAV) axes: 7 3 × 1 3 × 3 3 × 1 d V R ( t + t p ) ] = [ C B R ( t + t p ) · d V B ( t + t p ) ] 1000 Hz : 8 3 × 1 3 × 1 Δ P R ( t + t p ) ] = ∑ t = t k t + t p dV ( t + t p ) , dt k = 5 ms ( 200 Hz ) 3 × 1 3 × 1 3 × 1 3 × 3 3 × 1 P R ( t + t p ) ] = P R ( t k ) ] + Δ P R ( t + t p ) ] + [ C R B ( t + t p ) ] · R B L ]
[0221] Line-of-Sight (LOS): 9 3 × 1 e LOS i R ] = Direction Cosines to Sat i ( 1 Hz ) 1 × 1 1 × 3 3 × 1 φ e i = e LOS i R ] T · P R ( t + t p ) ] ( dot product )
[0222] The first step is to determine the direction cosine (DC) matrix from body (B) to navigation reference (R) axes at the predicted time t+tp. This is accomplished by accumulating angular increments along body axes from the last navigation solution at time tk to the predicted time t+tp. Using small angle approximations, the DC matrix at the last navigation solution time tk is then rotated through the accumulated angle &Dgr;&thgr;B obtain an approximate DC matrix [CBR(t+tp)] at 1000 HZ. This matrix is then used to convert the last dVB increment at time t+tp to navigation reference axis increment dVR(t+tp) at 1000 Hz. These increments along navigation reference axes are accumulated to obtain delta position of the IM from last navigation solution time tk to predicted time t+tp.
[0223] By adding these positions, the total position of the IMU is obtained. It remains to add the relative position of the GPS antenna due to the lever arm vector RLB] from the IM to the GPS antenna. Although RLB] is fixed in body axes, it must be resolved along navigation reference axes, as was dVB]. The resulting position is then resolved along the LOS to each satellite, using the slowly varying satellite LOS direction cosines ei] relative to the navigation reference axes. These direction cosines are already computed from the navigation filter at 1 Hz.
[0224] Because of the aiding from the slow (3 per minute) AIME KF to the fast (1 Hz) AJ-AIME KF and the aiding from this fast Kalman filter (1 Hz) to the Extrapolator (1000 Hz), positions computed at different rates are being added. In order to provide continuous 1000 Hz phase, it is necessary to low-pass filter each aiding position and to sample at a much higher rate before adding to the faster filter at each level.
[0225] Since only relative change in phase is needed, the positions are periodically reduced by an amount corresponding to the change since the last slow computer time tk to time t+tp, and only the position change is resolved along the LOS to add to the phase at the last computer time tk. Whenever the position is reduced, the low pass filter for that position is reduced by the same amount, in order that the filter output is continuous. In addition, the phase is reduced by the phase at the last slow computer time tk. Since the phase is computed in fractions of a revolution, modulus-1 revolution, the change in phase is computed correctly, even though the total accumulated phase never exceeds one revolution.
Claims
1. A method for adjusting the phase and frequency of a received GPS signal in an inertial-GPS navigation system comprising an inertial measurement unit (IMU) and a GPS receiver, the GPS receiver having an input port for inputting a delta-phase at delta-time intervals into a received satellite signal, the GPS receiver having an output port for outputting a carrier phase error, the carrier phase error being the difference between the actual phase of the received GPS signal and a reference value, the GPS receiver having an output port for outputting a pseudorange associated with the received GPS signal, the IMU having an output port for outputting acceleration and angular velocity of the IMU, the delta-phase being the sum of at least two delta-phase components, the method comprising the step:
- (a) determining a Kalman delta-phase component, a plurality of candidate Kalman delta-phase components being obtained by performing a first set of more than one Kalman filter processes, the inputs to each first-set Kalman filter process including the carrier phase error and a position vector associated with the IMU, a candidate Kalman delta-phase component being produced as an output of each first-set Kalman filter process, the Kalman delta-phase component being determined from a candidate Kalman delta-phase components selected from the plurality of candidate Kalman delta-phase components by applying a predetermined Kalman delta-phase component selection rule;
- (b) determining an IMU delta-phase component from the EMU outputs and a direction-cosine matrix that translates the body coordinates of the IMU into the navigation coordinates of the inertial-GPS navigation system.
2. The method of claim 1 wherein step (a) comprises the step:
- (a1) identifying the present value of the Kalman delta-phase component with a future value of the selected candidate delta-phase component.
3. The method of claim 1 wherein step (a) comprises the steps:
- (a1) obtaining a plurality of candidate error-state vectors by performing a second set of more than one Kalman filter processes, a candidate error-state vector being produced as an output of each second-set Kalman filter process utilizing a plurality of prior pseudorange values and the present position vector;
- (a2) selecting the error-state vector from the candidate error-state vectors by applying a predetermined error-state vector selection rule;
- (a3) determining the position vector using IMU outputs and the error-state vector.
4. The method of claim 3 wherein in step (a1) each Kalman filter process in the second set is tuned to recognize either (1) the absence of any failure conditions or (2) the presence of a particular failure condition, a failure condition being the failure of a group of one or more functional elements of the inertial-GPS navigation system to perform within specifications.
5. The method of claim 3 wherein in step (a2) the selected error-state vector is the error-state vector produced by the Kalman filter process producing a value of a failure-condition quantity that is statistically distinguishable from the values of the failure-condition quantity produced by the other second-set Kalman filter processes.
6. The method of claim 5 whereby the failure-condition quantity is derived from weighted residuals summed over a plurality of Kalman filter process update cycles.
7. The method of claim 1 wherein step (b) comprises the steps:
- (b1) estimating future values of IMLJ acceleration and angular velocity from the IMU outputs;
- (b2) translating the future values of IMW acceleration and angular velocity into the IMU delta-phase component utilizing the direction-cosine matrix that translates the body coordinates of the IMU into the navigation coordinates of the inertial-GPS navigation system.
8. The method of claim 7 wherein step (b2) comprises the steps:
- (b2-1) obtaining a plurality of candidate error-state vectors by performing a second set of more than one Kalman filter processes, a candidate error-state vector being produced as an output of each second-set Kalman filter process utilizing a plurality of prior pseudorange values and the present position vector;
- (b2-2) selecting the error-state vector from the candidate error-state vectors by applying a predetermined error-state vector selection rule;
- (b2-3) determining the direction-cosine matrix using IMU outputs and the error-state vector.
9. The method of claim 8 wherein in step (b2-1) each Kalman filter process in the second set is tuned to recognize either (1) the absence of any failure conditions or (2) the presence of a particular failure condition, a failure condition being the failure of a group of one or more functional elements of the inertial-GPS navigation system to perform within specifications.
10. The method of claim 8 wherein in step (b2-2) the selected error-state vector is the error-state vector produced by the Kalman filter process producing a value of a failure-condition quantity that is statistically distinguishable from the values of the failure-condition quantity produced by the other second-set Kalman filter processes.
11. The method of claim 10 whereby the failure-condition quantity is derived from weighted residuals summed over a plurality of Kalman filter process update cycles.
12. Apparatus for practicing the method of claim 1.
13. Apparatus for adjusting the phase and frequency of a received GPS signal in an inertial-GPS navigation system comprising an inertial measurement unit (IMU) and a GPS receiver, the GPS receiver having an input port for inputting a delta-phase at delta-time intervals into a received satellite signal, the GPS receiver having an output port for outputting a carrier phase error, the carrier phase error being the difference between the actual phase of the received GPS signal and a reference value, the GPS receiver having an output port for outputting a pseudorange associated with the received GPS signal, the IMW having an output port for outputting acceleration and angular velocity of the IMU, the delta-phase being the sum of at least two delta-phase components, the apparatus comprising:
- (a) a processor for determining a Kalman delta-phase component, a plurality of candidate Kalman delta-phase components being obtained by performing a first set of more than one Kalman filter processes, the inputs to each first-set Kalman filter process including the carrier phase error and a position vector associated with the IMU, a candidate Kalman delta-phase component being produced as an output of each first-set Kalman filter process, the Kalman delta-phase component being determined from a candidate Kalman delta-phase components selected from the plurality of candidate Kalman delta-phase components by applying a predetermined Kalman delta-phase component selection rule;
- (b) a processor for determining an IMU delta-phase component from the IMU outputs and a direction-cosine matrix that translates the body coordinates of the IMU into the navigation coordinates of the inertial-GPS navigation system.
14. The apparatus of claim 13 wherein processor (a) includes:
- (a1) a means for identifying the present value of the Kalman delta-phase component with a future value of the selected candidate delta-phase component.
15. The apparatus of claim 13 wherein processor (a) includes:
- (a1) a means for obtaining a plurality of candidate error-state vectors by performing a second set of more than one Kalman filter processes, a candidate error-state vector being produced as an output of each second-set Kalman filter process utilizing a plurality of prior pseudorange values and the present position vector;
- (a2) a means for selecting the error-state vector from the candidate error-state vectors by applying a predetermined error-state vector selection rule;
- (a3) a means for determining the position vector using IMa outputs and the error-state vector.
16. The apparatus of claim 15 wherein each Kalman filter process in the second set is tuned to recognize either (1) the absence of any failure conditions or (2) the presence of a particular failure condition, a failure condition being the failure of a group of one or more functional elements of the inertial-GPS navigation system to perform within specifications.
17. The apparatus of claim 15 wherein the selected error-state vector is the error-state vector produced by the Kalman filter process producing a value of a failure-condition quantity that is statistically distinguishable from the values of the failure-condition quantity produced by the other second-set Kalman filter processes.
18. The apparatus of claim 17 whereby the failure-condition quantity is derived from weighted residuals summed over a plurality of Kalman filter process update cycles.
19. The apparatus of claim 13 wherein processor (b) includes:
- (b1) a means for estimating future values of IMU acceleration and angular velocity from the IMU outputs;
- (b2) a means for translating the future values of IMU acceleration and angular velocity into the IMU delta-phase component utilizing the direction-cosine matrix that translates the body coordinates of the IMU into the navigation coordinates of the inertial-GPS navigation system.
20. The apparatus of claim 19 wherein processor (b) includes:
- (b2-1) a means for obtaining a plurality of candidate error-state vectors by performing a second set of more than one Kalman filter processes, a candidate error-state vector being produced as an output of each second-set Kalman filter process utilizing a plurality of prior pseudorange values and the present position vector;
- (b2-2) a means for selecting the error-state vector from the candidate error-state vectors by applying a predetermined error-state vector selection rule;
- (b2-3) a means for determining the direction-cosine matrix using IMU outputs and the error-state vector.
21. The apparatus of claim 20 wherein each Kalman filter process in the second set is tuned to recognize either (1) the absence of any failure conditions or (2) the presence of a particular failure condition, a failure condition being the failure of a group of one or more functional elements of the inertial-GPS navigation system to perform within specifications.
22. The apparatus of claim 20 wherein the selected error-state vector is the error-state vector produced by the Kalman filter process producing a value of a failure-condition quantity that is statistically distinguishable from the values of the failure-condition quantity produced by the other second-set Kalman filter processes.
23. The apparatus of claim 22 wherein the failure-condition quantity is derived from weighted residuals summed over a plurality of Kalman filter process update cycles.
Type: Application
Filed: Mar 23, 2001
Publication Date: Aug 15, 2002
Inventor: John W. Diesel (Woodland Hills, CA)
Application Number: 09816798
International Classification: G01S005/14;
