Abstract:
A multi-stage maximum likelihood target estimator for use with radar and sonar systems is provided. The estimator is a software implemented algorithm having four computational stages. The first stage provides angle smoothing for data endpoints thereby reducing angle errors associated with tie-down times. The second stage performs a coarse grid search to obtain the initial approximate target state to be used as a starting point for stages  3  and  4 . The third stage is an endpoint Gauss-Newton type maximum likelihood target estimate which determines target range along two time lines. The final refinement of the target state is obtained by the fourth stage which is a Cartesian coordinate maximum likelihood target estimate. The four-stage processing allows the use of target historic data while reducing processing time and computation power requirement.

Description:
STATEMENT OF GOVERNMENT INTEREST 
   The invention described herein may be manufactured and used by or for the Government of the United States of America for Governmental purposes without the payment of any royalties thereon or therefor. 
   BACKGROUND OF THE INVENTION 
   (1) Field of the Invention 
   The present invention relates generally to the field of radar and sonar systems. In particular, the invention employs an algorithm in a process for enhanced target detection and tracking. 
   (2) Description of the Prior Art 
   State of the art combat systems rely heavily on target motion analysis (TMA) subcomponents. A target motion analysis subcomponent estimates the current position and velocity components of a contact. Estimates from target motion analysis are important to combat personnel because the estimate allows the personnel to predict the location of the hostile contact some time in the future. Precise determination of the future position of the contact is required for accurate targeting of weapons systems as well as for defensive maneuvering and evasion of the contact by friendly units. 
   In both radar and sonar detection systems, an antenna array receives a reflected signal. Preliminary processing then occurs and the locations of contacts are generated. An example of this type of processing is disclosed in Chang et al., Active Sonar Range-Beam Partitioner, U.S. Pat. No. 5,737,249 (Filed 7 Apr. 1998). 
   The next stage in processing is to determine range and bearing estimates for each target. Prior attempts have led to two distinct approaches for these determinations. The first approach, (sequential algorithms) uses an averaged measurement to reflect historic information and combines this average in a weighted manner with the most current measurement. This approach yields minimal computational needs due to the small size of the input dataset. Sequential algorithms also can respond quickly to targets that have rapidly varying direction of movement. However, the condensation of all historic measurements into a single set of input numbers results in a great loss in the granularity of the data. Sequential algorithms have not been able to utilize the complete historic dataset to dynamically recompute the output range and bearing as a cache set of input values is received. 
   Batch processing algorithms have developed to meet this precise need. However, batch processing algorithms have also been plagued with a plethora of problems. First, computational requirements have consistently been exceptionally high. As a result, algorithm designers have been limited in the amount of processing steps which could be performed while still providing real time output. In some circumstances, computational needs have been so high as to require limiting the number of individual historic input measurements which are processed. As such, all viable prior attempts have used a single stage algorithm for processing. 
   The first type of algorithm often used is grid searching. The grid search technique divides the target space into a number of cells. Contact range and bearing are computed by detecting movement between cells. In order for this technique to be successful, the resolution of the target grid must be very fine. This fine resolution has resulted in extreme computational power requirements. 
   The second type of algorithm is a stand-alone endpoint coordinate maximum likelihood estimation (MLE). In maximum likelihood estimation, an iterative least-squares technique is used to determine contact range and bearing. However, this approach has been subject to over-sensitivity, especially in cases where iterations on the quadratic solution lead to a divergence rather than a convergence. 
   SUMMARY OF THE INVENTION 
   Accordingly, it is an object of the present invention to provide a software algorithm which provides range and bearing estimates for target acquisition systems. 
   It is a further object of the present invention to minimize computational requirements while processing substantial historic data. 
   It is a still further object of the present invention to maintain a high granularity or resolution in the target field. 
   It is a still further object of the present invention to prevent divergence of the least-squares solution and of the target falses which result from such divergence. 
   In accordance with these and other objects, the invention is a process using a multi-stage algorithm for estimating the current position and velocity components of contacts. The algorithm comprises four major stages. In the first stage, pre-processing aimed at elimination of angle errors associated with the time measurements is developed for use in later stages. In the second stage, a coarse grid search, in endpoint coordinates, is performed to yield a refined range estimate at each of the time measurements. In the third stage, an endpoint Gauss-Newton type maximum likelihood estimation (MLE) solution is performed to yield an accurate range estimate. Finally, in the fourth stage, the computed range and bearing values are refined more precisely through a Cartesian coordinate MLE. 
   The four-stage process or method provides the advantage of allowing each stage of the algorithm to work with well-defined input data. Additionally, this method allows the overall algorithm to perform computationally heavy operations over a smaller data space. 
   Also, the initial stage of the operation is held to a coarse estimation requiring little processing power. In this way, the present invention is able to handle large amounts of historic target information without sacrificing resolution in the target space. Furthermore, the procedure of using preprocessing and early estimation stages before the least squares operations in the MLE stages, steps the algorithm from selecting iteration points at local minimums rather than true minimums. This procedure prevents divergence in the solution and prevents the resulting false radar and sonar targets. 

   
     BRIEF DESCRIPTION OF THE DRAWINGS 
     The foregoing objects and other advantages of the present invention will be more fully understood from the following detailed description and reference to the appended drawings wherein: 
       FIG. 1  is a block diagram depicting the multi-stage maximum likelihood estimation (MLE) method; 
       FIG. 2  is a geographic plot of MLE endpoints; and 
       FIG. 3  is a three-dimensional representation of the sonar beam and bottom reflected beam of a towed array sensor. 
   

   DESCRIPTION OF THE PREFERRED EMBODIMENTS 
   The multistage maximum likelihood estimator (MLE) processes sonar data and computes a target solution (range, bearing, course, speed) and a localization ellipse by processing data in several stages. The algorithm processes azimuthal bearing measurements, direct path or bottom-bounce conical angle measurements, horizontal range, direct path or bottom bounce frequency measurements from multiple trackers and sonar arrays. Frequency data from a maximum of 2 trackers may be processed. The algorithm constraints include a non-maneuvering target at a known depth, a flat ocean bottom, and an isovelocity environment (straight-line sound propagation). The propagation path is constrained to be either direct path or bottom bounce-on ray reversal and the measurement noise is assumed to be uncorrelated. When measurement data has been partitioned into segments, propagation path hypothesis testing is performed. 
   Referring now to  FIG. 1 , the overall process  10  is depicted showing the four major stages of the present invention, a endpoint angle smoothing stage  12 , a coarse endpoint coordinate grid search stage  22 , an endpoint Gauss-Newton type MLE  32 , and a fourth stage, the Cartesian coordinate MLE stage  42 . 
   In the first stage  12 , the algorithm calculates angle smoothing on the angle measurements at the endpoints of the data window in order to reduce angle errors associated with the tie down times (depicted in  FIG. 2  as time line  1  (t 1 ) and time line  2  (t 2 )) used by the endpoint coarse grid search and endpoint maximum likelihood estimator. 
   In the second stage  22  of  FIG. 1 , a coarse grid search in endpoint coordinates is performed to obtain a reasonable initial stage estimate of target range at the two times lines. Referring again to  FIG. 2 , the target position at time line  1  (t 1 ) and time line  2  (t 2 ) is constrained to lie on either the azimuthal bearing lines or conical angle hyperbolas for bottom bounce propagation or conical angle hyperbolic asymptotes for direct path propagation, thereby producing the constrained track of a target  26 . The actual track  28  is depicted showing the convergence of the solution. 
   These target restraints may be better visualized by reference to  FIG. 3 . In  FIG. 3 , ownship  62  is submerged at a submarine depth plane  68  with a representation of the sonar-emitted, cone-shaped beam  64 . The cone-shaped beam  64  either directly impinges a target or can be reflected off the ocean bottom  72 . As shown, the bottom reflection  66  produces a hyperbola. As a result, the reflected beam  70  is a conical angle hyperbola. 
   In the third stage  32 , an endpoint Gauss-Newton type MLE estimates target range at the two times lines along with a target base frequency for a maximum of two frequency trackers. Again, the target position at time line  1  and time line  2  is constrained to lie on either the azimuthal bearing lines, conical angle hyperbolas or conical angle hyperbolic asymptotes. 
   In the fourth stage  42 , the solution is further refined using the Cartesian coordinate MLE, which also provides errors bounds on various target parameters. The Cartesian coordinate MLE is also Gauss-Newton type MLE that estimates target x, y-position and velocity using the same assumptions made by the endpoint MLE. 
   Endpoint Angle Smoothing 
   The first stage, the endpoint angle smoothing stage receives input data from the target tracker, in this example, a sonar sensor, and provides preliminary data for follow-on stages. The algorithm performs angle smoothing on the angle measurements at the endpoints of the data window in order to reduce angle errors associated with the tie-down times (referred to as time line  1  and time line  2 ). 
   Because the coarse grid search constrains its target solution to lie on the azimuthal bearing lines or conical angle hyperbolae (bottom bounce) or conical angle hyperbolic asymptotes (direct path) at time line  1  and time line  2 , significantly noisy measurements at either timeline may result in a significantly biased target solution. In order to avoid biased solutions due to endpoint constraints, the coarse grid search constrains the target track to lie on the smoothed (vice measured) bearing lines or conical angle hyperbolae/asymptotes. The angle measurements from the tracker or trackers associated with time line  1  and time line  2  are smoothed by fitting measurement data collected within a specified time window of either time line  1  or time line  2  with a quadratic model using standard (normal equation) least-squares theory. Sophisticated orthogonalization techniques are simply not necessary in this application. 
   Assuming a quadratic model, the angle measurements from the tracker associated with time line  1  that are within 120 seconds of time line  1  (a 1 , a 2 , . . . , a m ) can be described as 
                   [           a   1               a   2             ⋮             a   m           ]     =       [         1         Δ   ⁢           ⁢     t   1               Δ   1   2     2             1         Δ   ⁢           ⁢     t   2               Δ   2   2     2             ⋮       ⋮       ⋮           1         Δ   ⁢           ⁢     t   m               Δ   m   2     2           ]     ⁡     [           a   0               a   0   ′               a   0   ″           ]               (   1   )               
or
   z=Hx   (2) 
where Δt i  is the time of the ith measurement of time line  1   
   a i  is the ith angle measurement (−π≦a i &lt;+π) 
   a 0  is the smoothed angle at time line  1   
   a′ 0  is the angle rate at time line  1   
   a″ 0  is the angle acceleration at time line  1   
   The curve fit coefficients (x) can be computed using a standard unweighted normal equation approach as
 
 x=[H   T   H]   −1   H   T   z   (3)
 
where the matrix inverse is performed using a standard Gaussian elimination method. In order to tie down to the smoothed angle at time line  1 , the smoothed angle estimate a 0  can be substituted for the measured angle at time line  1 .
 
   In similar fashion, a smoothed angle estimate at time line  2  can be generated using tracker data associated with the time line  2  tracker that is within 120 seconds of time line  2 , and this smoothed angle can also be substituted for the measured angle at time line  2 . 
   If the root mean square (RMS) error of the curve fit at either time line exceeds 3°, then the smoothed angle estimates shall be discarded. 
   Coarse Grid Search 
   The coarse grid search can process frequency data for up to two separate frequency trackers. For improved clarity, only a single frequency tracker is described. 
   1. Where at least three frequency measurements are available for a given frequency tracker, frequency data from that tracker is processed and the estimated base frequency for that tracker (Fb) is set to the most recent frequency measurement. 
   2. Set the minimum and maximum range at t 1  with respect to the sensor associated with time line  1  (R 1   min , R 1   max ) and the minimum and maximum range at t 2  with respect to the sensor associated with time line  2  (R 2   min , R 2   max ) as follows: 
   
       
       
         
           a. If the measurement at time line  1  is a bearing, set the minimum range at t 1  with respect to the sensor associated with time line  1  (R 1   min ) to the minimum range constraint which is defaulted to 100. 
           b. If the measurement at time line  1  is a conical angle, compute the minimum range at t 1  with respect to the sensor associated with time line  1  (R 1   min ). If R 1   min  is less than the minimum range constraint, set R 1   min  to the minimum range constraint which is defaulted to 100. The minimum range with respect to the sensor is computed as follows:
           i. Compute the plane depth (Rz) associated with a measurement as follows:
               1.) If the propagation path is direct (zero ray reversals), then the image plane depth is computed as follows:
 
 Rz=Zt−Zs   (4)
   
               
         
           where Zt is the assumed target depth and
           Zs is the sensor depth
               2.) If the propagation path is bottom bounce (one ray reversal), then the image plane depth is computed as follows:
 
 Rz= 2 Zb−Zs−Zt   (5)
   
               
         
           where Zb is the bottom depth.
           ii. Compute the maximum D/E angle with respect to the sensor (θ max). If the measured conical angle (βm) is between 0 and π/2 inclusive,
 
θ max =β n   −C   s   (6)
    where C s  is the sensor cant angle. If the measured conical angle is less than π,
 
