Abstract:
A target tracking method uses sensor(s) producing target signals subject to positional and/or angular bias, which are updated with sensor bias estimates to produce updated target-representative signals. Time propagation produces time-updated target states and sensor positional and angular biases. The Jacobian of the state dynamics of a target model produces the state transition matrix for extended Kalman filtering. Target state vector and bias covariances of the sensor are time propagated. The Kalman measurement residual is computed to produce state corrections, which are added to the time updated filter states to thereby produce (i) target state updates and (ii) sensor positional and angular bias updates. The covariance of a state vector comprising target states and sensor positional and angular biases is propagated, producing measurement updated state covariance including (i) target position and velocity measurement covariance updates and (ii) the sensor positional and angular bias measurement covariance updates.

Description:
This invention was made with Government support under Contract N00024-03-C-6110 awarded by the Department of the Navy. The Government has certain rights in this invention. 

   FIELD OF THE INVENTION 
   This invention relates to the tracking of targets by the use of sensors which may have positional and or angular bias or misalignment, and processing of information to produce unbiased estimates of target states. 
   BACKGROUND OF THE INVENTION 
   Collaborative sensor coordination among the systems of a System of Systems (SOS) is currently being pursued by the Missile Defense Agency (MDA) to enhance both targeting and cueing accuracies in support of ballistic missile countermeasures or defense. The Navy is deploying its Cooperative Engagement Coordination (CEC) system which is intended to enable Aegis destroyers to pass tactical data among or between elements of the battle group. The MDA will require sensor coordination in order to provide effective and layered tactical and strategic missile defense in a missile defense System of Systems (SOS). Collaborative sensor coordination requires each element of a Missile Defense System to register its sensor(s) to local geodetic coordinate systems in order to minimize tracking and guidance errors, thereby reducing system handover and guidance errors between the target tracking and/or cueing systems and the interceptor(s). This “sensor registration” ultimately provides additional margin to the weapon system&#39;s pointing and divert error budgets, which in turn expands the battle space and enhances the overall warfare capability. 
     FIG. 1  illustrates a scenario  10  in which a first ship  1  and a second ship  2  lie at distances from a land mass  3 . Item  6  represents the horizon. A communication satellite  4  is illustrated, and can communicate with both ships  1  and  2  by way of paths illustrated as “lightning bolts”  4   a  and  4   b . Positional calibration measurements for ships  1  and  2  can be provided by global positioning system (GPS) signals  5   s  flowing from GPS satellites such as  5   a  and  5   b , which follow various orbital paths, illustrated together as  5   t . In the scenario  10  of  FIG. 1 , a hostile missile or target  12  is launched from a location  12   s  on land mass  3 , and follows a path or trajectory  12   t . In this scenario, defensive ship  2  is located closer to the missile launch site  12   s  than ship  1 , and it acquires a sensor track earlier than ship  1 . In this case, the sensor target track may use information from radar or infrared sensors. The target track information generated by ship  2  may be communicated to ship  1  by way of communications satellite  4 , or it may be communicated by a direct path illustrated as  14 . The sensors aboard ship  1  can fuse the data provided from the sensors aboard ship  1  with data from ship  2  to aid in acquiring its own track of the target missile  12 . Ship  1  can then proceed to fire a weapon at the target missile  12 . In the scenario  10  of  FIG. 1 , the weapon is an antimissile vehicle  16 . Antimissile vehicle  16  follows a track  16   t  to intercept the target missile  12  at an intercept location  18 . 
   Current technology in multisensor data fusion assumes that sensor and system bias registration techniques can be either (a) accounted for through covariance inflation techniques or (b) mitigated through use of ‘buffer states.’ The Cooperative Engagement Capability (CEC) System developed by Johns Hopkins University/Applied Physics Lab (JHU/APL), is an example of the covariance inflation technique. An example of the buffer state mitigation technique is described in U.S. Pat. No. 7,026,980 entitled MISSILE IDENTIFICATION AND TRACKING SYSTEM AND METHOD (MDOTS) and issued Apr. 11, 2006 in the name of Mavroudakis et al., This method uses the Unified Unbiased Rocket Equation Extended Kalman Algorithm (UUREEKA) described in U.S. patent application Ser. No. 10/972,943 entitled Computerized Method for Generating Low-Bias Estimates of Position of a Vehicle From Sensor Data, filed on Oct. 25, 2004 in the name of Boka et al. These techniques may under certain circumstances result in less-than-optimal fused track states attributable to sensor registration bias error. 
   The current art in sensor bias registration methods can be categorized into either real-time and non-real-time, and can alternatively be categorized as angular bias methods and positional bias methods. ARCHER, developed by Computer Science Corporation (CSC) and System Calibration Using Satellites (SCUS), developed by Lockheed Martin (LMCO), are examples of non-real-time method for angular registration bias estimation. Both of these methods make use of data in the form of satellite ephemeris to provide a reference which is used to estimate the angular bias error. SCUS and Instantaneous Sensor Alignment Auto-Calibration (ISAAC) described in U.S. patent application Ser. No. 11/149,692, filed Jun. 10, 2005 in the name of Boka et al are examples of angular bias registration methods. Sensor positional bias registration error amelioration or correction is described in U.S. patent application Ser. No. 11/504,561 and entitled “Method for Compensating for the Positional Errors of a Sensor,” (GPSLess) filed on or about Aug. 14, 2006 in the name of Mookerjee et al. 
   Improved or alternative sensor registration techniques and or methods are desired. 
   SUMMARY OF THE INVENTION 
   A method according to an aspect of the invention is for target tracking. The method comprises the step of sensing a target to be tracked with at least one sensor, which sensor or sensors is/are subject to positional bias and angular bias. The sensor(s) produce target representative signals subject to sensor positional and angular biases. The sensor positional bias and the sensor angular bias are updated with any sensor positional bias updates and sensor angular bias updates, respectively, to thereby produce sensor positional-bias and angular-bias updated target representative signals. State time propagation is performed, including propagation of at least the sensor positional and angular bias and the target positional and velocity states of the sensor positional-bias and angular-bias updated target representative signals to produce time updated target states and sensor positional and angular biases. The Jacobian is computed of the state dynamics of a target model to produce the state transition matrix for extended Kalman filtering. Covariance time propagation is performed by time propagating the covariance of a state vector comprising at least position and velocity of the target and positional and angular bias of the sensor, to thereby produce time updated state covariance. A determination is made as to whether a sensor measurement update is available. If the sensor measurement update is not available, the steps of time propagating the state estimates, computing the Jacobian, and time propagating the state covariance matrix are repeated. If the sensor measurement update is available, the Kalman filter gain is determined and the Kalman measurement residual is computed. The Kalman measurement residual is weighted with the gain to produce state corrections, and the state corrections are added to the time updated target states and sensor positional and angular biases, to thereby produce updates of 
   (i) target position and velocity state estimates, and 
   (ii) sensor positional bias updates and sensor angular bias state estimates. 
   Using the Kalman filter gain, updates are made to the state covariance matrix which consists of 
   (i) target position and velocity measurement covariance updates, and 
   (ii) sensor positional bias measurement covariance updates and sensor angular bias measurement covariance updates. 
   In an advantageous mode, the method further comprises the step of transformation of coordinates of the state vector and state covariance of the Kalman filter. The transformation of coordinates may be between the sensor bias filter frame and an unbiased filter frame. 
   A method according to another aspect of the invention is for compensating for the positional and angular alignment errors of a sensor tracking a target with known acceleration. This aspect of the method comprises the step of defining the estimator state given by 
             s   _     =     {             X   _       E   ~                     X   .     _       E   ~                 δ   ⁢       R   _       E   ~                   δ   ⁢     θ   _             }           
