Apparatus For An Automated Aerial Refueling Boom Using Multiple Types Of Sensors

Abstract

A system for automated control of a refueling boom coupled to a tanker aircraft is provided. The system includes: a first inertial measurement unit (IMU) providing inertial measurements for the tanker aircraft; a first GPS receiver providing a GPS location for a GPS antenna attached to the tanker aircraft; and a processor adapted to calculate a first inertial navigation state for the tanker aircraft through integration of the inertial measurements, the processor being further adapted to calculate a first inertial navigation state error relative to the GPS location and to filter the first inertial navigation state error and the first inertial navigation state based upon noise characteristics of the first IMU and the first GPS receiver to provide an updated inertial navigation state for the tanker aircraft, the processor being further adapted to control the refueling boom relative to a receiver aircraft based upon the first and updated inertial navigation states.

Description

TECHNICAL FIELD

The present invention relates generally to the control of aerial refueling booms, and more particularly to automated control of aerial refueling booms using multiple sensors.

BACKGROUND

Aerial tankers use refueling booms to fuel other aircraft in mid-flight. In this fashion, the refueled (receiver) aircraft can fly extended missions during military or civilian operations. Generally, the aerial tanker and the receiver aircraft must be flown in close proximity to accomplish a refueling. Given this proximity, a manual operator in the aerial tanker may extend its refueling boom to mate with the fuel tank of the receiver aircraft.

In general, the refueling boom operation can tax the skill of even experienced boom operators. The possibility of midair accidents is ever present. In addition, the refueling boom may improperly mate with the receiver aircraft. For example, a stealth aircraft may have its low observable coating contacted such that it becomes observable to enemy radar. In such a case, the stealth aircraft may have to abort its mission and return for repair.

During the refueling operation, the boom operator must manually direct the refueling boom to mate with the receiver aircraft. This manual direction must be accomplished in varied lighting conditions from extremely bright sun light to complete darkness or minimum visibility weather conditions. Moreover, the manual direction may be further complicated by turbulence. Because of this manual operation, refueling operations are lengthy as compared to machine-guided refueling. In addition, the need for human boom operators makes unmanned aerial tankers unfeasible. Finally, manual operation is inevitably prone to error and mishap.

Conventional machine-guided boom controls typically use an electro-optic sensor to estimate the boom and receiver aircraft position. By removing the need for human boom operators, unmanned aerial tankers can be developed, which can lead to significant cost savings. For example, it has been estimated that manned aircraft use 80% of their service life to train aircrews. Conventional electro-optic boom automated boom control thus advantageously lessens the need for human operators. However, although electro-optic sensors can be quite accurate, they are range limited.

Accordingly, there is a need in the art for more robust machine-guided boom control systems.

SUMMARY

Apparatuses, systems, and methods are disclosed herein which provide automated refueling boom control in a cost-effective manner for a multitude of commercial and military applications. Specifically, apparatuses, devices, and methods are disclosed herein that relate to the integration of data from multiple sensors to provide more robust and accurate automated control of refueling booms.

In accordance with an embodiment, a system for automated control of a refueling boom coupled to a tanker aircraft is provided. The system includes: a first inertial measurement unit (IMU) providing inertial measurements for the tanker aircraft; a first GPS receiver providing a GPS location for a GPS antenna attached to the tanker aircraft; and a processor adapted to calculate a first inertial navigation state for the tanker aircraft through integration of the inertial measurements, the processor being further adapted to calculate a first inertial navigation state error relative to the GPS location and to filter the first inertial navigation state error and the first inertial navigation state based upon noise characteristics of the first IMU and the first GPS receiver to provide an updated inertial navigation state for the tanker aircraft, the processor being further adapted to control the refueling boom relative to a receiver aircraft based upon the first and updated inertial navigation states.

In accordance with another embodiment, a system for control of a refueling boom coupled to a tanker aircraft is provided. The system includes: an IMU providing tanker inertial measurements for the tanker aircraft; an IMU providing receiver inertial measurement for a receiver aircraft; and means for controlling the refueling boom so as to automatically mate with the receiver aircraft, wherein the means controls refueling boom by filtering the tanker and receiver inertial measurements relative to measurements from a sensor selected from the group consisting of a GPS receiver and an EO sensor.

In accordance with another embodiment, a method of controlling a refueling boom relative to a tanker aircraft and a receiver aircraft is provided. The method includes the acts of: deriving an inertial navigation state for the tanker aircraft; deriving an inertial navigation state for the receiver aircraft; comparing the inertial navigation states to additional sensor measurements to derive an inertial navigation state error for the tanker aircraft and an inertial navigation state error for the receiver aircraft; filtering the inertial navigation states and the inertial navigation state errors to provide an updated inertial navigation state for the receiver aircraft and an updated inertial navigation state for the tanker aircraft; and controlling the refueling boom based upon the updated inertial navigations states so as to automatically mate with the receiver aircraft.

The scope of the present invention is defined by the claims, which are incorporated into this section by reference. A more complete understanding of embodiments of the present invention will be afforded to those skilled in the art, as well as a realization of additional advantages thereof, by a consideration of the following detailed description. Reference will be made to the appended sheets of drawings that will first be described briefly.

BRIEF DESCRIPTION OF THE FIGURES

FIG. 1 shows tanker components for an automated refueling boom control system in accordance with an embodiment of the present invention.

FIG. 2 shows an receiver aircraft components for an automated refueling boom control system according to an embodiment of the present invention.

FIG. 3 illustrates a fault detection and reconfiguration process for the automated refueling boom control system of FIGS. 1 and 2.

Embodiments of the present invention and their advantages are best understood by referring to the detailed description that follows. It should be appreciated that like reference numerals are used to identify like elements illustrated in one or more of the figures.

DETAILED DESCRIPTION

Reference will now be made in detail to one or more embodiments of the invention. While the invention will be described with respect to these embodiments, it should be understood that the invention is not limited to any particular embodiment. On the contrary, the invention includes alternatives, modifications, and equivalents as may come within the spirit and scope of the appended claims. Furthermore, in the following description, numerous specific details are set forth to provide a thorough understanding of the invention. The invention may be practiced without some or all of these specific details. In other instances, well-known structures and principles of operation have not been described in detail to avoid obscuring the invention.

To provide a robust automated control system for aerial refueling, data from multiple sensors are integrated in a process that may be denoted as “sensor fusion” to accurately position a refueling boom with regard to both the aerial tanker and the receiver aircraft. In one embodiment, this integration uses inertial state information from inertial measurement units (IMUs) located on the aerial tanker and the receiver aircraft to propagate inertial navigation states for each aircraft. Each inertial navigation state has its own independent error dynamics. However, a combined state space may be formed which depends upon the error dynamics from each IMU. This combined state space provides a convenient format for mixing in other sensor information such as GPS sensor information or electro-optic (EO) sensor information to correct the inertial navigation state using a filter such as an extended Kalman filter. Because both navigational states are thereby corrected, the resulting extended Kalman filter may be referred to as a Global Extended Kalman Filter (GEKF).

In one embodiment, the tanker, the refueling boom, and the receiver aircraft are all processed through the GEKF so as to estimate the relative position velocity, and attitude between the tanker and the receiver aircraft, from the tanker to the boom, and ultimately from the boom tip to the receiver aircraft receptacle. The inertial state consists of the position, velocity, and attitude of each player in an inertial reference frame. It may also be expanded to include linear acceleration and angular rate. Each IMU provides measurements of linear acceleration and angular rate for its location. The tanker, the refueling boom, and the receiver aircraft (each IMU-tracked object may be denoted as a “player” in the following discussion) may have their own IMU measurements. For example, FIG. 1 illustrates tanker-based components of an exemplary automated refueling boom control system. The refueling boom (not illustrated) may be controlled by a boom control computer 100 that receives information from a GEKF 105. Positional information for the receiver aircraft is derived from an IMU 110. As will be explained further herein, velocity and angular acceleration measurements from IMU 110 may be integrated using, for example, strap down equations of motion to provide position and attitude (roll, pitch and yaw) information for the receiver aircraft. GEKF 105 updates or corrects the strap-down-equations-of-motion-derived positional information based upon measurements from other sensors such as from a GPS receiver 115. This position data will be denoted as “a priori” position data in the following discussion. Receiver 115 provides GPS-derived positional measurements based upon GPS signals received at a GPS antenna 120. For GEKF 105 to correct the a priori position data with the GPS-derived measurements, the position of the antenna 120 may be derived from the a priori position data through appropriate translation based upon a body frame lever arm 125 between IMU 110 and GPS antenna 120. GEKF 105 receives position data from other players such as the receiver aircraft (discussed with regard to FIG. 2) through an antenna 140 and a corresponding data link 145. The a priori position data for these other players with regard to IMU 110 may be corrected through GEKF 105 using measurements from electro-optic (EO) sensors such as a camera 130. To do so, the a priori position data is translated based upon a body frame lever arm 135 between IMU 110 and camera 130.

Turning now to FIG. 2, complementary components may be located on a receiver aircraft. A receiver aircraft IMU 200 provides measurements to a GEKF 205 so that a priori position data may be derived as discussed with regard to FIG. 1. This a priori position data may be updated based upon GPS measurements from a GPS receiver 210 having a GPS antenna 215. The a priori position data for the receiver aircraft is translated to the GPS antenna location using a body frame lever arm 220 between IMU 200 and GPS antenna 215. GEKF 205 also receives position data from other players by receiving signals through a data link 230 as received by an antenna 225. Referring back to FIG. 1, camera 130 has a field of view 150 that allows the imaging of reference points such as reference points 1 through N on the receiver aircraft. In addition, camera 130 will image similar reference points on the refueling boom (not illustrated). Each of the receiver aircraft reference points has a known body frame lever arm between the receiver aircraft IMU 220 and the corresponding reference point. Thus, the receiver aircraft a priori position data may be translated using the corresponding lever arm to each of the reference points so that GEKF 205 may update its position as will be explained further herein.

It may thus be seen that to estimate the inertial states of and the relative states between each player, some combination of measurements from Global Positioning System (GPS) receivers, Inertial Measurement Units (IMU's), wireless or wired communication, and Electro-Optic (EO) sensors are blended in a process denoted herein as “sensor fusion.” This process may be classified into two stages or phases. A first stage is referred to as the propagation phase. In this phase, the inertial states for each player are formed. The following discussion will assume that an inertial navigation state will be determined with regard to IMU measurements. However, it will be appreciated that the sensor fusion techniques discussed herein may be applied in systems without IMUs. For each player that has an IMU, the linear accelerations and angular rate measurements if available are utilized to aid in the propagation of the inertial navigation state for that vehicle. The acceleration and angular rate measurements are integrated in time using the strap down equations of motion to obtain the a priori position data, which may also be denoted as the inertial navigation states for each player. A second phase consists of utilizing other sensor measurements to adjust the inertial navigation states of each player (as determined in the first phase). For example, GPS measurements may be utilized to correct the absolute position estimates of each state. Differential GPS measurements provide precise corrections of the relative position estimates. EO measurements provide precise correction to the relative position and attitude estimates. Finally, if IMU data is available, errors in the IMU such as bias errors, scale factor errors, and misalignment errors may be calibrated and removed in order to improve the propagation phase of the estimation process. The navigation states thus created by the multi sensor system may then be provided to a conventional Boom Control Unit (BCU) which in turn commands the boom actuators to move the boom toward the target, and into contact. Given this overview, the first phase of sensor fusion will be discussed as follows.

Propagation Phase for the Inertial States

Each player in the methodology, whether boom, tanker, or receiver, is represented with an inertial navigation state. This state consists of the position, velocity, and attitude relative to an inertial reference frame. In one embodiment, the inertial reference frame consists of a set of axes fixed in inertial space. For example, an Earth Centered Earth Fixed (ECEF) coordinate frame and consists of an orthogonal set of coordinate axes emanating from the center of the earth. The positive z-axis points through the North Pole, the positive x-axis points through the equator at the Greewich Meridian, and the y-axis completes the right-hand coordinate frame. The ECEF moves relative to the inertial frame at a constant rotation rate (defined by the Earth's rotation). The state may be expanded to include the angular rate and acceleration relative to this frame.

Given this definition of ECEF coordinates, a player's inertial state x may be represented as

$\begin{array}{cc}x=\left[\begin{array}{c}\begin{array}{c}{P}^E\\ V^E\end{array}\\ Q_B^E\end{array}\right]& \left(\mathrm{Eq}.1\right)\end{array}$

In this embodiment, the state x consists of a position P^E in the ECEF frame, a velocity V^E in the ECEF frame, and a quaternion Q_B^E defining a body frame attitude of the vehicle relative to the ECEF frame. In such embodiment, the body frame is defined separately for each player. For example, the body frame may be consistent with typical “aircraft coordinates” such that it consists of an orthogonal coordinate axis frame with a positive x axis through the nose of the aircraft, a positive y-axis through the starboard side of the aircraft and a z axis positive down.

Note that IMU measurements consist of the measurement of acceleration and angular rates in the body frame of the IMU which may or may not be consistent with the vehicle body frame. However the IMU is rigidly mounted to the vehicle and a constant rotation may be employed to relate the IMU to the vehicle frame.

Referring back to FIGS. 1 and 2, IMUs 200 and 110 provide acceleration and angular rates. These measurements may be processed to obtain a derivative of the inertial navigation state x (Eq. 1), designated as x. This derivative is integrated to obtain the propagated state using, for example, the strap down equations of motion:

$\begin{array}{cc}\begin{array}{c}\stackrel{.}{x}=\left[\begin{array}{c}\begin{array}{c}{\stackrel{.}{P}}^E\\ {\stackrel{.}{V}}^E\end{array}\\ {\stackrel{.}{Q}}_B^E\end{array}\right]\\ =\left[\begin{array}{c}\begin{array}{c}{V}^E\\ C_B^Ea^B-{\omega }_{\mathrm{IE}}^E\times \left({\omega }_{\mathrm{IE}}^E\times P^E\right)-2{\omega }_{\mathrm{IE}}^E\times V^E+g^E\end{array}\\ \frac{1}{2}\left[{\Omega }_{\mathrm{EB}}^B\times \right]Q_B^E\end{array}\right]\end{array}& \left(\mathrm{Eq}.2\right)\end{array}$

where a matrix C_B^E is the rotation matrix calculated from the quaternion Q_B^E, a specific force vector a^B represents the linear acceleration exhibited on the vehicle in the vehicle body frame, a gravity vector g^E is represented in the ECEF frame, an earth rotation rate ω_IE^E represents the rotation of the ECEF frame relative to the inertial reference frame, a vector cross product between two vectors a and b is represented as a×b, and a quaternion rate matrix [Ω_EB^B×] is defined as:

$\begin{array}{cc}\left[{\Omega }_{\mathrm{EB}}^B\times \right]=\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]{\omega }_{\mathrm{EB}}^B=\left[\begin{array}{c}\begin{array}{c}{\omega }_x\\ {\omega }_y\end{array}\\ {\omega }_z\end{array}\right]& \left(\mathrm{Eq}.3\right)\end{array}$

where the angular velocity term ω_EB^B represents the angular velocity of the vehicle body frame relative to the ECEF frame represented in the vehicle body frame.

The measurements from each IMU are the linear acceleration in the body frame a^B and the angular velocity of the body frame relative to the inertial frame, ω_IB^B. The following definition holds for the angular velocities:

ω_IB^B=ω_IE^E+ω_EB^B  (Eq. 4)

Using these definitions, it is possible to integrate the strap down equations of motion using the IMU measurements, if available. If not, the angular velocity and acceleration can be estimated using the corrections from the GEKF defined herein.

Error in the Propagated State

The strap down equations of motion (Eq. 2) are susceptible to errors in the acceleration or angular rate data. The errors grow as a function of time. This growth is predictable using a linearization. The linearization estimates the error growth over short periods of time. An error growth may be defined as:

$\begin{array}{cc}\left[\begin{array}{c}\begin{array}{c}\delta {\stackrel{.}{P}}^E\\ \delta {\stackrel{.}{V}}^E\end{array}\\ \delta {\stackrel{.}{q}}_B^{\stackrel{\_}{B}}\end{array}\right]=\left[\begin{array}{ccc}0& I& 0\\ G-{\left[{\omega }_{\mathrm{IE}}^E\times \right]}^2& -2\left[{\omega }_{\mathrm{IE}}^E\times \right]& -2C_B^Ea^B\\ 0& 0& -\left[{\omega }_{\mathrm{IB}}^B\times \right]\end{array}\right]\left[\begin{array}{c}\begin{array}{c}\delta P^E\\ \delta V^E\end{array}\\ \delta q_B^{\stackrel{\_}{B}}\end{array}\right]+\hspace{1em}\left[\begin{array}{c}\begin{array}{c}0\\ C_B^Ew_a\end{array}\\ w_g\end{array}\right]& \left(\mathrm{Eq}.5\right)\end{array}$

where a gravity gradient G is a function of ECEF position defined by an Earth gravity model such as the J2 gravity model, a linearized quaternion error q_B^Brepresents the linearized error in the full quaternion Q_B^E, a noise term w_a represents the error in the accelerometer measurements, a noise term w_g represents the error in the angular rate measurements, and a quaternion error is defined as:

C _B ^E =C _B ^E(I+2[δq_B^B×])  (Eq. 6)

In this case C_B^E is the a priori estimate of attitude calculated from an a prior estimate Q_B^E of Q_B^E. Eq. 6 also implies a method of correcting the a priori rotation once the error is calculated using filtering methods derived below.

Note that Eq. (5) can be expanded to include other error sources. For instance, accelerometer bias, scale factor and misalignment may be included as additional states. In addition, the same errors may be included for the gyroscope measurements (if used). Finally, GPS clock errors may be included. Adding other error sources further refines the error model, thus improving knowledge of how errors in the navigation state grow in time.

These dynamics are used in the Global Extended Kalman Filter (GEKF) as discussed further herein. Note that each player will have an inertial state x as derived from integration of Eq. (2). Each state is propagated using the available estimates of acceleration and angular velocity either from an IMU or from estimation using GPS/EO. The propagation of each state occurs independently of any other vehicle. The error growth of the state estimate is defined in Eq. (5). These dynamics may be simplified to the form:

δ{dot over (x)}=Aδx+Bw  (Eq. 7)

Matrix A and vector B may be derived through a comparison of Eqs. (5) and (7). Note that noise terms (summarized as vector w) for the IMU measurements are known through, for example, a manufacturer-provided analysis of each IMU. Thus, the unknown is the error term δx in the a priori position data/inertial navigation state x. Each player (such as the tanker of FIG. 1 and the receiver aircraft of FIG. 2) has its own independent error term. However, a combined state space may be formed as:

$\begin{array}{cc}\left[\begin{array}{c}\delta {\stackrel{.}{x}}_1\\ \delta {\stackrel{.}{x}}_2\end{array}\right]=\left[\begin{array}{cc}{A}_1& 0\\ 0& A_2\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]+\left[\begin{array}{cc}{B}_1& 0\\ 0& B_2\end{array}\right]\left[\begin{array}{c}{w}_1\\ w_2\end{array}\right]& \left(\mathrm{Eq}.8\right)\end{array}$

where δx₁ is the error in the inertial navigation state of a first vehicle and δx₂ is the error in the state of a second vehicle. A simple rotation can be used to redefine the error as:

$\begin{array}{cc}\left[\begin{array}{c}\delta {\stackrel{.}{x}}_1\\ \delta \Delta \stackrel{.}{x}\end{array}\right]=\left[\begin{array}{cc}{A}_1& 0\\ A_1-A_2& A_2\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}\right]+\left[\begin{array}{cc}{B}_1& 0\\ B_1& -B_2\end{array}\right]\left[\begin{array}{c}{w}_1\\ \Delta w\end{array}\right]& \left(\mathrm{Eq}.9\right)\end{array}$