θ max =π−β m   +C   s   (7)
   iii. The minimum range with respect to the sensor (R min ) can then be computed as   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   R 
                   min 
                 
                 = 
                 
                   
                     R 
                     z 
                   
                   
                     tan 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       max 
                     
                   
                 
               
             
             
               
                 ( 
                 8 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where R z  is the image plane depth. 
             
           
           c. Set the maximum range t 1  with respect to the sensor associated with time line  1  (R 1   max ) to the maximum range constraint which is defaulted to 200000. 
           d. If the measurement at time line  2  is a bearing, set the minimum range at t 2  with respect to the sensor associated with time line  2  (R 2   min ) to the minimum range constraint which is defaulted to 100. 
           e. If the measurement at time line  2  is a conical angle, compute the minimum range at t 2  with respect to the sensor associated with time line  2  (R 2   min ). If R 2   min  is less than the minimum range constraint, set R 2   min  to the minimum constraint which is defaulted to 100. The minimum range with respect to the sensor is computed as for equations (4) thru (8). 
           f. Set the maximum range at t 2  with respect to the sensor associated with time line  2  (R 2   max ) to the maximum range constraint which is defaulted to 200000.
 
3. Compute three values of range at t 1  with respect to the sensor associated with time line  1  (R 1   j , j=1, . . . , 3) and three values of range at t 2  with respect to the sensor associated with time line  2  (R 2   k , k=1, . . . , 3) as follows:
 
 R 1 j   =R 1 min+ 5000 j   (9)
 
 R 2 k   =R 2 min+ 5000 k   (10)
 
If R 1   j &gt;R 1   max , set R 1   j  to R 1   max . If R 2   k &gt;R 2   max , set R 2   k  to R 2   max .
 
4. If frequency data is being processed from a particular tracker, then five base frequency estimates (Fb l , l=1, . . . , 5) are computed as follows:
 
 Fb   l   =Fr   avg +0.005( l− 1)  (11)
 
where Fr avg  is averaged measured frequency measurement between t 1  and t 2 .
 
5. For each combination of R 1   j ,R 2   k ,Fb 1  compute the Endpoint coordinate performance index (PI jkl ) as follows:
 
           a. Compute Endpoint Parameters as follows:
           i. If the measurement at time line  1  is a bearing, set true bearing at t 1  with respect to the sensor associated with time line  1  (B 1 ) to the bearing estimate at time line  1 .   ii. If the measurement at time line  1  is a conical angle,
               1.) Compute the target image depth at t 1  with respect to the sensor associated with time line  1  (Rz 1 ) as described in for equations (4) and (5).   2.) Compute the maximum depression/elevation (D/E) angle at t 1  with respect to the sensor associated with time line  1  (θ 1   max ) as described for equations (6) thru (8).   3.) Compute the slant range at t 1  with respect to the sensor associated with time line  1  (Rs 1 ):
 
 Rs 1 =√{square root over (R1 2 +Rz1 2 )}   (12)
   4.) Compute the D/E angle at t 1  with respect to the sensor associated with time line  1  (θ 1 ):   
               
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   1 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       Rz1 
                       Rs1 
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 13 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) If θ 1 &gt;θ 1   max , the D/E angle is invalid and processing shall terminate. 
                   6.) Compute the cosine of relative bearing at t 1  with respect to the sensor associated with time line  1  (cBr 1 ) as follows: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   r 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   1 
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       1 
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       C 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       θ 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       1 
                     
                   
                   
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     C 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     s 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     1 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     θ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     1 
                   
                 
               
             
             
               
                 ( 
                 14 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               where Cs 1  is the cant angle at t 1  of the sensor associated with time line  1 
               β 1  is the conical angle estimate at time line  1 .   7.) Insure that −0.99999&lt;cBr 1 &lt;0.99999.   8.) Compute the relative bearing at t 1  with respect to the sensor associated with time line  1  (Br 1 ) as follows:
 
 Br 1=cos −1   cBr 1  (15)
   9.) If the port/starboard assumption for time line  1  indicates port, set Br 1 =2π−Br 1 .   10.) Compute the true bearing at t 1  with respect to the sensor associated with time line  1  (B 1 ) as follows:
 
 B 1 =Br 1 +Hs 1  (16)
   
             
               where Hs 1  is the heading at t i  of the sensor associated with time line  1 . 
               iii. If the measurement at time line  2  is a bearing, set true bearing at t 2  with respect to the sensor associated with time line  2  (B 2 ) to the bearing estimate at time line  2 . 
               iv. If the measurement at time line  2  is a conical angle,
               1.) Compute the target image depth at t 2  with respect to the sensor associated with time line  2  (Rz 2 ) as described for equations (4) and (5).   2.) Compute the maximum D/E angle at t 2  with respect to the sensor associated with time line  2  (θ 2   max ) as described for equations (6) thru (8).   3.) Compute the slant range at t 2  with respect to the sensor associated with time line  2  (Rs 2 ):
 
 Rs 2 =√{square root over (R2 2 +Rz2 2 )}   (17)
   4.) Compute the D/E angle at t 2  with respect to the sensor associated with time line  2  (θ 2 ):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   2 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       Rz2 
                       Rz2 
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 18 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) If θ 2 ≧θ 2   max , the D/E angle is invalid and processing shall terminate. 
                   6.) Compute the cosine of relative bearing at t 2  with respect to the sensor associated with time line  2  (cBr 2 ) as follows: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   r 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   2 
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       2 
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       C 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       θ 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       2 
                     
                   
                   
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     C 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     s 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     2 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     θ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     2 
                   
                 
               
             
             
               
                 ( 
                 19 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where Cs 2  is the cant angle at t 2  of the sensor associated with time line  2   
                   β 2  is the conical angle estimate at time line  2 . 
                   7.) Insure that −0.99999&lt;cBr 2 &lt;0.99999. 
                   8.) Compute the relative bearing at t 2  with respect to the sensor associated with time line  2  (Br 2 ) as follows:
 
 Br 2=cos −1   cBr 2  (20)
 
                   9.) If the port/starboard assumption for time line  2  indicates port set Br 2 =2π−Br 2 . 
                   10.) Compute the true bearing at t 2  with respect to the sensor associated with time line  2  (B 2 ) as follows:
 
 B 2 =Br 2 +Hs 2  (21)
 
                    where Hs 2  is the heading at t 2  of the sensor associated with time line  2 . 
                 
               
             
           
           b. For each measurement in the batch:
           i. Compute the x-component of range t i  with respect to the sensor associated with the ith measurement (Rx i ) and the y-component of range at t i  with respect to the sensor associated with the ith measurement (Ry i ):   
         
         
       
     
  
                   T1   i     =         t   i     -   t1       t2   -   t1               (   22   )                 T 2 i =1 −T 1 1   (23)   Rx   i   =T 2 i   R 1 j  sin B1 +T 1 i   R 2 k  sin B2 +T 1 i ( Xs 2 −Xs 1)−( Xs   i   −Xs 1)  (24)   Ry   i   =T 2 i   R 1 j  cos B1 +T 1 i   R 2 k  cos  B 2 +T 1 i ( Ys 2 −Ys 1)−( Ys   i   −Ys 1)  (25)         where Xs i  is the x-coordinate of the position at t i  of the sensor associated with the ith measurement Ys i  is the y-coordinate of the position at t i  of the sensor associate with the ith measurement t i  the time of the ith measurement
           ii. If the ith measurement is a bearing, the following shall be performed:
               1.) Compute the true bearing at t i  with respect to the sensor associated with the ith measurement (B i ):   
               
               
   
     
       
         
           
             
               
                 
                   B 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         Rx 
                         i 
                       
                       
                         Ry 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 26 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the bearing residual (RESB i ) such that −π≦RESB i ≦π:
 
 RESB   i   =Bm   i   −B   i   (27)
 
                    where Bm i  is the measured bearing at t i    
                   3.) Compute the normalized bearing residual (  RESB i   ): 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     RESB 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESB 
                     i 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       B 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 28 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               where σB i  is the standard deviation of the measured bearing at t i    
               iii. If the ith measurement is a conical angle, the following shall be performed:
               1.) Compute the target image depth at t i  with respect to the sensor associated with ith measurement (Rz i ) as described for equations (4) and (5).   2.) Compute the maximum D/E angle at t i  with respect to the sensor associated with the ith measurement (θ maxi ) as described for equations (6) thru (8).   3.) Compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   Rs 
                   i 
                 
                 = 
                 
                   
                     
                       Rx 
                       i 
                       2 
                     
                     + 
                     
                       Ry 
                       i 
                       2 
                     
                     + 
                     
                       Rz 
                       i 
                       2 
                     
                   
                 
               
             
             
               
                 ( 
                 29 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) Compute the D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ): 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   i 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         Rz 
                         i 
                       
                       
                         Rs 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 30 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) If θ i &lt;θ maxi , the D/E angle is valid and the following shall be performed:
                     a  Compute the x-component of range at t i  with respect to the sensor associated with the ith measurement (xta i ), the y-component of range at t i  with respect to the sensor associated with the ith measurement (yta i ) and the z-component of range at t i  with respect to the sensor associated with the ith measurement rotated to the axis of the array (zta i )
   xta   i   =Rx   i  cos  Hs   i   −Ry   i  sin  Hs   i   (31)   yta   i =( Rx   i  sin  Hs   i   +Ry   i  cos  Hs   i )cos  Cs   i   −Rz   i  sin  Cs   i   (32)   zta   i =( Rx   i  sin  Hs   i   +Ry   i  cos  Hs   i )sin  Cs   i   +Rz   i  cos  Cs   i   (33)    where CS i  is the cant angle at t i  of the sensor associated with the ith measurement   Hs i  is the heading at t i  of the sensor associated with the ith measurement     b  Compute the conical angle at ti with respect to the sensor associated with the ith measurement (β i ):   If yta i ≠0   
                 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   β 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         
                           
                             xta 
                             i 
                             2 
                           
                           + 
                           
                             zta 
                             i 
                             2 
                           
                         
                       
                       
                         yta 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 34 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        otherwise 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   β 
                   i 
                 
                 = 
                 
                   π 
                   2 
                 
               
             
             
               
                 ( 
                 35 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                         c . Compute the conical angle residual (RESβ i ) such that −π≦RESβ i ≦π:
   RESβ   i   =βm   i −β 1   (36) 
                        where βm i  is the measured conical angle at t i . 
                         d . Compute the normalized conical angle residual (  RESβ i   ): 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     RES 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       β 
                       i 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     RES 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       β 
                       i 
                     
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 37 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        where σβ i  is the standard deviation of the measured conical angle at t i . 
                     
                   
                 
               
               iv. If the ith measurement is a horizontal range:
               1.) Compute the range at t i  with respect to the sensor associated with the ith measurement (R i ):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   R 
                   i 
                 
                 = 
                 
                   
                     
                       Rx 
                       i 
                       2 
                     
                     + 
                     
                       Ry 
                       i 
                       2 
                     
                   
                 
               
             
             
               
                 ( 
                 38 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the range residual (RESR i ):
 
 RESR   i   =Rm   i   −R   i   (39)
 
                    where Rm i  is the measured range at t i . 
                   3.) Compute the normalized range residual (  RESR i   ): 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     RESR 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESR 
                     i 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       R 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 40 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σR i  is the standard deviation of the measured range at t i . 
                 
               
               v. If the ith measurement is a frequency and frequency data are being processed:
               1.) Compute the target image at t i  with respect to the sensor associated with the ith measurement (Rz i ) as described for equations (4) and (5).   2.) Compute the maximum D/E angle at t i  with respect to the sensor associated with the ith measurement (θ maxi ) as described for equations (6) thru (8).   3.) Compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   Rs 
                   i 
                 
                 = 
                 
                   
                     
                       Rx 
                       i 
                       2 
                     
                     + 
                     
                       Ry 
                       i 
                       2 
                     
                     + 
                     
                       Rz 
                       i 
                       2 
                     
                   
                 
               
             
             
               
                 ( 
                 41 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) Compute the D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ): 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   i 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         Rz 
                         i 
                       
                       
                         Rs 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 42 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) If θ i &lt;θ maxi , the D/E angle is valid and the following shall be performed:
                     a  Compute the x-component of target velocity (Vxt) and the y-component of target velocity (Vyt):   
                 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 Vxt 
                 = 
                 
                   
                     
                       
                         R2 
                         k 
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     Xs2 
                     - 
                     
                       
                         R1 
                         j 
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     Xs1 
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 43 
                 ) 
               
             
           
           
             
               
                 Vyt 
                 = 
                 
                   
                     
                       
                         R2 
                         k 
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     Ys2 
                     - 
                     
                       
                         R1 
                         j 
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     Ys2 
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 44 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                         b . Compute the frequency at t i  with respect to the sensor associated with the ith measurement (F i ): 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   F 
                   i 
                 
                 = 
                 
                   Fb 
                   ⁢ 
                   
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         
                           Vxs 
                           i 
                         
                         ⁢ 
                         
                           Rx 
                           i 
                         
                       
                       + 
                       
                         
                           Vys 
                           i 
                         
                         ⁢ 
                         
                           Ry 
                           i 
                         
                       
                     
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         VxtRx 
                         i 
                       
                       + 
                       
                         VytRy 
                         i 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 45 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        where c is the average speed of sound. 
                         c . Compute the frequency residual (  RESF i   ):
   RESF   i   =Fm   i   −F   i   (46) 
                        where Fm i  is the measured frequency at t i . 
                         d . Compute the normalized frequency residual (  RESF i   ): 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     RESF 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESF 
                     i 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       F 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 47 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        where σF i  is the standard deviation of the measured frequency at t i . 
                     
                   
                 
               
             
           
           c. If a range constraint is being imposed, then the following computations shall be performed:
           i. Compute the target range (R) as follows:   
         
         
       
     
  
   
     
       
         
           
             
               
                 R 
                 = 
                 
                   
                     
                       Rx 
                       i 
                       2 
                     
                     + 
                     
                       Ry 
                       i 
                       2 
                     
                   
                 
               
             
             
               
                 ( 
                 48 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               ii. Compute the range residual (RESR):
 
 RESR=Rc−R   (49)
 
                where Rc is the assumed target range. 
               iii. Compute the normalized range residual (  RESR ): 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   RESR 
                   _ 
                 
                 = 
                 
                   RESR 
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     R 
                   
                 
               
             
             
               
                 ( 
                 50 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σR is the standard deviation of the assumed target range. 
             
           
           d. If a speed constraint is being imposed, then the following computations shall be performed:
           i. Compute the x-component of target velocity (Vxt) and the y-component of target velocity (Vyt):   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   V 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   x 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   t 
                 
                 = 
                 
                   
                     
                       
                         R2 
                         k 
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     
                       X 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                     
                     - 
                     
                       
                         R1 
                         j 
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     
                       X 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 51 
                 ) 
               
             
           
           
             
               
                 
                   V 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   y 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   t 
                 
                 = 
                 
                   
                     
                       
                         R2 
                         k 
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     
                       Y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                     
                     - 
                     
                       
                         R1 
                         j 
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     
                       Y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 52 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               ii. Compute the target speed (V):
 
 V=√{square root over (Vxt 2 +Vyt 2 )}   (52)
 
               iii. Compute the speed residual (RESV):
 
 RESV=Vc−V   (54)
 
                where Vc is the assumed target speed. 
               iv. Compute the normalized speed residual (  RESV ): 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   RESV 
                   _ 
                 
                 = 
                 
                   RESV 
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     V 
                   
                 
               
             
             
               
                 ( 
                 55 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σV is the standard deviation of the assumed target speed. 
             
           
           e. Compute the Endpoint coordinate performance index (PI jkl ) as the square root of the mean of the squared normalized residuals, which include measurements as well as constraints.
 
6. Select the value of R 1   j , R 2   k  and Fb l  associated with the smallest PI jkl .
 
Then assuming zero mean unit variance measurements, R −1 =I,
 
         
       
     
  
   The non-linear, least-squares algorithm, which employs Householder transformations, applies to both the third and fourth stages of the target estimator, the endpoint MLE and the Cartesian coordinate MLE. The sequence of operations are: 
                                   Initialization           x 1  = x 0         1 = 1, NITER   Gauss-Newton iterations       i = l,m   measurement loop m = # of           measurements       H = ∂h(x 1−1 )/∂x   Jacobian matrix m × ns           ns = # of state variables       Z = z − h(x 1−1 )   residual vector m × 1       H = [H|z]   augmented Jacobian m × (ns + 1)       A = QH Householder Transformation                       =     [         U       Y           0       d         ]             U upper triangular ns × nsY is normalized residual ns × 1               P = U −1 U −t     state covariance matrix ns × ns       Δx = U −1 Y   correction vector ns × 1       PI = ½[z − h(x 1−1 )]R −1 [z − h(x 1−1 )]   initial performance index (scalar)       X 1  = x 1−1  + αΔx   state update           α = stepsize via line search       PI′ = ½[z − h(x 1 )] R −1 [z − h(x 1 )]   updated performance index       ΔPI = (PI − PI′)/PI′   change in performance index       if ΔPI &lt; threshold, exit loop   convergence test                    
Endpoint Coordinate MLE
 
   The endpoint coordinate MLE can process frequency data for up to two separate frequency trackers. For improved clarity, only a single frequency tracker is described. 
   1. Initialize the following Endpoint coordinate MLE solution parameters to zero: 
   Roc (range with respect to own ship at current time) 
   Boc (bearing with respect to own ship at current time) 
   Ct (target course) 
   Vt (target speed) 
   Fb (target base frequency) 
   2. Initialize the number of Gauss-Newton iterations to zero. A maximum of twenty-five Gauss-Newton iterations shall be performed as described in paragraphs 15a through 15r. 
   3. Determine the number of state variables as follows: If a least three frequency measurements are available, then frequency data will be processed, target base frequency shall be estimated and the number of states (ns) shall be set to three. Otherwise, the number of state variables shall be two, frequency data shall not be processed and target base frequency shall not be estimated.
 
4. Initialize values for range at t 1  with respect to the sensor associated with time line  1  (R 1 ) and range at t 2  with respect to the sensor associated with time line  2  (R 2 ) using the outputs from the coarse grid search.
 
 R 1 =R 1 init   (56)
 
 R 2 =R 2 init   (57)
 
where R 1   init  and R 2   init  are output by the grid search algorithm
 
   t 1  is the time line  1  time 
   t 2  is the time line  2  time 
   5. If frequency is being processed, initialize the base frequency state (Fb) with the base frequency output by the coarse gird search algorithm. 
   6. Compute the Endpoint coordinate performance index (PI) based on the initial states as follows: 
   
       
       
         
           a. First compute endpoint parameters:
           i. If the measurement at time line  1  is a bearing, set true bearing at t 1  with respect to the sensor associated with time line  1  (B 1 ) to the bearing estimate at time line  1 .   ii. If the measurement at time line  1  is a conical angle,
               1.) Compute the target image depth at t 1  with respect to the sensor associated with time line  1  (Rz 1 ) as described for equations (4) and (5).   2.) Compute the maximum depression/elevation (D/E) angle at t 1  with respect to the sensor associated with time line  1  (θ 1  max) as described for equations (6) thru (8).   3.) Compute the slant range at t 1  with respect to the sensor associated with time line  1  (Rs 1 ):
 
 Rs 1 =√{square root over (R1 2 +Rz1 2 )}   (58)
   4.) Compute the D/E angle at t 1  with respect to the sensor associated with time line  1  (θ 1 ):   
               
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   1 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         z1 
                       
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         s1 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 59 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) If θ 1 &gt;θ 1   max , the D/E angle is invalid and processing shall terminate. 
                   6.) Compute the cosine of relative bearing at t 1  with respect to the sensor associated with time  1  (cBr 1 ) as follows: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   r1 
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       1 
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       C 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       θ 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       1 
                     
                   
                   
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     C 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     s1 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     θ1 
                   
                 
               
             
             
               
                 ( 
                 60 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where Cs 1  is the cant angle at t 1  of the sensor associated with time line  1   
                   β 1  is the conical angle estimate at time line  1   
                   7.) Insure that −0.99999&lt;cBr 1 &lt;0.99999. 
                   8.) Compute the relative bearing at t 1  with respect to the sensor associated with time line  1  (Br 1 ) as follows:
 
 Br 1=cos −1   cBr 1  (61)
 
                   9.) If the port/starboard assumption for time line  1  indicates port, set Br 1 =2π−Br 1 . 
                   10.) Compute the tear bearing at t 1  with respect to the sensor associated with time line  1  (B 1 ) as follows:
 
 B 1 =Br 1 +Hs 1  (62)
 
                    where Hs 1  is the heading at t 1  of the sensor associated with time line  1 . 
                 
               
               iii. If the measurement at time line  2  is a bearing, set true bearing at t 2  with respect to the sensor associated with time line  2  (B 2 ) to the bearing estimate at time line  2 . 
               iv. If the measurement at time line  2  is a conical angle,
               1.) Compute the target image depth at t 2  with respect to the sensor associated with time line  2  (Rz 2 ) as described for equations (4) and (5).   2.) Compute the maximum D/E angle at t 2  with respect to the sensor associated with time line  2  (θ 2   max ) as described for equations (6) thru (8).   3.) Compute the slant range at t 2  with respect to the sensor associated with time line  2  (Rs 2 ):
 
 Rs 2 =√{square root over (R2 2 +Rz2 2 )}   (63)
   4.) Compute the D/E angle at t 2  with respect to the sensor associated with time line  2  (θ 2 ):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   2 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         z2 
                       
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         s2 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 64 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) If θ 2 &gt;θ 2   max , the D/E angle is invalid and processing shall terminate. 
                   6.) Compute the cosine of relative bearing at t 2  with respect to the sensor associated with time line  2  (cBr 2 ) as follows: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 CBr2 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       2 
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       C 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       θ 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       2 
                     
                   
                   
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     C 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     s 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     θ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     2 
                   
                 
               
             
             
               
                 ( 
                 65 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where Cs 2  is the cant angle at t 2  of the sensor associated with time line  2  and β 2  is the conical angle estimate at time line  2   
                   7.) Insure that −0.99999&lt;cBr 2 &lt;0.99999. 
                   8.) Compute the relative bearing at t 2  with respect to the sensor associated with time line  2  (Br 2 ) as follows:
 
 Br 2=cos −1   cBr 2  (66)
 
                   9.) If the port/starboard assumption for time line  1  indicates port, set Br 2 =2π−Br 2 . 
                   10.) Compute the true bearing at t 2  with respect to the sensor associated with time line  2  (B 2 ) as follows:
 
 B 2 =Br 2 +Hs 2  (67)
 
                 
               
             
           
           where Hs 2  is the heading at t 2  of the sensor associated with time line  2   
           b. Second, for each measurement in the batch:
           i. Compute the x-component of range at t i  with respect to the sensor associated with the ith measurement (Rx i ) and the y-component of range at t i  with respect to the sensor associated with the ith measurement (Ryi):   
         
         
       
     
  
                     T1   ⁢             i     =         t   i     -   t1       t2   -           ⁢   t1               (   68   )                 T 2 i =1 −T 1 i   (69)   Rx   i   =T 2 i   R 1 sin  B 1 +T 1 i   R 2 sin  B 2 +T 1 i ( Xs 2 −Xs 1)−( Xs   i   −Xs 1)  (70)   Ry   i   =T 2 i   R 1 cos  B 1 +T 1 i   R 2 cos  B 2 +T 1 i ( Ys 2 −Ys 1)−( Ys   i   −Ys 1)  (71)              where Xs i  is the x-coordinate of the position at t i  of the sensor associated with the ith measurement   Ys i  is the y-coordinate of the position at t i  of the sensor associated with the ith measurement   t i  is the time of the ith measurement   ii. If the ith measurement is a bearing, the following shall be performed:
               1.) Compute the true bearing at t i  with respect to the sensor associated with the ith measurement (B i ):   
                       
   
     
       
         
           
             
               
                 
                   B 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           x 
                           i 
                         
                       
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           y 
                           i 
                         
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 72 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the bearing residual (RESb i ) such that
 