and comprising the target position X {tilde over (E)} , target velocity {dot over (X)} {tilde over (E)} , and the positional bias δ R   {tilde over (E)}  and angular bias δ θ  of the sensor. This aspect of the method also comprises the step of applying to the sensed information any sensor positional bias update information and angular bias information, to produce updated sensed information. This updated sensed information ultimately provides improved target state information. State estimates are propagated from the previous time to the current time. From the Jacobian, the state transition matrix is computed for the extended Kalman filter algorithm. Using the state transition matrix, the covariance of a state vector is time propagated, comprising the position and velocity states of the target and the positional bias of the sensor
 
   In this advantageous mode, the step of propagating the state of the updated sensed information to produce time updated state estimates of the target position may further comprise the step of calculating a nonlinear propagation equation making use of the Jacobian matrix. Also in this advantageous mode, the step of propagating the state of the updated sensed information to produce time updated state estimates of the target position may further comprise the step of generating a Jacobian matrix which provides observability of the sensor positional bias or error and angular bias (registration error) through gravitation and coriolis forces. Further, the step of time propagating the covariance of the state vector of the target may comprise the step of time propagating a composite state vector, where the composite state vector comprises the position and velocity states of the target and the position bias and angular bias of the sensor. The advantageous mode may further comprise, after the step of time propagating the covariance of a state vector comprising the position and velocity states of the target, the step of determining if target position information is available, and if target position is available, performing the further steps of calculating gain of the Kalman filter to generate Kalman filter gain. This advantageous mode may further comprise, after the step of calculating gain of the Kalman filter to generate Kalman filter gain, the step of generating updates of the state estimates of the target and of the covariance of the states. 
   A method for estimating the position of a target with the aid of a sensor the position and angular orientation is not known accurately according to another aspect of the invention comprises the steps of operating the sensor to generate sensed information relating to the position of the target, and adding to the sensed data any sensor positional bias update information and sensor angular bias information to produce updated sensed information. The state of the updated sensed information is propagated in time to produce time updated state estimates of the target position and velocity. The Jacobian of the state dynamics of the target is computed. The state transition matrix for the extended Kalman filter algorithm is computed. The covariance of a state vector comprising the position and velocity states of the target is propagated in time to thereby produce positional error information and angular bias information relating to the target tracking. 
   A method for estimating the position of a sensor and the angular bias orientation of a sensor according to another aspect of the invention comprises the steps of operating a sensor to generate sensed data relating to a target, which data is contaminated by sensor positional bias errors and angular bias registration errors, and adding to the sensed data any sensor positional bias update information and sensor angular bias update information to produce updated sensed information. The state of the updated sensed information is propagated in time to produce time updated state estimates of the target position and velocity. The Jacobian of the state dynamics of the target is computed. The state transition matrix for the extended Kalman filter algorithm is computed. The covariance of a state vector comprising the position and velocity states of the target is time propagated. 
   A method for estimating the error in the position of a sensor and the angular bias orientation of the sensor according to another aspect of the invention comprises the steps of operating a sensor to generate data relating to a target, which data is contaminated by sensor positional bias errors and angular bias registration errors. The method comprises the steps of operating the sensor to generate sensed information relating to the target, and adding to the sensed data any sensor positional bias update information and sensor angular bias update information to produce updated sensed information. The state of the updated sensed information is propagated in time to produce time updated state estimates of the target position and velocity. The Jacobian of the state dynamics of the target is computed. The state transition matrix for the extended Kalman filter algorithm is computed. The covariance of a state vector comprising the position and velocity states of the target is propagated in time to thereby produce positional error information relating to the target. 

   
     BRIEF DESCRIPTION OF THE DRAWING 
       FIG. 1  is a representation of a scenario in which a target is tracked by two or more spaced-apart sensors; 
       FIG. 2  is a notional representation of a system according to an aspect of the invention; 
       FIG. 3  is a representation of transformations among stable and biased coordinate systems; 
       FIG. 4  is a functional block diagram illustrating operation of a system according to an aspect of the invention. 
   

   DESCRIPTION OF THE INVENTION 
   It would be desirable to be able to perform sensor tracking and fusion with combined angular and positional registration bias estimation capability independent of supplemental inputs such as GPS and or satellite ephemeris. According to an aspect of the invention, a multi-sensor measurement fusion method produces fused ballistic inertial target track state estimates nominally free of navigational errors attributable to angular and positional sensor registration biases. A method according to an aspect of the invention, given the name Unified Navigation and Inertial Tracking Estimation System (UNITES), may be implemented with the aid of a computer algorithm. The method incorporates an extended Kalman filter algorithm incorporating sensor registration bias scheme requiring no ephemeris or GPS inputs (although they may be used if desired). With sufficient computational resources, the sensor registration bias aspect operates in real time and provides real-time or nominally instantaneous estimate of the angular and positional sensor registration bias errors. The result is that the target is tracked with reduced errors attributable to sensor positional and angular biases. This improved tracking, in turn, allows the target to be attacked with a greater likelihood of success. 
   Angular sensor bias registration has been addressed through state augmentation, as described in U.S. patent application Ser. No. 11/149,692, filed Jun. 10, 2005 in the name of Boka et al. and entitled “Instantaneous Multisensor Angular Bias Autoregistration” (ISAAC), and “System Calibration using Satellites (SCUS)” represented by U.S. Pat. No. 5,729,234, entitled “Remote alignment system” issued Mar. 17, 1998 in the name of Stetson et al. Sensor positional bias registration error amelioration or correction is described in U.S. patent application Ser. No. 11/504,561 and entitled “Method for Compensating for the Positional Errors of a Sensor,” (GPSLess) filed on or about Aug. 14, 2006 in the name of Mookerjee et al. 
   The method of the present invention in effect merges the capabilities of both ISAAC and GPSLess by new algorithms which enable the angular and positional sensor registration bias errors to be observed separately.  FIG. 2  is a simplified notional representation  200  of the uniting of ISAAC with GPSLess to produce an improved UNITES result. In  FIG. 2 , representation  200  includes a source, illustrated as a block  210 , of multi-sensor information representing information about the target(s). This information may be updated occasionally or periodically. The information is made available from sensor block  210  to the Unified Navigation and Inertial Estimation System (UNITES), illustrated as a block  212 . UNITES block  212  includes fusion and estimation algorithms in a block  214 . The fusion and estimation algorithms fuse or combine some the angular registration bias features of provided by ISAAC  216  with positional registration bias features of GPSLess  218 . These fused or combined features are combined with sensor registration processes illustrated as a block  220  and with target track state processing illustrated as a block  222  to produce (a) multisensor positional bias registration bias estimates, (b) multisensor angular registration bias estimates, and (c) target track state and covariance estimates. The multisensor positional bias registration bias estimates, multisensor angular registration bias estimates, and target track state and covariance estimates are, in turn, used by combat and weapon systems (not illustrated), such as those aboard ships  1  and or  2  of  FIG. 1 , to mitigate the effects of sensor registration biases in the overall fire control process. 
     FIG. 3  represents coordinate frame topology and vector definitions for analysis, depicted or couched in terms of true and nominal (biased) coordinate frame topologies. These conventions are consistent with Aegis sea-based systems without loss of generality. In  FIG. 3 , starting at the left end of the upper (Truth) transformation line, the E frame represents the stable space frame, that is, Earth-Centered Earth-Fixed (ECEF), East-North Up (ENU), or Earth-Centered Inertial (ECI). The earth&#39;s angular velocity vector ω E  is known exactly in this frame. The estimate  {circumflex over (R)}   E  of the sensor location vector  R   E  is known in this frame, where the underscore represents a vector quantity, and the overcaret (^) represents an estimate. The estimate  {circumflex over (R)}   E  differs from the actual location or position based on
     R     E   = {circumflex over (R)}     E   +δ {circumflex over (R)}     E   (1) 