where δΔx=δx₁−δx₂. This dynamic state is now in terms of the absolute navigation error of the first vehicle and the relative navigation error between the first vehicle and the second vehicle. The error shows how two inertial navigation states drift relative to each other. This relative state is a convenient format for mixing measurements from, for example, EO sensors and GPS sensors for correction of the inertial navigation states. Each of these sensors may be provide a sensor position estimate that may be compared to a IMU-derived position estimate for the sensor (through appropriate translation) of the IMU inertial navigation state x. These other sensors also have a known noise behavior as discussed with regard to the IMUs. A filter such as an Extended Kalman filter may then process the sensor position estimate, the IMU-derived sensor position estimate, and the noise behaviors for the IMU and the sensor to determine the error term δx of Eq. (7). The inertial navigation state x may then be corrected. In this fashion, although the sensor and IMU measurements are noisy, the inertial navigation state is estimated having an error that is optimal in a least-means-squared sense. Assuming that the combined error space from Eq. (9) is used, a resulting Extended Kalman filter may be referred to as a Global Extended Kalman Filter (GEKF) as shown in FIGS. 1 and 2 because of the error terms for both players are determined simultaneously.

Note the advantages of such an automated boom control system. Because IMU measurements are fused with other sensor measurements in a statistically optimal fashion, very accurate automatic control of the refueling boom is made possible for the first time. In this fashion, unmanned aerial refueling tankers are enabled, providing significant cost savings for both commercial and military applications. The solution is quite flexible in that multiple types of sensor measurements may be fused together. Not only is a statistically optimal control solution provided, but fault detection methods are also disclosed. Should faults be detected in, for example, acceleration sensors in the IMU or GPS satellite transmissions, an appropriate reconfiguration of the automated refueling boom control system is disclosed. Sensor fusion with regard to GPS measurements will be addressed first in the following discussion.

Correction of the Propagated Inertial States Using GPS Measurements

GPS measurements include measurements of a position and a velocity for a GPS receiver antenna. These measurements may be used to correct an inertial state provided that the lever arm between the GPS antenna and the location of the inertial state reference point is known. For instance, the strap down equations of motion (Eq. 1.1.2) are typically integrated at the location of the IMU. Transfer of the IMU position estimates to the GPS antenna location is performed as follows:

P _GPS ^E =P _IMU ^E +C _B ^E L _12G  (Eq. 10)

where P_GPS^E is the position of the GPS antenna location in the ECEF coordinate frame, P_IMU^E is the position of the IMU in the ECEF coordinate frame matrix, and L_12G represents the vector distance from the IMU to the GPS antenna in the vehicle body frame. Velocity estimates may be transferred using similar methods:

V _GPS ^E =V _IMU ^E +C _B ^E(ω_IB^B×L_12G)−ω_IE^E×C_B^EL_12G  (Eq. 11)

where V_GPS^E is the velocity for the GPS antenna in the ECEF coordinate frame, and V_IMU^E is the velocity for the IMU in the ECEF coordinate frame. Referring back to FIG. 1, an IMU-derived position for IMU 110 may thus be translated to provide an IMU-derived position estimate for GPS antenna 120 based upon lever arm 125. For a given GPS antenna and an IMU inertial state, the GPS position measurements may be modeled as a function of the inertial navigation state error using the perturbation methods defined previously with regard to Eq. (7). In that regard, GPS receiver 115 provides a GPS-derived position estimate for GPS antenna 120 that may be denoted as {tilde over (P)}_GPS^E. The inertial-navigation-state-derived position (which is typically an IMU-derived position) may be denoted as P_GPS^E. Using the inertial navigation state error term error term δx, {tilde over (P)}_GPS^E may thus be defined using the following measurement function:

{tilde over (P)} _GPS ^E = P _GPS ^E +C _P δx+ν  (Eq. 12)

where ν is measurement noise, and a sensitivity matrix C_P is defined as:

C _P =[I0−2C_B^E[L_12G×]]  (Eq. 13)

The resulting cross product matrix in Eq. (13) operates against the attitude error in the inertial state error vector.

Note that more precise models may be incorporated. For instance, if pseudorange measurements are utilized, then the state error dynamics must be augmented to include a clock error model and the position measurements are augmented to include a clock bias. This example is included for brevity and clarity and is in no way limiting. Further, a similar matrix may be constructed for the use of velocity measurements or Doppler measurements from the GPS. If Doppler measurements are included, then the GPS clock bias error must be expanded to include a clock rate term and possibly a clock acceleration term.

Using the simple error model defined in Eq. (7) and combined with the dynamics defined in Eq. (12), a Global Extended Kalman Filter (GEKF) may be utilized to estimate the state error δx as discussed further below. The state error may then be used to correct the a priori state estimate and improve the performance of the navigation solution. Note that GPS measurements are often available at a much lower rate than the IMU data. The IMU data is typically used to drive the strap down equations of motion. The error covariance of the GEKF is propagated at the same rate (or slower if computational limitations exist) as the IMU. When GPS measurements are available, the GPS is used to correct the IMU state. The state is then propagated forward in time using the IMU measurements until the next GPS measurement is available.

Correction of the Propagated Inertial States Using Differential GPS Measurements

Differential GPS involves the difference between two sets of GPS measurement from different GPS receiver in relatively close proximity. Because of the resulting elimination of common mode errors, differential GPS is typically more accurate than conventional GPS. However, a disadvantage is that differential GPS measurements lose their absolute position information. Therefore a combination of absolute and relative GPS measurements may be utilized.

Given two sets of GPS measurements, for example, from GPS receivers 115 and 210, Eq. (12) takes on the following form:

$\begin{array}{cc}\left[\begin{array}{c}{\tilde{P}}_{\mathrm{GPS}1}^E\\ {\tilde{P}}_{\mathrm{GPS}2}^E\end{array}\right]=\left[\begin{array}{c}{\stackrel{\_}{P}}_{\mathrm{GPS}1}^E\\ {\stackrel{\_}{P}}_{\mathrm{GPS}2}^E\end{array}\right]+\left[\begin{array}{cc}{C}_{P1}& 0\\ 0& C_{P2}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]+\left[\begin{array}{c}{b}_c\\ b_c\end{array}\right]+\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right]& \left(\mathrm{Eq}.14\right)\end{array}$

Each of the measurements is independent of the other. However, they are both corrupted by the same common mode errors b_c. Using a rotation similar to the one employed Eq. (9), the relative state errors may modified to the following form:

$\begin{array}{cc}\left[\begin{array}{c}{\tilde{P}}_{\mathrm{GPS}1}^E\\ \Delta {\tilde{P}}_{\mathrm{GPS}}^E\end{array}\right]=\hspace{1em}\left[\begin{array}{c}{\stackrel{\_}{P}}_{\mathrm{GPS}1}^E\\ \Delta {\stackrel{\_}{P}}_{\mathrm{GPS}}^E\end{array}\right]+\left[\begin{array}{cc}{C}_{P1}& 0\\ C_{P1}-C_{P2}& C_{P2}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}\right]+\left[\begin{array}{c}{b}_c\\ 0\end{array}\right]+\left[\begin{array}{c}{v}_1\\ \Delta v\end{array}\right]& \left(\mathrm{Eq}.15\right)\end{array}$

where a differential measurement is defined as Δ{tilde over (P)}_GPS^E={tilde over (P)}_GPS1^E−{tilde over (P)}_GPS2^E. The rotation removes the effect of the common mode errors in the relative measurements. This new measurement function combined with the dynamics of Eq. (9) can be used to define what is henceforward referred to as the Global Extended Kalman Filter (GEKF). The filter is referred to as a global filter since it incorporates the absolute and relative state of both vehicles and is corrected with the absolute and differential GPS measurements using the model in Eq. (15).

It will be appreciated that numerous modifications may be performed. For instance, raw pseudorange measurements from the GPS receiver may be used instead of position estimates. In this case, the state errors must be augmented with a GPS clock bias (as discussed before) but also with a relative clock bias estimate. If velocity or Doppler measurements are employed, then a state must be added for the clock drift and modeled appropriately. In addition, it is possible to process some or all of the available GPS measurements as they arrive. For instance, it is possible to process the absolute measurements in the GEKF without the differential measurements until such time as the differential GPS measurements from both vehicles arrive.

Correction of the Propagated Inertial States Using GPS Carrier Phase Measurements

A very special subclass of measurements exists for GPS receivers equipped to provide carrier phase measurements because such measurements may be accurate to a few centimeters.

However, to achieve this accuracy, an integer ambiguity problem must be solved. Once solved, the differential carrier phase measurements may be integrated into the GEKF and processed accordingly. The integer ambiguity problem arises because an unknown integer number of wavelengths exists between each vehicle. This unknown integer is different for each satellite that both receivers track and must be estimated on a per-satellite basis. The integer ambiguity problem is typically solved from GPS antenna to GPS antenna on each vehicle. For a carrier phase of a given wavelength λ, the range to a GPS satellite i from a vehicle (designated as vehicle 1) is defined as:

λ({tilde over (φ)}₁ⁱ+N₁ⁱ)= ρ₁ⁱ+δP₁^E+τ+b_c+ν₁  (Eq. 16)

where the carrier phase measurement at receiver 1 for satellite i is defined as {tilde over (φ)}₁ⁱ, the integer number of wavelengths between the receiver and satellite is N₁ⁱ, and an a priori estimate of the range to the satellite is ρ₁ⁱ. This range is calculated from the known satellite locations and the a priori estimate of the GPS antenna either computed from the inertial state or from the GPS position estimates or the pseudorange measurements. The carrier phase measurement is corrupted by errors in the estimate of the GPS antenna position δP₁^E that influences the error through a line of sight matrix C_LOSⁱ This matrix is defined as the vector line of sight from the GPS antenna on vehicle 1 to satellite i. In addition, a GPS receiver clock bias r corrupts the measurements along with the receiver noise and common mode errors.

In the previous discussion, a single difference was defined as the difference between two GPS measurements from two different vehicles. Now a double difference is defined for the purpose of eliminating the common mode errors and the clock bias. The double difference is defined as:

∇Δ{tilde over (φ)}={tilde over (φ)}₁ⁱ−{tilde over (φ)}₁^j−({tilde over (φ)}₂ⁱ−{tilde over (φ)}₂^j)  (Eq. 17)

In this case, satellite i and satellite j are different satellites currently in view by both receivers 1 and 2. The resulting measurement error is defined as:

λ(∇Δ{tilde over (φ)}+∇ΔN)=∇Δ ρ+(C_LOSⁱ−C_LOS^j)ΔδP^E+∇Δν_φ  (Eq. 18)

The definition of the double difference is applied to each term in Eq. (16). The common mode and receiver clock errors are now eliminated. A similar error model may be applied to create double differenced pseudorange GPS measurements.

∇Δ{tilde over (ρ)}=∇Δ ρ+(C_LOSⁱ−C_LOS^j)ΔδP^E+∇Δν_ρ  (Eq. 19)

Using the combination of these two measurements, it is possible to estimate the double differenced integer ambiguity and then utilize the double differenced carrier phase measurements in the GEKF. Several methods are available for performing this task. One way is to perform code minus carrier smoothing. Essentially, the difference between the pseudorange and carrier phase measurements is utilized to estimate the integer bias through averaging over time:

$\begin{array}{cc}\nabla \Delta N=\frac{1}{\lambda }\nabla \Delta \tilde{\rho }-\nabla \Delta \tilde{\phi }& \left(\mathrm{Eq}.20\right)\end{array}$

Since ∇ΔN is assumed constant, Eq. (20) is merely averaged over time until the covariance is sufficiently small.

A more complex and precise method is to utilize the Wald Test for the integer ambiguity problem. The Wald Test is a known statistical hypothesis testing scheme. In this case, a set of possible integer ambiguities is hypothesized and tested against the available GPS measurements. Using a combination of all of the available code and carrier phase measurements available to both GPS receivers, a set of residuals are constructed and used to estimate the probability that a particular hypothesis is correct. The residual formed as:

$\begin{array}{cc}{r}_k=\left[\begin{array}{c}\lambda \left(\nabla \Delta \tilde{\phi }+\nabla \Delta N_k\right)-\nabla \Delta \tilde{\rho }\\ E\lambda \left(\nabla \Delta \tilde{\phi }+\nabla \Delta N_k\right)\end{array}\right]& \left(\mathrm{Eq}.21\right)\end{array}$

In this equation, the residual r_k is generated using a hypothesized integer ambiguity ∇ΔN_k where k is the index number of the particular hypothesis. The measurements and residuals are now assumed to incorporate all of the satellites measurements available to both GPS receivers. The number of hypotheses can be quite large since a range of integers must be hypothesized for each satellite. A special function E is a matrix constructed as the left annihilator of the measurement sensitivity function. E is defined such that

E(∇C_LOS)=0  (Eq. 22)

where ∇C_LOS is the collection of the line of sight vector differences. The residual r_k has an assumed Gaussian probability density function, although the Wald Test allows for other densities to be utilized. If the integer hypothesis k is correct, then the residual will have a probability density function. Given a probability that the hypothesis is correct F_k, and assumed density function for the residual ƒ_k, the Wald Test calculates the probability that the hypothesis is correct using the residuals in recursive manner.