−π≦ RESb   i ≦π:
 
 RESb   i   =Bm   i   −B   i   (73)
 
                    where Bm i  is the measured bearing at t i    
                   3.) Compute the normalized bearing residual (  RESb i   ): 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     R 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     E 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     S 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       b 
                       i 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     R 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     E 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     S 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       b 
                       i 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     b 
                   
                 
               
             
             
               
                 ( 
                 74 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σb i  is the standard deviation of the measured bearing at t i . 
                 
               
               iii. If the ith measurement is a conical angle, the following shall be performed:
               1.) Compute the target image depth at t i  with respect to the sensor associated with the ith measurement (Rz i ) as described for equations (4) and (5).   2.) Compute the maximum D/E angle at t i  with respect to the sensor associated with the ith measurement (θ maxi ) as described for equations (6) thru (8).   3.) Compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ).   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   R 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     s 
                     i 
                   
                 
                 = 
                 
                   
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         x 
                         i 
                         2 
                       
                     
                     + 
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         y 
                         i 
                         2 
                       
                     
                     + 
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         z 
                         i 
                         2 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 75 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) Compute the D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ): 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   i 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           z 
                           i 
                         
                       
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           s 
                           i 
                         
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 76 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) If θ i &lt;θ maxi , the D/E angle is valid and the following shall be performed:
                     a . Compute the x-component of range at t i  with respect to the sensor associated with the ith measurement (xta i ), the y-component of range at t i  with respect to the sensor associated with the ith measurement (yta i ) and the z-component of range at t i  with respect to the sensor associated with the ith measurement (zta i ) rotated to the axis of the array:
   xta   i   =Rx   i  cos  Hs   i   −Ry   i  sin  Hs   i   (77)   yta   i =( Rx   i  sin  Hs   i   +Ry   i  cos  Hs   i )cos  Cs   i   −Rz   i  sin  Cs   i   (78)   zta   i =( Rx   i  sin  Hs   i   +Ry   i  cos  Hs   i )sin  Cs   i   −Rz   i  cos  Cs   i   (79)    where Cs i  is the cant angle at t i  of the sensor associated with the ith measurement and Hs i  is the heading at t i  of the sensor associated with the ith measurement     b . Compute the conical angle at t i  with respect to the sensor associated with the ith measurement (β i ):   If yta i ≠0:   
                 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   β 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         
                           
                             x 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             t 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               a 
                               i 
                               2 
                             
                           
                           + 
                           
                             z 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             t 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               a 
                               i 
                               2 
                             
                           
                         
                       
                       
                         y 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         t 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           a 
                           i 
                         
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 80 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                       otherwise: 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   β 
                   i 
                 
                 = 
                 
                   π 
                   2 
                 
               
             
             
               
                 ( 
                 81 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                         c . Compute the conical angle residual (RESβ i ) such that −π≦RESβ i ≦π:
   RESβ   i   =βm   i −β i   (82) 
                        where βm i  is the measured conical angle at t i    
                         d . Compute the normalized conical angle residual (  RESβ i   ): 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   RES 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     β 
                     i 
                   
                 
                 = 
                 
                   
                     RES 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       β 
                       i 
                     
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 83 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        where σβ i  is the standard deviation of the measured conical angle at t i    
                     
                   
                 
               
               iv. If the ith measurement is a horizontal a range:
               1.) Compute the range residual (RESr i ):
 
 RESr   i   =Rm   i   −R   i  
   
             
               where Rm i  is the measured range at t i .
               2.) Compute the normalized range residual (  RESr i   ):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     R 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     E 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     S 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       r 
                       i 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     R 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     E 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     S 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       r 
                       i 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       r 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 84 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σr i  is the standard deviation of the measured range at t i . 
                 
               
               v. If the ith measurement is a frequency and frequency data are being processed, then the following shall be performed:
               1.) Compute the target image depth at t i  with respect to the sensor associated with the ith measurement (Rz i ).   2.) Compute the maximum D/E angle at t i  with respect to the sensor associated with the ith measurement (θ maxi ).   3.) Compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   R 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     s 
                     i 
                   
                 
                 = 
                 
                   
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         x 
                         i 
                         2 
                       
                     
                     + 
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         y 
                         i 
                         2 
                       
                     
                     + 
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         z 
                         i 
                         2 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 85 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) Compute the D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ): 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   i 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           z 
                           i 
                         
                       
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           s 
                           i 
                         
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 86 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) If θ i &lt;θ maxi  the D/E angle is valid and the following shall be performed:
                     a . Compute the x-component of target velocity (Vxt) and the y-component of target velocity (Vyt):   
                 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   V 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   x 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   t 
                 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     
                       X 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                     
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     
                       X 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 87 
                 ) 
               
             
           
           
             
               
                 
                   V 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   y 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   t 
                 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     
                       Y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                     
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     
                       Y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 88 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                         b . Compute the frequency at t i  with respect to the sensor associated with the ith measurement (f i ): 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   f 
                   i 
                 
                 = 
                 
                   F 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   b 
                   ⁢ 
                   
                     
                       
                         c 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           s 
                           i 
                         
                       
                       + 
                       
                         V 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         x 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           s 
                           i 
                         
                         ⁢ 
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           x 
                           i 
                         
                       
                       + 
                       
                         V 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         y 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           s 
                           i 
                         
                         ⁢ 
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           y 
                           i 
                         
                       
                     
                     
                       
                         c 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           s 
                           i 
                         
                       
                       + 
                       
                         V 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         x 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         t 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           x 
                           i 
                         
                       
                       + 
                       
                         V 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         y 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         t 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           y 
                           i 
                         
                       
                     
                   
                 
               
             
             
               
                 ( 
                 89 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        where c is the average speed of sound. 
                         c . Compute the frequency residual (RESf i ):
   RESf   i   =fm   i   −f   i   (90) 
                        where fm i  is the measured frequency at t i . 
                         d . Compute the normalized frequency residual (  RESf i   ): 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     RESf 
                     _ 
                   
                   i 
                 
                 = 
                 
                   
                     RESf 
                     i 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       f 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 91 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        where σf i  is the standard deviation of the measured frequency at t i . 
                     
                   
                 
               
             
           
           c. If a range constraint is being imposed, then the following processing shall be performed:
           i. Compute the range residual (RESr):
 
 RESr=Rc−R   (92)
    where Rc is the assumed target range.   ii. Compute the normalized range residual (  RESr ):   
         
         
       
     
  
                   RESr   _     =     RESr     σ   ⁢           ⁢   R               (   93   )               
where σR is the assumed target range standard deviation.
         d. If a speed constraint is being imposed, then the following processing shall be performed:
           i. Compute the x-component of target velocity (Vxt) and the y-component of target velocity (Vyt):   
               
   
     
       
         
           
             
               
                 
                   V 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   x 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   t 
                 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     
                       X 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                     
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     
                       X 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 94 
                 ) 
               
             
           
           
             
               
                 
                   V 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   y 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   t 
                 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     
                       Y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                     
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     
                       Y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 95 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               ii. Compute the target speed (V):
 
 V=√{square root over (Vxt 2 +Vyt 2 )}   (96)
 
               iii. Compute the speed residual (RESv):
 
 RESv=Vc−V   (97)
 
                where V is the assumed target speed. 
               iv. Compute the normalized speed residual (  RESv ): 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     R 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     E 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     S 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     v 
                   
                   _ 
                 
                 = 
                 
                   
                     R 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     E 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     S 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     v 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     V 
                   
                 
               
             
             
               
                 ( 
                 98 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σV is the assumed target speed standard deviation. 
             
           
           e.) Compute the Endpoint coordinate performance index as the square root of the means of the squared normalized residuals.
 
7. Set the minimum and maximum range at t 1  with respect to the sensor associated with time line  1  (R 1   min ,R 1   max ) and the minimum and maximum range at t 2  with respect to the sensor associated with time line  2  (R 2   min ,R 2   max ) as follows:
 
           a. If the measurement at time line  1  is a bearing, set the minimum range at t 1  with respect to the sensor associated with time line  1  (R 1   min ) to the minimum range constraint which is defaulted to 100. 
           b. If the measurement at time line  1  is a conical angle, compute the minimum range at t 1  with respect to the sensor associated with time line  1  (R 1   min ). If R 1   min  is less than the minimum range constraint, set R 1   min  to the minimum range constraint which is defaulted to 100. The minimum range with respect to the sensor is computed as described in equations (4) thru (8). 
           c. Set the maximum range at t 1  with respect to the sensor associated with time line  1  (R 1   max ) to the maximum range constraint with is defaulted to 200000. 
           d. If the measurement at time line  2  is a bearing, set the minimum range at t 2  with respect to the sensor associated with time line  2  (R 2   min ) to the minimum range constraint which is defaulted to 100. 
           e. If the measurement at time line  2  is a conical angle, compute the minimum range at t 2  with respect to the sensor associated with time line  2  (R 2   min ). If R 2   min  is less than the minimum range constraint, set R 2   min  to the minimum range constraint which is defaulted to 100. The minimum range with respect to the sensor is computed as described in equations (4) thru (8). 
           f. Set the maximum range at t 2  with respect to the sensor associated with time line  2  (R 2   max ) to the maximum range constraint which is defaulted to 200000.
 