The matrix T E   P (t) represents the coordinate transformation from the E frame to the P frame, where (t) denotes a time dependency. The P frame represents the platform frame (e.g. Aegis ship deck frame) that may be moving (e.g. wave motion) relative to the E stable space frame. The matrix T P   S ( θ ) represents the coordinate transformation from the P or platform frame to the S or sensor frame, where  θ  denotes the actual (unknown) rotation angles between P and S. The S sensor frame represents the measurement sensor coordinate system (i.e. measurement vector X S   m  known exactly in this frame). Note that δ {circumflex over (R)}   E  is the estimated sensor registration position bias represented in the E frame.
 
   The bottom line of  FIG. 3 , which is the nominal or biased transformation line, represents the nominal (biased) coordinate reference system transformations. The {tilde over (E)} frame represents the biased stable space frame (e.g. biased ECEF, ENU, or ECI) that a filter would mistakenly work within after transforming the measurement vector X S   m  from the S sensor frame to the expected stable frame E using the nominal coordinate transformation matrices T {tilde over (P)}   S ( {tilde over (θ)} ) and T {tilde over (E)}   {tilde over (S)} (t)(=T E   S (t)) (These last two coordinate frame transformations refer to the transformation between the frames as denoted by the subscript and superscript of each term) where {tilde over (P)} denotes the nominal (biased) platform frame, and the overtilde (˜) represents a bias. Note that the biased platform transformation matrix T P   {tilde over (P)} (δ{tilde over (θ)}) can be represented as a bias in the E frame using the similarity transformation 
   
     
       
         
           
             
               
                 
                   T 
                   E 
                   
                     E 
                     ~ 
                   
                 
                 = 
                 
                   
                     
                       T 
                       P 
                       E 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       T 
                       P 
                       
                         P 
                         ~ 
                       
                     
                     ⁡ 
                     
                       ( 
                       
                         δ 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           
                             θ 
                             _ 
                           
                           ~ 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       T 
                       E 
                       P 
                     
                     ⁡ 
                     
                       ( 
                       t 
                       ) 
                     
                   
                 
               
             
             
               
                 ( 
                 2 
                 ) 
               
             
           
         
       
     
   
   It should be noted that there may be a bias between the E and P frames of  FIG. 3 , which may be taken into account if the bias is significant. If the bias is significant, the UNITES state vector may be augmented to include that bias, as by using a modeling technique similar to that used for  θ . For simplicity of description, however, and without the loss of generality, the transformation from the E frame to the P frame is taken as being un-biased and only time dependent. 
     FIG. 4  is a simplified functional block diagram illustrating the operation of an extended Kalman filter system according to an aspect of the invention. In  FIG. 4 , the process begins with application to an input port  410  of an initial position measurement X S   m  from a sensor (not illustrated in  FIG. 4 ) tracking a ballistic target. In order to develop the filter equations, a model is assumed in which the target is either ballistic (falling under the force of gravity) or else the specific force (i.e. thrust acceleration) is known exactly and can be compensated for. Additionally, it is assumed that atmospheric drag effects are negligible due to the high altitudes at which target tracking occurs, or that atmospheric drag can be properly compensated for. Given these assumptions, the following equations describe the model used for the target kinematics: 
                             X   ¨     _     E     =           -   μ     ⁢       Z   _     E                  Z   _     E          3       +       A   _     E     -         ω   _     E     ×     (         ω   _     E     ×       Z   _     E       )       -     2   ⁢           ⁢       ω   _     E     ×         X   _     .     E                         Z   _     E     =         R   _     E     +       X   _     E                     (   3   )               
where:
 
   μ is the Earth gravitational constant; 
   A E  is the known specific force which includes (but is not limited to) effects such as higher order gravitational effects; and 
   X E  is the sensed target position reported in the E frame. 
   For simplicity of explanation, and without loss of generality, the remaining description omits the A E  term for conciseness and includes only the dominant gravity term μ. 
   The initial sensor-generated position vector X S   m  at time t m , representing the position of the target, is applied from port  410  of  FIG. 4  to an input port  412   i   1  of a measurement position bias update function illustrated as a block  412 . Block  412  sets the initial value of the estimated sensor position vector in the stable space frame ( {circumflex over (R)}   E ) to the nominal bias value ( {tilde over (R)}   E ),  {circumflex over (R)}   E = {tilde over (R)}   E  Measurement position bias update function block  412  also updates the estimated sensor position vector using the sensor position bias error estimate applied to second input port  412   i   2  from the previous UNITES bias estimation event or iteration. Note that block  412  performs this function at every cycle, not just at filter start-up. From block  412 , the logic of system  400  of  FIG. 4  flows to an input  414   i   1  of a measurement angular bias update block  414 . Block  414  initializes the measurement at time t m  using the nominal value for alignment biases  {circumflex over (θ)} = {tilde over (θ)} . The measurement angular bias update function block  414  also receives at its second input port  414   i   2  the sensor alignment bias error estimate δ{tilde over (θ)} from the previous UNITES bias estimation event, and updates the sensor alignment bias δ{tilde over (θ)} for the next cycle. 
   From block  414 , the logic  400  of  FIG. 4  flows to a measurement update or next update cycle function represented by a block  420 . Block  420  determines whether the state and covariance estimates for the current time iteration will include a measurement update. The UNITES function  400  of  FIG. 4  nominally operates at a predetermined cycle rate, with measurement updates occurring asynchronously as they are available. If updates are not available, block  420  merely sends the logic on to a state propagation function represented by a block  422 . 
   The target state vector s is: 
                   s   _     =     {             X   _       E   ~                     X   .     _       E   ~                 δ   ⁢       R   _       E   ~                   δ   ⁢           ⁢     θ   _             }             (   4   )               
State vector s includes target position, velocity, and positional and angular registration biases for all reporting sensors. Position and velocity vectors are referenced relative to the biased stable space frame {tilde over (E)}. The angular registration bias state vector δ θ  is referenced relative to the platform frame P, and the positional registration bias δ R   E  is referenced relative to the biased stable space frame and is constant in that frame.
 
   The dynamics equations (i.e. the nonlinear state derivative equations) are set forth in equations (5). These equations are based on the assumed target kinematics described in relation to equation (3). Additionally, it is assumed that all sensor positional and angular registration biases are constant. 
   
     
       
         
           
             
               
                 
                   
                     s 
                     . 
                   
                   _ 
                 
                 = 
                 
                   
                     { 
                     
                       
                         
                           
                             
                               
                                 X 
                                 _ 
                               
                               . 
                             
                             
                               E 
                               ~ 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 X 
                                 ¨ 
                               
                               _ 
                             
                             
                               E 
                               ~ 
                             
                           
                         
                       
                       
                         
                           
                             δ 
                             ⁢ 
                             
                               
                                 
                                   R 
                                   . 
                                 
                                 _ 
                               
                               
                                 E 
                                 ~ 
                               
                             
                           
                         
                       
                       
                         
                           
                             δ 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               
                                 θ 
                                 _ 
                               
                               . 
                             
                           
                         
                       
                     
                     } 
                   
                   = 
                   
                     { 
                     
                       
                         
                           
                             
                               
                                 
                                   
                                     
                                       
                                         
                                           
                                             X 
                                             _ 
                                           
                                           . 
                                         
                                         
                                           E 
                                           ~ 
                                         
                                       
                                     
                                   
                                   
                                     
                                       
                                         
                                           
                                             
                                               - 
                                               μ 
                                             
                                             ⁢ 
                                             
                                                 
                                             
                                             ⁢ 
                                             
                                               
                                                 Z 
                                                 _ 
                                               
                                               
                                                 E 
                                                 ~ 
                                               
                                             
                                           
                                           
                                             
                                                
                                               
                                                 
                                                   Z 
                                                   _ 
                                                 
                                                 
                                                   E 
                                                   ~ 
                                                 
                                               
                                                
                                             
                                             3 
                                           
                                         
                                         - 
                                         
                                           
                                             
                                               ω 
                                               _ 
                                             
                                             
                                               E 
                                               ~ 
                                             
                                           
                                           × 
                                           
                                             ( 
                                             
                                               
                                                 
                                                   ω 
                                                   _ 
                                                 
                                                 
                                                   E 
                                                   ~ 
                                                 
                                               
                                               × 
                                               
                                                 
                                                   Z 
                                                   _ 
                                                 
                                                 
                                                   E 
                                                   ~ 
                                                 
                                               
                                             
                                             ) 
                                           
                                         
                                         - 
                                         
                                           2 
                                           ⁢ 
                                           
                                             
                                               ω 
                                               _ 
                                             
                                             
                                               E 
                                               ~ 
                                             
                                           
                                           × 
                                           
                                             
                                               
                                                 X 
                                                 . 
                                               
                                               _ 
                                             
                                             
                                               E 
                                               ~ 
                                             
                                           
                                         
                                       
                                     
                                   
                                 
                               
                             
                             
                               
                                 
                                   
                                     0 
                                     _ 
                                   
                                   
                                     3 
                                     × 
                                     1 
                                   
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               0 
                               _ 
                             
                             
                               3 
                               × 
                               1 
                             
                           
                         
                       
                     
                     } 
                   
                 
               
             
             
               
                 ( 
                 5 
                 ) 
               
             
           
         
       
     
   
   From block  420  of  FIG. 4 , the logic  400  flows to a state propagation function illustrated as a block  422 . The state propagation block  422  of logic  400  of  FIG. 4  propagates (in time) the UNITES state vector in equation (4) to produce time updated state estimates of the target position and velocity. Time propagation of the state vector is performed in block  422  by numerically integrating the state derivative vector from the previous time t i-1 , to the current time t i , where the subscript i refers to the filter cycle iteration: 
                       s   _     ^     ⁡     (     t   i     )       =           s   _     ^     ⁡     (     t     i   -   1       )       +       ∫     t     i   -   1         t     i   -   1   +     Δ   ⁢           ⁢   t           ⁢             s   .     ^     _     ⁡     (   τ   )       ⁢           ⁢     ⅆ   τ                   (   6   )               
For the integration process, a high order numerical integration algorithm, such as the 2 nd  order or 4 th  order Runge Kutta algorithm might be used. The incremental time step Δt refers to either the nominal update cycle time or the incremental time step from the last cycle time to the current measurement time t m  (i.e., Δt=t m −t i-1 ) At the first iteration, the state propagation function of block  422  also initializes the state for the subsequent iterations. From state propagation block  422 , the logic of  FIG. 4  flows to a Jacobian computation represented by a block  424 .
 
   The Jacobian computation function  424  of  FIG. 4  computes the Jacobian of the state dynamics of the target, and subsequently computes, from the state dynamics of the target, the state transition matrix for the UNITES extended Kalman filter algorithm. The UNITES Jacobian matrix formulation of the state dynamics provides observability into the sensor alignment bias through the gravitational and coriolis forces. The Jacobian J of the state dynamics equation is given by 
                 J   =       [           ⁢       ∂       s   _     .         ∂     s   _         ]     =       [           ⁢         ∂       s   .     _         ∂       X   _       E   ~           ⁢           ⁢       ∂       s   .     _         ∂         X   _     .       E   ~           ⁢           ⁢       ∂       s   .     _           ∂   δ     ⁢       R   _       E   ~           ⁢           ⁢       ∂       s   _     .           ∂   δ     ⁢     θ   _           ]     =     [           ⁢           0     3   ×   3             I     3   ×   3             0     3   ×   3             0     3   ×   3                   ∂         X   _     ¨       E   ~           ∂       X   _       E   ~                   ∂         X   _       E   ~       ¨         ∂         X   _       E   ~       .                 ∂         X   _     ¨       E   ~             ∂   δ     ⁢       R   _       E   ~                   ∂         X   _     ¨       E   ~             ∂   δ     ⁢     θ   _                   0     3   ×   3             0     3   ×   3             0     3   ×   3             0     3   ×   3                 0     3   ×   3             0     3   ×   3             0     3   ×   3             0     3   ×   3             ⁢           ]                 (   7   )               
where:
 
                     ∂         X   ¨     _       E   ~           ∂       X   _       E   ~           =       -       μ       (           Z   _     ^       E   ~       ·         Z   _     ^       E   ~         )       3   2         ⁡     [       I     3   ×   3       -       3     (           Z   _     ^       E   ~       ·         Z   _     ^       E   ~         )       ⁡     [           Z   _     ^       E   ~       ·         Z   _     ^       E   ~     T       ]         ]         -       〚         ω   ^     _       E   ~       〛     ·     〚         ω   ^     _       E   ~       〛                         ∂         X   ¨     _       E   ~           ∂         X   _     .       E   ~           =       -   2     ·     〚         ω   ^     _       E   ~       〛                       ∂       X   _     ¨           ∂   δ     ⁢       R   _     E         =     0     3   ×   1                         ∂         X   _     ¨       E   ~             ∂   δ     ⁢     θ   _         =         -       μ       (           Z   _     ^       E   ~       ·         Z   _     ^       E   ~         )       3   2         ⁡     [       I     3   ×   3       -       3     (           Z   _     ^       E   ~       ·         Z   _     ^       E   ~         )       ⁡     [           Z   _     ^       E   ~       ·         Z   _     ^       E   ~     T       ]         ]         ·     〚         R   _     ^       E   ~       〛     ·         T   ^       p   ~       E   ~       ⁡     (     t   i     )         -     2   ·     [     [           X   .     ^     _       E   ~       ]     ]     ·     
     ⁢           ⁢     〚         ω   ^     _       E   ~       〛     ·         T   ^       p   ~       E   ~       ⁡     (     t   i     )         +         [         〚         ω   ^     _       E   ~       〛     ·     〚         X   ^     _       E   ~       〛     ·     〚         ω   ^     _       E   ~       〛       +       〚         ω   ^     _       E   ~       〛     ·     〚         R   ^     _       E   ~       〛     ·     〚         ω   ^     _       E   ~       〛       +     
     ⁢           ⁢       〚       〚         ω   ^     _       E   ~       〛     ·         X   ^     _       E   ~         〛     ·     〚         ω   ^     _       E   ~       〛       +       〚       〚         ω   ^     _       E   ~       〛     ·         R   ^     _       E   ~         〛     ·     〚         ω   ^     _       E   ~       〛       -       〚         ω   ^     _       E   ~       〛     ·     〚         ω   ^     _       E   ~       〛     ·     〚         R   ^     _       E   ~       〛         ]     ·         T   ^       p   ~       E   ~       ⁡     (     t   i     )         ⁢           ⁢   and         ⁢     
     ⁢                 R   _     ^       E   ~       =         T   ^     E     E   ~       ·         R   ^     _     E                         ω   ^     _       E   ~       =         T   ^     E     E   ~       ·       ω   _     E                       T   ^     E     E   ~       =         T   P   E     ⁡     (     t   i     )       ·     [       I     3   ×   3       +     〚       θ   _     ^     〛       ]     ·       T   E   P     ⁡     (     t   i     )                             
The            •          notation denotes a skew symmetric matrix of the vector argument.
 
The state transition matrix used for the time propagation of the UNITES error covariance can be approximated including up to 2 nd  order terms:
 Φ≈ I+JΔt+ 0.5 J   2   Δt   2   (8)
   From Jacobian computation block  424 , the logic  400  of  FIG. 4  flows to a block  426 , which represents the use of the state transition matrix to time propagate the covariance of a state vector comprising the position and velocity states of the target. The covariance propagation function performed by block  426  is the traditional Kalman filter time propagation of the state covariance matrix utilizing the state transition matrix. This function will nominally operate faster than the measurement processing to reduce non-linear effects when measurement updates occur at slow update rates. At the first iteration, the covariance propagation function of block  426  also initializes the state covariance for the subsequent iterations. Time propagation of the UNITES error covariance matrix P(t i ) is performed with the following equation:
 
 P ( t   i )=Φ P ( t   i-1 )Φ T   +Q   (9)
 
where:
 
   Q is the UNITES state noise matrix. 
   The state noise matrix Q may be determined using the following equation: 
                 Q   =       ∫   0     Δ   ⁢           ⁢   t       ⁢     Φ   ⁢           ⁢   W   ⁢           ⁢     Φ   T     ⁢           ⁢     ⅆ   τ                 (   10   )               
where:
 
   W=E(w(τ)w(τ) T ); and 
   w(τ) is the 12×1 state noise vector of white noise. 
   From block  426  of  FIG. 4 , the logic  400  flows to a decision block  428 . Decision block  428  determines if a position measurement is currently available. If a measurement is not currently available, then time is incremented by the nominal Δt propagation time rate, the logic leaves decision block  428  by the “No” output, and control is passed by way of a path  430  to block  420  at the start of the iteration loop  406 . If, on the other hand, a measurement is available, then the logic of  FIG. 2  leaves decision block  428  by the “Yes” output, and proceeds to a block  432 , representing a gain computation. 
   The gain computation block  432  of  FIG. 4  constructs the standard Kalman filter gain matrix K using the measurement matrix and the error covariance matrix
 
 K=P ( t   i )· H   T ·( H·P ( t   i )· H   T   +R ) −1   (11)
 
where:
 
 H=[I   3×3  0 3×3    I   3×3  0 3×3 ]  (12)
 
   is the measurement matrix; and 
   R is the measurement noise covariance matrix associated with the currently reporting sensor defined in the stable space frame 
                 R   =           T   ^     P   E     ⁡     (     t   i     )       ·         T   ^     S   P     ⁡     (       θ   ^     _     )       ·   Σ   ·         T   ^     P   S     ⁡     (       θ   ^     _     )       ·         T   ^     E   P     ⁡     (     t   i     )                 (   13   )               
with Σ denoting the measurement noise matrix as represented in the sensor frame.
 
   It should be noted that the gain computation expressed by equations (11) and (12) is applicable only in the case in which the measurement update is of the target position only. If the measurement update includes both target position and target velocity information, the gain of the Kalman filter is generated in accordance with equation (11) with the measurement matrix H redefined as 
                 H   =     [           I     3   ×   3             0     3   ×   3             I     3   ×   3             0     3   ×   3                 0     3   ×   3             I     3   ×   3             0     3   ×   3             0     3   ×   3             ]             (   14   )               
and including the effect of the velocity in the second row.
 
   From gain computation block  432 , the logic of  FIG. 4  flows to a block  434 , representing the state measurement and state residuals update including the sensor registration bias estimate. The state vector is updated using
 
 ŝ=ŝ+K·Δm   (15)
 
where:
 
   the measurement residual Δm is defined as 
                   Δ   ⁢     m   _       =             T   ^     P   E     ⁡     (     t   i     )       ·         T   ^     S   P     ⁡     (       θ   _     ^     )       ·         X   _     S   m     ⁡     (     t   i     )         -     H   ·     s   _                 (   16   )               
The updated state measurements are made available by way of path  440  to sensor positional bias update switch function  416 M and sensor angular bias update switch function  418 M, for inclusion in the next iteration. The updated state measurements produced by block  434  include the updates to the target missile position X {tilde over (E)}  and velocity {dot over (X)} {tilde over (E)}  in the estimated bias frame {tilde over (Ê)}. The updated state measurements also include the update to the sensor biases for position δ R   {tilde over (E)}  and angle δ θ . The updated state measurements are made available by connections to path  440  for use by an antimissile targeting arrangement, not illustrated in  FIG. 4 . Note that, according to an advantageous result of this processing, as the updated angular bias estimates converge to the true bias, the biased frame converges toward the true sensor frame.
 
   From block  434  of  FIG. 4 , the logic  400  flows to a further block  436 , representing the updating of the covariance measurement. The covariance measurement update function of block  436  performs the measurement update of the state covariance matrix
 
 P ( t   i )=( I−K·H )· P ( t   i )·( I−K·H ) T   +K·R·K   T   (17)
 
which is made available by way of path  442  to a block  444  representing a transformation of the state and covariance to an estimated unbiased frame Ê. The state covariance in estimated biased frame {tilde over (Ê)} is also made available for external use for fire control or guidance purposes for missile targeting. The logic flow  400  continues with updating of the time t=tm+Δt, and the logic returns by way of path  438  to block  420  to begin another iteration around loop  406 .
 
   State and covariance transformation block  444  of  FIG. 4  performs transformations of target missile position, velocity, and covariance to the estimated stable space frame {tilde over (Ê)} using equations (18) 
                 X   ^     _       E   .       =       T     E   ~       E   .       ·         X   ^     _       E   ~                             X   .     ^     _       E   .       =       T     E   ~       E   .       ·           X   .     ^     _       E   ~                       P       X   .     ⁢     X   .         =       P       X   ~     ⁢     X   ~         +       〚         X   _     ^       E   ^       〛     ·       T   P     E   ^       ⁡     (     t   1     )       ·     P   θθ     ·       T     E   ^     P     ⁡     (     t   i     )       ·       [     [           X   .     ^     _       E   ^       ]     ]     T       +       T     E   ~       E   ^       ·     P         X   .     ~     ⁢   θ       ·       T     E   ^     P     ⁡     (     t   i     )       ·       [     [         X   ^     _       E   ^       ]     ]     T       +       [     [         X   ^     _       E   ^       ]     ]     ·       T   P     E   ^       ⁡     (     t   i     )       ·     P       X   ~     ⁢   θ     T     ·     T     E   ^       E   ~                         P         X   .     ^     ⁢       X   .     ^         =       P         X   .     ~     ⁢       X   .     ~         +       [     [           X   .     ^     _       E   ^       ]     ]     ·       T   P     E   ^       ⁡     (     t   i     )       ·     P   θθ     ·       T     E   ^     P     ⁡     (     t   i     )       ·       [     [           X   .     ^     _       E   ^       ]     ]     T       +       T     E   ~       E   ^       ·     P         X   .     ~     ⁢   θ       ·       T     E   ^     P     ⁡     (     t   i     )       ·       [     [           X   .     ^     _       E   ^       ]     ]     T       +       [     [           X   .     ^     _       E   ^       ]     ]     ·       T   P     E   ^       ⁡     (     t   i     )       ·     P         X   .     ~     ⁢   θ     T     ·     T     E   ^       E   ~                         where   :     
     ⁢     P   ⁡     (     t   i     )         =     [           P       X   ~     ⁢     X   ~               P       X   ~     ⁢       X   .     ~               P       X   ~     ⁢   δ   ⁢           ⁢   X             P       X   ~     ⁢   θ                 P       X   ~     ⁢       X   .     ~       T           P         X   .     ~     ⁢       X   .     ~               P         X   .     ~     ⁢   δ   ⁢           ⁢   X             P         X   .     ⁢   θ     ~                 P       X   ~     ⁢   δ   ⁢           ⁢   X     T           P         X   .     ~     ⁢   δ   ⁢           ⁢   X     T           P     δ   ⁢           ⁢   X   ⁢           ⁢   δX             P     δ   ⁢           ⁢   X   ⁢           ⁢   θ                 P       X   ~     ⁢   θ     T           P         X   .     ~     ⁢   θ     T           P     δ   ⁢           ⁢   X   ⁢           ⁢   θ     T           P   θθ           ]                     T   ^     E     E   ~       =         T   P   E     ⁡     (     t   i     )       ·     [       I     3   ×   3       +       〚       θ   _     ^     〛     ·       T   E   P     ⁡     (     t   i     )                     
The transformed position, velocity and covariance in the stable space frame are made available by way of paths  446  for use by an antimissile targeting arrangement, not illustrated in  FIG. 4 .
 
   At the end of each iteration through the logic  400  of  FIG. 4 , the sensor position vector estimate  {circumflex over (R)}   E  is updated using  {circumflex over (R)}   E = {circumflex over (R)}   E +δ {circumflex over (R)}   E . At initialization, the sensor position vector assumes the nominal value  {circumflex over (R)}   E = {tilde over (R)}   E . Also at the end of each iteration, the sensor angular bias estimate  {circumflex over (θ)}  is updated using  {circumflex over (θ)} = {circumflex over (θ)} +δ {circumflex over (θ)} . At initialization, the sensor angular bias estimate assumes the nominal value of  {circumflex over (θ)} = {tilde over (θ)} .