$\begin{array}{cc}{F}_k\left(t+\Delta t\right)=\frac{F_k\left(t\right)f_k\left(t+\Delta t\right)}{\sum _{k=0}^LF_k\left(t\right)f_k\left(t+\Delta t\right)}& \left(\mathrm{Eq}.23\right)\end{array}$

If the probability of one hypothesis F_k(t+Δt) reaches one, then that hypothesis is declared the correct hypothesis. At this point the double differenced carrier phase measurements can be utilized in the GEKF. The measurement model is updated utilizing the following form:

$\begin{array}{cc}[\begin{array}{c}{\tilde{P}}_{\mathrm{GPS}1}^E\\ \Delta {\tilde{P}}_{\mathrm{GPS}}^E\\ \lambda \left(\nabla \Delta \tilde{\phi }+\nabla \Delta N\right)\end{array}]=[\begin{array}{c}{\stackrel{\_}{P}}_{\mathrm{GPS}1}^E\\ \Delta {\stackrel{\_}{P}}_{\mathrm{GPS}}^E\\ \nabla \Delta \stackrel{\_}{\rho }\end{array}]+[\begin{array}{cc}{C}_{P1}& 0\\ C_{P1}-C_{P2}& C_{P2}\\ 0& \nabla C_{P2}\end{array}][\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}]+\left[\begin{array}{c}{b}_c\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}{v}_1\\ \Delta v\\ \nabla \Delta v_{\phi }\end{array}\right]& \left(\mathrm{Eq}.24\right)\end{array}$

This measurement function can now be used as part of the GEKF to estimate the relative and absolute state errors. It will be appreciated that this method applies for all of the different carrier phase wavelengths: for example, L1, L2, widelane, narrow lane, or any other linear combination. This above measurements may be modified to include double differenced code measurements as previously stated or to utilize again single differenced code or Doppler measurements provided that the state is augmented to incorporate the relative state error.

Sensor Fusion Through the Global Extended Kalman Filter (GEKF)

As discussed previously, Eq (9) provides a convenient format for mixing in measurements from sensors such as GPS sensors or EO sensors. For example, This sensor fusion process may use the measurement function of Eq. (24) so as to fuse in differential GPS measurements. Eq (9) is now repeated for convenience.

$\begin{array}{cc}[\begin{array}{c}\delta {\stackrel{.}{x}}_1\\ \delta \Delta \stackrel{.}{x}\end{array}]=\left[\begin{array}{cc}{A}_1& 0\\ A_1-A_2& A_2\end{array}\right][\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}]+\left[\begin{array}{cc}{B}_1& 0\\ B_1& -B_2\end{array}\right]\left[\begin{array}{c}{w}_1\\ \Delta w\end{array}\right]& \left(\mathrm{Eq}.25\right)\end{array}$

The dynamics of Equation (25) are driven by noise processes w₁ and Δw. The GEKF assumes that each of these is a Gaussian process with the following statistics:

$\begin{array}{cc}\begin{array}{c}E\left[\left[\begin{array}{c}{w}_1\\ \Delta w\end{array}\right]\right]=0;\\ E[\left[\begin{array}{c}{w}_1\\ \Delta w\end{array}\right]\left[\begin{array}{cc}{w}_1& \Delta w]\end{array}\right]=\left[\begin{array}{cc}{W}_1& W_1\\ W_1& W_1+W_2\end{array}\right]=W_{\mathrm{GEKF}}\end{array}& \left(\mathrm{Eq}.26\right)\end{array}$

where the covariance of the process noise for each inertial system is assumed independent of the other and have values denoted by W₁ and W₂. The term E [•] represents the taking of the statistical expectation value. The process noise is determined by the inertial measurement unit quality. The initial statistics for the error states are likewise:

$\begin{array}{cc}\begin{array}{c}E\left[[\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}]\right]=0;\\ E[[\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}]\left[\begin{array}{cc}\delta x_1& \delta \Delta x]\end{array}\right]=\left[\begin{array}{cc}{P}_1& P_1\\ P_1& P_1+P_2\end{array}\right]=P_{\mathrm{GEKF}}\end{array}& \left(\mathrm{Eq}.27\right)\end{array}$

The dynamic matrix for the inertial error dynamics are grouped together into a common form as:

$\begin{array}{cc}{A}_{\mathrm{GEKF}}=\left[\begin{array}{cc}{A}_1& 0\\ A_1-A_2& A_2\end{array}\right]& \left(\mathrm{Eq}.28\right)\end{array}$

The process noise sensitivity is defined as:

$\begin{array}{cc}{B}_{\mathrm{EKF}}=\left[\begin{array}{cc}{B}_1& 0\\ B_1& -B_2\end{array}\right]& \left(\mathrm{Eq}.29\right)\end{array}$

For a given time increment Δt, the state for each vehicle is integrated over time independently using Eq. (2) using a nonlinear integration method of choice such as Euler integration or a Runge-Kutta method. For example, the tanker may be referred to as vehicle 1 and have its state determined through integration of the following:

$\begin{array}{cc}\begin{array}{c}{\stackrel{.}{x}}_1=\left[\begin{array}{c}{\stackrel{.}{P}}_1^E\\ {\stackrel{.}{V}}_1^E\\ {\stackrel{.}{Q}}_{B1}^E\end{array}\right]\\ =\left[\begin{array}{c}{V}_1^E\\ C_{B1}^Ea_1^{B1}-{\omega }_{\mathrm{IE}}^E\times \left({\omega }_{\mathrm{IE}}^E\times P_1^E\right)-2{\omega }_{\mathrm{IE}}^E\times V_1^E+g_1^E\\ \frac{1}{2}\left[{\Omega }_{EB1}^{B1}\times \right]Q_{B1}^E\end{array}\right]\end{array}& \left(\mathrm{Eq}.30\right)\end{array}$

In this case, all terms denoted with a subscript “1” denote those value or states experienced by the tanker. Similarly, the receiver aircraft may be denoted as vehicle 2 and have corresponding subscripts:

$\begin{array}{cc}\begin{array}{c}{\stackrel{.}{x}}_2=\left[\begin{array}{c}{\stackrel{.}{P}}_2^E\\ {\stackrel{.}{V}}_2^E\\ {\stackrel{.}{Q}}_{B2}^E\end{array}\right]\\ =\left[\begin{array}{c}{V}_2^E\\ C_{B2}^Ea_2^{B2}-{\omega }_{\mathrm{IE}}^E\times \left({\omega }_{\mathrm{IE}}^E\times P_2^E\right)-2{\omega }_{\mathrm{IE}}^E\times V_2^E+g_2^E\\ \frac{1}{2}\left[{\Omega }_{EB2}^{B2}\times \right]Q_{B2}^E\end{array}\right]\end{array}& \left(\mathrm{Eq}.31\right)\end{array}$

Each state is propagated forward at a user defined interval or at the interval that the IMU is updated. At each new time interval, the values for α₁^B1 and ω_IB1^B1 are updated using the best estimates or measurements of the acceleration and angular velocity for the first vehicle that are either estimated or measured from the IMU. Likewise, the values for α₂^B2 and ω_IB2^B2 are updated using the best estimates or measurements of the acceleration and angular velocity for the second vehicle. However, the error covariance is propagated in time using the dynamic model described. The propagation is done as:

M _GEKF=Φ_GEKFP_GEKF(Φ_GEKF)^T+Γ_GEKFW_GEKFΓ_GEKF^T  (Eq. 32)

where state transition matrix Φ_GEKF is defined as follows:

Φ_GEKF=e^AGEKFΔt  (Eq. 33)

and where a discrete time process noise propagation matrix is defined as:

$\begin{array}{cc}{\Gamma }_{\mathrm{GEKF}}={\int }_0^{\Delta t}{}^{A_{\mathrm{GEKF}}\tau }B_{\mathrm{EKF}}\left(\tau \right)\tau & \left(\mathrm{Eq}.34\right)\end{array}$

which may be approximated using appropriate means, if necessary.The measurement model described for the GPS measurements is repeated:

$\begin{array}{cc}[\begin{array}{c}{\tilde{P}}_{\mathrm{GPS}1}^E\\ \Delta {\tilde{P}}_{\mathrm{GPS}}^E\\ \lambda \left(\nabla \Delta \tilde{\phi }+\nabla \Delta N\right)\end{array}]=\hspace{1em}[\begin{array}{c}{\stackrel{\_}{P}}_{\mathrm{GPS}1}^E\\ \Delta {\stackrel{\_}{P}}_{\mathrm{GPS}}^E\\ \nabla \Delta \stackrel{\_}{\rho }\end{array}]+[\begin{array}{cc}{C}_{P1}& 0\\ C_{P1}-C_{P2}& C_{P2}\\ 0& \nabla C_{P2}\end{array}][\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}]+[\begin{array}{c}{b}_c\\ 0\\ 0\end{array}]+\left[\begin{array}{c}{v}_1\\ \Delta v\\ \nabla \Delta v_{\phi }\end{array}\right]& \left(\mathrm{Eq}.35\right)\end{array}$

The measurement noise is lumped together and assumed to be Gaussian with the following statistics:

$\begin{array}{cc}E\left[\left[\begin{array}{c}{b}_c+v_1\\ \Delta v\\ \nabla \Delta v_{\phi }\end{array}\right]\right]=0;E\left[\left[\begin{array}{c}{b}_c+v_1\\ \Delta v\\ \nabla \Delta v_{\phi }\end{array}\right]\left[\begin{array}{ccc}{b}_c+v_1& \Delta v& \nabla \Delta v_{\phi }\end{array}\right]\right]=\hspace{1em}\left[\begin{array}{ccc}{V}_{\mathrm{bc}}+V_1& V_1& 0\\ V_1& V_1+V_2& 0\\ 0& 0& V_{\phi }\end{array}\right]=V_{\mathrm{GEKF}}& \left(\mathrm{Eq}.36\right)\end{array}$

The measurement sensitivity matrix is lumped together into a common matrix defined as:

$\begin{array}{cc}{C}_{\mathrm{GEKF}}=[\begin{array}{cc}{C}_{P1}& 0\\ C_{P1}-C_{P2}& C_{P2}\\ 0& \nabla C_{P2}\end{array}]& \left(\mathrm{Eq}.37\right)\end{array}$

Whenever a GPS measurement is available, the inertial states are propagated forward to the time of validity of the GPS measurements using the method outlined in Eq. (25) through Eq. (31). Then the GPS measurement is used to correct the state using the GEKF structure. The covariance is updated using the following covariance update formula:

P _GEKF =M _GEKF −M _GEKF C _GEKF ^T(C_GEKFM_GEKFC_GEKF^T+V_GEKF)⁻¹C_GEKFM_GEKF  (Eq. 38)

A Kalman gain K is then calculated as:

K=P _GEKF C _GEKF ^T V _GEKF ⁻¹  (Eq. 39)

The state correction can be calculated as:

$\begin{array}{cc}[\begin{array}{c}\delta {\hat{x}}_1\\ \delta \Delta \hat{x}\end{array}]=K(\left[\begin{array}{c}{\tilde{P}}_{\mathrm{GPS}1}^E\\ \Delta {\tilde{P}}_{\mathrm{GPS}}^E\\ \lambda \left(\nabla \Delta \tilde{\phi }+\nabla \Delta N\right)\end{array}\right]-\hspace{1em}[\begin{array}{c}{\stackrel{\_}{P}}_{\mathrm{GPS}1}^E\\ \Delta {\stackrel{\_}{P}}_{\mathrm{GPS}}^E\\ \nabla \Delta \stackrel{\_}{\rho }\end{array}])& \left(\mathrm{Eq}.40\right)\end{array}$

No a priori estimate of the error needs to be maintained since the correction is applied to the state estimate after each GPS measurement. The state is propagated forward in time using the nonlinear strap down equations of motion previously defined. The state corrections are defined in terms of the corrections for the navigation state:

$\begin{array}{cc}\left[\begin{array}{c}\delta {\hat{x}}_1\\ \delta \Delta \hat{x}\end{array}\right]=\left[\begin{array}{c}\delta P_1^E\\ \delta V_1^E\\ \delta q_{B1}^{\stackrel{\_}{B}1}\\ \delta P_1^E-\delta P_2^E\\ \delta V_1^E-\delta V_2^E\\ \delta q_{B1}^{\stackrel{\_}{B}1}-\delta q_{B2}^{\stackrel{\_}{B}2}\end{array}\right]& \left(\mathrm{Eq}.41\right)\end{array}$

The first vehicle state is corrected in the following manner to get the updated estimate:

$\begin{array}{cc}\left[\begin{array}{c}{\hat{P}}_1^E\\ {\hat{V}}_1^E\\ {\hat{Q}}_{B1}^E\end{array}\right]=\left[\begin{array}{c}{\stackrel{\_}{P}}_1^E+\delta P_1^E\\ {\stackrel{\_}{V}}_1^E+\delta V_1^E\\ {\stackrel{\_}{Q}}_{\stackrel{\_}{B}1}^E\otimes {\hat{Q}}_{\hat{B}1}^{\stackrel{\_}{B}1}\end{array}\right]& \left(\mathrm{Eq}.42\right)\end{array}$

where a quaternion Q_B1^E′ is the a priori attitude (before the GPS measurement is applied). The term (A{circle around (X)}B) represents the standard rotation between two quaternions A and B. In this case, the a priori quaternion is corrected by applying the corrective rotation {circumflex over (Q)}_{{circumflex over (B)}1}^B1 which is defined using the quaternion correction δq_B1^B1 estimated from the GEKF.

$\begin{array}{cc}{\hat{Q}}_{\hat{B}1}^{\stackrel{\_}{B}1}=\left[\begin{array}{c}1.0-\delta q_{B1}^{\stackrel{\_}{B}1}\\ \delta q_{B1}^{\stackrel{\_}{B}1}\end{array}\right]& \left(\mathrm{Eq}.43\right)\end{array}$

Similarly, the state of the second vehicle is corrected using:

$\begin{array}{cc}\left[\begin{array}{c}{\hat{P}}_2^E\\ {\hat{V}}_2^E\\ {\hat{Q}}_{B2}^E\end{array}\right]=\left[\begin{array}{c}{\stackrel{\_}{P}}_2^E+\delta P_2^E\\ {\stackrel{\_}{V}}_2^E+\delta V_2^E\\ {\stackrel{\_}{Q}}_{\stackrel{\_}{B}2}^E\otimes {\hat{Q}}_{\hat{B}2}^{\stackrel{\_}{B}2}\end{array}\right]\mathrm{where}\text{:}& \left(\mathrm{Eq}.44\right)\\ {\hat{Q}}_{\hat{B}2}^{\stackrel{\_}{B}2}=\left[\begin{array}{c}1.0-\delta q_{B2}^{\stackrel{\_}{B}2}\\ \delta q_{B2}^{\stackrel{\_}{B}2}\end{array}\right]& \left(\mathrm{Eq}.45\right)\end{array}$

Note that the process as shown assumed absolute position, single differenced position, and double differenced carrier phase measurements. If only some of these measurements are available, then the measurement sensitivity matrix C_GEKF and measurement noise V_GEKF are modified to include the proper number of measurements actually available and the processing proceeds as defined. Alternatively, the loosely coupled versions in which the GPS provides absolute position estimates rather than range measurements may be utilized by modifying the measurement function appropriately. Note also that the rate of propagation and correction do not have to be the same. The propagation process may be repeated numerous times between state updates copying M_GEKF into P_GEKF and propagating the states at the same rate.

Sensor Fusion Using EO Measurements

EO measurements form a redundant set of measurements for the relative state estimation error. The EO sensor typically operates on one vehicle and is utilized to image and provide measurements of the other vehicle or player. This EO system may be, for instance, utilized on the tanker to image a receiver aircraft and/or the boom. Typical EO measurements come in one of two forms. In a first form, the EO system provides bearings measurements (elevation and azimuth) from the sensor to a reference point on the imaged player. In a second form, the EO system attempts to perform pattern matching between an image of the player and a set of recorded images. The first form is referred to as reference point mapping. The second form is referred to as silhouette matching because the EO system attempts to match a possibly three-dimensional (3D) model to a two-dimensional image by first projecting the 3D model onto the assumed image plane and forming a two-dimensional silhouette.