8. Compute the endpoint parameters as follows:
 
           a. If the measurement at time line  1  is a bearing, set true bearing at t 1  with respect to the sensor associated with time line  1  (B 1 ) to the smoothed bearing estimate at time line  1  output by the endpoint smoother algorithm. 
           b. If the measurement at time line  1  is a conical angle,
           i. Compute the target image depth at t 1  with respect to the sensor associated with time line  1  (Rz 1 ) as described for equations (4) and (5).   ii. Compute the maximum depression/elevation (D/E) angle at t 1  with respect to the sensor associated with time line  1  (θ 1   max ) as described for equations (6) thru (8).   iii. Compute the slant range at t 1  with respect to the sensor associated with time line  1  (Rs 1 ):
 
 Rs 1 =√{square root over (R1 2 +Rz1 2 )}   (99)
   iv. Compute the D/E angle t 1  with respect to the sensor associated with time line  1  (θ 1 ):   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   1 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         z1 
                       
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         s1 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 100 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               v. If θ&gt;θ 1   max , the D/E angle is invalid and processing shall terminate. 
               vi. Compute the cosine of relative bearing at t 1  with respect to the sensor associated with time line  1  (cBr 1 ) as follows: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   Br1 
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β1 
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       C 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       θ 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       1 
                     
                   
                   
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     C 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     s1cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     θ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     1 
                   
                 
               
             
             
               
                 ( 
                 101 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where Cs 1  is the cant angle at t 1  of the sensor associated with time line  1   
               β 1  is the smoothed conical angle estimate at time line  1  output by the endpoint smoother algorithm. 
               vii. Insure that −0.99999&lt;cBr 1 &lt;0.99999. 
               viii. Compute the relative bearing at t 1  with respect to the sensor associated with time line  1  (Br 1 ) as follows:
 
 Br 1=cos −1   cBr 1  (102)
 
               ix. If the port/starboard assumption for time line  1  indicates port, set Br 1 =2π−Br 1 . 
               x. Compute the true bearing at t 1  with respect to the sensor associated with time line  1  (B 1 ) as follows:
 
 B 1 =Br 1 +Hs 1  (103)
 
                where Hs 1  is the heading at t 1  of the sensor associated with time line  1   
             
           
           c. If the measurement at time line  2  is a bearing, set true bearing at t 2  with respect to the sensor associated with time line  2  (B 2 ) to the smoothed bearing estimate at time line  2  output by the endpoint smoother algorithm. 
           d. If the measurement at time line  2  is a conical angle,
           i. Compute the target image depth at t 2  with respect to the sensor associated with time line  2  (Rz 2 ) as described for equations (4) and (5).   ii. Compute the maximum D/E angle at t 2  with respect to the sensor associated with time line  2  (θ 2   max ) as described for equations (6) thru (8).   iii. Compute the slant range at t 2  with respect to the sensor associated with time line  2  (Rs 2 ):   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   R 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s2 
                 
                 = 
                 
                   
                     
                       R2 
                       2 
                     
                     + 
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         z2 
                         2 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 104 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               iv. Compute the D/E angle at t 2  with respect to the sensor associated with time line  2  (θ 2 ): 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   2 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         z2 
                       
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         s2 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 105 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               v. If θ 2 &gt;θ 2   max , the D/E angle is invalid and processing shall terminate. 
               vi. Compute the cosine of relative bearing at t 2  with respect to the sensor associated with time line  2  (cBr 2 ) as follows: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   r2 
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       2 
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       C 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       θ 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       2 
                     
                   
                   
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     C 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     s2 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     θ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     2 
                   
                 
               
             
             
               
                 ( 
                 106 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where Cs 2  is the cant angle at t 2  of the sensor associated with time line  2 . 
               β 2  is the smoothed conical angle estimate at time line  2  output by the endpoint smoother algorithm. 
               vii. Insure that −0.99999&lt;cBr 2 &lt;0.99999. 
               viii. Compute the relative bearing at t 2  with respect to the sensor associated with time line  2  (Br 2 ) as follows:
 
 Br 2=cos −1   cBr 2  (107)
 
               ix. If the port/starboard assumption for time line  2  indicates port, set Br 2 =2π−Br 2 . 
               x. Compute the true bearing at t 2  with respect to the sensor associated with time line  2  (B 2 ) as follows:
 
 B 2 =Br 2 +Hs 2  (108)
 
                where Hs 2  is the heading at t 2  of the sensor associated with time line  2 .
 
9. Compute the initial x-component of target velocity (Vxt) and initial y-component of target velocity (Vyt):
 
             
           
         
       
     
  
                   V   ⁢           ⁢   x   ⁢           ⁢   t     =         R2   ⁢           ⁢   sin   ⁢           ⁢   B2     +     X   ⁢           ⁢   s2     -     R1   ⁢           ⁢   sin   ⁢           ⁢   B1     -     X   ⁢           ⁢   s1         t2   -   t1               (   109   )                 V   ⁢           ⁢   y   ⁢           ⁢   t     =         R2   ⁢           ⁢   cos   ⁢           ⁢   B2     +     Y   ⁢           ⁢   s2     -     R1   ⁢           ⁢   cos   ⁢           ⁢   B1     -     Y   ⁢           ⁢   s1         t2   -   t1               (   110   )               
where Xs 2  is the x-coordinate of the position at t 2  of the sensor associated with time line  2           Ys 2  is the y-coordinate of the position at t 2  of the sensor associated with time line  2     Xs 1  is the x-coordinate of the position at t 1  of the sensor associated with time line  1     Ys 1  is the y-coordinate of the position at t 1  of the sensor associated with time line  1 
 
10. Compute the initial target course (Ct) and speed (Vt) estimates:
       
                   C   ⁢           ⁢   t     =       tan     -   1       ⁡     (       V   ⁢           ⁢   x   ⁢           ⁢   t         V   ⁢           ⁢   y   ⁢           ⁢   t     ⁢               )               (   111   )                 Vt=√{square root over (Vxt 2 +Vyt 2 )}   (112) 
11. Compute initial x-coordinate of target position at tc (Xtc) and initial y-coordinate of target position at tc (Ytc):
   Xtc=R 2 sin  B 2 +Xs 2 +Vxt ( t 2 −tc )  (113)   Ytc=R 2 cos  B 2 +Ys 2 +Vyt ( t 2 −tc )  (114) 
   where tc is current time 
   12. Compute initial x-component of range at tc with respect to own ship (Rxoc) and initial y-component of range at tc with respect to own ship (Ryoc):
 
 Rxoc=Xtc−Xoc   (115)
 
 Ryoc=Ytc−Yoc   (116)
 
   where Xoc is the x-coordinate of own ship position at tc 
   Yoc is the y-coordinate of own ship position at tc 
   13. Compute initial range at tc with respect to own ship (Roc) and true bearing at tc with respect to own ship (Boc):
 
 Roc=√{square root over (Rxoc 2 +Ryoc 2 )}   (117)
 
                   B   ⁢           ⁢   o   ⁢           ⁢   c     ⁢           =       tan     -   1       ⁡     (       R   ⁢           ⁢   x   ⁢           ⁢   o   ⁢           ⁢   c       R   ⁢           ⁢   y   ⁢           ⁢   o   ⁢           ⁢   c       )               (   118   )               
14. If a range constraint is being imposed, limit the initial range at tc with respect to own ship to the maximum target range constraint. If a speed constraint is being imposed, limit the initial target speed estimate (Vt) to the maximum target speed constraint.
 
15. Gauss-Newton iterations shall be performed as described in paragraphs a through r below, until the algorithm converges as described in paragraph r or until twenty-five iterations have been performed.
         a. If the measurement at time line  1  is a conical angle, compute endpoint parameters at the time of the measurement at time line  1 :
           i. Limit the range at t 1  with respect to the sensor associated with time line  1  (R 1 ) to a minimum of R 1   min +0.1.   ii. Compute the target image depth at t 1  with respect to the sensor associated with time line (Rz 1 ) as described for equations (4) and (5).   iii. Compute the maximum depression/elevation (D/E) angle at t 1  with respect to the sensor associated with time line  1  (θ 1   max ) as described for equations (6) thru (8).   iv. Compute the slant range at t 1  with respect to the sensor associated with time line  1  (Rs 1 ):
 
 Rs 1 =√{square root over (R1 2 +Rz1 2 )}   (119)
   v. Compute the D/E angle at t 1  with respect to the sensor associated with time line  1  (θ 1 ):   
               
   
     
       
         
           
             
               
                 
                   θ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   1 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         z1 
                       
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         s1 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 120 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               vi. If θ 1 &lt;θ 1   max , perform the following:
               1.) Compute the cosine of relative bearing at t 1  with respect to the sensor associated with time line  1  (cBr 1 ) as follows:   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   r1 
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       1 
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       C 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       θ 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       1 
                     
                   
                   
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     C 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     s1 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     θ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     1 
                   
                 
               
             
             
               
                 ( 
                 121 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Insure that −0.99999&lt;cBr 1 &lt;0.99999. 
                   3.) Compute the relative bearing at t 1  with respect to the sensor associated with time line (Br 1 ) as follows:
 
 Br 1=cos −1   cBr 1  (122)
 
                   4.) If the port/starboard assumption for time line  1  indicates port, set Br 1 =2π−Br 1 . 
                   5.) Compute the true bearing at t 1  with respect to the sensor associated with time line  1  (B 1 ) as follows:
 
 B 1 =Br 1 +Hs 1  (123)
 
                   6.) Compute the slant range at t 1  respect to the sensor associated with time line  1  (Rs 1 ) as follows:
 
 Rs 1 =√{square root over (R1 2 +Rz1 2 )}   (124)
 
                 
               
             
           
           vii. If θ 1 &gt;θ 1   max , terminate all processing. 
           b. If the measurement at time line  2  is a conical angle, compute endpoint parameters at the time of the measurement at time line  2 :
           i. Limit the range at t 2  with respect to the sensor associated with time line  2  (R 2 ) to a minimum of R 2   min +0.1.   ii. Compute the target image depth at t 2  with respect to the sensor associated with time line  2  (Rz 2 ) as described for equations (4) and (5).   iii. Compute the maximum D/E angle at t 2  with respect to the sensor associated with time line  2  (θ 2   max ) as described for equations (6) thru (8).   iv. Compute the slant range at t 2  with respect to the sensor associated with time line  2  (Rs 2 ):
 
 Rs 2 =√{square root over (R2 2 +Rz2 2 )}   (125)
   v. Compute the D/E angle at t 2  with respect to the sensor associated with time line  2  (θ 2 ):   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   θ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   2 
                 
                 = 
                 
                   
                     sin 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         z2 
                       
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         s2 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 126 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               vi. If θ 2 &lt;θ 2   max , perform the following:
               1.) Compute the cosine of relative bearing at t 2  with respect to the sensor associated with time line  2  (cBr 2 ) as follows:   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 cBr2 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β2 
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       Cs2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       θ2 
                     
                   
                   
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     Cs2 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     θ2 
                   
                 
               
             
             
               
                 ( 
                 127 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Insure that −0.99999&lt;cBr 2 &lt;0.99999. 
                   3.) Compute the relative bearing at t 2  with respect to the sensor associated with time line  2  (Br 2 ) as follows:
 
 Br 2=cos −1   cBr 2  (128)
 
                   4.) If the port/starboard assumption for time line  2  indicates port, set Br 2 =2π−Br 2 . 
                   5.) Compute the true bearing at t 2  with respect to the sensor associated with time line  2  (B 2 ) as follows:
 
 B 2 =Br 2 +Hs 2  (129)
 
                   6.) Compute the slant range at t 2  respect to the sensor associated with time line  2  (Rs 2 ) as follows:
 
 Rs 2 =√{square root over (R2 2 +Rz2 2 )}   (130)
 
                 
               
               vii. If θ 2 &gt;θ 2   max , terminate all processing. 
             
           
           c. For each measurement in the batch:
           i. Compute the x-component of range at t i  with respect to the sensor associated with the ith measurement (Rx i ) and the y-component of range at t i  with respect to the sensor associated with the ith measurement (Ry i ):   
         
         
       
     
  
                   T1   i     =         t   i     -   t1       t2   -   t1               (   131   )                 T 2 i =1 −T 1 1   (132)   Rx   i   =T 2 i   R 1 sin  B 1 +T 1 i   R 2 sin  B 2 +T 1 i ( Xs 2 −Xs 1)−( Xs   i   −Xs 1)  (133)   Ry   i   =T 2 i   R 1 cos  B 1 +T 1 i   R 2 cos  B 2 +T 1 i ( Ys 2 −Ys 1)−( Ys   i   −Ys 1)  (134)              where Xs i  is the x-coordinate of the position at t i  of the sensor associated with the ith measurement   Ys i  is the y-coordinate of the position at t i  of the sensor associated with the ith measurement t i  is the time of the ith measurement   ii. Compute the range at t i  with respect to the sensor associated with the ith measurement (R i ) and bearing at t i  with respect to the sensor associated with the ith measurement (B i ):           
   
     
       
         
           
             
               
                 
                   R 
                   i 
                 
                 = 
                 
                   
                     
                       Rx 
                       i 
                       2 
                     
                     + 
                     
                       Ry 
                       i 
                       2 
                     
                   
                 
               
             
             
               
                 ( 
                 135 
                 ) 
               
             
           
           
             
               
                 
                   B 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         Rx 
                         i 
                       
                       
                         Ry 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 136 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               iii. Compute the target image depth at t i  with respect to the sensor associated with the ith measurement (Rz i ) and D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ) as described for equations (75) and (76). 
               iv. If the measurement at time line  1  is a bearing, the following shall be performed:
               1.) Compute the partial derivative of the x-component of target range at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1     
             
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 Rx 
                 i 
               
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the y-component of target range at t i  with respect to range at t 1  with respect to the sensor associated with line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         Ry 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Rx 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     T2 
                     i 
                   
                   ⁢ 
                   sin 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B1 
                 
               
             
             
               
                 ( 
                 137 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Ry 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     T2 
                     i 
                   
                   ⁢ 
                   cos 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B1 
                 
               
             
             
               
                 ( 
                 138 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the partial derivative of target horizontal range at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       
                         Rx 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Rx 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                     + 
                     
                       
                         Ry 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Ry 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 139 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   3.) Compute the partial derivative of the bearing at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       B 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       T2 
                       i 
                     
                     ⁢ 
                     
                       sin 
                       ⁡ 
                       
                         ( 
                         
                           B1 
                           - 
                           
                             B 
                             i 
                           
                         
                         ) 
                       
                     
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 140 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) Compute the partial derivative of the sine of true bearing at t 1  with respect to the sensor associated with time line  1  with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               sB1 
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the cosine of true bearing at t 1  with respect to the sensor associated with time line  1  with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       cB1 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     sB1 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 0 
               
             
             
               
                 ( 
                 141 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     cB1 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 0 
               
             
             
               
                 ( 
                 142 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               v. If the measurement at time line  1  is a conical angle, the following shall be performed:
               1.) Compute the partial derivative of the sine of true bearing at t 1  with respect to the sensor associated with time line  1  with respect to range at t 1  with respect to the sensor associated with time line  1     
             
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               sB1 
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the cosine of true bearing at t 1  with respect to the sensor associated with time line  1  with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       cB1 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 TMP1 
                 = 
                 
                   
                     - 
                     Rz1 
                   
                   
                     
                       R1 
                       2 
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       Cs1 
                       ⁡ 
                       
                         ( 
                         
                           
                             
                               Rz1 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               cos 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               β1 
                             
                             Rs1 
                           
                           + 
                           
                             sin 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             Cs1 
                           
                         
                         ) 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 143 
                 ) 
               
             
           
           
             
               
                 TMP2 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           cos 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           Br1 
                         
                         
                           sin 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           Br1 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   TMP1 
                 
               
             
             
               
                 ( 
                 144 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     cB1 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     TMP1 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     Hs1 
                   
                   - 
                   
                     TMP 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     sin 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     Hs1 
                   
                 
               
             
             
               
                 ( 
                 145 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     sB1 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           cos 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           B1 
                         
                         
                           sin 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           B1 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       cB1 
                     
                     
                       ∂ 
                       R1 
                     
                   
                 
               
             
             
               
                 ( 
                 146 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the partial derivative of the x-component of range at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 Rx 
                 i 
               
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the y-component of range at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         Ry 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Rx 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     T2 
                     i 
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R1 
                         ⁢ 
                         
                           
                             ∂ 
                             sB1 
                           
                           
                             ∂ 
                             R1 
                           
                         
                       
                       + 
                       
                         sin 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B1 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 147 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Ry 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     T2 
                     i 
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R1 
                         ⁢ 
                         
                           
                             ∂ 
                             cB1 
                           
                           
                             ∂ 
                             R1 
                           
                         
                       
                       + 
                       
                         cos 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B1 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 148 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   3.) Compute the partial derivative of horizontal range at t i  with respect to the sensor associated with the ith measurement with respect to the range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       
                         Rx 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Rx 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                     + 
                     
                       
                         Ry 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Ry 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 147 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) Compute the partial derivative of true bearing at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       B 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       
                         Ry 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Rx 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                     - 
                     
                       
                         Rx 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Ry 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                   
                   
                     R 
                     i 
                     2 
                   
                 
               
             
             
               
                 ( 
                 150 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               vi. If the measurement at time line  2  is a bearing, the following shall be performed:
               1.) Compute the partial derivative of the x-component of target range at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2     
             
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 Rx 
                 i 
               
             
             
               ∂ 
               R2 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the y-component of target range at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         Ry 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Rx 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     T1 
                     i 
                   
                   ⁢ 
                   sin 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B2 
                 
               
             
             
               
                 ( 
                 151 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Ry 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     T1 
                     i 
                   
                   ⁢ 
                   cos 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B2 
                 
               
             
             
               
                 ( 
                 152 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the partial derivative of target horizontal range at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       
                         Rx 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Rx 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     + 
                     
                       
                         Ry 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Ry 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 153 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   3.) Compute the partial derivative of the bearing at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       B 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       T1 
                       i 
                     
                     ⁢ 
                     
                       sin 
                       ⁡ 
                       
                         ( 
                         
                           B2 
                           - 
                           
                             B 
                             i 
                           
                         
                         ) 
                       
                     
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 154 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) Compute the partial derivative of the sine of true bearing at t 2  with respect to the sensor associated with time line  2  with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               sB2 
             
             
               ∂ 
               R2 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the cosine of true bearing at t 2  with respect to the sensor associated with time line  2  with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       cB2 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     sB2 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 0 
               
             
             
               
                 ( 
                 155 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     cB2 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 0 
               
             
             
               
                 ( 
                 156 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               vii. If the measurement at time line  2  is a conical angle, the following shall be performed:
               1.) Compute the partial derivative of the sine of true bearing at t 2  with respect to the sensor associated with time line  2  with respect to range at t 2  with respect to the sensor associated with time line  2     
             
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               sB2 
             
             
               ∂ 
               R2 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the cosine of true bearing at t 2  with respect to the sensor associated with time line  2  with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       cB2 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 TMP1 
                 = 
                 
                   
                     - 
                     Rz2 
                   
                   
                     
                       R2 
                       2 
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       Cs2 
                       ⁡ 
                       
                         ( 
                         
                           
                             
                               Rz2 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               cos 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               β2 
                             
                             Rs2 
                           
                           + 
                           
                             sin 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             Cs2 
                           
                         
                         ) 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 157 
                 ) 
               
             
           
           
             
               
                 TMP2 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           cos 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           Br2 
                         
                         
                           sin 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           Br2 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   TMP1 
                 
               
             
             
               
                 ( 
                 158 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     cB2 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     TMP1 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     Hs2 
                   
                   - 
                   
                     TMP2 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     sin 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     Hs2 
                   
                 
               
             
             
               
                 ( 
                 159 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     sB2 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           cos 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           B2 
                         
                         
                           sin 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           B2 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       cB2 
                     
                     
                       ∂ 
                       R2 
                     
                   
                 
               
             
             
               
                 ( 
                 160 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the partial derivative of the x-component of range at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 Rx 
                 i 
               
             
             
               ∂ 
               R2 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the y-component of range at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         Ry 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Rx 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     T1 
                     i 
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R2 
                         ⁢ 
                         
                           
                             ∂ 
                             sB2 
                           
                           
                             ∂ 
                             R2 
                           
                         
                       
                       + 
                       
                         sin 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B2 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 161 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Ry 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     T1 
                     i 
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         R2 
                         ⁢ 
                         
                           
                             ∂ 
                             cB2 
                           
                           
                             ∂ 
                             R2 
                           
                         
                       
                       + 
                       
                         cos 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B2 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 162 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   3.) Compute the partial derivative of horizontal range at t i  with respect to the sensor associated with the ith measurement with respect to the range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       
                         Rx 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Rx 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     + 
                     
                       
                         Ry 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Ry 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 163 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) Compute the partial derivative of true bearing at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       B 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       
                         Ry 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Rx 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     - 
                     
                       
                         Rx 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Ry 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                   
                   
                     R 
                     i 
                     2 
                   
                 
               
             
             
               
                 ( 
                 164 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               viii. If the ith measurement is a bearing, then the following shall be performed:
               1.) Compute the bearing residual (RESb i ) such that −π≦RESb i ≦π:
 
 RESb   i   =Bm   i   −B   i   (165)
    where Bm i  is the measured bearing at t i      2.) Compute the normalized bearing residual (  RESb i   ) and normalized partial derivatives   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       
                         
                           ∂ 
                           
                             B 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                       , 
                       
                         
                           ∂ 
                           
                             B 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     _ 
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     RESb 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESb 
                     i 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       b 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 166 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       b 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 167 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       b 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 168 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σb i  is the standard deviation of the bearing measurement 
                   3.) If frequency data are not being processed, set the next row of the augmented Jacobian H Matrix as follows: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       
                         ∂ 
                         R1 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       
                         ∂ 
                         R2 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESb 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 169 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   If frequency data are being processed, set the next row of the augmented Jacobian H matrix as follows: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       
                         ∂ 
                         R2 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       
                         ∂ 
                         R2 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   O 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESb 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 170 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               ix. If the ith measurement is a conical angle and the D/E-mark indicates a valid D/E:
               1.) Compute the true bearing at t i  with respect to the sensor associated with the ith measurement (B i ):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   B 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         Rx 
                         i 
                       
                       
                         Ry 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 171 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ): 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   Rs 
                   i 
                 
                 = 
                 
                   
                     
                       Rx 
                       i 
                       2 
                     
                     + 
                     
                       Ry 
                       i 
                       2 
                     
                     + 
                     
                       Rz 
                       i 
                       2 
                     
                   
                 
               
             
             
               
                 ( 
                 172 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   3.) If the measurement at time line  1  is a conical angle:
                     a  Compute the partial derivative of slant range at t 1  with respect to the sensor associated with time line  1  with respect to range at t 1  with respect to the sensor associated with time line  1     
                 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       Rs1 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     Rs1 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   R1 
                   Rs1 
                 
               
             
             
               
                 ( 
                 173 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                         b  Compute the partial derivative of cosine of relative bearing at t 1  with respect to the sensor associated with time line  1  with respect to range at t 1  with respect to the sensor associated with time line 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               cBr1 
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        and the partial derivative of sine of relative bearing at t 1  with respect to the sensor associated with time line  1  with respect to range at t 1  with respect to the sensor associated with time line  1   
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       sBr1 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     cBr1 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           β 
                           ⁢ 
                           1 
                         
                         ⁡ 
                         
                           ( 
                           
                             
                               R1 
                               ⁢ 
                               
                                 
                                   ∂ 
                                   Rs1 
                                 
                                 
                                   ∂ 
                                   R1 
                                 
                               
                             
                             - 
                             Rs1 
                           
                           ) 
                         
                       
                     
                     - 
                     
                       Rz1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       Cs1 
                     
                   
                   
                     
                       R1 
                       2 
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     Cs1 
                   
                 
               
             
             
               
                 ( 
                 174 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     sBr1 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     - 
                     
                       
                         cos 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         Br1 
                       
                       
                         sin 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         Br1 
                       
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       cBr1 
                     
                     
                       ∂ 
                       R1 
                     
                   
                 
               
             
             
               
                 ( 
                 175 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) If the measurement at time line  2  is a conical angle:
                     a  Compute the partial derivative of slant range at t 2  with respect to the sensor associated with time line  2  with respect to range at t 2  with respect to the sensor associated with time line  2     
                 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       Rs2 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     Rs2 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   R2 
                   Rs2 
                 
               
             
             
               
                 ( 
                 176 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                         b  Compute the partial derivative of cosine of relative bearing at t 2  with respect to the sensor associated with time line  2  with respect to range at t 2  with respect to the sensor associated with time line  2   
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               cBr2 
             
             
               ∂ 
               R2 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        and the partial derivative of sine of relative bearing at t 2  with respect to the sensor associated with time line  2  with respect to range at t 2  with respect to the sensor associated with time line  2   
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       sBr2 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     cBr2 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           β 
                           ⁢ 
                           r 
                           ⁢ 
                           2 
                         
                         ⁡ 
                         
                           ( 
                           
                             
                               R2 
                               ⁢ 
                               
                                 
                                   ∂ 
                                   Rs2 
                                 
                                 
                                   ∂ 
                                   R2 
                                 
                               
                             
                             - 
                             Rs2 
                           
                           ) 
                         
                       
                     
                     - 
                     
                       Rz2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       Cs2 
                     
                   
                   
                     
                       R2 
                       2 
                     
                     ⁢ 
                     cos 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     Cs2 
                   
                 
               
             
             
               
                 ( 
                 177 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     sBr2 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     - 
                     
                       
                         cos 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         Br2 
                       
                       
                         sin 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         Br2 
                       
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       cBr2 
                     
                     
                       ∂ 
                       R2 
                     
                   
                 
               
             
             
               
                 ( 
                 178 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) Compute the partial derivative of slant range at t i  with respect to the sensor associated with the ith measurement with respect to the x-component of range at t i  with respect to the sensor associated with the ith measurement 
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               
                 ∂ 
                 
                   Rs 
                   i 
                 
               
               . 
             
             
               ∂ 
               
                 Rx 
                 i 
               
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of slant range at t i  with respect to the sensor associated with the ith measurement with respect to the y-component of range at t i  with respect to the sensor associated with the ith measurement 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         Rs 
                         i 
                       
                     
                     
                       ∂ 
                       
                         Ry 
                         i 
                       
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Rs 
                       i 
                     
                   
                   
                     ∂ 
                     
                       Rx 
                       i 
                     
                   
                 
                 = 
                 
                   
                     Rx 
                     i 
                   
                   
                     Rs 
                     i 
                   
                 
               
             
             
               
                 ( 
                 179 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Rs 
                       i 
                     
                   
                   
                     ∂ 
                     
                       Ry 
                       i 
                     
                   
                 
                 = 
                 
                   
                     Ry 
                     i 
                   
                   
                     Rs 
                     i 
                   
                 
               
             
             
               
                 ( 
                 180 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   6.) Compute the partial derivative of relative at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 Br 
                 i 
               
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of relative bearing at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         Br 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Br 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       
                         Ry 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Rx 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                     - 
                     
                       
                         Rx 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Ry 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                   
                   
                     R 
                     i 
                     2 
                   
                 
               
             
             
               
                 ( 
                 181 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Br 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       
                         Ry 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Rx 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     - 
                     
                       
                         Rx 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Ry 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                   
                   
                     R 
                     i 
                     2 
                   
                 
               
             
             
               
                 ( 
                 182 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   7.) Compute the partial derivative of D/E at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 θ 
                 i 
               
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of D/E angle at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         θ 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       θ 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       - 
                       
                         Rz 
                         i 
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         R1 
                       
                     
                   
                   
                     Rs 
                     i 
                     2 
                   
                 
               
             
             
               
                 ( 
                 183 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       θ 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       - 
                       
                         Rz 
                         i 
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         R2 
                       
                     
                   
                   
                     Rs 
                     i 
                     2 
                   
                 
               
             
             
               
                 ( 
                 184 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   8.) Compute the sine and cosine of D/E angle at t i  with respect to the sensor associated with the ith measurement: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   sin 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     i 
                   
                 
                 = 
                 
                   
                     Rz 
                     i 
                   
                   
                     Rs 
                     i 
                   
                 
               
             
             
               
                 ( 
                 185 
                 ) 
               
             
           
           
             
               
                 
                   cos 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     i 
                   
                 
                 = 
                 
                   
                     R 
                     i 
                   
                   
                     Rs 
                     i 
                   
                 
               
             
             
               
                 ( 
                 186 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   9.) Compute the relative bearing at t i  with respect to the sensor associated with the ith measurement (Br i ):
 
 Br   i   =B   i   −Hs   i   (187)
 
                   10.) Compute the conical angle at t i  from a horizontal array with respect to the sensor associated with the ith measurement (βh i ):
 
β h   i =cos −1 (cos θ i  cos  Br   i )  (188)
 
                   11.) Is sin βh i ≠0, compute the partial derivative of the conical angle at t i  from a horizontal array with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               
                 ∂ 
                 β 
               
               ⁢ 
               
                   
               
               ⁢ 
               
                 h 
                 i 
               
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the conical angle at t i  from a horizontal array with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       
                         ∂ 
                         β 
                       
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         h 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       β 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       h 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         Br 
                         i 
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         θ 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             θ 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         Br 
                         i 
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         θ 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Br 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                   
                   
                     sin 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     β 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       h 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 189 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       β 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       h 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         Br 
                         i 
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         θ 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             θ 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         Br 
                         i 
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         θ 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             Br 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                   
                   
                     sin 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     β 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       h 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 190 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   12.) Compute the x-component of range at t i  with respect to the sensor associated with the ith measurement at the time of the ith measurement (xta i ), the y-component of range at t i  with respect to the sensor associated with the ith measurement at the time of the ith measurement (yta i ) and the z-component of range at t i  with respect to the sensor associated with the ith measurement at the time of the ith measurement (zta i ) rotated to the axis of the array:
 
 xta   i   =Rx   i  cos  Hs   i   −Ry   i  sin  Hs   i   (191)
 
 yta   i =( Rx   i  sin  Hs   i   +Ry   i  cos  Hs   i )cos  Cs   i   −Rz   i  sin  Cs   i   (192)
 
 zta   i =( Rx   i  sin  Hs   i   +Ry   i  cos  Hs   i )sin  Cs   i   −Rz   i  cos  Cs   i   (193)
 
                   13.) Compute the conical angle at t i  with respect to the sensor associated with the ith measurement (β i ): 
                   If yta i ≠0, 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   β 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         
                           
                             xta 
                             i 
                             2 
                           
                           + 
                           
                             zta 
                             i 
                             2 
                           
                         
                       
                       
                         yta 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 194 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   otherwise, 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   β 
                   i 
                 
                 = 
                 
                   π 
                   2 
                 
               
             
             
               
                 ( 
                 195 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   14.) If sinβ i ≠0, compute the partial derivative of the conical angle at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 β 
                 i 
               
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the conical angle at t i  with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       
                         
                           cos 
                           ⁢ 
                           Cs 
                         
                         i 
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         h 
                         i 
                       
                       ⁢ 
                       
                         
                           
                             ∂ 
                             β 
                           
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             h 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         Cs 
                         i 
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         θ 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             θ 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                   
                   
                     sin 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       β 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 196 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       
                         
                           cos 
                           ⁢ 
                           Cs 
                         
                         i 
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       β 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         h 
                         i 
                       
                       ⁢ 
                       
                         
                           
                             ∂ 
                             β 
                           
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             h 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         Cs 
                         i 
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         θ 
                         i 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             θ 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                   
                   
                     sin 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       β 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 197 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   15.) Compute the conical angle residual (RESβ i ) such that −π≦RESβ i ≦π:
 
 RESβ   i   =βm   i −β i   (198)
 
                    where βm i  is the measured conical angle at t i . 
                   16.) Compute the normalized conical angle residual  (RESβ i )  and normalized partial derivatives 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       
                         
                           ∂ 
                           
                             β 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                       , 
                       
                         
                           ∂ 
                           
                             β 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     _ 
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     RESβ 
                     _ 
                   
                   i 
                 
                 = 
                 
                   
                     RESβ 
                     i 
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 199 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 200 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 201 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σβ i  is the standard deviation of the conical angle measurement. 
                   17.) If frequency data are not being processed, set the next row of the augmented Jacobian H matrix to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           β 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           β 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ⁢ 
                   
                     
                       RESβ 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 202 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   If frequency data are being processed, set the next row of the augmented Jacobian H matrix to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           β 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           β 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ⁢ 
                   0 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESβ 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 203 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               x. If the ith measurement is a horizontal range:
               1.) Compute the range residual (RESr i ):
 
 RESr   i   =Rm   i   −R   i   (204)
    where Rm i  is the measured horizontal range at t i .   2.) Compute the normalized range residual (  RESr i   ) and normalized partial derivatives   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       
                         
                           ∂ 
                           
                             R 
                             i 
                           
                         
                         
                           ∂ 
                           R1 
                         
                       
                       , 
                       
                         
                           ∂ 
                           
                             R 
                             i 
                           
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     _ 
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     RESr 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESr 
                     i 
                   
                   
                     
                       σ 
                       ⁢ 
                       r 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 205 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       r 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 206 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       r 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 207 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σr i  is the standard deviation of the range measurement. 
                   3.) If frequency data are not being processed, set the next row of the augmented Jacobian H matrix to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ⁢ 
                   
                     
                       RESr 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 208 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   If frequency data are being processed, set the next row of the augmented Jacobian H matrix to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ⁢ 
                   0 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESr 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 209 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               xi. If frequency data are being processed and the D/E mark associated with the ith measurement indicates a valid D/E, then the following shall be performed:
               1.) Compute the x-component of target velocity (Vxt) and the y-component of target velocity (Vyt):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 Vxt 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     Xs2 
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     Xs1 
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 210 
                 ) 
               
             
           
           
             
               
                 Vyt 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     Ys2 
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     Ys1 
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 211 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the x-component of target velocity at t i  relative to the sensor associated with the ith measurement (Vx i ) and the y-component of target velocity at t i  relative to the sensor associated with the ith measurement (Vy i )
 
 Vx   i   =Vxt−Vxs   i   (212)
 
 Vy   i   =Vyt−Vys   i   (213)
 
                    where Vxs i  is the x-component of velocity of the sensor associated with the ith measurement 
                   Vys i  is the y-component of velocity of the sensor associated with the ith measurement 
                   3.) Compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ) 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   Rs 
                   i 
                 
                 ⁢ 
                 
                   
                     
                       R 
                       i 
                       2 
                     
                     + 
                     
                       Rz 
                       i 
                       2 
                     
                   
                 
               
             
             
               
                 ( 
                 214 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   4.) Compute the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to the x-component of range at t i  with respect to the sensor associated with the ith measurement 
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 f 
                 i 
               
             
             
               ∂ 
               
                 Rx 
                 i 
               
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to the y-component of range at t i  with respect to the sensor associated with the ith measurement 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                     
                 
                 ⁢ 
                 
                   
                     ( 
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Ry 
                           i 
                         
                       
                     
                     ) 
                   
                   ⁢ 
                   
                     : 
                   
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     
                       Rx 
                       i 
                     
                   
                 
                 = 
                 
                   
                     Fb 
                     
                       
                         ( 
                         
                           
                             cRs 
                             i 
                           
                           + 
                           
                             
                               Vxs 
                               i 
                             
                             ⁢ 
                             
                               Rx 
                               i 
                             
                           
                           + 
                           
                             
                               Vys 
                               i 
                             
                             ⁢ 
                             
                               Ry 
                               i 
                             
                           
                           + 
                           
                             
                               Rx 
                               i 
                             
                             ⁢ 
                             
                               Vx 
                               i 
                             
                           
                           + 
                           
                             
                               Ry 
                               i 
                             
                             ⁢ 
                             
                               Vy 
                               i 
                             
                           
                         
                         ) 
                       
                       2 
                     
                   
                   · 
                   
                     ( 
                     
                       
                         
                           ( 
                           
                             
                               
                                 Rx 
                                 i 
                               
                               ⁢ 
                               
                                 Vx 
                                 i 
                               
                             
                             + 
                             
                               
                                 Ry 
                                 i 
                               
                               ⁢ 
                               
                                 Vy 
                                 i 
                               
                             
                           
                           ) 
                         
                         ⁢ 
                         
                           ( 
                           
                             
                               
                                 c 
                                 
                                   Rs 
                                   i 
                                 
                               
                               ⁢ 
                               
                                 Rx 
                                 i 
                               
                             
                             + 
                             
                               Vxs 
                               i 
                             
                           
                           ) 
                         
                       
                       - 
                       
                         
                           ( 
                           
                             
                               cRs 
                               i 
                             
                             + 
                             
                               
                                 Vxs 
                                 i 
                               
                               ⁢ 
                               
                                 Rx 
                                 i 
                               
                             
                             + 
                             
                               
                                 Vys 
                                 i 
                               
                               ⁢ 
                               
                                 Ry 
                                 i 
                               
                             
                           
                           ) 
                         
                         ⁢ 
                         
                           Vx 
                           i 
                         
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 215 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     
                       Ry 
                       i 
                     
                   
                 
                 = 
                 
                   
                     Fb 
                     
                       
                         ( 
                         
                           
                             cRs 
                             i 
                           
                           + 
                           
                             
                               Vxs 
                               i 
                             
                             ⁢ 
                             
                               Rx 
                               i 
                             
                           
                           + 
                           
                             
                               Vys 
                               i 
                             
                             ⁢ 
                             
                               Ry 
                               i 
                             
                           
                           + 
                           
                             
                               Rx 
                               i 
                             
                             ⁢ 
                             
                               Vx 
                               i 
                             
                           
                           + 
                           
                             
                               Ry 
                               i 
                             
                             ⁢ 
                             
                               Vy 
                               i 
                             
                           
                         
                         ) 
                       
                       2 
                     
                   
                   · 
                   
                     ( 
                     
                       
                         
                           ( 
                           
                             
                               
                                 Rx 
                                 i 
                               
                               ⁢ 
                               
                                 Vx 
                                 i 
                               
                             
                             + 
                             
                               
                                 Ry 
                                 i 
                               
                               ⁢ 
                               
                                 Vy 
                                 i 
                               
                             
                           
                           ) 
                         
                         ⁢ 
                         
                           ( 
                           
                             
                               
                                 c 
                                 
                                   Rs 
                                   i 
                                 
                               
                               ⁢ 
                               
                                 Ry 
                                 i 
                               
                             
                             + 
                             
                               Vys 
                               i 
                             
                           
                           ) 
                         
                       
                       - 
                       
                         
                           ( 
                           
                             
                               cRs 
                               i 
                             
                             + 
                             
                               
                                 Vxs 
                                 i 
                               
                               ⁢ 
                               
                                 Rx 
                                 i 
                               
                             
                             + 
                             
                               
                                 Vys 
                                 i 
                               
                               ⁢ 
                               
                                 Ry 
                                 i 
                               
                             
                           
                           ) 
                         
                         ⁢ 
                         
                           Vy 
                           i 
                         
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 216 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   5.) Compute the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to the x-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement 
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 f 
                 i 
               
             
             
               ∂ 
               
                 Vx 
                 i 
               
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to the y-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                     
                 
                 ⁢ 
                 
                   
                     ( 
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Vy 
                           i 
                         
                       
                     
                     ) 
                   
                   ⁢ 
                   
                     : 
                   
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     
                       Vx 
                       i 
                     
                   
                 
                 = 
                 
                   
                     Fb 
                     
                       
                         ( 
                         
                           
                             cRs 
                             i 
                           
                           + 
                           
                             
                               Vxs 
                               i 
                             
                             ⁢ 
                             
                               Rx 
                               i 
                             
                           
                           + 
                           
                             
                               Vys 
                               i 
                             
                             ⁢ 
                             
                               Ry 
                               i 
                             
                           
                           + 
                           
                             
                               Rx 
                               i 
                             
                             ⁢ 
                             
                               Vx 
                               i 
                             
                           
                           + 
                           
                             
                               Ry 
                               i 
                             
                             ⁢ 
                             
                               Vy 
                               i 
                             
                           
                         
                         ) 
                       
                       2 
                     
                   
                   · 
                   
                     ( 
                     
                       
                         ( 
                         
                           
                             cRs 
                             i 
                           
                           + 
                           
                             
                               Vxs 
                               i 
                             
                             ⁢ 
                             
                               Rx 
                               i 
                             
                           
                           + 
                           
                             
                               Vys 
                               i 
                             
                             ⁢ 
                             
                               Ry 
                               i 
                             
                           
                         
                         ) 
                       
                       ⁢ 
                       
                         Rx 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 217 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     
                       Vy 
                       i 
                     
                   
                 
                 = 
                 
                   
                     Fb 
                     
                       
                         ( 
                         
                           
                             cRs 
                             i 
                           
                           + 
                           
                             
                               Vxs 
                               i 
                             
                             ⁢ 
                             
                               Rx 
                               i 
                             
                           
                           + 
                           
                             
                               Vys 
                               i 
                             
                             ⁢ 
                             
                               Ry 
                               i 
                             
                           
                           + 
                           
                             
                               Rx 
                               i 
                             
                             ⁢ 
                             
                               Vx 
                               i 
                             
                           
                           + 
                           
                             
                               Ry 
                               i 
                             
                             ⁢ 
                             
                               Vy 
                               i 
                             
                           
                         
                         ) 
                       
                       2 
                     
                   
                   · 
                   
                     ( 
                     
                       
                         ( 
                         
                           
                             cRs 
                             i 
                           
                           + 
                           
                             
                               Vxs 
                               i 
                             
                             ⁢ 
                             
                               Rx 
                               i 
                             
                           
                           + 
                           
                             
                               Vys 
                               i 
                             
                             ⁢ 
                             
                               Ry 
                               i 
                             
                           
                         
                         ) 
                       
                       ⁢ 
                       
                         Ry 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 218 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   6.) Compute the partial derivative of the x-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with the measurement at time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 Vx 
                 i 
               
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the y-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         Vy 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Vx 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       R1 
                       ⁢ 
                       
                         
                           
                             ∂ 
                             sin 
                           
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           B1 
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                     + 
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 219 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Vy 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       R1 
                       ⁢ 
                       
                         
                           
                             ∂ 
                             cos 
                           
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           B1 
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                     + 
                     
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 220 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   7.) Compute the partial derivative of the x-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2  at t 2   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 Vx 
                 i 
               
             
             
               ∂ 
               R2 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of the y-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         Vy 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Vx 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   - 
                   
                     
                       
                         R2 
                         ⁢ 
                         
                           
                             ∂ 
                             sB2 
                           
                           
                             ∂ 
                             R2 
                           
                         
                       
                       + 
                       
                         sin 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B2 
                       
                     
                     
                       t2 
                       - 
                       t1 
                     
                   
                 
               
             
             
               
                 ( 
                 221 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       Vy 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   - 
                   
                     
                       
                         R2 
                         ⁢ 
                         
                           
                             ∂ 
                             cB2 
                           
                           
                             ∂ 
                             R2 
                           
                         
                       
                       + 
                       
                         cos 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B2 
                       
                     
                     
                       t2 
                       - 
                       t1 
                     
                   
                 
               
             
             
               
                 ( 
                 222 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   8.) Compute the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to range at t 1  with respect to the sensor associated with time line  1   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   f 
                   i 
                 
               
               
                 ∂ 
                 R1 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to range at t 2  with respect to the sensor associated with time line  2   
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 f 
                 i 
               
             
             
               ∂ 
               R2 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to base frequency 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Fb 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Rx 
                           i 
                         
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           Rx 
                           i 
                         
                       
                       
                         ∂ 
                         R1 
                       
                     
                   
                   + 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Ry 
                           i 
                         
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           Ry 
                           i 
                         
                       
                       
                         ∂ 
                         R1 
                       
                     
                   
                   + 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Vx 
                           i 
                         
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           Vx 
                           i 
                         
                       
                       
                         ∂ 
                         R1 
                       
                     
                   
                   + 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Vy 
                           i 
                         
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           Vy 
                           i 
                         
                       
                       
                         ∂ 
                         R1 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 223 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Rx 
                           i 
                         
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           Rx 
                           i 
                         
                       
                       
                         ∂ 
                         R2 
                       
                     
                   
                   + 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Ry 
                           i 
                         
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           Ry 
                           i 
                         
                       
                       
                         ∂ 
                         R2 
                       
                     
                   
                   + 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Vx 
                           i 
                         
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           Vx 
                           i 
                         
                       
                       
                         ∂ 
                         R2 
                       
                     
                   
                   + 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         
                           Vy 
                           i 
                         
                       
                     
                     ⁢ 
                     
                       
                         ∂ 
                         
                           Vy 
                           i 
                         
                       
                       
                         ∂ 
                         R2 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 224 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     Fb 
                   
                 
                 = 
                 
                   
                     
                       cRs 
                       i 
                     
                     + 
                     
                       
                         Vxs 
                         i 
                       
                       ⁢ 
                       
                         Rx 
                         i 
                       
                     
                     + 
                     
                       
                         Vys 
                         i 
                       
                       ⁢ 
                       
                         Ry 
                         i 
                       
                     
                   
                   
                     ( 
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         
                           Vxs 
                           i 
                         
                         ⁢ 
                         
                           Rx 
                           i 
                         
                       
                       + 
                       
                         
                           Vys 
                           i 
                         
                         ⁢ 
                         
                           Ry 
                           i 
                         
                       
                       + 
                       
                         
                           Rx 
                           i 
                         
                         ⁢ 
                         
                           Vx 
                           i 
                         
                       
                       + 
                       
                         
                           Ry 
                           i 
                         
                         ⁢ 
                         
                           Vy 
                           i 
                         
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 225 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   9.) Compute the estimated frequency at t i  with respect to the sensor associated with the ith measurement at the time of the ith measurement: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   f 
                   i 
                 
                 = 
                 
                   Fb 
                   ⁢ 
                   
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         
                           Vxs 
                           i 
                         
                         ⁢ 
                         
                           Rx 
                           i 
                         
                       
                       + 
                       
                         
                           Vys 
                           i 
                         
                         ⁢ 
                         
                           Ry 
                           i 
                         
                       
                     
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         VxtRx 
                         i 
                       
                       + 
                       
                         VytRy 
                         i 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 226 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   10.) Compute the frequency residual (RESf i ):
 
 RESf   i   =fm   i   −f   i   (227)
 
                    where fm i  is the measured frequency at t i . 
                   11.) Compute the normalized frequency residual (  RESf i   ) and normalized partial derivatives 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       
                         
                           ∂ 
                           
                             f 
                             i 
                           
                         
                         _ 
                       
                       
                         ∂ 
                         R1 
                       
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             f 
                             i 
                           
                         
                         _ 
                       
                       
                         ∂ 
                         R2 
                       
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             f 
                             i 
                           
                         
                         _ 
                       
                       
                         ∂ 
                         Fb 
                       
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     RESf 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESf 
                     i 
                   
                   
                     
                       σ 
                       ⁢ 
                       f 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 228 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       R1 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       f 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 229 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       R2 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       f 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 230 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Fb 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Fb 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       f 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 231 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σf i  is the standard deviation of the frequency measurement. 
                   12.) Set the next row of the augmented Jacobian H matrix to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Fb 
                     
                   
                   ⁢ 
                   
                     
                       RESf 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 232 
                 ) 
               
             
           
         
       
     
       
       
         
           d. If a range constraint is being imposed, then the following processing shall be performed:
           i. Compute the range residual (RESR):
 
 RESR=Rc−R   (233)
    where Rc is the assumed target range.   ii. Compute the normalized range residual  (RESR) :   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   RESR 
                   _ 
                 
                 = 
                 
                   RESR 
                   
                     σ 
                     ⁢ 
                     R 
                   
                 
               
             
             
               
                 ( 
                 234 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σR is the assumed target range standard deviation. 
               iii. If frequency data are not being processed, set the next row of the augmented Jacobian H Matrix to: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         
                           ∂ 
                           R 
                         
                         ⁢ 
                         
                           : 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ⁢ 
                   
                     
                       
                         
                           ∂ 
                           R 
                         
                         ⁢ 
                         
                           : 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ⁢ 
                   
                     RESR 
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 235 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               If frequency data are being processed, set the next row of the augmented Jacobian H matrix to: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         
                           ∂ 
                           R 
                         
                         ⁢ 
                         
                           : 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ⁢ 
                   
                     
                       
                         
                           ∂ 
                           R 
                         
                         ⁢ 
                         
                           : 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ⁢ 
                   0 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     RESR 
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 236 
                 ) 
               
             
           
         
       
     
       
       
         
           e. If a speed constraint is being imposed, then the following processing shall be performed:
           i. Compute the x-component of target velocity (Vxt) and the y-component of target velocity (Vyt):   
         
         
       
     
  
   
     
       
         
           
             
               
                 Vxt 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     Xs2 
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     Xs1 
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 237 
                 ) 
               
             
           
           
             
               
                 Vyt 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     Ys2 
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     Ys1 
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 238 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               ii. Compute the partial derivative of the x-component of target velocity with respect to range at t 1  with respect to the sensor associated with the measurement at time line  1   
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               Vxt 
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
                and the partial derivative of the y-component of target velocity with respect to range t 1  with respect to the sensor associated with time line  1   
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       Vyt 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     Vxt 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   - 
                   
                     
                       
                         R1 
                         ⁢ 
                         
                           
                             ∂ 
                             sB1 
                           
                           
                             ∂ 
                             R1 
                           
                         
                       
                       + 
                       
                         sin 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B1 
                       
                     
                     
                       t2 
                       - 
                       t1 
                     
                   
                 
               
             
             
               
                 ( 
                 239 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     Vxt 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   - 
                   
                     
                       
                         R1 
                         ⁢ 
                         
                           
                             ∂ 
                             sB1 
                           
                           
                             ∂ 
                             R1 
                           
                         
                       
                       + 
                       
                         cos 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B1 
                       
                     
                     
                       t2 
                       - 
                       t1 
                     
                   
                 
               
             
             
               
                 ( 
                 240 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               iii. Compute the partial derivative of the x-component of target velocity with respect to range at t 2  with respect to the sensor associated with time line  2   
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               Vxt 
             
             
               ∂ 
               R2 
             
           
           ) 
         
       
     
       
       
         
           
             
                and the partial derivative of the y-component of target velocity with respect to range at t 2  with respect to the sensor associated with time line  2   
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       
                         ∂ 
                         V 
                       
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       t 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       V 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     x 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     t 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   - 
                   
                     
                       
                         R2 
                         ⁢ 
                         
                           
                             
                               
                                 ∂ 
                                 s 
                               
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               B 
                             
                             ⁢ 
                             
                                 
                             
                           
                           
                             ∂ 
                             R2 
                           
                         
                       
                       + 
                       
                         sin 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B2 
                       
                     
                     
                       t2 
                       - 
                       t1 
                     
                   
                 
               
             
             
               
                 ( 
                 241 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       V 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     y 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     t 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   - 
                   
                     
                       
                         R2 
                         ⁢ 
                         
                           
                             
                               ∂ 
                               c 
                             
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             B2 
                           
                           
                             ∂ 
                             R2 
                           
                         
                       
                       + 
                       
                         cos 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         B2 
                       
                     
                     
                       t2 
                       - 
                       t1 
                     
                   
                 
               
             
             
               
                 ( 
                 242 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               iv. Compute the partial derivative of target speed with respect to range at t 1  with respect to the sensor associated with time line  1   
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               v 
             
             
               ∂ 
               R1 
             
           
           ) 
         
       
     
       
       
         
           
             
                and the partial derivative of target speed with respect to range at t 2  with respect to the sensor associated with time line  2   
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       v 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     v 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       V 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       x 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       t 
                       ⁢ 
                       
                         
                           
                             ∂ 
                             V 
                           
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           x 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           t 
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                     + 
                     
                       V 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       t 
                       ⁢ 
                       
                         
                           
                             ∂ 
                             V 
                           
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           y 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           t 
                         
                         
                           ∂ 
                           R1 
                         
                       
                     
                   
                   
                     
                       
                         V 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         x 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           t 
                           2 
                         
                       
                       + 
                       
                         V 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         y 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           t 
                           2 
                         
                       
                     
                   
                 
               
             
             
               
                 ( 
                 243 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     v 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       V 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       x 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       t 
                       ⁢ 
                       
                         
                           
                             ∂ 
                             V 
                           
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           x 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           t 
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                     + 
                     
                       V 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       t 
                       ⁢ 
                       
                         
                           
                             ∂ 
                             V 
                           
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           y 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           t 
                         
                         
                           ∂ 
                           R2 
                         
                       
                     
                   
                   
                     
                       
                         V 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         x 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           t 
                           2 
                         
                       
                       + 
                       
                         V 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         y 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           t 
                           2 
                         
                       
                     
                   
                 
               
             
             
               
                 ( 
                 244 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               v. Compute the estimated target speed:
 
 V=√{square root over (Vxt 2 +Vyt 2 )}   (245)
 
               vi. Compute the speed residual (RESv):
 
 RESv=Vc−V   (246)
 
                where Vc is the assumed target speed. 
               vii. Compute the normalized speed residual (  RESv ) and normalized partial derivatives 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 ( 
                 
                   
                     
                       
                         ∂ 
                         v 
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   , 
                   
                     
                       
                         ∂ 
                         v 
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                 
                 ) 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     R 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     E 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     S 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     v 
                   
                   _ 
                 
                 = 
                 
                   
                     R 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     E 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     S 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     v 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     v 
                   
                 
               
             
             
               
                 ( 
                 247 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       v 
                     
                     _ 
                   
                   
                     ∂ 
                     R1 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       v 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     v 
                   
                 
               
             
             
               
                 ( 
                 248 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       v 
                     
                     _ 
                   
                   
                     ∂ 
                     R2 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       v 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     v 
                   
                 
               
             
             
               
                 ( 
                 249 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σv is the standard deviation of the assumed target speed. 
               viii. If frequency data are not being processed, set the next row of the augmented Jacobian H matrix to: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         v 
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         v 
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ⁢ 
                   
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       E 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       S 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       v 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 250 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               If frequency data are being processed, set the next row of the augmented Jacobian H matrix to: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         v 
                       
                       _ 
                     
                     
                       ∂ 
                       R1 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         v 
                       
                       _ 
                     
                     
                       ∂ 
                       R2 
                     
                   
                   ⁢ 
                   0 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       E 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       S 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       v 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 251 
                 ) 
               
             
           
         
       
     
       
       
         
           f. Reorder the rows of the H matrix such that a zero-valued partial derivative does not appear along the diagonal. 
           g. Perform the Householder transformation on the m-by-ns+1 matrix H:
           i. Compute values of s, μ, and β as:   
         
         
       
     
  
                 s   =       -   s     ⁢           ⁢   g   ⁢           ⁢     n   ⁡     (     H   ⁡     (     1   ,   1     )       )       ⁢     (       ∑     i   =   1     m     ⁢     [       H   ⁡     (     i   ,   1     )       2     ]       )               (   252   )                 u (1)= H (1,1)− s   (253)   u ( i )= H ( i, 1)  i= 2, . . . ,m  (254) 
   
     
       
         
           
             
               
                 β 
                 = 
                 
                   1 
                   
                     s 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       u 
                       ⁡ 
                       
                         ( 
                         1 
                         ) 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 255 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               ii. For j=2, . . . , ns+1, evaluate the following equations (apply Householder transformation to the successive columns of H): 
             
           
         
       
     
  
                 γ   =     β   ⁢       ∑     i   =   1     m     ⁢       u   ⁡     (   i   )       ⁢     H   ⁡     (     i   ,   j     )                     (   256   )                 H ( i,j )= H ( i,j )+γ u ( i ) i= 1 , . . . , m   (257)         h. Extract the upper triangular matrix R from the upper left hand corner of the transformed matrix H:
 
 R ( i,i )= H ( i,i ) i= 1 , . . . ,ns   (258)
   i. Compute R −1  by back substitution:
           i. Compute R −1  (1,1) as follows:   
               
   
     
       
         
           
             
               
                 
                   
                     R 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       1 
                       , 
                       1 
                     
                     ) 
                   
                 
                 = 
                 
                   1 
                   
                     R 
                     ⁡ 
                     
                       ( 
                       
                         1 
                         , 
                         1 
                       
                       ) 
                     
                   
                 
               
             
             
               
                 ( 
                 259 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               ii. For j=2, . . . , ns perform the following: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   U 
                   ⁡ 
                   
                     ( 
                     
                       j 
                       , 
                       j 
                     
                     ) 
                   
                 
                 = 
                 
                   1 
                   
                     R 
                     ⁡ 
                     
                       ( 
                       
                         j 
                         , 
                         j 
                       
                       ) 
                     
                   
                 
               
             
             
               
                 ( 
                 260 
                 ) 
               
             
           
           
             
               
                 
                   
                     U 
                     ⁡ 
                     
                       ( 
                       
                         k 
                         , 
                         j 
                       
                       ) 
                     
                   
                   = 
                   
                     
                       - 
                       
                         ( 
                         
                           
                             ∑ 
                             
                               l 
                               = 
                               k 
                             
                             
                               j 
                               - 
                               1 
                             
                           
                           ⁢ 
                           
                             
                               U 
                               ⁡ 
                               
                                 ( 
                                 
                                   k 
                                   , 
                                   l 
                                 
                                 ) 
                               
                             
                             ⁢ 
                             
                               R 
                               ⁡ 
                               
                                 ( 
                                 
                                   l 
                                   , 
                                   j 
                                 
                                 ) 
                               
                             
                           
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       U 
                       ⁡ 
                       
                         ( 
                         
                           j 
                           , 
                           j 
                         
                         ) 
                       
                     
                   
                 
                 , 
                 
                   k 
                   = 
                   1 
                 
                 , 
                 … 
                 ⁢ 
                 
                     
                 
                 , 
                 
                   j 
                   - 
                   1 
                 
               
             
             
               
                 ( 
                 261 
                 ) 
               
             
           
         
       
     
       
       
         
           j. Set the Y vector to the last column of the transformed matrix H:
 
 Y ( i )= H ( i,n+ 1) i= 1 , . . . ,ns   (262)
 
           k. Compute the gain vector (G):
 
 G=R   −1   Y   (263)
 
           l. Determine if the gain vector is near zero. If both |G( 1 )| and |G( 2 )| are less than or equal to 0.1, then the algorithm has converged and Gauss-Newton iterations shall terminate, and processing shall resume as described in paragraph g. 
           Otherwise, processing shall continue as described below. 
           m. Limit the range changes such that the updated range estimates will be within bounds as follows:
           i. If |G( 1 )|&gt;10000 or |G( 2 )|&gt;10000, perform the following calculations up to twenty times which divide ΔR 1  and ΔR 2  by 2 until the updated range estimates will be within bounds:
 
α=1(Initialization)  (264)
 
 R 1 temp   =R 1 +αG (1)  (265)
 
 R 2 temp   =R 2 +αG (2)  (266)
   If R 1   min &lt;R 1   temp &lt;R 1   max  and R 2   min &lt;R 2   temp &lt;R 2   max , update gain vector as follows:
 
 G=αG   (267)
    and continue as described in paragraph 16n below. Otherwise, divide α by 2 and repeat the process.   ii. If |G(1)|&lt;10000 or |G(2)|&lt;10000, perform the following calculations up to twenty times which decreases ΔR 1  and ΔR 2  by 5% until the updated range estimates will be within bounds:
 
 ii= 0(Initialization)  (268)
   
         
         
       
     
  
                 α   =       100   -     5   ⁢   i   ⁢           ⁢   i     +   5     100             (   269   )                 R 1 temp   =R 1 +αG (1)  (270)   R 2 temp   =R 2 +αG (2)  (271)             If R 1   min &lt;R 1   temp &lt;R 1   max  and R 2   min &lt;R 2   temp &lt;R 2   max , update gain vector as follows:
 
 G=αG   (272)
    and continue as described in paragraph 16 below. Otherwise, increase ii by 1 and repeat the process.       n. Compute the stepsize (s) via the quadratic fit type line search as follows:
           i. This following procedure provides a method for selecting the stepsize a l  in the modified Gauss-Newton iterative formula
 
 x   l+1   =x   1   +a   1   Δx   1   (273)
    where Δx l  is the correction vector. Actually, because it is not normalized, the correction Δx l  also contributes to the size of the step. It is convenient to redefine equation (273) as
 
 x   l+1   =x   l   +a   j   Δx   l   (274)
    where a j  denotes the jth value of the step size at the lth Gauss-Newton iteration.   ii. Once Δx l  is found from the Gauss-Newton equations, the performance index PI l  is a function only of a j ,
 
 PI   l ( a   j )= PI   l ( x   l   +a   j   Δx   l )  (275)
    and this is minimized by a judicious selection of a j . Here, a j  is defined by the minimum of a quadratic polynomial which passes through three data points (a j , PI(a j ), j=1, 2, 3). For equally spaced values of a j , the step size occurring at the minimum of this quadratic is given by   
               
   
     
       
         
           
             
               
                 
                   a 
                   m 
                 
                 = 
                 
                   
                     
                       
                         ( 
                         
                           
                             a 
                             2 
                           
                           + 
                           
                             a 
                             3 
                           
                         
                         ) 
                       
                       ⁢ 
                       P 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           I 
                           l 
                         
                         ⁡ 
                         
                           ( 
                           
                             a 
                             1 
                           
                           ) 
                         
                       
                     
                     - 
                     
                       2 
                       ⁢ 
                       
                         ( 
                         
                           
                             a 
                             1 
                           
                           + 
                           
                             a 
                             3 
                           
                         
                         ) 
                       
                       ⁢ 
                       P 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           I 
                           l 
                         
                         ⁡ 
                         
                           ( 
                           
                             a 
                             2 
                           
                           ) 
                         
                       
                     
                     + 
                     
                       
                         ( 
                         
                           
                             a 
                             2 
                           
                           + 
                           
                             a 
                             1 
                           
                         
                         ) 
                       
                       ⁢ 
                       P 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           I 
                           l 
                         
                         ⁡ 
                         
                           ( 
                           
                             a 
                             3 
                           
                           ) 
                         
                       
                     
                   
                   
                     
                       2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       P 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           I 
                           l 
                         
                         ⁡ 
                         
                           ( 
                           
                             a 
                             1 
                           
                           ) 
                         
                       
                     
                     - 
                     
                       4 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       P 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           I 
                           l 
                         
                         ⁡ 
                         
                           ( 
                           
                             a 
                             2 
                           
                           ) 
                         
                       
                     
                     + 
                     
                       2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       P 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           I 
                           l 
                         
                         ⁡ 
                         
                           ( 
                           
                             a 
                             3 
                           
                           ) 
                         
                       
                     
                   
                 
               
             
             
               
                 ( 
                 276 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where a 3 &gt;a 2 ≧0. 
               iii. The first of these data points is readily available, namely, a 1 =(0,PI l (x 1 )); and if
 
 PI   l (1)&lt; PI   l (0),  (277)
 
                then a 2 =1 gives the second data point and a 3 =2a 2  gives the third. However, if equation (302) is not satisfied, the length of the interval is reduced by selecting a 2 =½ and a 3 =2a 2 =1, provided
 
 PI   l (½)&lt; PI   l (0)  (278)
 
               iv. If this is successful, the next selection is a 2 =¼ and a 3 =2a 2 =  1 / 2 , and subsequent selections are given by repeatedly dividing a 2  by 2. This continues until PI l (a 2 )&lt;PI l (a 1 ) or a threshold is crossed which causes termination of the line search. After a m  is found, then PI l (a m ), PI l (a 1 ) and PI l (a 3 ) are compared to determine which of these is the smallest. This is necessary because the quadratic polynomial may not always provide a good fit to the cost function and PI l (a 2 ) or PI l (a 3 ) may be smaller than PI l (a m ). 
             
           
           o. Update the states using the selected stepsize:
           i. Update the range states:
 
 R 1 new   =R 1 old   +sG (1)  (279)
 
 R 2 new   =R 2 old   +sG (2)  (280)
    and insure R 1   min +0.1&lt;R 1   new &lt;R 1   max −0.1 and R 2   min +0.1&lt;R 2   new &lt;R 2   max −0.1.   ii. If frequency data is being processed update the frequency state:
 
 Fb   new   =Fb   old   +sG (3)  (281)
    and insure 1&lt;Fb new .   
         
           p. Compute the new performance index (PI new ) based on the updated states (R 1   new ,R 2   new ,Fb new ). 
           q. Compute range, bearing, course and speed at tc:
           i. Compute the x-component of target velocity (Vxt) and the y-component of target velocity (Vyt):   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   V 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   x 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   t 
                 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     
                       X 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                     
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     
                       X 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 282 
                 ) 
               
             
           
           
             
               
                 
                   V 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   y 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   t 
                 
                 = 
                 
                   
                     
                       R2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B2 
                     
                     + 
                     
                       Y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s2 
                     
                     - 
                     
                       R1 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       B1 
                     
                     - 
                     
                       Y 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s1 
                     
                   
                   
                     t2 
                     - 
                     t1 
                   
                 
               
             
             
               
                 ( 
                 283 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               ii. Compute target course (Ct) and target speed (Vt): 
             
           
         
       
     
  
                   C   ⁢           ⁢   t     =       tan     -   1       ⁡     (       V   ⁢           ⁢   x   ⁢           ⁢   t       V   ⁢           ⁢   y   ⁢           ⁢   t       )               (   284   )                 Vt=√{square root over (Vxt 2 +Vyt 2 )}   (285)             iii. Compute x-component of target position at tc (Xtc) and y-component of target position at tc (Ytc):
 
 Xtc=R 2 sin  B 2 +Xs 2 +Vxt ( t 2 −tc )  (286)
 
 Ytc=R 2 cos  B 2 +Ys 2 +Vyt ( t 2 −tc )  (287)
   iv. Compute x-component of range at tc with respect to own ship (Rxoc) and y-component of range at tc with respect to own ship (Ryoc):
 
 Rxoc=Xtc−Xoc   (289)
 
 Ryoc=Ytc−Yoc   (290)
   v. Compute range at tc with respect to own ship (Roc) and true bearing at tc with respect to own ship (Boc):
 
 Roc=√{square root over (Rxoc 2 +Ryoc 2 )}   (291)
           
   
     
       
         
           
             
               
                 Boc 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       Rxoc 
                       Ryoc 
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 292 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               vi. Limit the range at tc with respect to own ship to the maximum target range constraint. 
             
           
         
       
     
  
   Propagation path hypothesis testing can be performed by the endpoint MLE algorithm on up to a maximum of four data segments which may be from different sonar arrays, and the endpoint MLE algorithm is capable of processing an additional six azimuthal bearings only or azimuthal bearing/horizontal range segments (from any array) which may be direct path only. Each segment which contains either conical angle or frequency measurements is tested to determine whether the best propagation path is a direct path or is a bottom bounce single ray reversal path. Propagation path testing is performed by Alternating the propagation path for each segment to be tested from a direct path to a bottom bounce path, running the endpoint MLE algorithm for each propagation path combination and each appropriate port/starboard combination and by saving the four best solution based on the performance index, along with the associated port/starboard indicators at the time lines and propagation paths for each segment. Thus, if there are four conical angle only segments and six azimuthal bearing segments, then the endpoint MLE may be invoked up to sixty-four times if testing all possible port/starboard combinations. If the selected time lines are associated with conical angle measurement and bearing measurements are available close in time to the conical angle measurements which can remove all port/starboard ambiguity, then the endpoint MLE will tie down to the bearing measurements and port/starboard hypothesis testing will not be performed. 
   Once the endpoint MLE has computed the four best solutions, the best solution is used to initialize the Cartesian coordinate MLE which will refine the solution using the optimal propagation path combinations. The Cartesian coordinate MLE shall be allowed to change the port/starboard designations if a particular part/starboard combination has been specified. 
   Cartesian Coordinate MLE 
   1. Initialize the number of Gauss-Newton iterations to zero. 
   2. Determine the number of state variables as follows: 
   
       
       
         
           If at least three frequency measurements are available, then frequency data will be processed, target base frequency shall be estimated and the number of state variables (ns) shall be set to five. Otherwise the number of state variables shall be four, frequency data shall not be processed and target base frequency shall not be estimated.
 
3. Initialize values for x-coordinate of target position at tm (Xtm), y-component of target position at tm (Ytm), x-component of target velocity (Vxt) and y-component of target velocity (Vyt) using the outputs from the Endpoint MLE as follows:
 
 Xtm=Roc  sin  Boc+Xoc−Vxt ( tc−tm )  (293)
 
 Ytm=Roc  cos  Boc+Yoc−Vyt ( tc−tm )  (294)
 
 Vxt=Vt  sin  Ct   (295)
 
 Vyt=Vt  cos  Ct   (296)
 
where Roc is the range at tc with respect to own ship
 
         
       
     
  
   Boc is the true bearing at tc with respect to own ship 
   Ct is the target course 
   Vt is the target speed 
   Xoc is the x-coordinate of own ship position at tc 
   Yoc is the y-coordinate of own ship position at tc 
   tm is the time of the most recent measurement 
   tc is current time 
   If frequency data are being processed, initialize the base frequency (Fb) to the base frequency output by the endpoint MLE. 
   4. Compute the Cartesian coordinate performance index (PI) based on the initial states as follows: 
   
       
       
         
           a. First, for each measurement in the batch:
           i. Compute the x-component of range at t i  with respect to the sensor associated with the ith measurement (Rx i ) and the y-component of range at t i  with respect to the sensor associated with the ith measurement (Ry i ):
 
 Rx   i   =Xtm−Vxt ( tm−t   i )− Xo   i   (297)
 
 Ry   i   =Ytm−Vyt ( tm−t   i )− Yo   i   (298)
   where Xo i  is x-coordinate of own ship position at t i      Yo i  is y-coordinate of own ship position at t i      t i  is the time of the ith measurement   tm is the time of the latest measurement   ii. Compute the range at t i  with respect to the sensor associated with the ith measurement (R i );
 
 R   i   =√{square root over (Rx i   2 +Ry i   2 )}   (299)
   iii. Compute the target image depth at t i  with respect to the sensor associated with the ith measurement (Rz i ) and D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ).   iv. If the ith measurement is an azimuthal bearing:
               1.) Compute the true bearing at t i  with respect to the sensor associated with the ith measurement (B i ):   
               
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   B 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         Rx 
                         i 
                       
                       
                         Ry 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 300 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the bearing residual (RESb i ) such that −π≦RESb i ≦π:
 
 RESb   i   =Bm   i   −B   i   (301)
 
                    where Bm i  is the ith measured bearing 
                   3.) Compute the normalized bearing residual (  RESb i   ): 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     RESb 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESb 
                     i 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       b 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 302 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σb i  is the measured bearing standard deviation 
               v. If the ith measurement is a conical angle:
               1.) Compute the target image depth at t i  with respect to the sensor associated with the ith measurement (Rz i ) and D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ).   2.) If the D/E angle associated with the conical angle measurement is valid:
                     a . Compute the true bearing at t i  with respect to the sensor associated with the ith measurement (B i ):   
                   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   B 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         Rx 
                         i 
                       
                       
                         Ry 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 303 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                         b . Compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ):
   Rs   i   =√{square root over (Rx i   2 +Ry i   2 +Rz i   2 )}   (304) 
                         c . Compute the conical angle at t i  with respect to the sensor associated with the ith measurement (β i ): 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   β 
                   i 
                 
                 = 
                 
                   
                     cos 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         ( 
                         
                           
                             cos 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               
                                 Cs 
                                 i 
                               
                               ⁡ 
                               
                                 ( 
                                 
                                   
                                     
                                       Rx 
                                       i 
                                     
                                     ⁢ 
                                     sin 
                                     ⁢ 
                                     
                                         
                                     
                                     ⁢ 
                                     
                                       Hs 
                                       i 
                                     
                                   
                                   + 
                                   
                                     
                                       Ry 
                                       i 
                                     
                                     ⁢ 
                                     cos 
                                     ⁢ 
                                     
                                         
                                     
                                     ⁢ 
                                     
                                       Hs 
                                       i 
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                           - 
                           
                             sin 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               Cs 
                               i 
                             
                             ⁢ 
                             
                               Rz 
                               i 
                             
                           
                         
                         ) 
                       
                       
                         Rs 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 305 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                       where Cs i  is the sensor cant angle at the ith measurement 
                       Hs i  is the sensor heading at the ith measurement 
                         d . Compute the conical angle (RESβ i ) such that −π≦RESβ i ≦π:
   RESβ   i   =βm   i −β i   (306) 
                        where βm i  is the ith measured conical angle 
                         e . Compute the normalized conical angle residual (  RESβ i   ): 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     RESb 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESb 
                     i 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       b 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 307 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        where σβ i  is the measured conical angle standard deviation. 
                     
                   
                 
               
               vi. If the ith measurement is a range:
               1.) Compute the range residual (RESr i ):
 
 RESr   i   =Rm   i   −R   i   (308)
    where Rm i  is the ith measured range   2.) Compute the normalized range residual (  RESr i   ):   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     RESr 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESr 
                     i 
                   
                   
                     
                       σ 
                       ⁢ 
                       r 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 309 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σr i  is the measured range standard deviation 
                 
               
               vii. If frequency data are being processed and the ith measurement is a frequency:
               1.) Compute the x-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement (Vx i ) and the y-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement (Vy i ):
 
 Vx   i   =Vxt−Vxs   i   (310)
 
 Vy   i   =Vyt−Vys   i   (311)
    where Vxs i  is the x-component of sensor velocity at t i  
                   Vys i  is the y-component of sensor velocity at t i      
                   
             
               2.) Compute the target image depth at t i  with respect to the sensor associated with the ith measurement (Rz i ) and D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ) 
               3.) If the D/E angle associated with the frequency is valid, compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ):
 
 Rs   i   =√{square root over (R i   2 +Rz i   2 )}   (312)
 
               4.) Compute the estimated frequency at t i  with respect to the sensor associated with the ith measurement: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   f 
                   i 
                 
                 = 
                 
                   Fb 
                   ⁢ 
                   
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         
                           Vxs 
                           i 
                         
                         ⁢ 
                         
                           Rx 
                           i 
                         
                       
                       + 
                       
                         
                           Vys 
                           i 
                         
                         ⁢ 
                         
                           Ry 
                           i 
                         
                       
                     
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         VxtRx 
                         i 
                       
                       + 
                       
                         VytRy 
                         i 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 313 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               5.) Compute the frequency residual (RESf i )
 
 RESf   i   =fm   i   −f   i   (314)
 
                where fm i  is the ith measured frequency 
               6.) Compute the normalized frequency residual (  RESf i   ): 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     RESf 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESf 
                     i 
                   
                   
                     
                       σ 
                       ⁢ 
                       f 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 315 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σf i  is the measured frequency standard deviation. 
             
           
           b. If a range constraint is being imposed, then the following computations shall be performed:
           i. Compute the range residual (RESr):
 
 RESr=Rc−R   (316)
    where Rc is the assumed target range   ii. Compute the normalized speed residual (  RESr ):   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   RESr 
                   _ 
                 
                 = 
                 
                   RESr 
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     R 
                   
                 
               
             
             
               
                 ( 
                 317 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σR is the assumed target range standard deviation. 
             
           
           c. If a speed constraint is being imposed, then the following computations shall be performed.
           i. Compute the estimated target speed:
 
 V=√{square root over (Vxt 2 +Vyt 2 )}   (318)
   ii. Compute the speed residual (RESv):
 
 RESv=Vc−V   (319)
    where Vc is the assumed target speed   iii. Compute the normalized speed residual (  RESv ):   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   RESv 
                   _ 
                 
                 = 
                 
                   RESv 
                   
                     σ 
                     ⁢ 
                     V 
                   
                 
               
             
             
               
                 ( 
                 320 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σV is the assumed target speed standard deviation. 
             
           
           d. Compute the performance index as one half of the sum of the squared normalized residuals.
 
5. Gauss-Newton iterations shall be performed as described in paragraphs a through n below, until the algorithm converges as described in paragraph n or until twenty-five iterations have been performed.
 
           a. For each measurement in the batch:
           i. Compute the x-component of range at t i  with respect to the sensor associated with the ith measurement (Rx i ) and the y-component of range at t i  with respect to the sensor associated with the ith measurement (Ry i ):
 
 Rx   i   =Xtm−Vxt ( tm−t   i )− Xo   i   (321)
 
 Ry   i   =Ytm−Vyt ( tm−t   i )− Yo   i   (322)
   where Xo i  is x-coordinate of own ship position at t i      Yo i  is y-coordinate of own ship position at t i      t i  is the time of the ith measurement   ii. Compute the range at t i  with respect to the sensor associated with the ith measurement (R i ):   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   R 
                   i 
                 
                 = 
                 
                   
                     
                       Rx 
                       i 
                       2 
                     
                     + 
                     
                       Ry 
                       i 
                       2 
                     
                   
                 
               
             
             
               
                 ( 
                 323 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               iii. Compute the target image depth at t i  with respect to the sensor associated with the ith measurement (Rz i ) and D/E angle at t i  with respect to the sensor associated with the ith measurement (θ I ) 
               iv. If the ith measurement is an azimuthal bearing:
               1.) Compute the partial derivative of true bearing at t i  with respect to the sensor associated with the ith measurement with respect to the x-coordinate of target position at   
             
             
           
         
       
     
  
   
     
       
         
           
             
               t 
               m 
             
             ⁡ 
             
               ( 
               
                 
                   ∂ 
                   
                     B 
                     i 
                   
                 
                 
                   ∂ 
                   Xtm 
                 
               
               ) 
             
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    the partial derivative of true bearing at t i  with respect to the sensor associated with the ith measurement with respect to the y-coordinate of target position at t m   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   B 
                   i 
                 
               
               
                 ∂ 
                 Ytm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    the partial derivative of true bearing at t i  with respect to the sensor associated with the ith measurement with respect to the x-component of target velocity 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   B 
                   i 
                 
               
               
                 ∂ 
                 Vxt 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of true bearing at t i  with respect to the sensor associated with the ith measurement with respect to the y-component of target velocity 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       Yxt 
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       B 
                       i 
                     
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 
                   
                     Ry 
                     i 
                   
                   
                     R 
                     i 
                     2 
                   
                 
               
             
             
               
                 ( 
                 324 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       B 
                       i 
                     
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 
                   - 
                   
                     
                       Rx 
                       i 
                     
                     
                       R 
                       i 
                       2 
                     
                   
                 
               
             
             
               
                 ( 
                 325 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       B 
                       i 
                     
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                 
               
             
             
               
                 ( 
                 326 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       B 
                       i 
                     
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                 
               
             
             
               
                 ( 
                 327 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the bearing residual (RESb i ) such that −π≦RESb i ≦π:
 
 RESb   i   =Bm   i   −B   i   (328)
 
                    where Bm i  is the ith measured bearing 
                   3.) Compute the normalized bearing residual (  RESb i   ) and normalized partial derivative 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       
                         
                           ∂ 
                           
                             B 
                             i 
                           
                         
                         _ 
                       
                       
                         ∂ 
                         Xtm 
                       
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             B 
                             i 
                           
                         
                         _ 
                       
                       
                         ∂ 
                         Ytm 
                       
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             B 
                             i 
                           
                         
                         _ 
                       
                       
                         ∂ 
                         Vxt 
                       
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             B 
                             i 
                           
                         
                         _ 
                       
                       
                         ∂ 
                         Vyt 
                       
                     
                   
                   ) 
                 
                 ⁢ 
                 
                   : 
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     RESb 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESb 
                     i 
                   
                   
                     
                       σ 
                       ⁢ 
                       b 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 329 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       b 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 330 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       b 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 331 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       b 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 332 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         B 
                         i 
                       
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       b 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 333 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σb i  is the measured bearing standard deviation 
                   4.) If frequency data are not being processed, then set the next row of the augmented Jacobian matrix H to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ⁢ 
                   
                     
                       RESB 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 334 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   If frequency data are being processed, then set the next row of the augmented Jacobian matrix H to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ⁢ 
                   0 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESB 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 335 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               v. If the ith measurement is a conical angle:
               1.) Compute the target image depth at t i  with respect to the sensor associated with the ith measurement (Rz i ) and D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ).   2.) If the D/E angle associated with the conical angle measurement is valid:
                     a  Compute the true bearing at t i  with respect to the sensor associated with the ith measurement (B i ):   
                   
             
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   B 
                   i 
                 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         Rx 
                         i 
                       
                       
                         Ry 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 336 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                         b  Compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ):
   Rs   i   =√{square root over (Rx i   2 +Ry i   2 +Rz i   2 )}   (337) 
                         c  Compute the conical angle at t i  with respect to the sensor associated with the ith measurement (β i ): 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   β 
                   i 
                 
                 = 
                 
                   
                     cos 
                     
                       - 
                       2 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       
                         ( 
                         
                           
                             cos 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               
                                 Cs 
                                 i 
                               
                               ⁡ 
                               
                                 ( 
                                 
                                   
                                     
                                       Rx 
                                       i 
                                     
                                     ⁢ 
                                     sin 
                                     ⁢ 
                                     
                                         
                                     
                                     ⁢ 
                                     
                                       Hs 
                                       i 
                                     
                                   
                                   + 
                                   
                                     
                                       Ry 
                                       i 
                                     
                                     ⁢ 
                                     cos 
                                     ⁢ 
                                     
                                         
                                     
                                     ⁢ 
                                     
                                       Hs 
                                       i 
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                           - 
                           
                             sin 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               Cs 
                               i 
                             
                             ⁢ 
                             
                               Rz 
                               i 
                             
                           
                         
                         ) 
                       
                       
                         Rs 
                         i 
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 338 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        where Cs i  is the sensor cant angle at the ith measurement and Hs i  is the sensor heading at the ith measurement 
                         d  Compute the partial derivative of conical angle at t i  with respect to the sensor associated with the ith measurement with respect to the x-coordinate of target position at t m   
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   β 
                   i 
                 
               
               
                 ∂ 
                 Xtm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        the partial derivative of conical angle at t i  with respect to the sensor associated with the ith measurement with respect to the y-coordinate of target position at tm 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   β 
                   i 
                 
               
               
                 ∂ 
                 Ytm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        the partial derivative of conical angle at t i  with respect to the sensor associated with the ith measurement with respect to the x-component of target velocity 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   β 
                   i 
                 
               
               
                 ∂ 
                 Vxt 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        and the partial derivative of conical angle at t i  with respect to the sensor associated with the ith measurement with respect to the y-component of target velocity 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   β 
                   i 
                 
               
               
                 ∂ 
                 Vyt 
               
             
             ) 
           
           ⁢ 
           
             : 
           
         
       
     
     
       
         
           
             
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 
                   - 
                   
                     
                       
                         
                           
                             ( 
                             
                               cos 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               
                                 
                                   Cs 
                                   i 
                                 
                                 ( 
                                 
                                   
                                     
                                       ( 
                                       
                                         
                                           Ry 
                                           i 
                                           2 
                                         
                                         + 
                                         
                                           Rz 
                                           i 
                                           2 
                                         
                                       
                                       ) 
                                     
                                     ⁢ 
                                     sin 
                                     ⁢ 
                                     
                                         
                                     
                                     ⁢ 
                                     
                                       Hs 
                                       i 
                                     
                                   
                                   - 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   
                                     Rx 
                                     i 
                                   
                                   ⁢ 
                                   
                                     Ry 
                                     i 
                                   
                                   ⁢ 
                                   cos 
                                   ⁢ 
                                   
                                       
                                   
                                   ⁢ 
                                   
                                     Hs 
                                     i 
                                   
                                 
                                 ) 
                               
                               + 
                               
                                 
                                   Rx 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Rz 
                                   i 
                                 
                                 ⁢ 
                                 sin 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   Cs 
                                   i 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         β 
                         i 
                       
                       ⁢ 
                       
                         R 
                         i 
                         3 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 339 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 
                   - 
                   
                     
                       
                         
                           
                             ( 
                             
                               cos 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               
                                 
                                   Cs 
                                   i 
                                 
                                 ( 
                                 
                                   
                                     
                                       ( 
                                       
                                         
                                           Rx 
                                           i 
                                           2 
                                         
                                         + 
                                         
                                           Rz 
                                           i 
                                           2 
                                         
                                       
                                       ) 
                                     
                                     ⁢ 
                                     cos 
                                     ⁢ 
                                     
                                         
                                     
                                     ⁢ 
                                     
                                       Hs 
                                       i 
                                     
                                   
                                   - 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   
                                     Rx 
                                     i 
                                   
                                   ⁢ 
                                   
                                     Ry 
                                     i 
                                   
                                   ⁢ 
                                   sin 
                                   ⁢ 
                                   
                                       
                                   
                                   ⁢ 
                                   
                                     Hs 
                                     i 
                                   
                                 
                                 ) 
                               
                               + 
                               
                                 
                                   Ry 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Rz 
                                   i 
                                 
                                 ⁢ 
                                 sin 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   Cs 
                                   i 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                     
                       sin 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         β 
                         i 
                       
                       ⁢ 
                       
                         R 
                         i 
                         3 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 340 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                 
               
             
             
               
                 ( 
                 341 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                 
               
             
             
               
                 ( 
                 342 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                         e  Compute the conical angle (RESβ i ) such that −π≦RESβ i ≦π:
   RESβ   i   =βm   i −β i   (343) 
                        where βm i  is the ith measured conical angle. 
                         f  Compute the normalized conical angle residual (  RESβ i   ) and normalized partial derivatives 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   _ 
                 
                 
                   ∂ 
                   Xtm 
                 
               
               , 
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   _ 
                 
                 
                   ∂ 
                   Ytm 
                 
               
               , 
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   _ 
                 
                 
                   ∂ 
                   Vxt 
                 
               
               , 
               
                 
                   
                     ∂ 
                     
                       β 
                       i 
                     
                   
                   _ 
                 
                 
                   ∂ 
                   Vyt 
                 
               
             
             ) 
           
           ⁢ 
           
             : 
           
         
       
     
     
       
         
           
             
               
                 
                   
                     RESβ 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESβ 
                     i 
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 344 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 345 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 346 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 347 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         β 
                         i 
                       
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   
                     σβ 
                     i 
                   
                 
               
             
             
               
                 ( 
                 348 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                        where σβ I  is the measured conical angle standard deviation 
                         g  If frequency data are not being processed, set the next row of the augmented Jacobian matrix H to: 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ⁢ 
                   
                     
                       RESβ 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 349 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   
                     
                       If frequency data are being processed, then set the next row of the augmented Jacobian matrix H to: 
                     
                   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           B 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ⁢ 
                   0 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESβ 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 350 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               vi. If the ith measurement is a range:
               1.) Compute the partial derivative of range at t i  with respect to the sensor associated with the ith measurement with respect to the x-coordinate of target position at t m     
             
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   R 
                   i 
                 
               
               
                 ∂ 
                 Xtm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    the partial derivative of range at t i  with respect to the sensor associated with the ith measurement with respect to the y-coordinate of target position at t m   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   R 
                   i 
                 
               
               
                 ∂ 
                 Ytm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    the partial derivative of range at t i  with respect to the sensor associated with the ith measurement with respect to the x-component of target velocity 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   R 
                   i 
                 
               
               
                 ∂ 
                 Vxt 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of range at t i  with respect to the sensor associated with the ith measurement with respect to the y-component of target velocity 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   R 
                   i 
                 
               
               
                 ∂ 
                 Vyt 
               
             
             ) 
           
           ⁢ 
           
             : 
           
         
       
     
     
       
         
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 
                   
                     Rx 
                     i 
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 351 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 
                   
                     Ry 
                     i 
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 352 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                 
               
             
             
               
                 ( 
                 353 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                 
               
             
             
               
                 ( 
                 354 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) Compute the range residual (RESr i ):
 
 RESr   i   =Rm   i   −R   i   (355)
 
                    where Rm i  is the ith measured range 
                   3.) Compute the normalized range residual (  RESr i   ) and normalized partial derivative 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   _ 
                 
                 
                   ∂ 
                   Xtm 
                 
               
               , 
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   _ 
                 
                 
                   ∂ 
                   Ytm 
                 
               
               , 
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   _ 
                 
                 
                   ∂ 
                   Vxt 
                 
               
               , 
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   _ 
                 
                 
                   ∂ 
                   Vyt 
                 
               
             
             ) 
           
           ⁢ 
           
             : 
           
         
       
     
     
       
         
           
             
               
                 
                   
                     RESr 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESr 
                     i 
                   
                   
                     
                       σ 
                       ⁢ 
                       r 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 356 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       r 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 357 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       r 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 358 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       r 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 359 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   
                     
                       σ 
                       ⁢ 
                       r 
                     
                     i 
                   
                 
               
             
             
               
                 ( 
                 360 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σr i  is the measured range standard deviation. 
                   4.) If frequency data are not being processed, then set the next row of the augmented Jacobian matrix H to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         Xtm 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         Ytm 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         Vxt 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         Vyt 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESr 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 361 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   If frequency data are being processed, then set the next row of the augmented Jacobian matrix H to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         Xtm 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         Ytm 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         Vxt 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       
                         ∂ 
                         Vyt 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   0 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESr 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 362 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               vii. If frequency data are being processed and the ith measurement is a frequency:
               1.) Compute the x-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement (Vx i ) and the y-component of target relative velocity at t i  with respect to the sensor associated with the ith measurement (Vy i ):
 
 Vx   i   =Vxt−Vxs   i   (363)
 
 Vy   i   =Vyt−Vys   i   (364)
    where Vxs i  is the x-component of sensor velocity at t i  and Vys i  is the y-component of sensor velocity at t i .   2.) Compute the target image depth at t i  with respect to the sensor associated with the ith measurement (Rz i ) and D/E angle at t i  with respect to the sensor associated with the ith measurement (θ i ).   3.) If the D/E angle associated with the frequency is valid, compute the slant range at t i  with respect to the sensor associated with the ith measurement (Rs i ):
 
 Rs   i   √{square root over (R i   2 +Rz i   2 )}   (365)
   4.) Compute the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to the x-coordinate of target position at t m     
             
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   f 
                   i 
                 
               
               
                 ∂ 
                 Xtm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to the y-coordinate of target position at t m   
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   f 
                   i 
                 
               
               
                 ∂ 
                 Ytm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to the x-component of target velocity 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   f 
                   i 
                 
               
               
                 ∂ 
                 Vxt 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
               
                 
                    the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to the y-component of target velocity 
                 
               
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               
                 f 
                 i 
               
             
             
               ∂ 
               Vyt 
             
           
           ) 
         
       
     
       
       
         
           
             
               
                 
                    and the partial derivative of frequency at t i  with respect to the sensor associated with the ith measurement with respect to base frequency 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     ( 
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         Fb 
                       
                     
                     ) 
                   
                   : 
                 
                 ⁢ 
                 
                     
                 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         Xtm 
                       
                     
                     = 
                     
                       Fb 
                       
                         
                           ( 
                           
                             
                               cRs 
                               i 
                             
                             + 
                             
                               
                                 Vxs 
                                 i 
                               
                               ⁢ 
                               
                                 Rx 
                                 i 
                               
                             
                             + 
                             
                               
                                 Vys 
                                 i 
                               
                               ⁢ 
                               
                                 Ry 
                                 i 
                               
                             
                             + 
                             
                               
                                 Rx 
                                 i 
                               
                               ⁢ 
                               
                                 Vx 
                                 i 
                               
                             
                             + 
                             
                               
                                 Ry 
                                 i 
                               
                               ⁢ 
                               
                                 Vy 
                                 i 
                               
                             
                           
                           ) 
                         
                         2 
                       
                     
                   
                     
                 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                     
                   
                     • 
                     ⁢ 
                     
                         
                         
                     
                     ⁢ 
                     
                       ( 
                       
                         
                           
                             ( 
                             
                               
                                 
                                   Rx 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Vx 
                                   i 
                                 
                               
                               + 
                               
                                 
                                   Ry 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Vy 
                                   i 
                                 
                               
                             
                             ) 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 
                                   c 
                                   
                                     Rs 
                                     i 
                                   
                                 
                                 ⁢ 
                                 
                                   Rx 
                                   i 
                                 
                               
                               + 
                               
                                 Vxs 
                                 i 
                               
                             
                             ) 
                           
                         
                         - 
                         
                           
                             ( 
                             
                               
                                 cRs 
                                 i 
                               
                               + 
                               
                                 
                                   Vxs 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Rx 
                                   i 
                                 
                               
                               + 
                               
                                 
                                   Vys 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Ry 
                                   i 
                                 
                               
                             
                             ) 
                           
                           ⁢ 
                           
                             Vx 
                             i 
                           
                         
                       
                       ) 
                     
                   
                 
               
             
             
               
                 ( 
                 366 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         Ytm 
                       
                     
                     = 
                     
                       Fb 
                       
                         
                           ( 
                           
                             
                               cRs 
                               i 
                             
                             + 
                             
                               
                                 Vxs 
                                 i 
                               
                               ⁢ 
                               
                                 Rx 
                                 i 
                               
                             
                             + 
                             
                               
                                 Vys 
                                 i 
                               
                               ⁢ 
                               
                                 Ry 
                                 i 
                               
                             
                             + 
                             
                               
                                 Rx 
                                 i 
                               
                               ⁢ 
                               
                                 Vx 
                                 i 
                               
                             
                             + 
                             
                               
                                 Ry 
                                 i 
                               
                               ⁢ 
                               
                                 Vy 
                                 i 
                               
                             
                           
                           ) 
                         
                         2 
                       
                     
                   
                     
                 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                     
                   
                     • 
                     ⁢ 
                     
                         
                         
                     
                     ⁢ 
                     
                       ( 
                       
                         
                           
                             ( 
                             
                               
                                 
                                   Rx 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Vx 
                                   i 
                                 
                               
                               + 
                               
                                 
                                   Ry 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Vy 
                                   i 
                                 
                               
                             
                             ) 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 
                                   c 
                                   
                                     Rs 
                                     i 
                                   
                                 
                                 ⁢ 
                                 
                                   Ry 
                                   i 
                                 
                               
                               + 
                               
                                 Vys 
                                 i 
                               
                             
                             ) 
                           
                         
                         - 
                         
                           
                             ( 
                             
                               
                                 cRs 
                                 i 
                               
                               + 
                               
                                 
                                   Vxs 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Rx 
                                   i 
                                 
                               
                               + 
                               
                                 
                                   Vys 
                                   i 
                                 
                                 ⁢ 
                                 
                                   Ry 
                                   i 
                                 
                               
                             
                             ) 
                           
                           ⁢ 
                           
                             Vy 
                             i 
                           
                         
                       
                       ) 
                     
                   
                 
               
             
             
               
                 ( 
                 367 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                 
               
             
             
               
                 ( 
                 368 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                 
               
             
             
               
                 ( 
                 369 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       f 
                       i 
                     
                   
                   
                     ∂ 
                     Fb 
                   
                 
                 = 
                 
                   
                     
                       cRs 
                       i 
                     
                     + 
                     
                       
                         Vxs 
                         i 
                       
                       ⁢ 
                       
                         Rx 
                         i 
                       
                     
                     + 
                     
                       
                         Vys 
                         i 
                       
                       ⁢ 
                       
                         Ry 
                         i 
                       
                     
                   
                   
                     ( 
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         
                           Vxs 
                           i 
                         
                         ⁢ 
                         
                           Rx 
                           i 
                         
                       
                       + 
                       
                         
                           Vys 
                           i 
                         
                         ⁢ 
                         
                           Ry 
                           i 
                         
                       
                       + 
                       
                         
                           Rx 
                           i 
                         
                         ⁢ 
                         
                           Vx 
                           i 
                         
                       
                       + 
                       
                         
                           Ry 
                           i 
                         
                         ⁢ 
                         
                           Vy 
                           i 
                         
                       
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 370 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where c is the average speed of sound 
                   5.) Compute the estimated frequency at t i  with respect to the sensor associated with the ith measurement: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   f 
                   i 
                 
                 = 
                 
                   Fb 
                   ⁢ 
                   
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         
                           Vxs 
                           i 
                         
                         ⁢ 
                         
                           Rx 
                           i 
                         
                       
                       + 
                       
                         
                           Vys 
                           i 
                         
                         ⁢ 
                         
                           Ry 
                           i 
                         
                       
                     
                     
                       
                         cRs 
                         i 
                       
                       + 
                       
                         VxtRx 
                         i 
                       
                       + 
                       
                         VytRy 
                         i 
                       
                     
                   
                 
               
             
             
               
                 ( 
                 371 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   6.) Compute the frequency residual (RESf i ):
 
 RESf   i   =fm   i   −f   i   (372)
 
                    where fm i  is the ith measured frequency 
                   7.) Compute the normalized frequency residual (  RESf i   ) and normalized partial derivatives 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   [ 
                   
                     
                       
                         
                           ∂ 
                           
                             f 
                             i 
                           
                         
                         
                           ∂ 
                           Xtm 
                         
                       
                       _ 
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             f 
                             i 
                           
                         
                         
                           ∂ 
                           Ytm 
                         
                       
                       _ 
                     
                     , 
                     
                         
                     
                     ⁢ 
                     
                       
                         
                           ∂ 
                           
                             f 
                             i 
                           
                         
                         
                           ∂ 
                           Vxt 
                         
                       
                       _ 
                     
                     , 
                     
                         
                     
                     ⁢ 
                     
                       
                         
                           ∂ 
                           
                             f 
                             i 
                           
                         
                         
                           ∂ 
                           Vyt 
                         
                       
                       _ 
                     
                     , 
                     
                         
                     
                     ⁢ 
                     
                       
                         
                           ∂ 
                           
                             f 
                             i 
                           
                         
                         
                           ∂ 
                           Fb 
                         
                       
                       _ 
                     
                   
                   ] 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       f 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 374 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       f 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 375 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       f 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 376 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       f 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 377 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Fb 
                     
                   
                   _ 
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         f 
                         i 
                       
                     
                     
                       ∂ 
                       Fb 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       f 
                       i 
                     
                   
                 
               
             
             
               
                 ( 
                 378 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                    where σf i  is the measured frequency standard deviation. 
                   8.) Set the next row of the augmented Jacobian matrix H to: 
                 
               
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         Xtm 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         Ytm 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         Vxt 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         Vyt 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           f 
                           i 
                         
                       
                       
                         ∂ 
                         Fb 
                       
                     
                     _ 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESf 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 379 
                 ) 
               
             
           
         
       
     
       
       
         
           b. If a range constraint is being imposed, the following computations shall be performed:
           i. Compute the partial derivative of range at t i  with respect to the sensor associated with the ith measurement with respect to the x-coordinate of target position at t m     
         
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   R 
                   i 
                 
               
               
                 ∂ 
                 Xtm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
                the partial derivative of range at t i  with respect to the sensor associated with the ith measurement with respect to the y-coordinate of target position at t m   
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   R 
                   i 
                 
               
               
                 ∂ 
                 Ytm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
                the partial derivative of range at t i  with respect to the sensor associated with the ith measurement with respect to the x-component of target velocity 
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 
                   R 
                   i 
                 
               
               
                 ∂ 
                 Vxt 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
                and the partial derivative of range at t i  with respect to the sensor associated with the ith measurement with respect to the y-component of target velocity 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 
                   
                     Rx 
                     i 
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 380 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 
                   
                     Ry 
                     i 
                   
                   
                     R 
                     i 
                   
                 
               
             
             
               
                 ( 
                 381 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                 
               
             
             
               
                 ( 
                 382 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     
                       R 
                       i 
                     
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     - 
                     
                       ( 
                       
                         
                           t 
                           m 
                         
                         - 
                         
                           t 
                           i 
                         
                       
                       ) 
                     
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                 
               
             
             
               
                 ( 
                 383 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               ii. Compute the range residual (RESr i ):
 
 RESr   i   =Rc−R   (384)
 
                where Rc is the assumed target range 
               iii Compute the normalized range residual (  RESr i   ) and normalized partial derivatives 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   [ 
                   
                     
                       
                         
                           ∂ 
                           
                             R 
                             i 
                           
                         
                         
                           ∂ 
                           Xtm 
                         
                       
                       _ 
                     
                     ⁢ 
                     
                         
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             R 
                             i 
                           
                         
                         
                           ∂ 
                           Ytm 
                         
                       
                       _ 
                     
                     , 
                     
                         
                     
                     ⁢ 
                     
                       
                         
                           ∂ 
                           
                             R 
                             i 
                           
                         
                         
                           ∂ 
                           Vxt 
                         
                       
                       _ 
                     
                     , 
                     
                         
                     
                     ⁢ 
                     
                       
                         
                           ∂ 
                           
                             R 
                             i 
                           
                         
                         
                           ∂ 
                           Vyt 
                         
                       
                       _ 
                     
                   
                   ] 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     RESr 
                     i 
                   
                   _ 
                 
                 = 
                 
                   
                     RESr 
                     i 
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     R 
                   
                 
               
             
             
               
                 ( 
                 385 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     R 
                   
                 
               
             
             
               
                 ( 
                 386 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     R 
                   
                 
               
             
             
               
                 ( 
                 387 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     R 
                   
                 
               
             
             
               
                 ( 
                 388 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     _ 
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       
                         R 
                         i 
                       
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     R 
                   
                 
               
             
             
               
                 ( 
                 389 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σR is the measured range standard deviation. 
               iv. If frequency data are not being processed, then set the next row of the augmented Jacobian matrix H to: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ⁢ 
                   
                     
                       RESr 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 390 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               If frequency data are being processed, then set the next row of the augmented Jacobian matrix H to: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         
                           R 
                           i 
                         
                       
                       _ 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   0 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       RESr 
                       i 
                     
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 391 
                 ) 
               
             
           
         
       
     
       
       
         
           c. If a speed constraint is being imposed, the following computations shall be performed:
           i. Compute the estimated target speed:
 
 V+√{square root over (Vxt 2 +Vyt 2 )}   (392)
   ii. Compute the partial derivative of target speed with respect to the x-coordinate of target position at t m     
         
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 V 
               
               
                 ∂ 
                 Xtm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
                the partial derivative of target speed with respect to the y-coordinate of target position at t m   
             
           
         
       
     
  
   
     
       
         
           
             ( 
             
               
                 ∂ 
                 V 
               
               
                 ∂ 
                 Ytm 
               
             
             ) 
           
           , 
         
       
     
       
       
         
           
             
                the partial derivative of target speed with respect to the x-component of target velocity 
             
           
         
       
     
  
   
     
       
         
           ( 
           
             
               ∂ 
               V 
             
             
               ∂ 
               Vxt 
             
           
           ) 
         
       
     
       
       
         
           
             
                and the partial derivative of target speed with respect to the y-component of target velocity 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   ( 
                   
                     
                       ∂ 
                       V 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ) 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   
                     ∂ 
                     V 
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 0 
               
             
             
               
                 ( 
                 393 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     V 
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 0 
               
             
             
               
                 ( 
                 394 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     V 
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   Vxt 
                   V 
                 
               
             
             
               
                 ( 
                 395 
                 ) 
               
             
           
           
             
               
                 
                   
                     ∂ 
                     V 
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   Vyt 
                   V 
                 
               
             
             
               
                 ( 
                 396 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               iii. Compute the speed residual (RESv):
 
 RES   v   =Vc−V   (397)
 
                where Vc is the assumed target speed 
               iv. Compute the normalized speed residual (  RESv ) and normalized partial derivatives 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 
                   [ 
                   
                     
                       
                         
                           ∂ 
                           V 
                         
                         _ 
                       
                       
                         ∂ 
                         Xtm 
                       
                     
                     , 
                     
                         
                     
                     ⁢ 
                     
                       
                         
                           ∂ 
                           V 
                         
                         _ 
                       
                       
                         ∂ 
                         Ytm 
                       
                     
                     , 
                     
                         
                     
                     ⁢ 
                     
                       
                         
                           ∂ 
                           V 
                         
                         _ 
                       
                       
                         ∂ 
                         Vxt 
                       
                     
                     , 
                     
                         
                     
                     ⁢ 
                     
                       
                         
                           ∂ 
                           V 
                         
                         _ 
                       
                       
                         ∂ 
                         Vyt 
                       
                     
                   
                   ] 
                 
                 : 
               
             
             
               
                   
               
             
           
           
             
               
                 
                   RESv 
                   _ 
                 
                 = 
                 
                   RESv 
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     R 
                   
                 
               
             
             
               
                 ( 
                 398 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       V 
                     
                     _ 
                   
                   
                     ∂ 
                     Xtm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       V 
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     V 
                   
                 
               
             
             
               
                 ( 
                 399 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       V 
                     
                     _ 
                   
                   
                     ∂ 
                     Ytm 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       V 
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     V 
                   
                 
               
             
             
               
                 ( 
                 400 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       V 
                     
                     _ 
                   
                   
                     ∂ 
                     Vxt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       V 
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     V 
                   
                 
               
             
             
               
                 ( 
                 401 
                 ) 
               
             
           
           
             
               
                 
                   
                     
                       ∂ 
                       V 
                     
                     _ 
                   
                   
                     ∂ 
                     Vyt 
                   
                 
                 = 
                 
                   
                     
                       ∂ 
                       V 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   
                     σ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     V 
                   
                 
               
             
             
               
                 ( 
                 402 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
                where σV is the assumed target speed standard deviation. 
               v. If frequency data are not being processed, then set the next row of the augmented Jacobian matrix H to: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         V 
                       
                       _ 
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         V 
                       
                       _ 
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         V 
                       
                       _ 
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         V 
                       
                       _ 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     RESv 
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 403 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               If frequency data are being processed, then set the next row of the augmented Jacobian matrix H to: 
             
           
         
       
     
  
   
     
       
         
           
             
               
                 [ 
                 
                   
                     
                       
                         ∂ 
                         V 
                       
                       _ 
                     
                     
                       ∂ 
                       Xtm 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         V 
                       
                       _ 
                     
                     
                       ∂ 
                       Ytm 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         V 
                       
                       _ 
                     
                     
                       ∂ 
                       Vxt 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         ∂ 
                         V 
                       
                       _ 
                     
                     
                       ∂ 
                       Vyt 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   0 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     RESv 
                     _ 
                   
                 
                 ] 
               
             
             
               
                 ( 
                 404 
                 ) 
               
             
           
         
       
     
       
       
         
           d. Reorder the rows of the matrix H such that a zero valued partial derivative does not appear along the diagonal. 
           e. Perform the Householder transformation on the m×n+1 matrix H. 
           f. Extract the upper triangular matrix R from the upper left hand corner of the transformed matrix H. 
           g. Compute R −1  by back-substitution. 
           h. Extract the Y vector from the upper right hand corner of the transformed matrix H. 
           i. Compute the gain vector (G):
 
 G=R   −1   Y   (405)
 
           j. Determine if the gain is near zero. If both |G( 1 )| and |G( 2 )| are less than 0.1 and |G( 3 )| and |G( 4 )| are less than 0.01, then the algorithm has converged, Gauss Newton iterations shall terminate, and processing shall be performed as described in paragraph 6. Otherwise, processing shall continue as described below. 
           k. Compute the stepsize as described in n. above. 
           l. Update the states using the optimal stepsize(s):
           i. Update the position and velocity states:
 
 Xtm=Xtm+sG (1)  (406)
 
 Ytm=Ytm+sG (2)  (407)
 
 Vxt=Vxt+sG (3)  (408)
 
 Vyt=Vyt+sG (4)  (409).
   ii. If frequency data are being processed, update the frequency state:
 
 Fb=Fb+sG (5)  (410)
   iii Compute range with respect to own ship at t m  (Rom) and target speed (Vt) as
 
 Rom =√{square root over (( Xtm−Xom ) 2 +( Ytm−Yom ) 2 )}{square root over (( Xtm−Xom ) 2 +( Ytm−Yom ) 2 )}  (411)
 
 Vt=√{square root over (Vxt 2 +Vyt 2 )}   (412)
   iv. Insure R min +0.1&lt;Rom&lt;R max  and V min +0.1&lt;Vt&lt;V max . If either Rm or Vt is out of bounds, limit the appropriate parameter and recompute Xtm, Ytm, Vxt and Vyt.   
         
           m. Compute the new performance index (PI new ) based on the updated states (Xtm, Ytm, Vxt, Vyt, Fb) 
           n. Compute range, bearing, course and speed at current time:
           i. Compute target course (Ct) and target speed (Vt):   
         
         
       
     
  
                 Ct   =       tan     -   1       ⁡     (     Vxt   Vyt     )               (   413   )                 Vt=√{square root over (Vxt 2 +Vyt 2 )}   (414)             ii. Compute x-coordinate of target position at tc(Xtc) and y-coordinate of target position at tc(Ytc):
 
 Xtc=Xtm+Vxt ( tc−tm )  (415)
 
 Ytc=Ytm+Vyt ( tc−tm )  (416)
   iii. Compute x-component of range at tc(Rxc) and y-component of range at tc with respect to own ship (Ryc):
 
 Rxc=Xtc−Xoc   (417)
 
 Ryc=Ytc−Yoc   (418)
    where Xoc is the x-coordinate of own ship position at tc and Yoc is the y-coordinate of own ship position at tc.   iv. Compute range at tc with respect to own ship (Rc) and true bearing at tc with respect to own ship (Bc):
 
 Rc=√{square root over (Rxc 2 +Ryc 2 )}   (419)
           
   
     
       
         
           
             
               
                 Bc 
                 = 
                 
                   
                     tan 
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     ( 
                     
                       Rxc 
                       Ryc 
                     
                     ) 
                   
                 
               
             
             
               
                 ( 
                 420 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               v. Limit range at tc with respect to own ship to a maximum of the target maximum range. 
               vi. Limit target speed to a maximum of the target maximum speed. 
             
           
           o. Determine if the change in the performance index is negligible. If so, processing shall terminate, otherwise, Gauss-Newton iterations shall continue.
           i. Compute change in the performance index (ΔPI):   
         
         
       
     
  
   
     
       
         
           
             
               
                 
                   
                     
                       
                         1 
                         ) 
                       
                       . 
                       
                           
                       
                       ⁢ 
                       If 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       PI 
                       old 
                     
                   
                   &gt; 
                   0 
                 
                 , 
               
             
             
               
                   
               
             
           
           
             
               
                 
                     
                 
                 ⁢ 
                 
                   
                     Δ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     PI 
                   
                   = 
                   
                     
                        
                       
                         
                           PI 
                           new 
                         
                         - 
                         
                           PI 
                           old 
                         
                       
                        
                     
                     
                       PI 
                       old 
                     
                   
                 
               
             
             
               
                 ( 
                 421 
                 ) 
               
             
           
         
       
     
       
       
         
           
             
               
                 
                   2.) If PI old =0,
 
Δ PI= 0  (422)
 
                 
               
               ii. If ΔPI≦0.00001 and PI new ≦threshold cc , stop iterating.
 
6. Compute the ns by ns Cartesian coordinate covariance matrix:
 
 P=R   −1   R   −T   (423)
 
7. Extrapolate the covariance matrix forward to current time:
 
             
           
           a. If frequency data are not being processed, the transition matrix Φ shall be defined as follows: 
         
       
     
  
   
     
       
         
           
             
               
                 ϕ 
                 = 
                 
                   [ 
                   
                     
                       
                         1 
                       
                       
                         0 
                       
                       
                         
                           tc 
                           - 
                           tm 
                         
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         1 
                       
                       
                         0 
                       
                       
                         
                           tc 
                           - 
                           tm 
                         
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                       
                         1 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                       
                         0 
                       
                       
                         1 
                       
                     
                   
                   ] 
                 
               
             
             
               
                 ( 
                 424 
                 ) 
               
             
           
         
       
     
       
       
         
           b. If frequency data are being processed, the transition matrix Φ shall be defined as follows: 
         
       
     
  
   
     
       
         
           
             
               
                 ϕ 
                 = 
                 
                   [ 
                   
                     
                       
                         1 
                       
                       
                         0 
                       
                       
                         
                           tc 
                           - 
                           tm 
                         
                       
                       
                         0 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         1 
                       
                       
                         0 
                       
                       
                         
                           tc 
                           - 
                           tm 
                         
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                       
                         1 
                       
                       
                         0 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                       
                         0 
                       
                       
                         1 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                       
                         0 
                       
                       
                         0 
                       
                       
                         1 
                       
                     
                   
                   ] 
                 
               
             
             
               
                 ( 
                 425 
                 ) 
               
             
           
         
       
     
       
       
         
           c. The covariance matrix at tc shall be extrapolated as follows:
 
 p=φPφ   T   (426)
 
8. Compute target range, bearing, course, speed and base frequency standard deviations:
 
         
       
     
  
                   σ   R     =             P   11     ⁢     Rxc   2       +     2   ⁢     P   12     ⁢   RxcRyc     +       P   22     ⁢     Ryc   2           Rc   2                 (   427   )                 σ   B     =             P   11     ⁢     Ryc   2       -     2   ⁢     P   12     ⁢   RxcRyc     +       P   22     ⁢     Rxc   2           Rc   4                 (   428   )                 σ   C     =             P   33     ⁢     Vyt   2       -     2   ⁢     P   34     ⁢   Vxt   *   Vyt     +       P   44     ⁢     Vyt   2           Vt   4                 (   429   )                 σ   S     =             P   33     ⁢     Vxt   2       +     2   ⁢     P   34     ⁢   Vxt   *   Vyt     +       P   44     ⁢     Vyt   2           Vt   2                 (   430   )               
If frequency data are being processed.
 σ F   =√{square root over (P 55 )}   (431) 
9. Compute major and minor localization ellipse axis length (X maj , X min ) and orientation of major axis from North (ORIEN):
 
                   λ   maj     =         P   11     +     P   22     +           (       P   11     -     P   22       )     2     +     4   ⁢     P   12   2             2             (   432   )                 λ     m   ⁢           ⁢   i   ⁢           ⁢   n       =         P   11     +     P   12     -           (       P   11     -     P   22       )     2     +     4   ⁢     P   12   5             2             (   433   )                 x   maj     =     2.1459   ⁢       λ   maj                 (   434   )                 x     m   ⁢           ⁢   i   ⁢           ⁢   n       =     2.1459   ⁢       λ     m   ⁢           ⁢   i   ⁢           ⁢   n                   (   435   )               ORIEN   =       tan     -   1       ⁡     [       P   12         λ   maj     -     P   11         ]               (   436   )               
10. Outputting to a display computer.
 
   It will be understood that many additional changes in the details, materials, steps and arrangement of parts, which have been herein described and illustrated in order to explain the nature of the invention, may be made by those skilled in the art within the principle and scope of the invention as expressed in the appended claims.