To perform sensor fusion between the EO sensor and the inertial navigation state previously defined, the lever arm distance from the inertial state location on the vehicle to the reference point in view must be known. For example, with regard to reference points 1 through N of FIG. 2 for the receiver aircraft, corresponding body frame lever arms between the receiver aircraft's IMU and the reference points should be known. Using either EO technique or combinations thereof, the measurements are related back to the location of the inertial state of the vehicle. Further, the EO sensor itself is often located at a fixed lever arm from the imaging vehicle inertial reference point (for example, lever arm 135 for camera 130 of FIG. 1). The geometries of both vehicles must be known or estimated as part of this procedure in order to relate the image processing to the inertial navigation solutions. The measurement model of the vision system is defined relative to the inertial system on each system. The relative position between an EO sensor such as a camera and a given reference point is defined in terms of the relative position between each IMU in the ECEF coordinate frame as:

ΔP_iC^E=P_i^E−P_C^E=P_IMU2^E+C_B2^EL_2-i^B2−P_IMU1^E−C_B1^EL_1-C^B1  (Eq. 46)

where the position of the camera in the ECEF coordinate frame is P_C^E, the position of the reference point i (such as one of reference points 1 through N of FIG. 2) on the target in the ECEF coordinate frame is P_i^E, and the relative position vector ΔP_iC^E is the difference between the two vectors. The relative position vector may be defined in terms of the secondary vehicle IMU state P_IMU2^E in the ECEF coordinate frame plus the lever arm between the IMU and a reference location L_2-i^B2 that must be rotated from the vehicle body frame into the ECEF frame using a cosine rotation matrix C_B2^E. Likewise, the primary vehicle's IMU position is defined as P_IMU1^E and is located relative to the camera system through the lever arm L_1-C^B1 rotated from the primary vehicle body frame to the ECEF coordinate frame through cosine rotation matrix C_B1^E. Both C_B2^E and C_B1^E are calculated using the attitude of the respective vehicle. This relationship relates the camera system to the associated inertial measurement unit through a reference frame that is common to the camera system, the IMU, and also the GPS system. Finally, a measurement model can be constructed which incorporates the GEKF state estimates and enables the integration of the camera system within the existing GEKF structure. A relative position vector in the camera frame is defined as:

$\begin{array}{cc}\Delta P_{\mathrm{iC}}^C=\left[\begin{array}{c}\Delta X_{\mathrm{iC}}^C\\ \Delta Y_{\mathrm{iC}}^C\\ \Delta Z_{\mathrm{iC}}^C\end{array}\right]=\left[\begin{array}{c}{x}_1^C-x_{2i}^C\\ y_1^C-y_{2i}^C\\ z_1^C-z_{2i}^C\end{array}\right]& \left(\mathrm{Eq}.47\right)\end{array}$

Each of the terms is one component of the relative position vector between the camera lens and the reference point on the receiver related in the camera reference frame.

EO Bearing Measurements

The measurements from an EO system may provide complete position estimates to active beacons or a camera system may simply identify points and provide bearings measurements from the camera to the reference point. Bearings measurements may be made up of two angles defined as:

$\begin{array}{cc}\left[\begin{array}{c}{\tilde{\alpha }}_{\mathrm{iC}}\\ {\tilde{\beta }}_{\mathrm{iC}}\end{array}\right]=\left[\begin{array}{c}{\tan }^{-1}\left(\frac{\Delta Z_{\mathrm{iC}}^C}{\Delta X_{\mathrm{iC}}^C}\right)\\ {\tan }^{-1}\left(\frac{\Delta Y_{\mathrm{iC}}^C}{\Delta X_{\mathrm{iC}}^C}\right)\end{array}\right]+\left[\begin{array}{c}{\upsilon }_{\alpha }\\ {\upsilon }_{\beta }\end{array}\right]& \left(\mathrm{Eq}.47a\right)\end{array}$

where C_E^C α_iC represents the azimuth angle of the reference point i relative to the camera C in the camera frame, and β_iC^C represents the elevation angle of the target. The additive noise terms are zero mean, Gaussian with noise variance associated with the error models described previously including pixel noise and blurring effects. Each of the terms is one component of the relative position vector between the camera lens and the reference point on the receiver related in the camera reference frame.

A Modified Gain Extended Kalman Filter (MGEKF) may be used to translate the elevation and azimuth angles to a Cartesian position error in the camera frame. Advantageously, this filter will have zero mean steady state error. The error function for the measurements (in the EO sensor frame) is now defined. First, the residual process is defined as the difference between the measured and a priori estimates of the angles as:

$\begin{array}{cc}\begin{array}{c}{r}_i=\left[\begin{array}{c}{\alpha }_i-{\stackrel{\_}{\alpha }}_i\\ {\beta }_i-{\stackrel{\_}{\beta }}_i\end{array}\right]\\ =\left[\begin{array}{c}{\tan }^{-1}\left(\frac{y_1^C-y_{2i}^C}{x_1^C-x_{2i}^C}\right)-{\tan }^{-1}\left(\frac{{\stackrel{\_}{y}}_1^C-{\stackrel{\_}{y}}_{2i}^C}{{\stackrel{\_}{x}}_1^C-{\stackrel{\_}{x}}_{2i}^C}\right)\\ {\tan }^{-1}\left(\frac{z_1^C-z_{2i}^C}{x_1^C-x_{2i}^C}\right)-{\tan }^{-1}\left(\frac{{\stackrel{\_}{z}}_1^C-{\stackrel{\_}{z}}_{2i}^C}{{\stackrel{\_}{x}}_1^C-{\stackrel{\_}{x}}_{2i}^C}\right)\end{array}\right]•\left[\begin{array}{c}{\tan }^{-1}\left(\Theta \right)\\ {\tan }^{-1}\left(\Psi \right)\end{array}\right]\end{array}& \left(\mathrm{Eq}.48\right)\end{array}$

where the angles Θ and Ψ are defined using the following relationship:

$\begin{array}{cc}{\tan }^{-1}\left(a\right)-{\tan }^{-1}\left(b\right)={\tan }^{-1}\left(\frac{a-b}{1+\mathrm{ab}}\right)& \left(\mathrm{Eq}.49\right)\end{array}$

Therefore the residual process may be re-written in the following way:

$\begin{array}{cc}\begin{array}{c}{r}_i=\left[\begin{array}{c}{\tilde{\alpha }}_i-{\stackrel{\_}{\alpha }}_i\\ {\tilde{\beta }}_i-{\stackrel{\_}{\beta }}_i\end{array}\right]\\ =\left[\begin{array}{cc}-D_1{\tan }^{-1}\left(\Theta \right)/\Theta & 0\\ 0& -D_2{\tan }^{-1}\left(\Psi \right)/\Psi \end{array}\right]\\ \left[\begin{array}{ccc}\sin \left({\tilde{\alpha }}_i\right)& 0& -\cos \left({\tilde{\alpha }}_i\right)\\ \sin \left(\widetilde{{\beta }_i}\right)& -\cos \left({\tilde{\beta }}_i\right)& 0\end{array}\right]\left[\begin{array}{c}{x}_1^C-x_{2i}^C\\ y_1^C-y_{2i}^C\\ z_1^C-z_{2i}^C\end{array}\right]\end{array}\mathrm{where}\text{:}& \left(\mathrm{Eq}.50\right)\\ D_1=1/\left[\cos \left({\tilde{\alpha }}_i\right)\left({\stackrel{\_}{x}}_1^C-{\stackrel{\_}{x}}_{2i}^C\right)+\sin \left({\tilde{\alpha }}_i\right)\left({\stackrel{\_}{z}}_1^C-{\stackrel{\_}{z}}_{2i}^C\right)\right]D_2=1/\left[\cos \left(\widetilde{{\beta }_i}\right)\left({\stackrel{\_}{x}}_1^C-{\stackrel{\_}{x}}_{2i}^C\right)+\sin \left(\widetilde{{\beta }_i}\right)\left({\stackrel{\_}{y}}_1^C-{\stackrel{\_}{y}}_{2i}^C\right)\right]& \left(\mathrm{Eq}.51\right)\end{array}$

The modified gain for the measurement function may be defined as:

$\begin{array}{cc}{H}_{\mathrm{MGEKFi}}=\left[\begin{array}{cc}-D_1{\tan }^{-1}\left(\Theta \right)/\Theta & 0\\ 0& -D_2{\tan }^{-1}\left(\Psi \right)/\Psi \end{array}\right]& \left(\mathrm{Eq}.52\right)\end{array}$

The line of sight matrix formed directly from the measurements

$\begin{array}{cc}{H}_{\mathrm{Los\_i}}=\left[\begin{array}{ccc}\sin \left({\tilde{\alpha }}_i\right)& 0& -\cos \left({\tilde{\alpha }}_i\right)\\ \sin \left(\widetilde{{\beta }_i}\right)& -\cos \left({\tilde{\beta }}_i\right)& 0\end{array}\right]& \left(\mathrm{Eq}.53\right)\end{array}$

The error in the bearings measurements in the camera frame may now be written as:

$\begin{array}{cc}\left[\begin{array}{c}{\tilde{\alpha }}_i-{\stackrel{\_}{\alpha }}_i\\ {\tilde{\beta }}_i-{\stackrel{\_}{\beta }}_i\end{array}\right]=H_{\mathrm{MGEKFi}}H_{\mathrm{Los\_i}}\left[\begin{array}{c}{x}_1^C-x_{2i}^C\\ y_1^C-y_{2i}^C\\ z_1^C-z_{2i}^C\end{array}\right]& \left(\mathrm{Eq}.54\right)\end{array}$

In this fashion, the measurement model becomes:

$\begin{array}{cc}\left[\begin{array}{c}{\tilde{\alpha }}_{\mathrm{iC}}\\ {\tilde{\beta }}_{\mathrm{iC}}\end{array}\right]-\left[\begin{array}{c}{\stackrel{\_}{\alpha }}_{\mathrm{iC}}\\ {\stackrel{\_}{\beta }}_{\mathrm{iC}}\end{array}\right]=H_{\mathrm{MGEKFi}}H_{\mathrm{Los\_i}}H_{\mathrm{IMUtoC}}\left[\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}\right]+\left[\begin{array}{c}{\upsilon }_{\alpha }\\ {\upsilon }_{\beta }\end{array}\right]& \left(\mathrm{Eq}.55\right)\end{array}$

In the above discussion, the a priori values for the angles are calculated using the a priori state estimates from the GEKF. The matrix H_Los—i has the line-of-sight vector for the ith reference point and the matrix H_MGEKF is generated as part of the MGEKF and is designed such that the residual calculated is zero mean in the ECEF Cartesian coordinate frame. Finally, the matrix H_IMUtoC is a matrix which translates the inertial error states of the Global Extended Kalman Filter to the position error in the camera frame and is similar to the GPS measurement matrix but utilizing the camera and reference frame lever arms instead of the lever arms to the GPS antenna. This measurement model therefore relates the camera system elevation and azimuth angles of the reference point to the error in the inertial system on the primary vehicle as the camera as well as the relative inertial error between the primary vehicle and the secondary vehicle.

The matrix H_IMUtoC is defined using a similar methodology as the translation from the IMU to the GPS antenna location. However, the results are not as trivial. To estimate the error between each inertial frame in the EO sensor reference frame defined in Eq. (47), the perturbation must be taken around the relative state vector:

ΔP_iC^C=C_B1^CC_E^B1(P_i^E−P_C^E)=C_B1^CC_E^B1(P₂^E+C_B2^EL_2-i^B2−P₁^E−C_B1^EL_1-C^B1)  (Eq. 56)

In this case, the lever arms L_2-i^B2 and L_1-C^B1 are assumed known as is the orientation of the camera relative to the inertial system on vehicle 1, C_B1^E.The following perturbations are defined as:

P ₁ ^E = P ₁ ^E +δP ₁  (Eq. 57)

P ₂ ^E = P ₂ ^E +δP ₂  (Eq. 58)

C _B1 ^E =C _B1 ^E(I+2[δq_B1^B1×])  (Eq. 59)

C _B2 ^E =C _B2 ^E(I+2[δq_B2^B2×])  (Eq. 60)

Then the perturbations are substituted back into Eq. (56) to form:

$\begin{array}{cc}\Delta P_{\mathrm{iC}}^C=C_{B1}^C\left(I-2\left[\delta q_1\times \right]\right)C_E^{{\stackrel{\_}{B}}_1}\left({\stackrel{\_}{P}}_2^E+\delta P_2-{\stackrel{\_}{P}}_1^E-\delta P_1\right)-C_{B_1}^VL_{1-C}^{B_1}+C_{B_1}^C\left(I-2\left[\delta q_1\times \right]\right)C_E^{{\stackrel{\_}{B}}_1}C_{{\stackrel{\_}{B}}_2}^E\left(I+2\left[\delta q_2\times \right]\right)L_{2-i}^{B2}-\left(C_{B_1}^VC_E^{B1}\left({\stackrel{\_}{P}}_2^E-{\stackrel{\_}{P}}_1^E\right)-C_{B1}^CL_{1-C}^{B1}+C_{B1}^CC_E^{{\stackrel{\_}{B}}_1}C_{{\stackrel{\_}{B}}_2}^EL_{2-i}^{B2}\right)& \left(\mathrm{Eq}.61\right)\end{array}$

Regrouping the terms of Eq. (61) into the GEKF state space and excluding higher order terms of perturbations produces:

$\begin{array}{cc}\Delta P_{\mathrm{iC}}^C\approx \Delta {\stackrel{\_}{P}}_{\mathrm{iC}}^C+C_{B1}^C\left[\begin{array}{cccccc}0& 0& H_{Q1}& C_E^{\stackrel{\_}{B}1}& 0& \left(H_{Q1}+H_{Q2}\right)\end{array}\right]\left[\begin{array}{c}\delta P_1\\ \delta V_1\\ \delta q_{B1}^{\stackrel{\_}{B}1}\\ \Delta \delta P\\ \Delta \mathrm{\delta V}\\ \Delta \delta q\end{array}\right]& \left(\mathrm{Eq}.62\right)\end{array}$

The a priori estimate Δ P_iC^C is defined as:

ΔP_iC^C=C_B1^CC_E^B1(P₂^E+C_B2^EL_2-i^B2− P₁^E−C_B1^EL_1-C^B1)  (Eq. 63)

The term H_Q1 is defined as:

H _Q1=2([C_E^B1(P₁^E− P₂^E)×]+[(C_E^B1C_B2^EL_2-i^B2)×])  (Eq. 64)

The term H_Q2 is defined as:

H _Q2=2C_E^B1C_B2^E[(L_2-i^B2)×]  (Eq. 65)

A notable portion of Eq. (62) is that, since the camera measurements are related through the inertial frame, the term H_Q1 actually provides limited observability of the error in the absolute state of vehicle 1. In other words, the absolute state of vehicle 1 relative to the Earth is observable through the camera measurements. This additional observability is a direct result of the fact that the camera measurements which provide relative state information are related through a common inertial reference frame.

A matrix H_IMU2C may be defined as:

H _IMU2C =C _B1 ^C[00H_Q1C_E^B10(H_Q1+H_Q2)]  (Eq. 66)

The measurement function in Eq. (55) is sufficient to provide relative navigation solutions to the GEKF previously defined given sufficient numbers of reference points. Each reference point may take the form of an active beacon on the viewed vehicle or a small, recognizable portion of the vehicle with known location relative to the inertial system.

EO Silhouette Matching Measurements

A single silhouette measurement produces a complete state of the following form:

$\begin{array}{cc}{\tilde{y}}_i=\left[\begin{array}{c}{\tilde{\rho }}_i\\ {\tilde{\alpha }}_i\\ {\tilde{\beta }}_i\\ Q_{B_2}^C\end{array}\right]& \left(\mathrm{Eq}.67\right)\end{array}$

where {tilde over (α)}_i and {tilde over (β)}_i have the same angle definition as before. The new variable {tilde over (ρ)}_i represents the scalar range from the vision system to the target along the vector line defined by {tilde over (α)}_i and {tilde over (β)}_i. The quaternion is the estimated attitude of the target relative to the camera system.The total measurement equation is:

$\begin{array}{cc}\left[\begin{array}{c}{\tilde{\rho }}_i\\ {\tilde{\alpha }}_i\\ {\tilde{\beta }}_i\end{array}\right]=\left[\begin{array}{c}{\stackrel{\_}{\rho }}_i\\ {\stackrel{\_}{\alpha }}_i\\ {\stackrel{\_}{\beta }}_i\end{array}\right]+\left[\begin{array}{c}{H}_{\rho i}\\ H_{C\alpha }\\ H_{C\beta }\end{array}\right]H_{\mathrm{IMU}2C}\left[\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}\right]+\left[\begin{array}{c}{v}_{\rho }\\ v_{\alpha }\\ v_{\beta }\end{array}\right]& \left(\mathrm{Eq}.68\right)\end{array}$

The a priori relative range measurement is calculated as the norm of the relative state vector or:

ρ=∥ΔP_iC^C∥  (Eq. 69)

Here, the range measurement matrix H_ρi is defined as:

$\begin{array}{cc}{H}_{\rho i}=\left[\frac{\Delta {\stackrel{\_}{X}}_{\mathrm{iC}}^C}{\Delta {\stackrel{\_}{P}}_i^C}\frac{\Delta {\stackrel{\_}{Y}}_{\mathrm{iC}}^C}{\Delta {\stackrel{\_}{P}}_i^C}\frac{\Delta {\stackrel{\_}{Z}}_{\mathrm{iC}}^C}{\Delta {\stackrel{\_}{P}}_i^C}\right]& \left(\mathrm{Eq}.70\right)\end{array}$

The combined bearings measurement matrices are equivalent to the previous forms as:

$\begin{array}{cc}\left[\begin{array}{c}{H}_{C\alpha }\\ H_{C\beta }\end{array}\right]=H_{\mathrm{MGEKF}}H_{\mathrm{LOS\_i}}& \left(\mathrm{Eq}.71\right)\end{array}$

The matrix H_IMU2C is as defined in Eq. (66).

The error from the attitude is treated separately since the residual is a set of relative attitude angles and is not related to the relative distance as the range, azimuth, and elevation are related to range. The attitude determined from an image defines the rotation from the vehicle body frame of the imaged vehicle to the camera frame. The measurement function is nonlinear:

{tilde over (C)} _{B 2} ^C =C _{B 2} ^C(I+2[δq₂×])(I+[ν_q×])  (Eq. 72)

In this case, the term {tilde over (C)}_B2^C is the rotation matrix determined from the estimated relative yaw, pitch and roll of the imaged vehicle in the camera produced image. The a priori estimate of the rotation is C_B2^C. Two errors are associated with the rotation. The first is due to the error in the estimate of the imaged vehicle. This is comprised of the usual attitude quaternion perturbation δq₂. An additional, nonlinear rotation error is due to errors in the image processing ν_q. The measurement equation may be re-written in terms of a linearized residual in which higher order terms of the rotation error and noise are neglected:

$\begin{array}{cc}\begin{array}{c}{r}_q=\left[\begin{array}{c}{\left({\tilde{C}}_{{\tilde{B}}_2}^CC_C^{{\stackrel{\_}{B}}_2}\right)}_{32}\\ {\left({\tilde{C}}_{{\tilde{B}}_2}^CC_C^{{\stackrel{\_}{B}}_2}\right)}_{13}\\ {\left({\tilde{C}}_{{\tilde{B}}_2}^CC_C^{{\stackrel{\_}{B}}_2}\right)}_{21}\end{array}\right]\\ =H_q\left[\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}\right]+\left[\begin{array}{c}{v}_{\Phi }\\ v_{\Theta }\\ v_{\Psi }\end{array}\right]\end{array}& \left(\mathrm{Eq}.73\right)\end{array}$

where the form ({tilde over (C)}_{{tilde over (B)}2}^CC_C^B2)_ij is the ith row and jth column of the 3×3 matrix {tilde over (C)}_{{tilde over (B)}2}^CC_C^B2.The quaternion measurement matrix H_q is simply defined as:

H _q=[002I00−2I]  (Eq. 74)

The measurement noise ν_q is decomposed into the axial components as:

$\begin{array}{cc}{v}_q=\left[\begin{array}{c}{v}_{\Phi }\\ v_{\Theta }\\ v_{\Psi }\end{array}\right]& \left(\mathrm{Eq}.75\right)\end{array}$

Therefore, the residual is formed using Eq. (73) and the associated error models. The error models are then utilized to form and correct the GEKF already defined.

Note that the above algorithm assumes that vehicle 1 images vehicle 2 using either reference points or silhouette matching. However, it is trivial to expand the results to include additional measurements of vehicle 2 imaging vehicle 1. For example, the receiver aircraft may include a camera (not illustrated in FIG. 2). Further, multiple silhouettes may be generated for different parts of the vehicle or player in view. Using either bearings only with multiple reference points or a silhouette matching scheme or combinations thereof, it is possible to use the measurement models defined in Eq. (68) and/or Eq. (55) to correct the inertial errors with error modeling defined in Eq. (15). Finally, the results may be simplified to the case where only a range measurement is available.

Processing Additional Sensor Measurements Through the GEKF

The Global Extended Kalman Filter (GEKF) has been defined for processing the GPS, EO, and INS measurements. However, the GEKF may be extended to include additional measurements such as relative range, bearings measurements, and relative attitude that may be provided from an Electro-Optic sensor or vision processing. The measurement functions derived can be used to correct the GEKF presented previously. The complete measurement function becomes:

$\begin{array}{cc}\left[\begin{array}{c}{\tilde{\rho }}_i-{\stackrel{\_}{\rho }}_i\\ {\tilde{\alpha }}_i-{\stackrel{\_}{\alpha }}_i\\ \begin{array}{c}{\tilde{\beta }}_i-{\stackrel{\_}{\beta }}_i\\ r_q\end{array}\end{array}\right]=\left[\begin{array}{c}{H}_{\rho i}H_{\mathrm{IMU}2C}\\ H_{C\alpha }H_{\mathrm{IMU}2C}\\ \begin{array}{c}{H}_{C\beta }H_{\mathrm{IMU}2C}\\ H_q\end{array}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta \Delta x\end{array}\right]+\left[\begin{array}{c}\begin{array}{c}{v}_{\rho }\\ v_{\alpha }\\ v_{\beta }\end{array}\\ v_q\end{array}\right]& \left(\mathrm{Eq}.76\right)\end{array}$

The measurement noise is assumed to be Gaussian with the following statistics.

$\begin{array}{cc}E\left[\left[\begin{array}{c}\begin{array}{c}{v}_{\rho }\\ v_{\alpha }\\ v_{\beta }\end{array}\\ v_q\end{array}\right]\right]=0;E\left[\left[\begin{array}{c}\begin{array}{c}{v}_{\rho }\\ v_{\alpha }\\ v_{\beta }\end{array}\\ v_q\end{array}\right]\left[v_{\rho }v_{\alpha }v_{\beta }v_q\right]\right]=V_{\mathrm{EO}}& \left(\mathrm{Eq}.77\right)\end{array}$

Note that V_EO is a covariance which contains any and all of the correlations between the noise processes, as determined by the method utilized to generate the range, angles, and attitude measurements. It is up to the designer to select appropriate values for V_EO Likewise, the measurement sensitivity is grouped together into a single matrix for convenience of notation:

$\begin{array}{cc}{H}_{\mathrm{EO}}=\left[\begin{array}{c}{H}_{\rho i}H_{\mathrm{IMU}2C}\\ H_{C\alpha }H_{\mathrm{IMU}2C}\\ \begin{array}{c}{H}_{C\beta }H_{\mathrm{IMU}2C}\\ H_q\end{array}\end{array}\right]& \left(\mathrm{Eq}.78\right)\end{array}$

Therefore, the GEKF can be modified to include the vision system measurements. The inertial navigation state is propagated forward in time by an amount Δt using the process and methods described in Eq. (25) through Eq. (34). At that time, if GPS measurements are available, the navigation state may be updated and corrected using the GPS measurement function and EKF corrections outlined in Eq. (35) through Eq. (45). After the state and covariance are updated, the system may check for EO measurements. If GPS measurements were available and applied, then the state is the most up to date state using the most recent GPS and IMU measurements. The covariance P_GEKF is copied into M_GEKF before processing the EO measurements.

If electro-optic (EO) sensor measurements are available, then the following method may be used to correct the navigation state through a GEKF process with or without GPS measurements. First, the covariance is updated using the following method:

P _GEKF =M _GEKF −M _GEKF H _EO ^T(H_EOM_GEKFH_EO^T+V_EO)⁻¹H_EOM_GEKF  (Eq. 79)

The Kalman Gain is then calculated as:

K=P _GEKF H _EO ^T V _EO ⁻¹  (Eq. 80)

The state correction can be calculated as:

$\begin{array}{cc}\left[\begin{array}{c}\delta {\hat{x}}_1\\ \delta \Delta \hat{x}\end{array}\right]=K\left(\left[\begin{array}{c}{\tilde{\rho }}_i-{\stackrel{\_}{\rho }}_i\\ {\tilde{\alpha }}_i-{\stackrel{\_}{\alpha }}_i\\ \begin{array}{c}{\tilde{\beta }}_i-{\stackrel{\_}{\beta }}_i\\ r_q\end{array}\end{array}\right]\right)& \left(\mathrm{Eq}.81\right)\end{array}$

No a priori estimate of the error needs to be maintained since the correction is applied to the state estimate. The state corrections are defined in terms of the corrections for the navigation state:

$\begin{array}{cc}\left[\begin{array}{c}\delta {\hat{x}}_1\\ \delta \Delta \hat{x}\end{array}\right]=\left[\begin{array}{c}\delta P_1^E\\ \delta V_1^E\\ \delta q_{B1}^{\stackrel{\_}{B}1}\\ \delta P_1^E-\delta P_2^E\\ \delta V_1^E-\delta V_2^E\\ \delta q_{B1}^{\stackrel{\_}{B}1}-\delta q_{B2}^{\stackrel{\_}{B}2}\end{array}\right]& \left(\mathrm{Eq}.82\right)\end{array}$

The correction process is applied as discussed for Eq. (42) through Eq. (45). Note that this correction may take place even if GPS measurements are not available. In this case, the INS states and covariances are propagated forward in time to the current reference time of the EO measurements utilizing the INS data. Then the EO measurement correction process is applied. Similarly, this correction process may take place before the GPS measurements are utilized to correct the state provided that the navigation state and covariance was corrected with the EO measurements before processing the GPS. Finally, a third option is that the measurement functions and noise covariances from both the EO and GPS measurements available are combined into one single update function for the GEKF. This last segment only applies if the GPS and EO measurements have the same reference time for the measurements. Otherwise, the state must be propagated forward in time to the next measurement whether it is the EO or GPS measurements.

Processing Radio Ranging Measurements Through the GEKF

Measurements from radio ranging devices or radio bearings devices may be used in the GEKF. In this case, the radio provides a measurement of the relative range to the other vehicle radio. Multiple antennae may provide elevation and/or azimuth angles from the first vehicle to the relative vehicle. Such applications include data links which provide range using time delay of arrival schemes or Auto Collision Avoidance Systems which attempt which provide time delay of arrival of message or angles to the source of a transmission. The measurement model is simply modified to include only those measurements present. Instead of an EO sensor, the measurements are taken at the radio antenna which must be referenced relative to the IMU in the same way that the EO sensor lever arm was defined. In other words L_1-C^B1 becomes the vector arm from the IMU to the radio antenna rather than the camera. Further, if elevation and azimuth are defined relative to an antenna, the relative attitude between the antenna and the inertial must be defined. In other words, C_B1^C is replaced with the rotation matrix from the IMU body frame to the antenna frame which should be constant for a rigid body. Likewise, the antenna location on the second vehicle must be surveyed relative to the second vehicle IMU. In other words, L_2-i^B2 is replaced with the lever arm from the IMU on the second vehicle to the radio antenna on the second vehicle. Using these three replacements, the measurement models remain valid and may be utilized to incorporate radio ranging from antenna to antenna on different vehicles.

Fault Detection

As discussed with regard to FIGS. 1 and 2, a statistically optimal automatic control methodology has been disclosed for the control of an aerial refueling boom. But it may be the case that sensor measurements are defective. For example, GPS signal errors include but are not limited to multi-path, satellite transmission errors, or simply low signal-to-noise due to partial blockage by the tanker. A receiver failure can be caused by a variety of factors, from signal blockage by the tanker to combat damage. Only the detection of more subtle GPS signal errors are discussed below since the detection of a failed receiver is trivial with such methods as utilizing watchdog timers on the output of the receiver. Turning now to FIG. 3, a fault detection and reconfiguration process is outlined. A bank of fault detection filters and residual generator 300 processes measurements from, for example, an IMU, a GPS receiver, and an EO system. Residuals from generator 300 are processed in a residual processor 305 and a fault reconstruction process 310 to provide fault probabilities and fault magnitudes, respectively. A fault tolerant estimator 315 processes the fault probabilities and fault magnitudes to provide fault-free inertial navigation state estimates. The fault estimation and reconfiguration process of FIG. 3 will be more fully explained below.

Absolute GPS Fault Detection

GPS signal errors introduce an effective, unknown bias on one of the GPS satellite range and/or carrier phase measurements relative to the other GPS measurements. A least squares approach may be used to detect and isolate failures in a single GPS satellite measurement and isolate it from the remaining GPS measurement sets. For example, a Multiple Hypothesis Shiryayev Sequential Probability Ratio Test (MHSSPRT) may be used for detection and isolation of these errors (this test is described further below).

A least squares GPS navigation solution is well known. The pseudo range measurements from four or more visible GPS satellites are combined to form an estimate of the GPS receiver position and clock bias relative to the GPS constellation. Given n satellites, the navigation solution can be computed with (n-1) satellites, cycling through the satellites to create (n-1) solutions. Comparing the residual of these solutions over time using the SSRPT creates a probability distribution that indicates the likelihood of a satellite failure. When the probability that a satellite is failing reaches 99.9% or a required alert probability, an alarm is triggered and the satellite is no longer used for computation of the navigation state.

The range from a GPS receiver to a satellite i is denoted by the symbol ρ_i and defined as:

ρ_i=√{square root over ((X_i−x)²+(Y_i−y)²+(Z_i−z)²)}{square root over ((X_i−x)²+(Y_i−y)²+(Z_i−z)²)}{square root over ((X_i−x)²+(Y_i−y)²+(Z_i−z)²)}+cτ  (Eq. 83)

The range measurement is a function of a satellite position vector P_i^E, a receiver position vector P^E, and a receiver clock bias τ. The variable c represents the speed of light and converts a clock bias in seconds to a range. The satellite position vector in the ECEF coordinate frame is defined as:

$\begin{array}{cc}{P}_i^E=\left[\begin{array}{c}\begin{array}{c}{X}_i\\ Y_i\end{array}\\ Z_i\end{array}\right]& \left(\mathrm{Eq}.84\right)\end{array}$

The receiver position in the Earth Centered Earth Fixed coordinate frame is defined as:

$\begin{array}{cc}{P}^E=\left[\begin{array}{c}\begin{array}{c}x\\ y\end{array}\\ z\end{array}\right]& \left(\mathrm{Eq}.85\right)\end{array}$

Given an a priori estimate of the receiver position and clock bias, the estimated range to the satellite is given by:

ρ=√{square root over ((X_i− x)²+(Y_i− y)²+(Z_i− z)²)}+c τ  (Eq. 86)

In this case the terms denoted by ( •) represent a priori estimates. The estimated range is in error and a linear perturbation defines the error between the estimated range and the true range.

ρ_i= ρ_i+H_iδx  (Eq. 87)

where the variable δx is the perturbed error in the state and consists of a perturbation in the state estimate which is to be estimated. The perturbation is defined as truth minus the estimated, or:

$\begin{array}{cc}\delta x=\left[\begin{array}{c}\begin{array}{c}x\\ y\end{array}\\ \begin{array}{c}z\\ c\tau \end{array}\end{array}\right]-\left[\begin{array}{c}\begin{array}{c}\stackrel{\_}{x}\\ \stackrel{\_}{y}\end{array}\\ \begin{array}{c}\stackrel{\_}{z}\\ c\stackrel{\_}{\tau }\end{array}\end{array}\right]& \left(\mathrm{Eq}.88\right)\end{array}$

A line of sight matrix H_i is defined as:

$\begin{array}{cc}{H}_i=\left[\frac{\left(X_i-\stackrel{\_}{x}\right)}{P_i^E-\stackrel{\_}{P}}\frac{\left(Y_i-\stackrel{\_}{y}\right)}{P_i^E-\stackrel{\_}{P}}\frac{\left(Z_i-\stackrel{\_}{z}\right)}{P_i^E-\stackrel{\_}{P}}1\right]& \left(\mathrm{Eq}.89\right)\end{array}$

where the term ∥•∥ denotes the 2-norm (sum of the squares) of the vector. From these definitions, a set of GPS measurements may be defined as:

{tilde over (ρ)}_i= ρ_i+H_iδx+μ_i+ν_i+b_i  (Eq. 90)

The GPS measurement of range {tilde over (ρ)} is corrupted by random noise ν_i which is zero mean and has variance V_i, as well as common mode errors b_i which include ionosphere and troposphere errors. The common mode errors are typically not zero mean, but are nearly the same for two GPS receivers within close proximity. In addition, an unknown fault μ_i may exist in the measurement such as multipath. A goal is to estimate the state error δx using a set of measurements {tilde over (ρ)}, one of which may include a fault μ_i in the ith measurement, and then detect and isolate the satellite with the fault. In the following discussion, only one satellite fault is assumed.

The tanker and receiver aircraft may each receive a set of GPS measurements of the form in Eq. (90).

{tilde over (ρ)}_tank= ρ_tank+H_tankδx_tank+ν_tank  (Eq. 91)

{tilde over (ρ)}_rec= ρ_rec+H_recδx_rec+ν_rec  (Eq. 92)

The common mode errors and fault modes are neglected for the moment. A weighted least squares solution is provided for the receiver and tanker aircraft below. The solutions presented represent the best estimate of the error for each vehicle using only the data available from each GPS receiver and assuming zero mean, Gaussian noise from each receiver.

δ{circumflex over (x)}_tank=(H_tank^TV_tank⁻¹H_tank)H_tank^TV_tank⁻¹({tilde over (ρ)}_tank− ρ_tank)  (Eq. 93)

δ{circumflex over (x)}_rec=(H_rec^TV_rec⁻¹H_rec)H_rec^TV_rec⁻¹({tilde over (ρ)}_rec− ρ_rec)  (Eq. 94)

The state estimates are then updated according to the following:

{circumflex over (x)} _tank = x _tank +δ{circumflex over (x)} _tank  (Eq. 95)

{circumflex over (x)} _rec = x _rec +δ{circumflex over (x)} _rec  (Eq. 96)

From the state, a posteriori residual may be formed in which the fault signal is now assumed to be present.

r={tilde over (ρ)}− ρ−Hδ{circumflex over (x)}−μ _i−ν_i  (Eq. 97)

A projector P is formed such that PH=0. For the matrix P to exist, the number of available satellites must be greater than 4; the size of the perturbed state space. Using this projector and assuming a no fault condition, then the residual may be modified as:

Pr=P({tilde over (ρ)}− ρ−Hδ{circumflex over (x)}−μ_i−ν)=P({tilde over (ρ)}− ρ−μ_i−ν)  (Eq. 98)

In sum, the effect of the estimation error has been annihilated, and assuming a reasonable first estimate ({tilde over (ρ)}− ρ≈0), then Eq. (98) becomes:

Pr=P(ν+μ_i)  (Eq. 99)

If no fault exists (μ_i=0), then the statistics of Eq. (99) are zero mean and Gaussian with covariance V. The projector P has a null space equal to the size of the state space δ{circumflex over (x)}. This may be removed from the residual since it is not relevant. The projector may be decomposed as:

$\begin{array}{cc}P=\left[\begin{array}{cc}{U}_1& U_2\end{array}\right]\left[\begin{array}{cc}I& 0\\ 0& 0\end{array}\right]\left[\begin{array}{c}{V}_1\\ V_2\end{array}\right]& \left(\mathrm{Eq}.100\right)\end{array}$

The residual in Eq. (99) may be rewritten neglecting the portion of the residual annihilated by the projector:

{tilde over (r)}=U ₁ ^T r=V ₁(ν+μ_i)  (Eq. 101)

The probability density function of the new residual {tilde over (r)} is defined as:

$\begin{array}{cc}{f}_{\tilde{r}}\left(\tilde{r}\right)=\frac{1}{{\left(2\pi \right)}^{11/2}{{\Lambda }_r}^{1/2}}{}^{\left(-\frac{1}{2}\left({\tilde{r}}^T{\Lambda }_r^{-1}\tilde{r}\right)\right)}& \left(\mathrm{Eq}.102\right)\end{array}$

This is the density function for a Gaussian random variable {tilde over (r)}=U₁^Tr with zero mean and covariance Λ_r=V₁VV₁^T. The value of n is equal to the number of GPS measurements used in the residual minus the state space δx size. The density function is utilized to test the estimates in the Multiple Hypothesis Shiryayev Sequential Probability Ratio Test (MHSSPRT). Those residuals conforming to the Gaussian characteristics are not likely to have a fault. However, those residuals that are consistently multiple standard deviations away from the mean value are likely to have a failure.

To detect a failure, the GPS satellites are ordered into subsets. Each subset includes all of the satellites except one. For each subset, the satellite that is excluded is assumed to be faulted. A separate least squares solution is computed for each subset of satellites using Eq. (860 through Eq. (96). The residual and probability density functions are then formed for each subset as in Eq. (99) through Eq. (102). Referring back to FIG. 3, the residuals are formed in residual generator 300. These probability density functions are tested using, for example, a MHSSPRT method in residual processor 305. This test sequentially updates the probability that each subset has a fault. If a satellite failure occurs, then all of the subsets will show a non-zero-mean probability density function except for the set that excluded the satellite. When the probability estimate of the case that excludes the faulted satellite reaches a set probability limit, a fault is declared and the satellite is excluded from the possible sets before use in the GEKF implemented in fault tolerant estimator 315. The tests are reset without that satellite and the process resumes looking for additional satellite failures.

Differential GPS Fault Detection

The methodology just discussed may be used to estimate failures in differential GPS measurements (DGPS). Calculating the relative state requires the differencing of state and pseudorange quantities. The relative state estimate becomes:

Δx= x_tank− x_rec  (Eq. 103)

An a priori relative range may be calculated as:

Δ ρ= ρ_tank− ρ_rec  (Eq. 104)

A single differenced GPS measurement is the difference of two range measurements from a common satellite. The receiver range measurement of satellite i is subtracted from the tanker measurement of satellite i to form the single difference range measurement. This measurement eliminated the common mode errors defined in Eq. (90) and is denoted as:

Δ{tilde over (ρ)}=( ρ_tank+H_tankδx_tank+ν_tank)−( ρ_rec+H_recδx_rec+ν_rec)  (Eq. 105)

Since the distance between the tanker and receiver is small in comparison to the distance to the satellites when the receiver is in contact position, it may be assumed that H_tank≈H_rec. A new relative range error may thus be defined as

Δ{tilde over (ρ)}= ρ_tank− ρ_rec+H_tank(δx_tank−δx_rec)+ν_tank−ν_rec=Δ ρ+H_tankΔδx+Δν  (Eq. 106)

The new, relative state estimate error Δδx is estimated using a weighted least squares solution to Eq. (106)

Δδ{circumflex over (x)}=[(H_tank^T(2ΔV⁻¹)H_tank^T(2ΔV⁻¹)](Δ{tilde over (ρ)}−Δ ρ)  (Eq. 107)

And the posteriori residual for testing in the MHSSPRT is described as:

r _Δ={tilde over (ρ)}− ρ−HΔδx−μ_i−Δν  (Eq. 108)

Note that the fault μ_i may be from either the tanker or receiver. This particular test does not isolate the fault to the tanker or receiver, but it does identify that a fault exists in the relative range estimates. The absolute estimates tests using the residuals defined in Eq. (97) are required to isolate a failure to either the tanker or receiver. However, the differential residual defined in Eq. (108) is not susceptible to common mode errors and the relative measurement covariance ΔV is typically an order of magnitude smaller than the absolute covariance V for each vehicle which must be increased substantially in order to compensate for the unmodeled atmospheric effects.

As discussed with regard to absolute GPS fault detection, a projector P is chosen such that PH=0 and used to modify the differential residual and eliminate the effect of the unknown estimation error. The results follow the previous development to form a new residual {tilde over (r)}_Δ=U₁r_Δ=V₁(Δν+μ_i). The probability density function of this residual assuming no fault may be defined by

$\begin{array}{cc}{f}_{{\tilde{r}}_{\Delta }}\left({\tilde{r}}_{\Delta }\right)=\frac{1}{{\left(2\pi \right)}^{11/2}{{\Lambda }_{r_{\Delta }}}^{1/2}}{}^{\left(-\frac{1}{2}\left({\tilde{r}}_{\Delta }^T{\Lambda }_{r_{\Delta }}^{-1}{\tilde{r}}_{\Delta }\right)\right)}& \left(\mathrm{Eq}.109\right)\end{array}$

As discussed with regard to absolute GPS measurements, residual generator 300 may generate the appropriate residuals having probability density functions as shown in Eq. (109). Residual processor 305 then examines whether the residuals remain within the density function or because the residuals provide insight into the health of the DGPS measurements. If the residuals remain within the density function, then the probability of a failure is small. If the residuals reside outside the density functions, then the probability of failure is more likely. Therefore, the double differenced measurements may be placed into sub-groups. Each subgroup utilizes all of the satellites except for one, with each subgroup choosing a different satellite measurement to exclude. The differential GPS residuals are then processed through the MHSSPRT that calculates the probability that each of the subgroups is healthy while the others are faulted. If one residual moves to a probability of one or to some pre-defined bound, then the satellite that was excluded from that subgroup is declared faulty and removed from use in the measurement set. New subgroups are formed from the remaining satellites, and the process continues.

The same process may be used for double differenced carrier phase measurements that may be tested independently or with the DGPS measurements in order to form additional measurements. For double differenced carrier phase measurements, the projector is different, however, and must be calculated based on the double differenced line of sight matrix. The MHSSPRT method will now be discussed in more detail.

Multiple Hypothesis Shiryayev Sequential Probability Ratio Test

The Multiple Hypothesis Shiryayev Sequential Probability Ratio Test (MHSSPRT) is a hypothesis testing scheme that may be performed in residual processor 305. The test is designed to detect changes in a residual history in minimum time based on assumed statistical properties. Given a residual history and associated probability density function for a set of hypotheses, the MHSSPRT calculates the probability that each hypothesis is true relative to the other hypotheses. The MHSSPRT assumes that a base condition exists which is healthy and then calculates the probability of a change to a different hypothesis assuming a probability of failure at each time step.

In equation form, the probability that a hypothesis j is correct at time 0 is given by an initial condition:

F_0,j=π_j  (Eq. 110)

The initial condition π_j is chosen as a design parameter. At each measurement time k, there is a finite probability that a failure has occurred between the time of the current measurement k and the previous time step, k-1. The propagation of the probability is:

φ_k,j=F_k,j+{tilde over (p)}_j(1−Σ_l=0^NF_k,j)  (Eq. 111)

The value of N is the total number of possible fault modes currently addressed in the MHSSPRT. For example, if 7 satellites are in view, then there are at least 7 fault modes considered, one for each satellite. The total health condition in which it is assumed that all satellites are healthy may or may not be included as a design choice.

When measurements become available, the residuals and associated density functions are calculated as previously described. The densities are updated utilizing the following update formula.

$\begin{array}{cc}{F}_{k+1,j}=\frac{{\varphi }_{k,j}f_{{\tilde{r}}_{k+1h_j}}\left({\tilde{r}}_{k+1}h_j\right)}{\sum _{l=0}^N{\varphi }_{k,j}f_{{\tilde{r}}_{k+1}h_j}\left({\tilde{r}}_{k+1}h_j\right)}& \left(\mathrm{Eq}.112\right)\end{array}$

After each update, the probability is tested against a value. If the probability exceeds a threshold (such as 0.99999) then that probability is declared true. The assumed hypothesis is assumed to be accurate. In this case, the subset of GPS measurements utilized which reaches this value is the set of measurements that is healthy since all other subsets (hypothesized healthy measurements) have a probability near zero, indicating that the satellite excluded by the first subset is unhealthy. In this way the MHSSPRT identifies the failure and isolates the failure from other possible failures.

EO Fault Detection

EO fault detection is similar to the differential GPS fault detection techniques presented previously. A least squares technique is presented as a means of utilizing excess observability in the vision systems as a means of self checking. Two different types of estimation are described.

Reference Point Least Squares Filter

Reference point estimation assumes that the EO system is capable of identifying fixed points on the receiver aircraft with known lever arms between the points. The result is that the EO system provides bearings measurements (elevation and azimuth) in the camera frame to each of the reference points. Reference points may be active such as beacon systems, passive such as known paint markings, or inherent in the receiver design such as the refueling receptacle. The relative position in the camera frame V from the camera to a reference point is given by:

$\begin{array}{cc}\begin{array}{c}{r}_{T,R_i}^V=\Delta P_i^V\\ =\left[\begin{array}{c}{x}_{R_i}-x_T\\ y_{R_i}-y_T\\ z_{R_i}-z_T\end{array}\right]\\ \left(\Delta P^V\right)+C_{B_R}^VL_{R_{\mathrm{INS}},R_i}^{B_R}\end{array}& \left(\mathrm{Eq}.113\right)\end{array}$

In this case, the relative position is calculated only in the vision sensor reference frame. The sensor frame is designed as a right handed Cartesian frame with x through the lens of the camera looking out towards the receiver, the y-axis is defined to starboard of the camera, and the z-axis points down. The axes are centered at the lens of the vision system. The rotation matrix C_BR^V defines the rotation from the receiver body frame to the tanker vision frame. The lever arm L_RINS,Ri^BR is the vector position in the receiver body frame from the receiver INS (or other common reference point) to the reference point i. This vector is assumed known a priori. Since no other instruments are involved, the lever arm from the tanker vision system to the GPS or IMU is ignored. This algorithm calculates the position and attitude of the receiver relative to the vision system. The values for x_T, y_T, and z_T form the origin of the reference frame and may be set to zero for convenience. The error in the relative position is defined as the difference between the a priori estimate of the relative position and a perturbation:

ΔP^V=Δ P^V+δΔP^V  (Eq. 114)

The error in the attitude is defined relative to the target body frame as:

C _{B R} ^V =C _{B R} ^V(I+2[δq_R×])→C_V^BR=(I−2[δq_R×])C_V^BR  (Eq. 115)

The definition for e^V, the error between the true and estimated position and attitude is

$\begin{array}{cc}\begin{array}{c}{e}_i^V=\left[\begin{array}{c}{e}_x^V\\ e_y^V\\ e_z^V\end{array}\right]\\ =\left[\begin{array}{c}{x}_{R_i}-x_T\\ y_{R_i}-y_T\\ z_{R_i}-z_T\end{array}\right]-\left[\begin{array}{c}{\stackrel{\_}{x}}_{R_i}-{\stackrel{\_}{x}}_T\\ {\stackrel{\_}{y}}_{R_i}-{\stackrel{\_}{y}}_T\\ {\stackrel{\_}{z}}_{R_i}-{\stackrel{\_}{z}}_T\end{array}\right]\\ =\Delta P^V-\Delta {\stackrel{\_}{P}}^V+C_{B_R}^VL_{R_{\mathrm{INS}},R_i}^{B_R}-C_{{\stackrel{\_}{B}}_R}^VL_{R_{\mathrm{INS}},R_i}^{B_R}\end{array}& \left(\mathrm{Eq}.116\right)\end{array}$

Substituting Eq. (114) and Eq. (115) into Eq. (116) produces the following equation in terms of the error perturbations:

e ^V =Δ P ^V +δΔP ^V −Δ P ^V +C _{B R} ^V(I+2[δq_R×])L_RINS,Ri^BR−C_BR^VL_RINS,Ri^BR

e ^V =δΔP ^V−2C_BR^V[L_RINS,Ri^BR×]δq_R  (Eq. 117)

The measurement matrix H_e is defined as

H _e =[I−2C_BR^V[L_RINS,Ri^BR×]]  (Eq. 118)

The state δx is now the reduced order state:

$\begin{array}{cc}\delta x=\left[\begin{array}{c}\delta \Delta P^V\\ \delta q_T\end{array}\right]& \left(\mathrm{Eq}.119\right)\end{array}$

The error equation in terms of the Cartesian location is of a form:

e^V=H_eδx  (Eq. 120)

If the measurements are in terms of a vector position of the reference point location relative to the camera (such as in the case of phase space measurements), then the residual error measurement becomes:

r _V ={tilde over (P)} ^V − P ^V =H _e δx+ν _V  (Eq. 121)

In this case, the measurement {tilde over (P)}^V is the position of the reference point in the vision reference frame measured by the phase space estimation system with additive noise ν_V.

However, if the vision or Electro-optical system produces angle measurements in the form of elevation and azimuth, then the measurements are defined as:

$\begin{array}{cc}{\alpha }_i={\tan }^{-1}\left(\frac{y_T-y_{R_i}}{x_T-x_{R_i}}\right)\mathrm{and}& \left(\mathrm{Eq}.122\right)\\ {\beta }_i={\tan }^{-1}\left(\frac{z_T-z_{R_i}}{x_T-x_{R_i}}\right)& \left(\mathrm{Eq}.123\right)\end{array}$

The conversion of specialized angle measurements to Cartesian errors may be performed using a Modified Gain Extended Kalman Filter as discussed previously. In this filter, a measurement matrix H_V is defined such that a residual of the angles for each reference point take the form:

$\begin{array}{cc}\begin{array}{c}{r}_{\mathrm{\alpha \beta }}=\left[\begin{array}{c}{\tilde{\alpha }}_i-{\stackrel{\_}{\alpha }}_i\\ {\tilde{\beta }}_i-{\stackrel{\_}{\beta }}_i\end{array}\right]\\ =H_VH_e\delta x+\left[\begin{array}{c}{v}_{\alpha }\\ v_{\beta }\end{array}\right]\end{array}& \left(\mathrm{Eq}.124\right)\end{array}$

In addition, the residuals which have measurements in spherical coordinates have linear errors in the Cartesian coordinates. The measurement noise ν_α and ν_β are typically modeled as pixel noise on the angles.

Using multiple reference points (multiple are required for full observability), it is possible to estimate the solution to Eq. (124) using the following least squares solution:

$\begin{array}{cc}\begin{array}{c}\delta \hat{x}={\left(H_e^TH_V^TV_V^{-1}H_VH_e\right)}^{-1}H_e^TH_V^TV_V^{-1}\left(\left[\begin{array}{c}\tilde{\alpha }-\stackrel{\_}{\alpha }\\ \tilde{\beta }-\stackrel{\_}{\beta }\end{array}\right]\right)\\ =\left[\begin{array}{c}\delta \Delta P^V\\ \delta q_{B_T}\end{array}\right]\end{array}& \left(\mathrm{Eq}.125\right)\end{array}$

The relative position Δ P^E is corrected using

Δ{circumflex over (P)}^E=Δ P^E+Δδ{circumflex over (P)}  (Eq. 126)

The relative attitude is corrected by first constructing the quaternion correction:

$\begin{array}{cc}{Q}_{{\hat{B}}_T}^{{\stackrel{\_}{B}}_T}=\left[\begin{array}{c}\sqrt{1-\left(\delta q_T^T\delta q_T\right)}\\ \delta q_T\end{array}\right]& \left(\mathrm{Eq}.127\right)\end{array}$

A corrected quaternion may then be formed by rotating the a priori quaternion by the corrected update quaternion:

Q_{{circumflex over (B)}T}^V=Q_BT^VQ_{{circumflex over (B)}T}^BT  (Eq. 128)

At this point the least squares solution to the relative navigation system is defined. The extension to fault detection is to assume that one of the reference points is invalid. A least squares fault detection filter as discussed above is then utilized to determine which reference point is invalid and exclude it from the estimation process.

Silhouette Matching Least Squares

The previous section assumed that the image processing algorithm identified reference points and calculated the bearings measurements to the vehicle. An alternative approach is to attempt to use silhouette matching algorithms in which the target is matched with a library of silhouettes of the target to estimate the range and orientation of the entire target or target area. The output of this type of algorithm provides additional information including a relative range and relative attitude of the target to the vision system. An additional score of how well the target and silhouette match is also included as a noise factor.

The benefit of silhouette matching is that it inherently takes into account the geometry of the lever arms over an area of the target and produces range and attitude information in addition to bearings measurements. The process may be repeated locally or globally for a given target geometry. For instance, the silhouette used may match the entire target geometry or just a portion of the target such as an aircraft canopy or tail section. Through the use of multiple silhouettes and sub-silhouettes for a given target, multiple relative ranges and attitudes may be used to estimate the total target range and relative attitude. However, a disadvantage of this type of algorithm is that each measurement is complex because it consists of complete relative position and attitude rather than just angles. The result is a highly correlated vector measurement. The capability of matching the silhouette to the image does not have a known, stochastic error model in terms of the measurements presented. Characterizing the measurements becomes more difficult.

A single silhouette measurement produces a complete state of the following form:

$\begin{array}{cc}{\tilde{y}}_i=\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}{\tilde{\rho }}_i\\ {\tilde{\alpha }}_i\end{array}\\ {\tilde{\beta }}_i\end{array}\\ Q_{B_{\stackrel{\_}{T}}}^V\end{array}\right]& \left(\mathrm{Eq}.129\right)\end{array}$

where {tilde over (α)}_i and {tilde over (β)}_i have the same angle definition as in Eq. (122) and Eq. (123). The new variable {tilde over (ρ)}_i represents the scalar range from the vision system to the target along the vector line defined by {tilde over (α)}_i and {tilde over (β)}_i. The quaternion is the estimated attitude of the target relative to the camera system as previously defined as part of Eq. (128).

Note that the silhouette matching algorithm still reflects the estimate of the range, attitude, and bearings from the camera to a particular point on or within the target. This location may be different from algorithm to algorithm. Regardless, the silhouette reference point may be different than the location of the target INS or other reference point. Therefore, the relative range from the camera to the target INS is still defined as:

$\begin{array}{cc}\begin{array}{c}{r}_{T,R_i}^V=\Delta P_i^V\\ =\left[\begin{array}{c}{x}_{R_i}-x_T\\ y_{R_i}-y_T\\ z_{R_i}-z_T\end{array}\right]\\ =\left(\Delta P^V\right)+C_{B_R}^VL_{R_{\mathrm{INS}},R_i}^{B_R}\end{array}& \left(\mathrm{Eq}.130\right)\end{array}$

The error model measurement for the measurements of {tilde over (α)}_i and {tilde over (β)}_i have been previously defined. The definition of the range measurement in the camera coordinate frame is:

$\begin{array}{cc}\begin{array}{c}{\rho }_i=\Delta P_i^V\\ =\left[\begin{array}{c}{x}_{R_i}-x_T\\ y_{R_i}-y_T\\ z_{R_i}-z_T\end{array}\right]\\ =\sqrt{\begin{array}{c}\begin{array}{c}{\left(x_{R_i}-x_T\right)}^2+\\ {\left(y_{R_i}-y_T\right)}^2+\end{array}\\ {\left(z_{R_i}-z_T\right)}^2\end{array}}\end{array}& \left(\mathrm{Eq}.131\right)\end{array}$

The a priori estimate of the range is determined from:

$\begin{array}{cc}\begin{array}{c}{\stackrel{\_}{\rho }}_i=\Delta {\stackrel{\_}{P}}_i^V\\ =\left[\begin{array}{c}{\stackrel{\_}{x}}_{R_i}-{\stackrel{\_}{x}}_T\\ {\stackrel{\_}{y}}_{R_i}-{\stackrel{\_}{y}}_T\\ {\stackrel{\_}{z}}_{R_i}-{\stackrel{\_}{z}}_T\end{array}\right]\\ =\sqrt{\begin{array}{c}\begin{array}{c}{\left({\stackrel{\_}{x}}_{R_i}-{\stackrel{\_}{x}}_T\right)}^2+\\ {\left({\stackrel{\_}{y}}_{R_i}-{\stackrel{\_}{y}}_T\right)}^2+\end{array}\\ {\left({\stackrel{\_}{z}}_{R_i}-{\stackrel{\_}{z}}_T\right)}^2\end{array}}\end{array}& \left(\mathrm{Eq}.132\right)\end{array}$

The range measurement is defined as truth plus an additive noise term:

{tilde over (ρ)}_i={tilde over (ρ)}_i+ν_ρ  (Eq. 133)

Taking a first order perturbation in ΔP^V, the true relative position is again defined relative to the a priori range as:

ΔP_i^V=Δ P_i^V+δΔP_i^V  (Eq. 134)

Taking a first order perturbation around the a priori estimate of range, the true range is approximated by:

ρ_i= ρ_i+H_ρiδΔP_i^V  (Eq. 135)

Where H_ρ is defined as:

$\begin{array}{cc}{H_{\rho i}=\frac{\partial {\rho }_i}{\partial \mathrm{\delta \Delta }P_i^V}}_{\Delta {\stackrel{\_}{P}}_i^V}=\left[\frac{{\stackrel{\_}{x}}_{R_i}-{\stackrel{\_}{x}}_T}{\Delta {\stackrel{\_}{P}}_i^V}\frac{{\stackrel{\_}{y}}_{R_i}-{\stackrel{\_}{y}}_T}{\Delta {\stackrel{\_}{P}}_i^V}\frac{{\stackrel{\_}{z}}_{R_i}-{\stackrel{\_}{z}}_T}{\Delta {\stackrel{\_}{P}}_i^V}\right]& \left(\mathrm{Eq}.136\right)\end{array}$

The measurement is therefore

{tilde over (ρ)}_i= P_i+H_ρiδΔP_i^V+ν_ρ  (Eq. 137)

For the reduced order state space defined in Eq. (119), the equivalent measurement function is

$\begin{array}{cc}{\tilde{\rho }}_i={\stackrel{\_}{\rho }}_i+H_{\rho i}\left[\begin{array}{cc}I& -2C_{{\stackrel{\_}{B}}_R}^V\end{array}\left[L_{R_{\mathrm{INS}},R_i}^{B_R}\times \right]\right]\left[\begin{array}{c}\mathrm{\delta \Delta }P^V\\ \delta q\end{array}\right]+v_{\rho }={\stackrel{\_}{\rho }}_i+H_{\rho i}H_e\delta x+v_{\rho }& \left(\mathrm{Eq}.138\right)\end{array}$

The quaternion measurement poses a problem in that simply converting to Euler angles and differencing the measurement with the a priori estimate is unacceptable because it obscures the inherent order of operations of the angles requires to correctly define the orientation. Further, the angles are difficult to reconcile with the quaternion errors presented earlier. However, using cosine rotation matrices, the quaternion error in the measurement may be defined. The definition of the true cosine rotation matrix in terms of the a priori estimate is defined as:

C _{B T} ^V =C _{B T} ^V(I+2[δq×])  (Eq. 139)

A similar version exists for Euler angle errors, but not the difference of the Euler angles. A measurement of Euler angles can be converted to a cosine rotation matrix with an error in the rotation due to noise.

The measurement matrix as a function of truth becomes:

C _{{tilde over (B)} T} ^V =C _{B T} ^V(I+[ν_q])  (Eq. 140)

In this case, the noise ν_q is assumed to be zero mean Gaussian noise on the Euler angles and small in comparison to the angles values. The vector ν_q is defined as:

$\begin{array}{cc}{v}_q=\left[\begin{array}{c}\begin{array}{c}{v}_{\Phi }\\ v_{\Theta }\end{array}\\ v_{\Psi }\end{array}\right]& \left(\mathrm{Eq}.141\right)\end{array}$

The covariance for ν_q is defined as V_q. Substituting Eq. (139) into Eq. (140) gives a nonlinear expression for the attitude error and noise:

C _{{tilde over (B)} T} ^V =C _{B T} ^V(I+2[δq×])(I+[ν_q×])→C_{{tilde over (B)}T}^VC_V^BT−I=2[δq×]+[ν_q×]+2[δq×][ν_q×]  (Eq. 142)

As can be seen in Eq. (142), the resulting nonlinear definition results in state dependent noise. Two options may be considered for handling this problem. The first is to ignore the cross coupling between the error in the quaternion and the measurement noise. This is not an unreasonable choice since the a priori estimate of the attitude error is zero so should not affect steady state performance. In that case, it is possible to write three equations for the measurement function as:

(C_{{tilde over (B)}T}^VC_V^BT)₃₂=2δq₁+ν_Φ

(C_{{tilde over (B)}T}^VC_V^BT)₁₃=2δq₂+ν_Θ

(C_{{tilde over (B)}T}^VC_V^BT)₂₁=2δq₃+ν_Ψ  (Eq. 143)

The notation (C_{{tilde over (B)}T}^VC_V^BT)_ij indicates the i^th row and j^th column of the 3×3 rotation matrix. The use of Eq. (143) is valid provided that the state dependent noise term in Eq. (142) is ignored. The resulting measurement residual to be processed in a least squares filter is then:

$\begin{array}{cc}\begin{array}{c}{r}_q=\left[\begin{array}{c}\begin{array}{c}{\left(C_{{\tilde{B}}_T}^VC_V^{{\stackrel{\_}{B}}_T}\right)}_{32}\\ {\left(C_{{\tilde{B}}_T}^VC_V^{{\stackrel{\_}{B}}_T}\right)}_{13}\end{array}\\ {\left(C_{{\tilde{B}}_T}^VC_V^{{\stackrel{\_}{B}}_T}\right)}_{21}\end{array}\right]\\ =\left[\begin{array}{cc}0& 2I\end{array}\right]\left[\begin{array}{c}\mathrm{\delta \Delta }P^V\\ \delta q\end{array}\right]+\left[\begin{array}{c}\begin{array}{c}{v}_{\Phi }\\ v_{\Theta }\end{array}\\ v_{\Psi }\end{array}\right]\\ =H_q\delta x+v_q\end{array}& \left(\mathrm{Eq}.144\right)\end{array}$

The measurement matrix H_q is defined as:

H_q=[02I]  (Eq. 145)

Six unknowns thus result. However, note that the rotation matrix in Eq. (142) provides nine equations which may be used to solve for the six unknowns. The set of equations is:

$\begin{array}{cc}C_{{\tilde{B}}_T}^VC_V^{{\stackrel{\_}{B}}_T}-I=[\begin{array}{ccc}\begin{array}{c}-v_{\Psi }\delta q_3-v_{\Theta }\delta q_2\\ \delta q_3+v_{\Psi }+v_{\Theta }\delta q_1\\ -\delta q_2-v_{\Theta }+v_{\Psi }\delta q_3\end{array}& \begin{array}{c}-\delta q_3-v_{\Psi }+v_{\Phi }\delta q_2\\ -v_{\Psi }\delta q_3-v_{\Phi }\delta q_1\\ \delta q_1-v_{\Phi }+v_{\Psi }\delta q_2\end{array}& \begin{array}{c}\delta q_2+v_{\Theta }+v_{\Phi }\delta q_3\\ -\delta q_1-v_{\Phi }+v_{\Theta }\delta q_3\\ -v_{\Theta }\delta q_2-v_{\Phi }\delta q_1\end{array}\end{array}]& \left(\mathrm{Eq}.146\right)\end{array}$

These could be solved explicitly. The noise vector may be correlated with the range and angular measurements defined previously which may make the solution to Eq. (146) ambiguous with relationship to the other noise factors. The total measurement equation is:

$\begin{array}{cc}\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}{\tilde{\rho }}_i\\ {\tilde{\alpha }}_i\end{array}\\ {\tilde{\beta }}_i\end{array}\\ Q_{B_{\tilde{T}}}^V\end{array}\right]=\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}{\stackrel{\_}{\rho }}_i\\ {\stackrel{\_}{\alpha }}_i\end{array}\\ {\stackrel{\_}{\beta }}_i\end{array}\\ Q_{B_{\stackrel{\_}{T}}}^V\end{array}\right]+\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}{H}_{\rho i}\\ H_{V\alpha }\end{array}\\ H_{V\beta }\end{array}\\ H_q\end{array}\right]H_e\left[\begin{array}{c}\mathrm{\delta \Delta }P^V\\ \delta q\end{array}\right]+\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}{v}_{\rho }\\ v_{\alpha }\end{array}\\ v_{\beta }\end{array}\\ v_q\end{array}\right]& \left(\mathrm{Eq}.147\right)\end{array}$

Using this set of measurement models, a least squares filter may be calculated that is similar to Eq. (125) so as to estimate the state using silhouette matching.

EO Fault Detection

A methodology for utilizing the vision system to detect failures in the measurements is presented. The methodology takes advantage of the least squares estimator structure presented previously. In this case, it is assumed that one of the reference points is invalid and the goal is to find the invalid measurement. The cause of the measurement is unknown as well as the signal.

Given a set of possibly nonlinear measurements of the form:

{tilde over (y)} _i =f(x)+ν_i  (Eq. 148)

where the measurement {tilde over (y)}_i is a nonlinear function of the state x and additive noise ν_i, then the generalized least squares fault detection problem is of the form:

{tilde over (y)} _i = y _i +H _i δx+ν _i+μ_i  (Eq. 149)

In this case, the measurement {tilde over (y)}_i is the ith measurement, the a priori estimate of the measurement is y_i=f( x), the associated state error δx is the first order perturbation of the error in y_i with the measurement matrix H_i defined as:

$\begin{array}{cc}{H_i=\frac{\partial y}{\partial x}}_{\stackrel{\_}{x}}& \left(\mathrm{Eq}.150\right)\end{array}$

The state x is the a priori estimate and the measurement matrix is linearized around this estimate. The measurement is corrupted by noise ν_i and an unknown fault μ_i may exist in the ith measurement. It is assumed that with N measurements, only one fault exists.

Given the N measurements and no fault, the optimal least squares solution to the problem in Eq. (149) is given by:

δ{circumflex over (x)}=(H^TV⁻¹H)⁻¹H^TV⁻¹({tilde over (y)}− y)  (Eq. 151)

In this case, the noise matrix V represents the covariance matrix of all of the measurements. The state is updated as

{circumflex over (x)}= x+δ{circumflex over (x)}  (Eq. 152)

The residual is the difference between the measurement and the a priori estimate. If a fault occurs, then the residual has takes the form:

r=y− y=Hδx+ν+μ  (Eq. 153)

In this case, μ is considered to be a column vector of zeros except for the fault signal μ_i on the ith row representing a fault in the ith measurement.

An annihilator may be constructed such that the effect of the error in the state estimate is annihilated from the residual. A common annihilator is:

D=I−H(H^TH)⁻¹H^T  (Eq. 154)

The annihilator is designed such that DH=0. The existence of the annihilator assumes that (H^TH)⁻¹ exists. This condition essentially requires that the state is fully observable by the set of measurements. It also implies that the measurement matrix H has full column rank. Using the annihilator on the residual produces:

Dr=D({tilde over (y)}− y)=D(Hδx+ν+μ)=D(ν+μ)  (Eq. 155)

To be an effective fault detection method, more measurements than state errors should be present. In other words, the measurement matrix H has full column rank and more rows than columns. If this is the case, then the annihilator may be decomposed using a singular value decomposition of the form:

$\begin{array}{cc}D=\left[\begin{array}{cc}{U}_1& U_2\end{array}\right]\left[\begin{array}{cc}I& 0\\ 0& 0\end{array}\right]\left[\begin{array}{c}{V}_1\\ V_2\end{array}\right]& \left(\mathrm{Eq}.156\right)\end{array}$

The identity matrix I is a square matrix of dimension equal to the state vector. Using this generalized form, a new residual may be constructed from the old one

{tilde over (r)}=U ₁ ^T r=V ₁(ν+μ)  (Eq. 157)

If it is assumed that the noise ν is a zero mean, Gaussian distributed random variable with covariance V, then the new residual has a Gaussian probability density function (assuming no fault) which is given by:

$\begin{array}{cc}{f}_{\tilde{r}}\left(\tilde{r}\right)=\frac{1}{{\left(2\pi \right)}^{11/2}{{\Lambda }_r}^{1/2}}{}^{\left(-\frac{1}{2}\left({\tilde{r}}^T{\Lambda }_r^{-1}\tilde{r}\right)\right)}& \left(\mathrm{Eq}.158\right)\end{array}$

Referring back to FIG. 3, residual generator 300 generates the residuals having the probability densities shown in Eq. (158). For this relationship, the covariance is given by:

Λ_r=V₁(V)⁻¹V₁  (Eq. 159)

The value of n is the dimension of {tilde over (r)}.

If a fault is present, then the residual statistical properties of the residual will not follow Eq. (158) so that a means of testing the residual set against a probability ratio test such as the MHSSPRT discussed previously is formed. Such a test may be carried out in residual processor 305 of FIG. 3.

For more than one fault, the measurement sets are subdivided into subsets. Each subset contains all of the measurements except one. Each subset is utilized to compute a separate estimate of the state. The residual is then tested using, for example, a MHSSPRT method. If a fault occurs in one measurement, then all of the subsets which incorporate that measurement will be affected and the respective statistical properties of the residuals will no longer be zero mean Gaussian. However, the subset without the fault will remain zero mean Gaussian effectively identifying the faulty measurement. The measurement is then excluded from the measurement set and new subsets are created for the remaining measurements until too few measurements remain to provide any analytic redundancy. A subset may be created to test the faulty measurement and reintroduce it when the fault disappears, detected when the residual process is again zero mean and Gaussian.

INS Fault Detection

Detecting failures in the INS is more subtle than detecting faults in the GPS and EO measurements. The fundamental measurements of an INS are angular velocity and acceleration in the vehicle's local body frame whereas the EO and GPS each measure relative position in a common coordinate frame. The EO system operates in the tanker camera frame and the GPS system operates in the Earth Centered Earth Fixed reference frame. Therefore, the only way to detect IMU instrument failures is through the dynamics of the inertial navigation system. This may be done separately for each IMU or through the GEKF.

INS Fault Detection through the GEKF

The GEKF may be extended to include INS faults. To do so, Eq. (1.1.7) is modified as follows:

δ{dot over (x)}=Aδx+Bw+fμ  (Eq. 160)

The error in the navigation state defined as:

$\begin{array}{cc}\delta x=\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}\begin{array}{c}\begin{array}{c}\delta P^E\\ \delta V^E\end{array}\\ \delta q\end{array}\\ \delta b_{\omega }\end{array}\\ \delta b_f\end{array}\\ c\mathrm{\delta \tau }\end{array}\right]& \left(\mathrm{Eq}.161\right)\end{array}$

where the perturbed error δx consists of 18 states: a position error δP^E in the ECEF frame, a velocity error δV^E, an attitude error δq, a bias in the rate gyros δb_ω, a bias in the accelerometers δb_f, and a bias in the GPS clock measurements cδτ which includes bias, drift, and clock acceleration terms. The errors are influenced by the noise in the inertial navigation system defined as Bw to be a zero mean Gaussian random variable with covariance W. A fault signal μ is introduced through a fault direction matrix F. A different fault matrix F is defined for each fault type, whether it is an x, y, z accelerometer, or an x, y, z gyro. Once the fault matrix is defined a separate Kalman filter is defined for each fault type to be tested.

The measurements take the form:

{tilde over (ρ)}= ρ+Hδx+ν  (Eq. 162)

This model assumes that all of the GPS measurements are healthy. The measurement matrix H includes the lever arm distance from the IMU to the GPS antenna.

Discrete Time Fault Detection

A filter is designed to provide the best estimate of the state δx and then block the effect of a failure μ on the state estimation process. The failure signal is not known, but the direction that the failure enters the system is defined by the matrix f in the dynamics of Eq. (160). The filter processes the GPS measurements in a means similar to the Kalman filter for a single vehicle GPS/INS system. However, the filter blocks the effect of the fault on the estimation process and provides a fault-free residual process with which to test against other filters which may not be tuned to block the particular fault. A filter that is not tuned to block the failure will exhibit non-Gaussian behavior while a filter tuned to block a particular failure will have a residual that remains Gaussian in the presence of the failure.

The discrete time fault detection filter of the form presented in Eq. (160) with measurements defined in Eq. (162) takes the following recursive form. First the dynamics and fault model to a discrete time form. The following approximations may be utilized for a generic time step Δt:

$\begin{array}{cc}\Phi ={}^{A\Delta t}\approx I+A\Delta t& \left(\mathrm{Eq}.163\right)\\ \Gamma =\left(I\Delta t+\frac{1}{2}A\Delta t^2\right)B& \left(\mathrm{Eq}.164\right)\\ F=\left(I\Delta t+\frac{1}{2}A\Delta t^2\right)f& \left(\mathrm{Eq}.165\right)\end{array}$

The new dynamics are then:

δx(t+Δt)=Φδx(t)+Γw+Fμ  (Eq. 166)

The state update thus becomes

δ{circumflex over (x)}=δ x+K({tilde over (ρ)}− ρHδ x)  (Eq. 167)

The gain K is selected in order to minimize the following cost function:

$\begin{array}{cc}\underset{\mu }{\max }\underset{x\left(0\right)}{\max }\frac{1}{2}\sum _0^{k\Delta t}\left(\begin{array}{c}{\tilde{\rho }-\stackrel{\_}{\rho }-H\delta \stackrel{\_}{x}}_{V^{-1}-}\\ {\mu }_{Q_F}^2-{\delta x-\delta \stackrel{\_}{x}}_{Q_s}\end{array}\right)-\frac{1}{2}{x\left(0\right)-\delta \stackrel{\_}{x}\left(0\right)}_{{\Pi }_0}& \left(\mathrm{Eq}.168\right)\end{array}$

The gain K may thus be constructed using the fault detection update defined as:

K=ΠH ^T(R+HΠH^T)⁻¹  (Eq. 169)

The value for R is a weighted version of the measurement noise covariance and is defined as

R=V ⁻¹ −HQ _s H ^T  (Eq. 170)

The matrix Q_s is a design parameter matrix designed to improve filter performance. Values must be chosen carefully in order to maintain positive definiteness. The a priori covariance Π is updated with the measurements using the following update formula:

M=Π−ΠH ^T(R+HΠH^T)⁻¹HΠ  (Eq. 171)

The state error is propagated forward in time using Eq. (166), unless the correction is applied to the state of the vehicle in which case the state error is zeroed after each correction. The covariance is propagated forward in time using:

$\begin{array}{cc}\Pi \left(t+\Delta t\right)=\Phi M{\Phi }^T+\frac{1}{\gamma }{\mathrm{FQ}}_FF^T+\Gamma W{\Gamma }^T& \left(\mathrm{Eq}.172\right)\end{array}$

This propagation is similar to a standard Kalman filter with the covariance modified by the discrete time dynamics Φ with additive process noise W. The additional term essentially “blocks” the fault direction F in the covariance, increasing the uncertainty in that direction by a weighting factor Q_F in order to force the filter to accept that there is larger uncertainty in that direction due to the possible existence of a failure. The weighting term γ is sometimes used as a design parameter to maintain reasonable filter performance, but is set to one for these sets of experiments.

Using this filter, the residual may be tested for failures. The posteriori residual is formed as:

{circumflex over (r)}={tilde over (ρ)}−{circumflex over (ρ)}  (Eq. 173)

A projector P is constructed to annihilate the effect of the state estimation error on the measurement residual, (PH=0). The annihilator has a null space associated with the fault direction. The matrix P may be decomposed as with other filter structures presented into the following form:

$\begin{array}{cc}P=\left[\begin{array}{cc}{U}_1& U_2\end{array}\right]\left[\begin{array}{cc}I& 0\\ 0& 0\end{array}\right]\left[\begin{array}{c}{V}_1\\ V_2\end{array}\right]& \left(\mathrm{Eq}.174\right)\end{array}$

The identity matrix I is a square matrix of dimension equal to the state vector. Using this generalized form, then a new residual may be constructed from the old one

{tilde over (r)}=U ₁ ^T {circumflex over (r)}=V ₁(ν+HFμ)  (Eq. 175)

If it is assumed that the measurement noise ν is a zero mean, Gaussian distributed random variable with covariance V, then the new residual has a Gaussian probability density function (assuming no fault) which is given by:

$\begin{array}{cc}{f}_{\tilde{r}}\left(\tilde{r}\right)=\frac{1}{{\left(2\pi \right)}^{11/2}{{\Lambda }_r}^{1/2}}{}^{\left(-\frac{1}{2}\left({\tilde{r}}^T{\Lambda }_r^{-1}\tilde{r}\right)\right)}& \left(\mathrm{Eq}.176\right)\end{array}$

In this case the covariance is given by:

Λ_r=V₁(V)⁻¹V₁  (Eq. 177)

The value of n is the dimension of {tilde over (r)}.

As discussed previously, the residual may be tested using the MHSSPRT method defined previously. A filter designed to block a particular failure will have a residual that remains zero mean and Gaussian even in the presence of a failure. However, other failures will exhibit non-Gaussian behavior indicating that a failure has occurred. The filter designed to block the failure remains healthy and the operation continues without interruption while other filters are discarded as having processed faulty data. The residual is used to detect and isolate the failure and the filter designed to block the failure is utilized to continue the estimation process in the even of the failure. Therefore, a bank of fault detection filters 300 may be used as shown in FIG. 3. Each filter is tuned to block one of the accelerometer axes or rate gyro axes. Six filters are required in addition to the healthy filter which assumes no fault. When a failure occurs all of the filters will show non-Gaussian statistics except for the filter tuned to block the failure. This filter will remain healthy despite the failure and can be used to continue estimation.

As seen in FIG. 3, a fault reconstruction process 310 identifies the fault magnitude introduced by, for example, a particular accelerometer of rate gyroscope. Fault tolerant estimator 315 would thus comprise a GEKF that excludes measurements from the faulty sensor. In this fashion, an automated control of a refueling boom is enabled despite IMU faults, GPS faults, EO faults, or faults in other sensors such as RF ranging sensors.

It will be obvious to those skilled in the art that various changes and modifications may be made without departing from this invention in its broader aspects. The appended claims encompass all such changes and modifications as fall within the true spirit and scope of this invention.

Claims

1. A system for automated control of a refueling boom coupled to a tanker aircraft, comprising: a first inertial measurement unit (IMU) providing inertial measurements for the tanker aircraft;a first GPS receiver providing a GPS location for a GPS antenna attached to the tanker aircraft; anda processor adapted to calculate a first inertial navigation state for the tanker aircraft through integration of the inertial measurements, the processor being further adapted to calculate a first inertial navigation state error relative to the GPS location and to filter the first inertial navigation state error and the first inertial navigation state based upon noise characteristics of the first IMU and the first GPS receiver to provide an updated inertial navigation state for the tanker aircraft, the processor being further adapted to control the refueling boom relative to a receiver aircraft based upon the first and updated inertial navigation states.

2. The system of claim 1, wherein the processor is adapted to calculate the first inertial navigation error by translating an IMU location from the first inertial navigation state to the GPS location.

3. The system of claim 1, wherein the processor is adapted to filter the first inertial navigation state using a Kalman filter.

4. The system of claim 3, wherein the Kalman filter is an extended Kalman filter.

5. The system of claim 1, further comprising: a second IMU providing inertial measurements the receiver aircraft; anda second GPS receiver providing a GPS location for a GPS antenna attached to the receiver aircraft; wherein the processor is further adapted to integrate the inertial measurements from the second IMU to provide a second inertial navigation state for the receiver aircraft, and wherein the processor is further adapted to calculate a second inertial navigation state error relative to the GPS location for the second GPS receiver and to filter the second inertial navigation state error and the second inertial navigation state based upon noise characteristics of the second IMU and the second GPS receiver to provide an updated inertial navigation state for the receiver aircraft, the processor being further adapted to control the refueling boom relative to a receiver aircraft based upon the first inertial navigation state, the second inertial navigation state, and the updated inertial navigation states.

6. The system of claim 5, wherein the processor is adapted to calculate the second inertial navigation error by translating an IMU location on the receiver aircraft from the second inertial navigation state to the GPS location for the GPS antenna on the receiver aircraft.

7. The system of claim 6, further comprising an electro-optic (EO) sensor adapted to image a plurality of reference points on the receiver aircraft, wherein the processor is further adapted to control the refueling boom relative to errors determined through comparison of (EO) sensor-derived locations for the reference points to second-inertial-navigation-state-derived locations for the reference points.

8. The system of claim 5, wherein the processor comprises at least a first processor on the tanker aircraft and a second processor on the receiver aircraft.

9. The system of claim 8, further comprising a wireless link between the receiver aircraft and the tanker aircraft adapted to permit the first and second processors to exchange positional information to aid the control of the refueling boom.

10. The system of claim 5, further comprising: an RF ranging sensor adapted to measure a range between the receiver aircraft and the tanker aircraft, wherein the processor is further adapted to control the refueling boom based upon the measured range.

11. A system for control of a refueling boom coupled to a tanker aircraft, comprising: an IMU providing tanker inertial measurements for the tanker aircraft;an IMU providing receiver inertial measurement for a receiver aircraft; andmeans for controlling the refueling boom so as to automatically mate with the receiver aircraft, wherein the means is adapted to control the refueling boom by filtering the tanker and receiver inertial measurements relative to measurements from a sensor selected from the group consisting of a GPS receiver and an EO sensor.

12. The system of claim 11, wherein the sensor is a tanker aircraft GPS receiver and a receiver aircraft GPS receiver.

13. The system of claim 12, wherein the means is adapted to filter the tanker and receiver inertial measurements through integration of the tanker and receiver inertial measurements to provide tanker and receiver inertial navigation states.

14. The system of claim 13, wherein the means is further adapted to calculate tanker and receiver inertial navigation errors through comparison of the tanker and receiver inertial navigations states to positions derived from the tanker aircraft GPS receiver and the receiver aircraft GPS receiver.

15. The system of claim 11, wherein the means is further adapted to control fuel transfer from the tanker aircraft to the receiver aircraft.