# Patent

## Title
Correlation and 3D-tracking of objects by pointing sensors

## Abstract

A method and system for tracking at least one object using a plurality of pointing sensors and a tracking system are disclosed herein. In a general embodiment, the tracking system is configured to receive a series of observation data relative to the at least one object over a time base for each of the plurality of pointing sensors. The observation data may include sensor position data, pointing vector data and observation error data. The tracking system may further determine a triangulation point using a magnitude of a shortest line connecting a line of sight value from each of the series of observation data from each of the plurality of sensors to the at least one object, and perform correlation processing on the observation data and triangulation point to determine if at least two of the plurality of sensors are tracking the same object. Observation data may also be branched, associated and pruned using new incoming observation data.

## Background
GOVERNMENT INTEREST
This invention was developed under Contract DE-AC04-94AL85000 between Sandia Corporation and the U.S. Department of Energy. The U.S. Government has certain rights in this invention.


TECHNICAL FIELD
The present disclosure relates object tracking. More specifically, the present disclosure is directed to two-dimensional (2D) correlation and three-dimensional (3D) tracking of objects.
BACKGROUND
Object tracking is commonly used in satellite, aerospace and other applications (e.g., land, sea), where sensors are arranged to track one or more objects. When a plurality of sensors are used, pointing errors may often cause sensors to misalign in their line-of-sight (LOS), which in turn causes tracking errors, since the two sensors do not recognize they are pointing at the same object. During 3D tracking, this may further introduce velocity errors in a tracking system. Accordingly, there is a need in the art to provide more efficient and/or accurate 2D and 3D tracking.
SUMMARY
In view of the foregoing and other exemplary problems, drawbacks, and disadvantages of the conventional methods and structures, a robust, novel approach is disclosed for determining whether two pointing sensors are detecting the same object. As mentioned above, pointing errors cause the LOS from the sensors to miss each other even when pointing to the same object. Knowledge of sensor pointing errors provides an estimate of the miss distance distribution and a confidence that the sensors are detecting the same object. Weighted least squares triangulation may be used to estimate the position of the object and its standardized residual is used to estimate final confidence that the objects are the same.
For moving objects, observations can be used to estimate position and velocity. Triangulation points from fits of the observations from the individual sensors are used to update the tracks of moving objects. This significantly reduces the velocity errors of the resulting 3D track estimates. The correlation algorithm is deployed in a new system and in a tracking system being developed that uses the triangulation updates of tracks.
A robust, novel approach is also disclosed for determining whether two pointing sensors are detecting the same object. In addition, for moving objects, a 3D track is disclosed using triangulation of the pointing vectors from the different sensors. The correlation of observations from different pointing sensors depends on characterization of the pointing errors of the sensors and the fact that without pointing errors the lines of sight from the object to the sensors would intersect in the point that is the current location of the object.
In first order effects, the distribution of the miss-distance of pointing vectors from two sensors can be determined from the pointing errors of the sensors and is normally distributed. The realized miss-distance for a pair of observations can thus be used to assess the consistency with the hypothesis that the sensors are detecting the same object. Furthermore, if one wants to accept as correlated at least 95% of those that actually are correlated, then the pair of observations should be accepted as correlated if the miss-distance is less than the top 5% of miss-distances according to the estimated distribution.
If second order effects cannot be ignored or if refraction significantly bends the lines of sight, weighted least squares iteration for the triangulation point can be used starting with the estimated intersection point from the miss-distance calculation. The standardized residual from the least squares triangulation is normally distributed and thus can be used to assess whether the sensors are detecting the same object. Using pointing errors expected from the sensors of interest, the correlation confidence based on the normalized miss-distance and based on the standardized residual may be very close. The correlation cases that are relevant include both moving and nonmoving objects.
For pairs of non-moving objects, the correlation calculation in one embodiment is done for a single time point in the middle of the observation set overlap for the two sensors. Fits are used, if possible, to smooth out the random errors with fit errors and tighten the miss-distance distribution. For a moving object and a nonmoving object, there are three cases: non-mover before, during or after-mover. For the non-mover during the mover case, fits are used to obtain observations at a common time and assessment is the same as for two non-movers. For the non-mover before or after the mover a fit to the mover 2D track is used to extrapolate backward or forward to the time of the non-mover and the assessment is performed using the extrapolated point. For two moving targets that overlap in time, a time near the beginning and a time near the end of the overlap are used to assess correlation and the correlation confidences are fused to make the final assessment of correlation. When two movers do not overlap in time and the gap between them is not too large, correlation is assessed by extrapolating from one to the other both ways, assessing correlation confidence for both extrapolations and fusing them to make the final assessment of correlation.
A 3D tracker is also disclosed for combining 2D observations from multiple sensors into 3D tracks. First, 2D observation tracks from one sensor are correlated to 2D observation track from another sensor using the correlation algorithm described above for movers that overlap in time. Assessing the correlation of 2D tracks significantly reduces the computation load from that required to assess the correlation of individual observations from the sensor and allows smoothing of the observations sets, which can reduce the state vector errors for the resulting 3D track. A set of triangulation points are generated to initialize an Extended Kalman Filter based 3D track. After track initialization, the track can be extended by adding 2D observations one at a time or by adding additional triangulation points. It has been found that adding triangulation points instead of single 2D observations reduces the error in estimated object velocity because the random observation error has been replaced by fit error from the triangulation. This also tends to reduce the position error in the estimated 3D track.
Further scope of applicability of the present disclosure will become apparent from the detailed description given hereinafter. However, it should be understood that the detailed description and specific examples, while indicating preferred embodiments, are given by way of illustration only, since various changes and modifications within the spirit and scope of the invention will become apparent to those skilled in the art from this detailed description.



BRIEF DESCRIPTION OF THE FIGURES
The present disclosure will become more fully understood from the detailed description given herein below and the accompanying drawings which are given by way of illustration only, and thus, do not limit the present disclosure, and wherein:
FIG. 1 illustrates an exemplary system for tracking objects using pointer sensors that are part of a processor-based tracking system;
FIG. 2 illustrates an embodiment of small angle approximation for angular errors perpendicular to the epipolar plane via a line of sight and triangulation point;
FIG. 3 is an embodiment illustrating that, for very small angular errors, errors in an intersection point in a plane perpendicular to a line of sight can be processed in terms of an angular error for a pointing vector in the epipolar plane, an angular error for a pointing vector in the epipolar plane and an angle between the lines of sight;
FIG. 4 illustrates an embodiment for a geometry for associating 2D tracks to establish a confidence that observations are consistent with being caused by the same source utilizing shortest distance between lines of sight, angular error, range from sensor to target, target position, sensor pointing vector, and sensor position vector; and
FIG. 5 is an exemplary process for 3D fusion tracking under an exemplary embodiment.



The figures and descriptions provided herein may have been simplified to illustrate aspects that are relevant for a clear understanding of the herein described devices, systems, and methods, while eliminating, for the purpose of clarity, other aspects that may be found in typical devices, systems, and methods. Those of ordinary skill may recognize that other elements and/or operations may be desirable and/or necessary to implement the devices, systems, and methods described herein. Because such elements and operations are known in the art, and because they do not facilitate a better understanding of the present disclosure, a discussion of such elements and operations may not be provided herein. However, the present disclosure is deemed to inherently include all such elements, variations, and modifications to the described aspects that would be known to those of ordinary skill in the art.
DETAILED DESCRIPTION
Turning to FIG. 1, an exemplary embodiment is illustrated showing a plurality of pointer sensors (201A-201n) that may be incorporated in one or more sensor arrays 201 that may be part of a satellite-based, aeronautical-based, land and/or water-based system comprising a processor-based tracking system 210. The sensors of 201 in this example may be tasked with detecting objects (OBJ1-OBJ3) and tracking and/or correlating specific objects of interest 202 (OBJ1, OBJ2) any of which may be still and/or or moving in specific directions (illustrated by arrows in FIG. 1).
Correlation of Events Seen by Multiple Pointing Sensors
The processes for determining whether detections from separated pointing sensors are due to the same physical event are described below. Observation errors may lead to a statistical approach for assessing correlation, and different time bases for the sensors require methods to estimate observation data from each sensor at common time. The minimum distance between the pointing lines of sight from two sensors at the common time is used in a screening step. Candidate correlations that pass the screen are accessed using the standardized residuals from weighted least squares triangulation for the location of the event at the common time.
In one exemplary embodiment, for each target (object), the pointing sensors (e.g., 201A-201B) under consideration produce a series of observations each of which contains sensor position, pointing vector and observation error characterization at a specified time. If there were no observation errors, the line of sight from one sensor would intersect the line of sight from the other sensor at the target location for any specified time. The observation errors lead to a finite minimum distance between the lines of sight even if the sensors are pointing at the same target. Similarly, least squares triangulation using the sensor position and pointing vectors has non-zero residual errors because of the observation errors. Since the time base and observation frequency of the separate sensors are independent, appropriate techniques, depending on the particular case, should be selected to obtain observations at a common time to perform the correlation assessment.
The sections below disclose various aspects of operation, including component uncertainties that contribute to the observation error and how they are combined into final covariance matrix for the sensor pointing vectors, screening based on the minimum distance between lines of sight, correlation assessment by weighted least squares triangulation of the pointing vectors and the application of the algorithms to various cases of stationary and moving targets (objects) where the observation sets from the sensors overlap in time and where the observations are separated in time.
Uncertainties
Sources of uncertainty for the pointing sensors of interest are

    
    
        Sensor position 3 dimensional error
        Platform attitude 3 dimensional error
        Instrument pointing 2 dimensional error
        Centroiding 2 dimensional error
        Distortion 2 dimensional error
    
    


The position, attitude and instrument pointing error vary slowly compared to the duration of observations for a typical target and can be treated as bias errors. The centroiding and distortion errors tend to vary with each observation and can be treated as random errors. The 3D errors can be projected onto a plane perpendicular to the pointing vector to determine the effective 2D covariance matrices which then can be combined with the other 2D errors to calculate the total pointing error covariances. An advantageous representation of the pointing error covariance matrix under one embodiment is as a 2D ellipse, where the axes are the square roots of the eigenvalues of the covariance matrix (i.e. the standard deviations) and the orientation angle, θ, is the rotation that diagonalizes the matrix. In the applications below it is assumed that component errors are independent and normally distributed. Bias errors may dominate for the sensors of interest.
When enough observations are available, numerical fits to the data are used to synthesize observations at a common time for correlation assessment. For interpolation cases where the observations from the sensors overlap in time small order polynomial fits are used and the resulting fit errors replace the random errors in the calculation of pointing error covariance. These fit errors follow a t-distribution rather than normal distribution under one embodiment. However, the t-distribution approaches the normal distribution if the number of fit points is large, and the normally distributed bias errors are dominant for the sensors of interest. This will be discussed further in the sections on applications to cases with temporally overlapping observations.
Kalman filters are used in one embodiment to predict an observation from one sensor at a time for which there is an observation from the other sensor when the sets of observations from the sensors do not overlap in time. In these cases, the Kalman estimation of the covariance matrix is used in place of the random error covariances in the calculation of total observation error covariance. This will be discussed further in the sections on applications to cases with temporally separated observations.
Screening Based on Lines of Sight Intersection Distance
Two different reported lines of sight for the same event would intersect if there were no measurement errors to form an epipolar plane that can be defined by the focal centers of the two sensors vj and vk, and the location of the event (i.e., object), or triangulation point Tjk.
Measurement errors may cause the lines of sight not to intersect. Due to the two-axis configuration of many gimbaled sensor systems, pointing errors are often measured in two orthogonal angular directions about a computed pointing vector pi and expressed in terms of a zero-mean bivariate normal distribution about pi. A confidence region associated with a bivariate normal distribution can be geometrically represented by an angular error ellipse centered at vi having normal pi. The following discussion assumes that atmospheric refraction can be ignored so that the pointing vector reported for a sensor observation of an event represents a straight line of sight. A later section will discuss some approaches for dealing with the bending of the light path due to refraction.
Minimum Distance Between Lines of Sight
Given measurement errors and assuming the lines of sight are not parallel, there will be a single shortest line connecting them. A closed form solution for that line exits and a selected point on the line will be used to initialize the full least squares determination of the intersection point.
For purposes of explanation, it is assumed that all vectors in the following discussion are expressed in a common coordinate system unless otherwise specified. Consider n>1 satellite (or spacecraft, or any suitable vehicle, craft or device utilizing tracking) each having a sensor viewing a potential common target. Further let vi, i=0 . . . n-1, be the Cartesian coordinates of the focal center of each of the respective sensors and pi, i=0 . . . n-1, the unit pointing vectors to the target from each of the respective satellites. There are 2n−1 pairs of sensors. The following considers only single pairs.
Two unique points may be determined, one on each of the three-space lines emanating from vj and vk, 0<=j<k<n, along the pointing vectors pj and pk respectively such that the distance between these points is a minimum. It should be noted that the line joining these two unique points is perpendicular to both of pj and pk. Thus, it suffices to find positive real values tj and tk such that

(vj+tjpj−vk−tkpk)•pj=0

and

(vj+tjpj−vk−tkpk)•pk=0

where • is three-dimensional dot (vector) product.

Expanding,

vj•pj+tjpj•pj−vk•pj−tkpk•pj=0

and

vj•pk+tjpj•pk−vk•pk−tkpk•pk=0.

Taking advantage of the fact that the pi are unit vectors

vj•pj+tj−vk•pj−tkpk•pj=0

and

vj•pk+tjpj•pk−vk•pk−tk=0

so that

tj=vk•pj+tkpk•pj−vj•pj=pj•(vk+tkpk−vj)

and

vj•pk+pj•(vk+tkpk−vj)pj•pk−vk•pk−tk=0.

Solving,

tk((pj•pk)2−1)=(vj•pj)(pj•pk)+vk•pk−vj•pk−(vk•pj)(pk•pj)

results in





  
    t
    k
  
  =
  
    
      
        
          (
          
            
              p
              j
            
            ·
            
              p
              k
            
          
          )
        
        ⁢
        
          (
          
            
              p
              j
            
            ·
            
              (
              
                
                  v
                  j
                
                -
                
                  v
                  k
                
              
              )
            
          
          )
        
      
      +
      
        
          p
          k
        
        ·
        
          (
          
            
              v
              k
            
            -
            
              v
              j
            
          
          )
        
      
    
    
      
        
          (
          
            
              p
              j
            
            ·
            
              p
              k
            
          
          )
        
        2
      
      -
      1
    
  




Note, pj•pk cannot be unity if vj and vk are distinct and pj and pk are pointing at the same target. The vector with minimum magnitude connecting lines of sight j and k is

Mjk=vk+tkpk−vj−tjpj j≠k 

Error analysis
Let Djk be magnitude of shortest line connecting the lines of sight, Mjk. If the two lines of sight do indeed intersect, the magnitude should be 0. Thus, Djk is a measure of the error parallel to Mjk and thus perpendicular to the epipolar plane.
Now, suppose that an elliptic error cone, eeci, in terms of an origin at vi and angular error ellipse ei, (ai,bi,θi) can be determined for each pointing vector pi, i=0, . . . ,n-1. Here, ai is the standard deviation in the major along the major axis, bi is the standard deviation minor axis and θi is the orientation angle of the major axis. Consider pair-wise distinct pointing vectors pj and pk, with scalars tj and tk defined as above. Then, an error ellipse corresponding to pj can be obtained by intersecting the elliptic error cone eecj with a plane located a distance tj (in the direction of pj) from the satellite and normal to pj. Similarly, define an error ellipse corresponding to pk. The convenient error axes for analyzing the j and k lines of sight are the normal to the epipolar plane, and a normal to pj in the epipolar plane for j and the normal to the epipolar plane, and a normal to pk in the epipolar plane for k. In the following, these will be denoted by the vj, hj, vk and hk subscripts respectively. Let γaj and γbj be the angular errors along the major and minor axis of ej and γak and γbk be the angular errors along the major and minor axis of ek. Define γvj as the projection of the angular errors γaj and γbj onto the normal to the epipolar plane and γhj as the projection of the angular errors γaj and γbj onto the epipolar plane. Similarly define γvk and γhk. Thus:

γvj=γaj cos θjk+γbj sin θjk 

γhj=γaj sin θjk+γbj cos θjk 

γvk=γak cos θkj+γbk sin θkj 

γhk=γak sin θkj+γbk cos θkj 

where θjk is the angle between the normal to the epipolar plane and the major axis of ej and θkj is the angle between the normal to the epipolar plane and the major axis of ek.

Turning to FIG. 2, it can be appreciated by those skilled in the art that using the small angle approximation for angular errors perpendicular to the epipolar plane γvj and γvk, the magnitude of Mjk in is

Djk=|tjγvj−tkγvk|,

where, as before, tj and tk are the ranges from the sensor to the shortest line connecting lines of sight Lj and Lk.

As is discussed in greater detail below in connection with “Second Order LOS Intersection Distance Errors”, when the second order terms are small




  (
  
    
      i
      .
      e
      .
      
        
          γ
          hj
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
    
    ⪡
    
      1
      ⁢
      
        
      
      ⁢
      and
      ⁢
      
        
      
      ⁢
      
        
          γ
          hk
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
    
    ⪡
    1
  
  )




the expression for Djk becomes:

Djk=γTE|=|tjAγaj cos θjk+tjAγbj sin θjk−tkAγak cos θkj+tkAγbk sin θkj|=|tjAγvj−tkAγvk|

where

γT=[γaj,γbj,γak,γbk],

ET=[tjA cos θjk,tjA sin θjk,tkA cos θkj,tkA sin θkj],

tjA and tkA are the actual ranges from the sensors to the target. Thus, to first order, only errors perpendicular to the epipolar plane contribute to Djk.

Initial Estimate of Triangulation Point for Two Pointing Sensors
The best estimate of the triangulation point, given the measured Djk, would minimize the sum of their weighted square residual errors under one exemplary embodiment. The quantity to be minimized may be:




  
    ∑
    
      e
      2
    
  
  ⁢
  
    =
    
      
        γ
        T
      
      ⁢
      
        C
        
          -
          1
        
      
      ⁢
      γ
    
  




where γ is the vector of angular errors and C is their covariance matrix.

Using only first order errors is equivalent to assuming that the triangulation point lies along somewhere on the shortest line between the lines of sight. This removal of the coupling between the errors in the epipolar plane and those perpendicular to it allows a simple least squares approach to generate an initial triangulation point for input to the full weighted least squares procedure described below, which takes the second order effects into account.
Assuming γvj and γvk are independent, the quantity to be minimized is now:




  
    ∑
    
      e
      2
    
  
  ⁢
  
    =
    
      
        
          (
          
            
              γ
              vj
            
            
              ρ
              vj
            
          
          )
        
        2
      
      +
      
        
          (
          
            
              γ
              vk
            
            
              ρ
              vk
            
          
          )
        
        2
      
    
  




where

ρvj2=(aj cos θjk)2+(bj sin θjk)2 and ρvk2=(ak cos θkj)2+(bk sin θkj)2 

Assuming that measurements have provided vj, vk, pj and pk, as well as estimates of the error ellipses ej and ek, then, estimates of Djk, tjA, tkA, ρvj and ρvk can be generated. These may be used to calculate values of γvj and γvk that minimize the weighted square angular errors for the estimated distance error Djk.
To first order errors




  
    γ
    vk
  
  =
  
    
      
        -
        
          D
          jk
        
      
      +
      
        
          t
          jA
        
        ⁢
        
          γ
          vj
        
      
    
    
      t
      kA
    
  




giving





  
    ∑
    
      e
      2
    
  
  ⁢
  
    =
    
      
        
          γ
          vj
          2
        
        ⁡
        
          (
          
            
              1
              
                ρ
                vj
                2
              
            
            +
            
              
                t
                jA
                2
              
              
                
                  t
                  kA
                  2
                
                ⁢
                
                  ρ
                  vk
                  2
                
              
            
          
          )
        
      
      -
      
        
          γ
          vk
        
        ⁡
        
          (
          
            
              2
              ⁢
              
                
              
              ⁢
              
                t
                jA
              
              ⁢
              
                D
                jk
              
            
            
              
                t
                kA
                2
              
              ⁢
              
                ρ
                ka
                2
              
            
          
          )
        
      
      +
      
        
          D
          jk
          2
        
        
          
            t
            kA
            2
          
          ⁢
          
            ρ
            vk
            2
          
        
      
    
  




setting





  
    
      d
      
        ∑
        
          e
          2
        
      
    
    
      d
      
        γ
        vj
      
    
  
  =
  
    
      
        2
        ⁢
        
          
            γ
            vj
          
          ⁡
          
            (
            
              
                1
                
                  ρ
                  vj
                  2
                
              
              +
              
                
                  t
                  jA
                  2
                
                
                  
                    t
                    kA
                    2
                  
                  ⁢
                  
                    ρ
                    vk
                    2
                  
                
              
            
            )
          
        
      
      -
      
        
          2
          ⁢
          
            t
            jA
          
          ⁢
          
            D
            jk
          
        
        
          
            t
            kA
            2
          
          ⁢
          
            ρ
            vk
            2
          
        
      
    
    =
    0
  




gives




  
    
      γ
      
        v
        ⁢
        
          
        
        ⁢
        j
      
    
    =
    
      
        
          t
          jA
        
        ⁢
        
          ρ
          vj
          2
        
        ⁢
        
          D
          jk
        
      
      
        
          
            t
            jA
            2
          
          ⁢
          
            ρ
            vj
            2
          
        
        +
        
          
            t
            kA
            2
          
          ⁢
          
            ρ
            vk
            2
          
        
      
    
  




  
    
      γ
      vk
    
    =
    
      
        
          
            -
            
              t
              kA
            
          
          ⁢
          
            ρ
            vk
            2
          
          ⁢
          
            D
            jk
          
        
        
          
            
              t
              jA
              2
            
            ⁢
            
              ρ
              vj
              2
            
          
          +
          
            
              t
              kA
              2
            
            ⁢
            
              ρ
              vk
              2
            
          
        
      
      .
    
  



Defining

σvj=tjAρvj 

σvk=tkAρvk 

and

σvjk=√{square root over (σvj2+σvk2)}

gives





  
    
      t
      jA
    
    ⁢
    
      γ
      vj
    
  
  =
  
    
      
        
          σ
          vj
          2
        
        ⁢
        
          D
          jk
        
      
      
        
          σ
          vj
          2
        
        +
        
          σ
          vk
          2
        
      
    
    =
    
      
        
          
            
              
                σ
                vj
                2
              
              ⁢
              
                D
                jk
              
            
            
              σ
              vjk
              2
            
          
          .
          
            

          
          ⁢
          
            t
            kA
          
        
        ⁢
        
          γ
          vk
        
      
      =
      
        
          
            
              σ
              vk
              2
            
            ⁢
            
              D
              jk
            
          
          
            
              σ
              vj
              2
            
            +
            
              σ
              vk
              2
            
          
        
        =
        
          -
          
            
              
                σ
                vk
                2
              
              ⁢
              
                D
                jk
              
            
            
              σ
              vjk
              2
            
          
        
      
    
  




The initial estimate of the triangulation point based on sensors j and k is




  
    T
    jk
  
  =
  
    
      v
      j
    
    +
    
      
        t
        jA
      
      ⁢
      
        p
        j
      
    
    +
    
      
        
          M
          jk
        
        
          D
          jk
        
      
      ⁢
      
        t
        jA
      
      ⁢
      
        γ
        vj
      
    
  




Correlation Screening Using Minimum Distance
The closed form solution for Djk in terms of vj, vk, pj and pk, together with estimates of ρvj and ρvk allow a screening statistical test of whether the error normal to the epipolar plane is consistent with the intersection of Lj and Lk (i.e. they are pointing to the same event). Recall that to first order errors,

Djk=|tjAγvj−tkAγvk|

Let N(μ,σ2) be the normal distribution with mean μ and standard deviation σ. Then since γvj and γvk are from different sensors, they are assumed to be independent. This leads to

tjAγvj−tkAγvk ̃N(0,σvj2+σvk2)=N(0,σvjk2)

Thus, the normalized minimum distance between the lines of sight

nDjk=Djk/σvjk ̃|N(0,1)|

provides a statistic for estimating the confidence that particular measured Djk from intersection of Lj and Lk is consistent with the hypothesis that two observations from different sensors are from the same event.

The confidence that nDjk is consistent with correlation of the two observations

Conf=1−P(x≦nDjk)=1−erf(nDjk/√{square root over (2)})

where x is the random variable for the normalized minimum distance between lines of sight and erf(s/√{square root over (2)}) is the cumulative probability for the absolute value of a normally distributed variable, s. For example, 5% confidence implies that 95% of the time actually correlated observations would have a normalized minimum distance between the lines of sight less than or equal to the observed nDjk.

The final assessment under one embodiment of whether of the Lj and Lk point to the same event must still wait until the full least squares procedure is completed because the pointing errors in the epipolar plane have not yet been taken into account and nor have possible refraction effects which bend the line of sight violating the straight line assumption of the minimum intersection distance calculation. Here, the confidence is set lower than the desired confidence cutoff because the minimum distance calculation is used as a screen to determine whether to perform the least squares triangulation that is employed to perform the final assessment of correlation. The screening calculation also provides an estimate of the triangulation point to initialize the least squares calculation, and estimates of parameters such as range from target to sensors that are used in refining some calculations.
Assessment Based on Triangulation Standardized Residuals
Weighted least squares approaches are particularly advantageous in multiple-image data point triangulation. In this section, a summary of the weighted least squares formulation is provided below followed application to the triangulation points for multiple sensors seeing the same event.
Exemplary Formulation
In one exemplary embodiment, assume

l={tilde over (l)}+e, 

where l is a vector of n unperturbed observations, e is a vector of a priori unknown residuals and the actual observations {tilde over (l)} are normally distributed observations with

{tilde over (l)} ̃N(l,Σll),

where l is the mean of the distribution and Σll denotes the covariance matrix. Letting x be a vector of u model parameters, the maximum likelihood estimators for model parameters are found by solving the nonlinear equality constrained problem

minimize[Θ=eTQll−1e]subject to f(x,l)=0.

where Qll is the cofactor matrix found from a normalization Σll=σ02Qll, σ02 is the a priori standard deviation of a unit weight observation. The inverted cofactor matrix can be interpreted as a weight matrix. Linearization of the problem allows an iterative solution given an initial guess of the model parameters {circumflex over (x)}(0) and with a first estimate {circumflex over (l)}(0)={tilde over (l)} for the unperturbed observations. The iteration is stopped if the improvement (s)={circumflex over (x)}(s+1)−{circumflex over (x)}(s) at step s is smaller than a specified threshold. The estimates for the unperturbed observations are updated at each step by (s)={circumflex over (l)}(s+1)−{circumflex over (l)}(s). When stopping the iteration at step s*, the final estimates of the unknowns are {circumflex over (l)}(s*+1) and {circumflex over (x)}(s*+1). A final estimate for the residuals e is obtained through e={circumflex over (l)}(s*+1)−{tilde over (l)}.

The linearized form of the constraint functions is

AΔx+BΔl+w=0,

where A=∂f/∂x is an r×u matrix and B=∂f/∂l is an r×n and w=−f({circumflex over (x)}(s),{circumflex over (l)}(s)) is a vector of r function residuals.

The solutions for  and  are

=(ATQww−1A)31 1ATQww−1w, 

and

=QllBTQww−1(−A(ATQww−1A)−1ATQww−1)w, 

where Qww=BQllBT simplifies the notation and can be interpreted as the cofactor matrix of the function residuals. The cofactors Qww can be converted to covariances Σww by multiplying Qww with





  
    
      
        σ
        0
        2
      
      ^
    
    =
    
      
        
          
            e
            T
          
          ^
        
        ⁢
        
          Q
          ll
          
            -
            1
          
        
        ⁢
        
          e
          ^
        
      
      v
    
  
  ,




where v is the number of degrees of freedom of the estimation problem. If the data and the model agree,  is an unbiased estimator of σ02. Note that multiplying Qww with σ02 instead of  results in a priori covariances. Thus, the solutions for  and  can be written

=(ATΣww−1A)−1ATΣww−1w, 

and

=ΣllBTΣww−1(−A(ATΣww−1A)−1ATΣww−1)w, 

where Σww=BΣllBT again simplifies the notation and can be interpreted as the a priori covariance matrix of the function residuals. In practice, {circumflex over (l)}(s+1) can often be calculated from a sensor model using the new value {circumflex over (x)}(s+1) removing the need to use the approximate  above.

Propagation of the cofactors of the function residuals into the spaces of the unknowns yields:




  
    
      Q
      
        
          
            Δ
            ⁢
            
              
            
            ⁢
            x
          
          ^
        
        ⁢
        
          
            Δ
            ⁢
            
              
            
            ⁢
            x
          
          ^
        
      
    
    =
    
      
        
          (
          
            
              A
              T
            
            ⁢
            
              Q
              ww
              
                -
                1
              
            
            ⁢
            A
          
          )
        
        
          -
          1
        
      
      =
      
        
          (
          
            
              
                
                  A
                  T
                
                ⁡
                
                  (
                  
                    
                      BQ
                      ll
                    
                    ⁢
                    
                      B
                      T
                    
                  
                  )
                
              
              
                -
                1
              
            
            ⁢
            A
          
          )
        
        
          -
          1
        
      
    
  
  ,




and





  
    Q
    
      
        
          Δ
          ⁢
          
            
          
          ⁢
          l
        
        ^
      
      ⁢
      
        
          Δ
          ⁢
          
            
          
          ⁢
          l
        
        ^
      
    
  
  =
  
    
      
        Q
        ll
      
      ⁢
      
        B
        T
      
      ⁢
      
        Q
        ww
        
          -
          1
        
      
      ⁢
      
        BQ
        ll
      
    
    -
    
      
        Q
        ll
      
      ⁢
      
        B
        T
      
      ⁢
      
        Q
        ww
        
          -
          1
        
      
      ⁢
      
        
          A
          ⁡
          
            (
            
              
                A
                T
              
              ⁢
              
                Q
                ww
                
                  -
                  1
                
              
              ⁢
              A
            
            )
          
        
        
          -
          1
        
      
      ⁢
      
        A
        T
      
      ⁢
      
        Q
        ww
        
          -
          1
        
      
      ⁢
      
        
          BQ
          ll
        
        .
      
    
  




Now




  
    Q
    
      
        x
        ^
      
      ⁢
      
        x
        ^
      
    
  
  ≈
  
    
      Q
      
        
          
            Δ
            ⁢
            
              
            
            ⁢
            x
          
          ^
        
        ⁢
        
          
            Δ
            ⁢
            
              
            
            ⁢
            x
          
          ^
        
      
    
    ⁢
    
      
    
    ⁢
    and
    ⁢
    
      
    
    ⁢
    
      Q
      
        
          e
          ^
        
        ⁢
        
          e
          ^
        
      
    
  
  ≈
  
    
      Q
      
        
          
            Δ
            ⁢
            
              
            
            ⁢
            l
          
          ^
        
        ⁢
        
          
            Δ
            ⁢
            
              
            
            ⁢
            l
          
          ^
        
      
    
    .
  




The covariances are obtained by multiplying by σ02 or . So the a priori covariances are





  
    
      ∑
      
        
          x
          ^
        
        ⁢
        
          x
          ^
        
      
    
    ⁢
    
      =
      
        
          
            (
            
              
                A
                T
              
              ⁢
              
                
                  
                    ∑
                    ww
                  
                  
                    -
                    1
                  
                
                ⁢
                A
              
            
            )
          
          
            -
            1
          
        
        =
        
          
            (
            
              
                
                  
                    A
                    T
                  
                  ⁡
                  
                    (
                    
                      B
                      ⁢
                      
                        
                          ∑
                          ll
                        
                        ⁢
                        
                          B
                          T
                        
                      
                    
                    )
                  
                
                
                  -
                  1
                
              
              ⁢
              A
            
            )
          
          
            -
            1
          
        
      
    
  
  ,




and





  
    
      ∑
      
        
          e
          ^
        
        ⁢
        
          e
          ^
        
      
    
    ⁢
    
      =
      
        
          ∑
          ll
        
        ⁢
        
          
            B
            T
          
          ⁢
          
            
              
                ∑
                
                  -
                  1
                
              
              ww
            
            ⁢
            
              B
              ⁢
              
                
                  ∑
                  ll
                
                ⁢
                
                  -
                  
                    
                      ∑
                      ll
                    
                    ⁢
                    
                      
                        B
                        T
                      
                      ⁢
                      
                        
                          
                            ∑
                            ww
                          
                          
                            -
                            1
                          
                        
                        ⁢
                        
                          
                            
                              A
                              ⁡
                              
                                (
                                
                                  
                                    A
                                    T
                                  
                                  ⁢
                                  
                                    
                                      
                                        ∑
                                        ww
                                      
                                      
                                        -
                                        1
                                      
                                    
                                    ⁢
                                    A
                                  
                                
                                )
                              
                            
                            
                              -
                              1
                            
                          
                          ⁢
                          
                            A
                            T
                          
                          ⁢
                          
                            
                              
                                ∑
                                ww
                              
                              
                                -
                                1
                              
                            
                            ⁢
                            
                              B
                              ⁢
                              
                                ∑
                                ll
                              
                            
                          
                        
                      
                    
                  
                
              
            
          
        
      
    
  
  ,
  
    

  
  ⁢
  
    
      ∑
      
        
          e
          ^
        
        ⁢
        
          e
          ^
        
      
    
    ⁢
    
      =
      
        
          ∑
          ll
        
        ⁢
        
          
            B
            T
          
          ⁢
          
            
              
                ∑
                ww
              
              
                -
                1
              
            
            ⁢
            
              B
              ⁢
              
                
                  ∑
                  ll
                
                ⁢
                
                  -
                  
                    
                      ∑
                      ll
                    
                    ⁢
                    
                      
                        B
                        T
                      
                      ⁢
                      
                        
                          
                            ∑
                            ww
                          
                          
                            -
                            1
                          
                        
                        ⁢
                        
                          A
                          ⁢
                          
                            
                              ∑
                              
                                
                                  x
                                  ^
                                
                                ⁢
                                
                                  x
                                  ^
                                
                              
                            
                            ⁢
                            
                              
                                A
                                T
                              
                              ⁢
                              
                                
                                  
                                    ∑
                                    ww
                                  
                                  
                                    -
                                    1
                                  
                                
                                ⁢
                                
                                  B
                                  ⁢
                                  
                                    
                                      ∑
                                      ll
                                    
                                    .
                                  
                                
                              
                            
                          
                        
                      
                    
                  
                
              
            
          
        
      
    
  




The standardized residual for use in test statistics is





  
    
      e
      _
    
    i
  
  =
  
    
      
        
          
            e
            ^
          
          i
        
        
          
            ∑
            
              
                e
                ^
              
              ⁢
              
                e
                ^
              
              ⁢
              ii
            
          
        
      
      ~
      
        N
        ⁡
        
          (
          
            0
            ,
            1
          
          )
        
      
    
    .
  




where





  
    ∑
    
      
        e
        ^
      
      ⁢
      
        e
        ^
      
      ⁢
      ii
    
  
  ⁢
  
    
  




is the ith diagonal element of





  
    ∑
    
      
        e
        ^
      
      ⁢
      
        e
        ^
      
    
  
  .




It is common to assume that the standardized residuals are distributed normally. However, this assumption should be tested for the particular applications.

Application to Events Seen by Multiple Pointing Sensors
In this embodiment, the data associated with a line of sight Li are vi, pi, and ei. The error ellipse is expressed in terms of major axis ai, minor axis bi, and orientation angle θi. This allows a simple formulation of the weighted least square approach to find the triangulation point (x≡T) in terms of these measurements that for m sensors minimizes




  Θ
  =
  
    
      
        e
        T
      
      ⁢
      
        Q
        ll
        
          -
          1
        
      
      ⁢
      e
    
    =
    
      
        
          σ
          0
          2
        
        ⁢
        
          e
          T
        
        ⁢
        
          
            
              ∑
              ll
            
            
              -
              1
            
          
          ⁢
          e
        
      
      =
      
        
          σ
          0
          2
        
        ⁢
        
          
            ∑
            i
          
          ⁢
          
            (
            
              
                
                  (
                  
                    
                      γ
                      ai
                    
                    
                      a
                      i
                    
                  
                  )
                
                2
              
              +
              
                
                  (
                  
                    
                      γ
                      bi
                    
                    
                      b
                      i
                    
                  
                  )
                
                2
              
            
            )
          
        
      
    
  




where γai is the angular error in the ai direction for sensor i, γbi is the angular error in the bi direction for sensor i and




  
    ∑
    ll
  
  ⁢
  
    =
    
      [
      
        
          
            
              a
              1
              2
            
          
          
            0
          
          
            ...
          
          
            0
          
          
            0
          
        
        
          
            0
          
          
            
              b
              1
              2
            
          
          
            ...
          
          
            0
          
          
            0
          
        
        
          
            ...
          
          
            ...
          
          
            ...
          
          
            ...
          
          
            ...
          
        
        
          
            0
          
          
            0
          
          
            ...
          
          
            
              a
              m
              2
            
          
          
            0
          
        
        
          
            0
          
          
            0
          
          
            ...
          
          
            0
          
          
            
              b
              m
              2
            
          
        
      
      ]
    
  




is the 2m×2m covariance matrix. Then there are two condition equations for each sensor:

Fai=γai=gai(vi,pi,ei,T)=0

Fbi=γbi=gbi(vi,pi,ei,T)=0

where gai calculates γai using vi, pi, ei and the estimate of T, and gbi calculates γai using vi, pi, ei and the estimate of T.

Now

{circumflex over (p)}i−{tilde over (p)}i={circumflex over (γ)}aiuai+{circumflex over (γ)}biubi 

where uai and ubi are the unit vectors in the directions of the ai and bi axes of ei. So the condition functions can be written as

gai(vi,pi,ei,T)=gai(vi,{tilde over (p)}i,γai,γbi,ei,T)

gbi(vi,pi,ei,T)=gbi(vi,{tilde over (p)}i,γai,γbi,ei,T)

leading to the identification





  l
  ≡
  
    [
    
      
        
          
            γ
            
              a
              ⁢
              
                
              
              ⁢
              1
            
          
        
      
      
        
          
            γ
            
              b
              ⁢
              
                
              
              ⁢
              1
            
          
        
      
      
        
          
            
              
                ⋮
              
            
            
              
                ⋮
              
            
          
        
      
      
        
          
            γ
            am
          
        
      
      
        
          
            γ
            bm
          
        
      
    
    ]
  




and





  B
  =
  
    
      
        ∂
        f
      
      
        ∂
        l
      
    
    =
    
      [
      
        
          
            
              
                ∂
                
                  f
                  1
                
              
              
                ∂
                
                  l
                  1
                
              
            
          
          
            ...
          
          
            0
          
        
        
          
            ...
          
          
            ...
          
          
            ...
          
        
        
          
            0
          
          
            ...
          
          
            
              
                ∂
                
                  f
                  m
                
              
              
                ∂
                
                  l
                  m
                
              
            
          
        
      
      ]
    
  




where





  
    
      ∂
      
        f
        i
      
    
    
      ∂
      
        l
        i
      
    
  
  =
  
    
      [
      
        
          
            
              
                ∂
                
                  g
                  ai
                
              
              
                ∂
                
                  γ
                  ai
                
              
            
          
          
            
              
                ∂
                
                  g
                  ai
                
              
              
                ∂
                
                  γ
                  bi
                
              
            
          
        
        
          
            
              
                ∂
                
                  g
                  bi
                
              
              
                ∂
                
                  γ
                  ai
                
              
            
          
          
            
              
                ∂
                
                  g
                  bi
                
              
              
                ∂
                
                  γ
                  bi
                
              
            
          
        
      
      ]
    
    .
  




Since gai alculates the current γai and gbi calculates the current γbi, which are assumed to be independent errors, ∂fi/∂li should be the identity matrix




  
    
      ∂
      
        f
        i
      
    
    
      ∂
      
        l
        i
      
    
  
  ≈
  
    
      [
      
        
          
            1
          
          
            0
          
        
        
          
            0
          
          
            1
          
        
      
      ]
    
    .
  




Now x≡T so




  A
  =
  
    
      
        ∂
        f
      
      
        ∂
        x
      
    
    =
    
      [
      
        
          
            
              
                ∂
                
                  g
                  
                    a
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  x
                
              
            
          
          
            
              
                ∂
                
                  g
                  
                    a
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  y
                
              
            
          
          
            
              
                ∂
                
                  g
                  
                    a
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  z
                
              
            
          
        
        
          
            
              
                ∂
                
                  g
                  
                    b
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  x
                
              
            
          
          
            
              
                ∂
                
                  g
                  
                    b
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  y
                
              
            
          
          
            
              
                ∂
                
                  g
                  
                    b
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  z
                
              
            
          
        
        
          
            ⋮
          
          
            ⋮
          
          
            ⋮
          
        
        
          
            ⋮
          
          
            ⋮
          
          
            ⋮
          
        
        
          
            
              
                ∂
                
                  g
                  am
                
              
              
                ∂
                
                  T
                  x
                
              
            
          
          
            
              
                ∂
                
                  g
                  am
                
              
              
                ∂
                
                  T
                  y
                
              
            
          
          
            
              
                ∂
                
                  g
                  am
                
              
              
                ∂
                
                  T
                  z
                
              
            
          
        
        
          
            
              
                ∂
                
                  g
                  bm
                
              
              
                ∂
                
                  T
                  x
                
              
            
          
          
            
              
                ∂
                
                  g
                  bm
                
              
              
                ∂
                
                  T
                  y
                
              
            
          
          
            
              
                ∂
                
                  g
                  bm
                
              
              
                ∂
                
                  T
                  z
                
              
            
          
        
      
      ]
    
  




Finally





  w
  =
  
    
      -
      f
    
    =
    
      
        [
        
          
            
              
                -
                
                  F
                  
                    a
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
            
          
          
            
              
                -
                
                  F
                  
                    b
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
            
          
          
            
              
                
                  
                    ⋮
                  
                
                
                  
                    ⋮
                  
                
              
            
          
          
            
              
                -
                
                  F
                  am
                
              
            
          
          
            
              
                -
                
                  F
                  bm
                
              
            
          
        
        ]
      
      .
    
  




At each iteration step, the current value of pi and the new value of T are used to calculate γai and γbi. The angle between the new and previous pi is the current angular error and is projected onto the ai and bi axes of the error ellipse to obtain γai and γbi. The new pointing vector can be found from

{circumflex over (T)}(s+1)=vi+{circumflex over (t)}(s+1){circumflex over (p)}i(s+1)

giving





  
    
      
        p
        ^
      
      i
    
    ⁡
    
      (
      
        s
        +
        1
      
      )
    
  
  =
  
    
      
        
          
            T
            ^
          
          ⁡
          
            (
            
              s
              +
              1
            
            )
          
        
        -
        
          v
          i
        
      
      
        
          t
          ^
        
        ⁡
        
          (
          
            s
            +
            1
          
          )
        
      
    
    =
    
      
        
          
            T
            ^
          
          ⁡
          
            (
            
              s
              +
              1
            
            )
          
        
        -
        
          v
          i
        
      
      
        
        
          
            
              T
              ^
            
            ⁡
            
              (
              
                s
                +
                1
              
              )
            
          
          -
          
            v
            i
          
        
        
      
    
  




Now

{circumflex over (p)}i(s+1)•{circumflex over (p)}i(s)=cos {circumflex over (γ)}i(s+1)

{circumflex over (γ)}i(s+1)=cos−1({circumflex over (p)}i(s+1)•{circumflex over (p)}i(s))

where {circumflex over (γ)}i is an estimate of the angular error for the current {circumflex over (T)}. Define





  
    
      
        
          
            
              u
              ^
            
            
              γ
              ⁢
              
                
              
              ⁢
              i
            
          
          ⁡
          
            (
            
              s
              +
              1
            
            )
          
        
        =
        
          
            
              
                
                  p
                  ^
                
                i
              
              ⁡
              
                (
                
                  s
                  +
                  1
                
                )
              
            
            -
            
              
                
                  p
                  ^
                
                i
              
              ⁡
              
                (
                s
                )
              
            
          
          
            
            
              
                
                  
                    p
                    ^
                  
                  i
                
                ⁡
                
                  (
                  
                    s
                    +
                    1
                  
                  )
                
              
              -
              
                
                  
                    p
                    ^
                  
                  i
                
                ⁡
                
                  (
                  s
                  )
                
              
            
            
          
        
      
    
  
  
    
      
        
          
            
              ρ
              ^
            
            i
          
          ⁡
          
            (
            
              s
              +
              1
            
            )
          
        
        =
        
          
            cos
            
              -
              1
            
          
          ⁡
          
            (
            
              
                
                  
                    u
                    ^
                  
                  
                    γ
                    ⁢
                    
                      
                    
                    ⁢
                    i
                  
                
                ⁡
                
                  (
                  
                    s
                    +
                    1
                  
                  )
                
              
              ·
              
                u
                ai
              
            
            )
          
        
      
    
  




where ûγi is the unit vector in the direction of {circumflex over (γ)}i, uai is the unit vector in the direction of major axis of the error ellipse ei and {circumflex over (ρ)}i is the angle between {circumflex over (γ)}i and the major axis of the error ellipse. Then

Fai(s+1)={circumflex over (γ)}ai(s+1)={circumflex over (γ)}i(s+1)cos ρi(s+1)

Fbi(s+1)={circumflex over (γ)}bi(s+1)={circumflex over (γ)}i(s+1)sin ρi(s+1)

Recalling that, after the final iteration, the final residuals are

e={circumflex over (l)}(s*+1)−{tilde over (l)}. 

As before

{circumflex over (p)}i(s*+1)•{tilde over (p)}i=cos i 

i=cos−1({circumflex over (p)}i(s*+1)•{tilde over (p)}i)

where i is an estimate of the angular error for the final estimate of the triangulation point . Define





  
    
      
        
          
            (
            
              u
              ′
            
            )
          
          
            γ
            ⁢
            
              
            
            ⁢
            i
          
        
        =
        
          
            
              
                
                  p
                  ^
                
                i
              
              ⁡
              
                (
                
                  
                    s
                    *
                  
                  +
                  1
                
                )
              
            
            -
            
              
                p
                ~
              
              i
            
          
          
            
            
              
                
                  
                    p
                    ^
                  
                  i
                
                ⁡
                
                  (
                  
                    
                      s
                      *
                    
                    +
                    1
                  
                  )
                
              
              -
              
                
                  p
                  ~
                
                i
              
            
            
          
        
      
    
  
  
    
      
        
          ρ
          ′
        
        =
        
          
            cos
            
              -
              1
            
          
          ⁡
          
            (
            
              
                
                  (
                  
                    u
                    ′
                  
                  )
                
                
                  γ
                  ⁢
                  
                    
                  
                  ⁢
                  i
                
              
              ·
              
                u
                ai
              
            
            )
          
        
      
    
  




where γi is the unit vector in the direction of i, and i is the angle between i and the major axis of the error ellipse. Then

ai=i cos i 

bi=i sin i 

The standardized residuals are




  
    
      
        
          
            γ
            _
          
          ai
        
        =
        
          
            
              
                γ
                ′
              
              ai
            
            
              
                ∑
                
                  
                    e
                    ^
                  
                  ⁢
                  
                    e
                    ^
                  
                  ⁢
                  ai
                
              
            
          
          ∼
          
            N
            ⁡
            
              (
              
                0
                ,
                1
              
              )
            
          
        
      
    
  
  
    
      
        
          
            γ
            _
          
          bi
        
        =
        
          
            
              
                γ
                ′
              
              bi
            
            
              
                ∑
                
                  
                    e
                    ^
                  
                  ⁢
                  
                    e
                    ^
                  
                  ⁢
                  bi
                
              
            
          
          ∼
          
            
              N
              ⁡
              
                (
                
                  0
                  ,
                  1
                
                )
              
            
            .
          
        
      
    
  




Simulations are used to verify that these standardized residuals are indeed normally distributed for the cases of interest.

Note, if B is the identity matrix, then




  
    
      ∑
      ww
    
    ⁢
    
      ∑
      ll
    
  
  ,
  
    

  
  ⁢
  
    
      ∑
      
        
          x
          ^
        
        ⁢
        
          x
          ^
        
      
    
    ⁢
    
      =
      
        
          
            (
            
              
                A
                T
              
              ⁢
              
                
                  ∑
                  ww
                  
                    -
                    1
                  
                
                ⁢
                A
              
            
            )
          
          
            -
            1
          
        
        =
        
          
            (
            
              
                A
                T
              
              ⁢
              
                
                  ∑
                  ll
                  
                    -
                    1
                  
                
                ⁢
                A
              
            
            )
          
          
            -
            1
          
        
      
    
  
  ,
  
    

  
  ⁢
  
    
      ∑
      
        
          e
          ^
        
        ⁢
        
          e
          ^
        
      
    
    ⁢
    
      =
      
        
          ∑
          ll
        
        ⁢
        
          
            -
            
              
                A
                ⁡
                
                  (
                  
                    
                      A
                      T
                    
                    ⁢
                    
                      
                        ∑
                        ll
                        
                          -
                          1
                        
                      
                      ⁢
                      A
                    
                  
                  )
                
              
              
                -
                1
              
            
          
          ⁢
          
            A
            T
          
        
      
    
  
  ,
  
    

  
  ⁢
  
    
      ∑
      
        
          e
          ^
        
        ⁢
        
          e
          ^
        
      
    
    ⁢
    
      =
      
        
          ∑
          ll
        
        ⁢
        
          
            -
            A
          
          ⁢
          
            
              ∑
              
                
                  x
                  ^
                
                ⁢
                
                  x
                  ^
                
              
            
            ⁢
            
              
                A
                T
              
              .
            
          
        
      
    
  




Correlation Assessment Based on Least Squares Statistics
Once the best triangulation point is found through iteration, the assessment of whether events seen by more than one sensor are the same event can be made. If the standardized residuals from triangulation are too large the lines of sight must not be from the same target. The assumption of normally distributed standardized residual leads to the same confidence calculation for the standardized residuals γxi as is used for the normalized minimum distance between the lines of sight:

Conf=1−P(x≦γxi)=1−erf(γxi/√{square root over (2)})

In practice, when observations from just two sensors are used, the least squares procedure results in all four standardized residuals (γa1,γb1,γa2,γb2) being equal. The confidence level required for a determination of correlation depends upon application needs but is typically set to 5%.
When the target density is high, the line of sight for a target on one sensor may be close to the lines of sight for more than one target on the other sensor and therefore more than one set of assignments could pass the confidence test. In this case, a means is provided to resolve the ambiguity and make a consistent set of assignments. An extended Munkres algorithm may be particularly advantageous to make assignments that minimize normalized square residuals.
When the covariances become large, the size of the residuals and equivalently the minimum distance between the lines of sight that will pass a confidence test grows increasing the possibility of spurious correlations. For example, at a 5% confidence level, the standardized residual or normalized distance between lines of sight that passes the test is about 1.96. This means that any Djk<1.96σvjk will pass the confidence test. Thus, σvjk is a good indicator of how close events can be together without causing a spurious correlation.
Consideration of Refraction
The discussion presented so far has assumed that the path of light from the detected event is a straight line. However, if the elevation angle from the event to the sensor is small enough Φ<Φc, where Φc≈200, refraction of light through the atmosphere become significant and must be addressed. In the current application, the reported pointing vector {tilde over (p)}′i is adjusted in the direction of the horizon to provide a straight line between the sensor and the event




  
    
      
        p
        ′
      
      ~
    
    i
  
  =
  
    
      
        
          
            T
            ′
          
          ~
        
        -
        
          v
          i
        
      
      
        
        
          
            
              T
              ′
            
            ~
          
          -
          
            v
            i
          
        
        
      
    
    =
    
      
        q
        
          ω
          ⁢
          
            
          
          ⁢
          i
        
        *
      
      ⁢
      
        
          p
          ~
        
        i
      
      ⁢
      
        q
        
          ω
          ⁢
          
            
          
          ⁢
          i
        
      
    
  




where qωi is quaternion rotation matrix through an angle ωi toward the horizon that is a function of the altitude of an assumed location point for the event {tilde over (T)}′ and {tilde over (p)}i is the pointing vector actually seen by the sensor. The conditions equations are still functions of the same variables but the expression for {circumflex over (p)}i(s+1) becomes





  
    
      
        p
        ^
      
      i
    
    ⁡
    
      (
      
        s
        +
        1
      
      )
    
  
  =
  
    
      
        q
        
          
            -
            ω
          
          ⁢
          
            
          
          ⁢
          i
        
        *
      
      ⁢
      
        
          
            
              p
              ′
            
            ^
          
          i
        
        ⁡
        
          (
          
            s
            +
            1
          
          )
        
      
      ⁢
      
        q
        
          
            -
            ω
          
          ⁢
          
            
          
          ⁢
          i
        
      
    
    =
    
      
        q
        
          
            -
            ω
          
          ⁢
          
            
          
          ⁢
          i
        
        *
      
      ⁢
      
        
          
            
              T
              ^
            
            ⁡
            
              (
              
                s
                +
                1
              
              )
            
          
          -
          
            v
            i
          
        
        
          
          
            
              
                T
                ^
              
              ⁡
              
                (
                
                  s
                  +
                  1
                
                )
              
            
            -
            
              v
              i
            
          
          
        
      
      ⁢
      
        q
        
          
            -
            ω
          
          ⁢
          
            
          
          ⁢
          i
        
      
    
  




Dependencies
Refraction strongly depends on surface temperature, temperature gradient with altitude and atmospheric pressure. Even if the calculation of the bending due to refraction is accurate, these inputs to the calculations are not known. Seasonal and diurnal variations for these inputs can be estimated from historic data bases as a function of latitude and longitude but the residual errors are not yet estimated.
Simulations indicate that the least squares triangulation procedure can accommodate the bending of light due to refraction using numerically calculated A matrix. But the lack of residual error estimation for the refraction calculation prevents assessment of correlation for targets where refraction correction and its residual error are significant for one or both of the paths from the target to sensor.
Temporally Overlapping Observations
When observations overlap in time, generation of test observations at a common time for correlation assessment involves interpolation. A fit used for the interpolation can smooth the random variations and the associated prediction error would replace the random error in the calculation of observation covariance. If there are too few observations from a sensor to obtain a good fit, a raw observation is used in the correlation assessment. If raw observations are used from both sensors, additional error is introduced due to the difference in observation times. However, such additional error may be negligible if the observation frequencies are high enough.
The cases under consideration for temporally overlapping observations include: stationary observation sets from both sensors, moving observation sets from both sensors, and a stationary observation set from one sensor and a moving set from the other sensor.
Results of simulation testing indicate that if there are enough points to generate a fit for both sensors, selection of the center of the observation overlap interval as the common observation time works well for correlation assessment for all three cases. Otherwise, use of the closest observation to the center of the overlap works well if the number of observations is too small for a good fit.
Temporally Separated Observations
There may be several instances where the observation sets from the sensors do not overlap in time: (a) a short stationary target from one sensor followed by a moving target from the other sensor, (b) a moving target from one sensor followed by a short stationary target from the other sensor, and (c) a moving target from one sensor followed by a moving target from the other sensor.
In each case, Kalman filters are used to generate a predicted observation from one sensor at a time for which an observation (e.g., generated with a polynomial fit) is available from the other sensor. The Kalman predicted covariance replaces the random error covariance in the calculation of total covariance for the correlation assessment in one embodiment. For the case of a stationary target followed by a moving target, the Kalman extrapolation is backward in time to a possible initiating event seen by the other sensor. For the case of a moving target followed by a stationary target, the Kalman extrapolation is forward in time to a possible termination event seen by the other sensor. For the case of two moving targets, two extrapolations are performed (one from each sensor to the other) and the resulting confidences are fused to provide a single estimate of the confidence that the observations are from the same physical target.
Linear Kalman Filter Embodiment
Kalman filters implement a recursive least squares approach for estimating the state of a system based on a series of observations. Let

    
    
        {circumflex over (X)}k be the state vector estimate at time tk,
        {circumflex over (P)}k be the error covariance matrix estimate at time tk,
        z be the vector of observations at time tk+1,
    
    





  
    
      Φ
      ⁡
      
        (
        
          
            t
            
              k
              +
              1
            
          
          ,
          
            t
            k
          
        
        )
      
    
    =
    
      
        ∂
        
          
            X
            ^
          
          
            k
            +
            1
          
        
      
      
        ∂
        
          
            X
            ^
          
          k
        
      
    
  




  
    
      H
      
        k
        +
        1
      
    
    =
    
      
        ∂
        z
      
      
        ∂
        
          
            X
            ^
          
          
            k
            +
            1
          
        
      
    
  



Then the predicted state at time tk+1 is

Xk+1=Φ{circumflex over (X)}k 

The predicted error covariance at time tk+1 is

Pk+1=Φ{circumflex over (P)}kΦT+Q 

where Q is the process noise.

The optimal estimates of the state and covariance given an observation vector, z at time tk+1 are

{circumflex over (X)}k+1=Xk+1+Kk+1[z−Hk+1Xk+1]

{circumflex over (P)}k+1=[I−Kk+1Hk+1]Pk+1 

where

Kk+1=Pk+1Hk+1T[Hk+1Pk+1Hk+1T+R]

is the Kalman gain and R is the measurement error.

Application to Observations from Pointing Sensors
In the current embodiment, the pointing observation vectors are expressed in spherical coordinates (θ,φ). Thus, z=(θ,φ). Using a six state Kalman filter, we have



  
    X
    =
    
      (
      
        θ
        ,
        
          θ
          .
        
        ,
        
          θ
           ̈
        
        ,
        φ
        ,
        
          φ
          .
        
        ,
        
          φ
           ̈
        
        ,
      
      )
    
  




  
    H
    =
    
      [
      
        
          
            1
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            1
          
          
            0
          
          
            0
          
        
      
      ]
    
  




  
    
      Φ
      ⁡
      
        (
        
          
            t
            
              k
              +
              1
            
          
          ,
          
            t
            k
          
        
        )
      
    
    =
    
      
        Φ
        ⁡
        
          (
          
            Δ
            ⁢
            
              
            
            ⁢
            t
          
          )
        
      
      =
      
        [
        
          
            
              1
            
            
              
                Δ
                ⁢
                
                  
                
                ⁢
                t
              
            
            
              
                
                  Δ
                  ⁢
                  
                    
                  
                  ⁢
                  
                    t
                    2
                  
                
                2
              
            
            
              
                
              
            
            
              
                
              
            
            
              
                
              
            
          
          
            
              0
            
            
              1
            
            
              
                Δ
                ⁢
                
                  
                
                ⁢
                t
              
            
            
              
                
              
            
            
              0
            
            
              
                
              
            
          
          
            
              0
            
            
              0
            
            
              1
            
            
              
                
              
            
            
              
                
              
            
            
              
                
              
            
          
          
            
              
                
              
            
            
              
                
              
            
            
              
                
              
            
            
              1
            
            
              
                Δ
                ⁢
                
                  
                
                ⁢
                t
              
            
            
              
                
                  Δ
                  ⁢
                  
                    
                  
                  ⁢
                  
                    t
                    2
                  
                
                2
              
            
          
          
            
              
                
              
            
            
              0
            
            
              
                
              
            
            
              
                0
                ⁢
                
                  
                
              
            
            
              1
            
            
              
                Δ
                ⁢
                
                  
                
                ⁢
                t
              
            
          
          
            
              
                
              
            
            
              
                
              
            
            
              
                
              
            
            
              0
            
            
              0
            
            
              1
            
          
        
        ]
      
    
  



R may be set to the random component of the measurement errors. The Q matrix is typically treated as a set of tuning parameters. However, in the applications of interest target characterization information may be available to help tune Q based on target type.

Here, Δt is the gap in time between the observations for one sensor and those for the other sensor. The calculation of the predicted Kalman covariance has a strong dependence on this gap. The growth of the predicted Kalman covariance with duration of the gap limits practical size of the size of the gap that can be accommodated because large covariance increases the chance of spurious correlations.
Fusion of Confidence
For the case of two moving targets separated in time, two confidences are obtained: one extrapolating from each set of sensor observations to the other. Although statistically correlated, these confidences are not the same. They need to be fused into a single confidence that the observation sets are from the same event. For this application, a gamma probabilistic fusion approach5 is used. A statistic is defined, in terms of the two confidences, that has a gamma distribution, which is then used to evaluate the fused confidence.
Let N be the number of confidences to be fused (N≧2) and define



  
    
      Y
      i
    
    =
    
      -
      
        log
        ⁡
        
          (
          
            Conf
            i
          
          )
        
      
    
  




  
    S
    =
    
      
        ∑
        
          i
          =
          1
        
        N
      
      ⁢
      
        Y
        i
      
    
  




  
    C
    =
    
      2
      ⁢
      
        
          ∑
          
            i
            =
            1
          
          
            N
            -
            1
          
        
        ⁢
        
          
            ∑
            
              j
              =
              
                i
                +
                1
              
            
            N
          
          ⁢
          
            ρ
            ij
          
        
      
    
  



where ρij is the statistical correlation between Confi and Confj. The gamma shape and scale parameters are





  
    
      
        
          r
          =
          
            
              N
              2
            
            
              N
              +
              C
            
          
        
        ,
      
    
    
      
        (
        shape
        )
      
    
  
  
    
      
        
          λ
          =
          
            N
            
              N
              +
              C
            
          
        
        ,
      
    
    
      
        (
        scale
        )
      
    
  




and the fused confidence is given by

Conff=1.0−G(S;r,λ)

where G is the incomplete gamma function integrated from the left.

In the fusion of the two confidences from extrapolation between the sensor observation sets, the correlation between the confidences, ρjk, is a function of the gap between the observation sets and must be estimated for the particular application.
Implementation Parameters
Several sets of tuning parameters are required to implement multiple pointing sensor correlation. The tuning parameter groups may be:

    
    
        Polynomial fit control
        Kalman filter control
        Minimum line of sight intersection distance screen control
        Least squares triangulation control, and
        Munkres assignment algorithm control
    
    


Simulation testing has been used to tune these parameters for optimizing performance of the correlation system. Excellent performance is seen for all of the cases described above over a wide set of target profiles and measurement uncertainty regimes. However, final tuning will occur after deployment the sensors because only then will the actual uncertainty regimes be determined. Furthermore, target profiles of interest are likely to evolve over the operational life of the sensors requiring periodic retuning of the parameter sets.
Second Order LOS Intersection Distance Errors
Turning to FIG. 3, an embodiment is illustrated showing that, for very small angular errors, the error in the intersection point in the plane perpendicular to Mjk can be analyzed as in FIG. 3 in terms of the angular error γhk for pk in the epipolar plane, the angular error γhj for pj in the epipolar plane and the angle between the lines of sight φjk. From the figure, the angular error γhk shifts in the apparent intersection point T′jk and results in errors in the ranges from the sensors:



  
    
      Δ
      ⁢
      
        
      
      ⁢
      
        t
        jhk
      
    
    =
    
      
        
          t
          k
        
        ⁢
        
          γ
          hk
        
      
      
        sin
        ⁢
        
          
        
        ⁢
        
          φ
          jk
        
      
    
  




  
    
      Δ
      ⁢
      
        
      
      ⁢
      
        t
        khk
      
    
    =
    
      
        
          
            
              t
              k
            
            ⁢
            
              γ
              hk
            
          
          
            sin
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
        
        ⁢
        cos
        ⁢
        
          
        
        ⁢
        
          φ
          jk
        
      
      =
      
        
          t
          k
        
        ⁢
        
          γ
          hk
        
        ⁢
        cot
        ⁢
        
          
        
        ⁢
        
          φ
          jk
        
      
    
  



Similarly, an angular error γhj results in errors:




  
    
      Δ
      ⁢
      
        
      
      ⁢
      
        t
        khj
      
    
    =
    
      
        
          
            t
            j
          
          ⁢
          
            γ
            hj
          
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            kj
          
        
      
      =
      
        -
        
          
            
              t
              j
            
            ⁢
            
              γ
              hj
            
          
          
            sin
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
        
      
    
  




  
    
      Δ
      ⁢
      
        
      
      ⁢
      
        t
        jhj
      
    
    =
    
      
        
          
            
              t
              j
            
            ⁢
            
              γ
              hj
            
          
          
            sin
            ⁢
            
              
            
            ⁢
            
              φ
              kj
            
          
        
        ⁢
        cos
        ⁢
        
          
        
        ⁢
        
          φ
          kj
        
      
      =
      
        
          -
          
            
              
                t
                j
              
              ⁢
              
                γ
                hj
              
              ⁢
              cos
              ⁢
              
                
              
              ⁢
              
                φ
                jk
              
            
            
              sin
              ⁢
              
                
              
              ⁢
              
                φ
                jk
              
            
          
        
        =
        
          
            -
            
              t
              j
            
          
          ⁢
          
            γ
            hj
          
          ⁢
          cot
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
    
  



Thus




  
    
      t
      j
    
    =
    
      
        t
        jA
      
      -
      
        
          
            cos
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
          
            sin
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
        
        ⁢
        
          t
          jA
        
        ⁢
        
          γ
          hj
        
      
      +
      
        
          
            t
            kA
          
          ⁢
          
            γ
            hk
          
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
    
  




  
    
      t
      k
    
    =
    
      
        t
        kA
      
      -
      
        
          
            t
            jA
          
          ⁢
          
            γ
            hj
          
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
      +
      
        
          
            cos
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
          
            sin
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
        
        ⁢
        
          t
          kA
        
        ⁢
        
          γ
          hk
        
      
    
  



where tjA and tkA are the actual ranges from the sensors. Therefore, in terms of the ranges from the sensors and the associated errors Djk becomes:




  
    
      D
      jk
    
    =
    
      
      
        
          
            (
            
              
                t
                jA
              
              -
              
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    
                      φ
                      jk
                    
                  
                  
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    
                      φ
                      jk
                    
                  
                
                ⁢
                
                  t
                  jA
                
                ⁢
                
                  γ
                  hj
                
              
              +
              
                
                  
                    t
                    kA
                  
                  ⁢
                  
                    γ
                    hk
                  
                
                
                  sin
                  ⁢
                  
                    
                  
                  ⁢
                  
                    φ
                    jk
                  
                
              
            
            )
          
          ⁢
          
            γ
            vj
          
        
        -
        
          
            (
            
              
                t
                kA
              
              -
              
                
                  
                    t
                    jA
                  
                  ⁢
                  
                    γ
                    hj
                  
                
                
                  sin
                  ⁢
                  
                    
                  
                  ⁢
                  
                    φ
                    jk
                  
                
              
              +
              
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    
                      φ
                      jk
                    
                  
                  
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    
                      φ
                      jk
                    
                  
                
                ⁢
                
                  t
                  kA
                
                ⁢
                
                  γ
                  hk
                
              
            
            )
          
          ⁢
          
            γ
            vk
          
        
      
      
    
  




  
    
      D
      jk
    
    =
    
      
      
        
          
            
              (
              
                
                  t
                  jA
                
                -
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      
                        φ
                        jk
                      
                    
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      
                        φ
                        jk
                      
                    
                  
                  ⁢
                  
                    
                      t
                      jA
                    
                    ⁡
                    
                      (
                      
                        
                          
                            γ
                            aj
                          
                          ⁢
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          
                            θ
                            jk
                          
                        
                        +
                        
                          
                            γ
                            bj
                          
                          ⁢
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          
                            θ
                            jk
                          
                        
                      
                      )
                    
                  
                
                +
                
                  
                    
                      t
                      kA
                    
                    ⁡
                    
                      (
                      
                        
                          
                            γ
                            ak
                          
                          ⁢
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          
                            θ
                            kj
                          
                        
                        +
                        
                          
                            γ
                            bk
                          
                          ⁢
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          
                            θ
                            kj
                          
                        
                      
                      )
                    
                  
                  
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    
                      φ
                      jk
                    
                  
                
              
              )
            
          
        
        
          
            
              
                (
                
                  
                    
                      γ
                      aj
                    
                    ⁢
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    
                      θ
                      jk
                    
                  
                  +
                  
                    
                      γ
                      bj
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    
                      θ
                      jk
                    
                  
                
                )
              
              -
              
                (
                
                  
                    t
                    kA
                  
                  -
                  
                    
                      
                        t
                        jA
                      
                      ⁡
                      
                        (
                        
                          
                            
                              γ
                              aj
                            
                            ⁢
                            sin
                            ⁢
                            
                              
                            
                            ⁢
                            
                              θ
                              jk
                            
                          
                          +
                          
                            
                              γ
                              bj
                            
                            ⁢
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            
                              θ
                              jk
                            
                          
                        
                        )
                      
                    
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      
                        φ
                        jk
                      
                    
                  
                  +
                
              
            
          
        
        
          
            
              
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      
                        φ
                        jk
                      
                    
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      
                        φ
                        jk
                      
                    
                  
                  ⁢
                  
                    
                      t
                      kA
                    
                    ⁡
                    
                      (
                      
                        
                          
                            γ
                            ak
                          
                          ⁢
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          
                            θ
                            kj
                          
                        
                        +
                        
                          
                            γ
                            
                              bk
                              ⁢
                              
                                
                              
                            
                          
                          ⁢
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          
                            θ
                            kj
                          
                        
                      
                      )
                    
                  
                
                )
              
              ⁢
              
                (
                
                  
                    
                      γ
                      ak
                    
                    ⁢
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    
                      θ
                      kj
                    
                  
                  +
                  
                    
                      γ
                      bk
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    
                      θ
                      kj
                    
                  
                
                )
              
            
          
        
      
      
    
  



It should be noted that when expanded, this takes the form

Djk=|γTE+γTMγ|

where

γT=[γaj,γbj,γak,γbk],

ET=[tjA cos θjk,tjA sin θjk,tkA cos θkj,tkA sin θkj],





  M
  =
  
    
      1
      
        sin
        ⁢
        
          
        
        ⁢
        
          φ
          jk
        
      
    
    ⁡
    
      [
      
        
          
            
              m
              11
            
          
          
            
              m
              12
            
          
          
            
              m
              13
            
          
          
            
              m
              14
            
          
        
        
          
            0
          
          
            
              m
              22
            
          
          
            
              m
              23
            
          
          
            
              m
              24
            
          
        
        
          
            0
          
          
            0
          
          
            
              m
              33
            
          
          
            
              m
              34
            
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            
              m
              44
            
          
        
      
      ]
    
  




with

m11=−tjA cos φjk sin θjk cos θjk,

m12=−tjA cos φjk,

m13=tkA sin θkj cos θjk+tjA sin θjk cos θkj,

m14=tkA cos θkj cos θjk+tjA sin θjk sin θkj,

m22=−tjA cos φjk sin θjk cos θjk,

m23=tjA cos θkj cos θjk+tkA sin θjk sin θkj,

m24=tjA sin θkj cos θjk+tkA sin θjk cos θkj,

m33=−tkA cos φjk sin θkj cos θkj,

m34=−tkA cos φjk,

m44=−tkA cos φjk sin θkj cos θkj.

Thus, when the second order terms are small




  (
  
    
      
        i
        .
        e
      
      ⁢
      
        
          γ
          hj
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
    
    ⪡
    
      1
      ⁢
      
        
      
      ⁢
      and
      ⁢
      
        
      
      ⁢
      
        
          γ
          hk
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
    
    ⪡
    1
  
  )




the first order expression for Djk can be used:

Djk=|γTE|=|tjAγaj cos θjk+tjAγbj sin θjk−tkAγak cos θkj+tkAγbk sin θkj|=|tjAγvj−tkAγvk|

In FIG. 3, the line B forms an angle β with line of sight Lj in the epipolar plane. The projections of the angular errors onto B are:




  
    
      B
      
        ∥
        hk
      
    
    =
    
      
        
          
            t
            k
          
          ⁢
          
            γ
            hk
          
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
      ⁢
      cos
      ⁢
      
        
      
      ⁢
      β
    
  
  ,
  
    

  
  ⁢
  
    
      B
      
        ∥
        hj
      
    
    =
    
      
        -
        
          
            
              t
              j
            
            ⁢
            
              γ
              hj
            
          
          
            sin
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
        
      
      ⁢
      
        cos
        ⁡
        
          (
          
            β
            -
            
              φ
              jk
            
          
          )
        
      
    
  




Thus, the combination of errors along B is:




  
    
      
    
    ⁢
    
      
        B
        ∥
      
      =
      
        
          
            -
            
              (
              
                
                  cos
                  ⁡
                  
                    (
                    
                      β
                      -
                      
                        φ
                        jk
                      
                    
                    )
                  
                
                
                  sin
                  ⁢
                  
                    
                  
                  ⁢
                  
                    φ
                    jk
                  
                
              
              )
            
          
          ⁢
          
            t
            jA
          
          ⁢
          
            γ
            hj
          
        
        +
        
          
            (
            
              
                cos
                ⁢
                
                  
                
                ⁢
                β
              
              
                sin
                ⁢
                
                  
                
                ⁢
                
                  φ
                  jk
                
              
            
            )
          
          ⁢
          
            t
            kA
          
          ⁢
          
            γ
            hk
          
        
      
    
  




  
    
      B
      ∥
    
    =
    
      
        
          -
          
            (
            
              
                cos
                ⁡
                
                  (
                  
                    β
                    -
                    
                      φ
                      jk
                    
                  
                  )
                
              
              
                sin
                ⁢
                
                  
                
                ⁢
                
                  φ
                  jk
                
              
            
            )
          
        
        ⁢
        
          
            t
            jA
          
          ⁡
          
            (
            
              
                
                  γ
                  aj
                
                ⁢
                sin
                ⁢
                
                  
                
                ⁢
                
                  θ
                  jk
                
              
              +
              
                
                  γ
                  bj
                
                ⁢
                cos
                ⁢
                
                  
                
                ⁢
                
                  θ
                  jk
                
              
            
            )
          
        
      
      +
      
        
          (
          
            
              cos
              ⁢
              
                
              
              ⁢
              β
            
            
              sin
              ⁢
              
                
              
              ⁢
              
                φ
                jk
              
            
          
          )
        
        ⁢
        
          
            t
            kA
          
          ⁡
          
            (
            
              
                
                  γ
                  ak
                
                ⁢
                sin
                ⁢
                
                  
                
                ⁢
                
                  θ
                  kj
                
              
              +
              
                
                  γ
                  bj
                
                ⁢
                cos
                ⁢
                
                  
                
                ⁢
                
                  θ
                  kj
                
              
            
            )
          
        
      
    
  



The projections of the angular errors perpendicular to B in the epipolar plane are:




  
    
      B
      
        ⊥
        hk
      
    
    =
    
      
        
          
            t
            k
          
          ⁢
          
            y
            hk
          
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
      ⁢
      sin
      ⁢
      
        
      
      ⁢
      β
    
  
  ,
  
    

  
  ⁢
  
    
      B
      
        ⊥
        hj
      
    
    =
    
      
        -
        
          
            
              t
              j
            
            ⁢
            
              γ
              hj
            
          
          
            sin
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
        
      
      ⁢
      
        sin
        ⁡
        
          (
          
            β
            -
            
              φ
              jk
            
          
          )
        
      
    
  




Thus, the combination of the angular errors perpendicular to B in the epipolar plane are:




  
    
      
    
    ⁢
    
      
        B
        ⊥
      
      =
      
        
          
            -
            
              (
              
                
                  sin
                  ⁡
                  
                    (
                    
                      β
                      -
                      
                        φ
                        jk
                      
                    
                    )
                  
                
                
                  sin
                  ⁢
                  
                    
                  
                  ⁢
                  
                    φ
                    jk
                  
                
              
              )
            
          
          ⁢
          
            t
            jA
          
          ⁢
          
            γ
            hj
          
        
        +
        
          
            (
            
              
                sin
                ⁢
                
                  
                
                ⁢
                β
              
              
                sin
                ⁢
                
                  
                
                ⁢
                
                  φ
                  jk
                
              
            
            )
          
          ⁢
          
            t
            kA
          
          ⁢
          
            γ
            hk
          
        
      
    
  




  
    
      B
      ⊥
    
    =
    
      
        
          -
          
            (
            
              
                sin
                ⁡
                
                  (
                  
                    β
                    -
                    
                      φ
                      jk
                    
                  
                  )
                
              
              
                sin
                ⁢
                
                  
                
                ⁢
                
                  φ
                  jk
                
              
            
            )
          
        
        ⁢
        
          
            t
            jA
          
          ⁡
          
            (
            
              
                
                  γ
                  aj
                
                ⁢
                sin
                ⁢
                
                  
                
                ⁢
                
                  θ
                  jk
                
              
              +
              
                
                  γ
                  bj
                
                ⁢
                cos
                ⁢
                
                  
                
                ⁢
                
                  θ
                  jk
                
              
            
            )
          
        
      
      +
      
        
          (
          
            
              sin
              ⁢
              
                
              
              ⁢
              β
            
            
              sin
              ⁢
              
                
              
              ⁢
              
                φ
                jk
              
            
          
          )
        
        ⁢
        
          
            t
            kA
          
          ⁡
          
            (
            
              
                
                  γ
                  ak
                
                ⁢
                sin
                ⁢
                
                  
                
                ⁢
                
                  θ
                  kj
                
              
              +
              
                
                  γ
                  bj
                
                ⁢
                cos
                ⁢
                
                  
                
                ⁢
                
                  θ
                  kj
                
              
            
            )
          
        
      
    
  



Now β=0 corresponds to Lj and results in:



  
    
      B
      ∥
    
    =
    
      
        
          -
          
            
              cos
              ⁢
              
                
              
              ⁢
              
                φ
                jk
              
            
            
              sin
              ⁢
              
                
              
              ⁢
              
                φ
                jk
              
            
          
        
        ⁢
        
          t
          j
        
        ⁢
        
          γ
          hj
        
      
      +
      
        
          
            t
            k
          
          ⁢
          
            γ
            hk
          
        
        
          sin
          ⁢
          
            
          
          ⁢
          
            φ
            jk
          
        
      
    
  




  
    
      B
      ⊥
    
    =
    
      
        t
        j
      
      ⁢
      
        γ
        hj
      
    
  



It is noted that, for this case, B∥ represents the error in tj.

Now β=φjk corresponds to Lk and results in:



  
    
      B
      ∥
    
    =
    
      
        -
        
          
            
              t
              j
            
            ⁢
            
              γ
              hj
            
          
          
            sin
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
        
      
      +
      
        
          
            cos
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
          
            sin
            ⁢
            
              
            
            ⁢
            
              φ
              jk
            
          
        
        ⁢
        
          t
          
            k
            ⁢
            
              
            
          
        
        ⁢
        
          γ
          hk
        
      
    
  




  
    
      B
      ⊥
    
    =
    
      
        t
        k
      
      ⁢
      
        γ
        hk
      
    
  



It is noted that, for this case, B∥ represents the error in tk.
Finally, if tjA≈tkA and ej≈ek then the normal to the epipolar plane, the direction of B for β=φjk/2 and the direction perpendicular to these vectors are the principle axes of the covariance matrix for Tjk.
3D Tracking
In certain embodiments, a 3D Fusion Trackers have been developed that use an Extended Kalman Filter (EKF) to build three dimensional tracks using both 2D and 3D observations from multiple sensors. Observation sequences from a sensor are associated with existing tracks using gating techniques to determine whether there is an existing track to which the observations should be added. Observation sequences from different sensors may be also associated with each other using gating techniques to determine if they are from the same target. If they are, a new track can be initiated.
When targets are closely spaced, a single sequence from one sensor may pass multiple gates for association with existing tracks or sequences from other sensors. The 3D Fusion Tracker disclosed herein uses a multi-hypothesis approach to maintain each possible track until additional information allows selection of the right track.
Extended Kalman Filter
While Kalman filters are generally known, advantageous embodiments in the present disclosure utilize 9 and 12-state Extended Kalman Filters (EKFs).
In one exemplary embodiment, let

    
    
        zk be the vector of observations at time tk=kT where T is the sample period,
        ηk be a vector of zero mean, independent identically distributed white Gaussian measurement noise with covariance R,
        xk be the state vector at time tk,
        {circumflex over (x)}k be the state vector estimate at time tk,
        vk be a vector of zero mean, independent identically distributed white Gaussian process noise with covariance Q,
        {circumflex over (P)}k be the error covariance matrix estimate at time tk,
        {tilde over (x)}k be the state vector prediction at time tk, and
        {tilde over (P)}k be the error covariance matrix prediction at time tk. Accordingly,
    
    





  
    
      
        z
        _
      
      k
    
    =
    
      
        h
        ⁡
        
          (
          
            
              x
              _
            
            k
          
          )
        
      
      +
      
        
          η
          _
        
        k
      
    
  




  
    
      H
      k
    
    =
    
      
        ∂
        
          h
          k
        
      
      
        ∂
        
          
            x
            ^
          
          k
        
      
    
  




  
    
      H
      k
    
    =
    
      
        ∂
        
          h
          k
        
      
      
        ∂
        
          
            x
            ^
          
          k
        
      
    
  




  
    
      F
      k
    
    =
    
      
        
          ∂
          
            
              x
              ^
            
            
              k
              +
              1
            
          
        
        
          ∂
          
            
              x
              ^
            
            k
          
        
      
      =
      
        
          ∂
          
            f
            ⁡
            
              (
              
                
                  x
                  ^
                
                k
              
              )
            
          
        
        
          ∂
          
            
              x
              ^
            
            k
          
        
      
    
  



It is noted that the predicted state at time tk+1 for a linear Kalman Filter is

{tilde over (x)}k+1+Fk{circumflex over (x)}k 

Predicted error covariance at time tk+1 is

{tilde over (P)}k+1=Fk{circumflex over (P)}kFkT+Qk 

Optimal estimates of the state and covariance given an observation vector, z, at time tk+1 are

{circumflex over (x)}k+1={tilde over (x)}k+1+Kk+1yk+1 

{circumflex over (P)}k+1=[I−Kk+1Hk+1]{tilde over (P)}k+1 

Sk+1=[Hk+1{tilde over (P)}k+1Hk+1T+Rk+1]

where

Kk+1={tilde over (P)}k+1Hk+1TSk+1 

is the Kalman gain and

yk+1=[z−Hk+1{tilde over (x)}k+1]

is the residual.

The normalized square residual (NSR) is given by

NSRk+1=└yk+1TSk+1−1yk+1┘,

which is normally distributed an therefore can be used to estimate the confidence that the observation belongs to the track. Here it assumed in the embodiment that the 2D pointing measurement errors can be characterized by a bias component that is constant over a set of observations and a random component that is different for each observation.

The subsections below describe in greater detail both a 9-state and a 12-state EKF that can be updated with either single 2D observations from the sensors or with 3D triangulations obtained using a least square error procedure to combine observations from multiple sensors.
9 State Kalman Filter
A 9 state filter for a target assumes that the state of the target may be described by its ECI position, velocity and acceleration, which is assumed to be constant over a time step. Random changes in acceleration are the process error over a time step.
The terms in the state transition matrix are assembled from:




  
    F
    k
  
  =
  
    
      
        ∂
        
          
            x
            ^
          
          
            k
            +
            1
          
        
      
      
        ∂
        
          
            x
            ^
          
          k
        
      
    
    =
    
      
        
          ∂
          
            f
            ⁡
            
              (
              
                
                  x
                  ^
                
                k
              
              )
            
          
        
        
          ∂
          
            
              x
              ^
            
            k
          
        
      
      =
      
        [
        
          
            
              1
            
            
              t
            
            
              
                
                  t
                  2
                
                /
                2
              
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              1
            
            
              t
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              t
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
            
              
                
                  t
                  2
                
                /
                2
              
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
            
              
                
                  t
                  2
                
                /
                2
              
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
          
        
        ]
      
    
  




where

{circumflex over (x)}k=[xk {dot over (x)}k {umlaut over (x)}k yk {dot over (y)}k ÿk zk żk {umlaut over (z)}k].

12 State Kalman Filter
A 12 state filter for a target assumes that the state of the target can be described by its ECI position, velocity, acceleration and jerk (rate of change of acceleration), which is assumed to be constant over a time step. Random changes in jerk are the process error over a time step.
Again, the terms in the state transition matrix are assembled from:




  
    F
    k
  
  =
  
    
      
        ∂
        
          
            x
            ^
          
          
            k
            +
            1
          
        
      
      
        ∂
        
          
            x
            ^
          
          k
        
      
    
    =
    
      
        
          ∂
          
            f
            ⁡
            
              (
              
                
                  x
                  ^
                
                k
              
              )
            
          
        
        
          ∂
          
            
              x
              ^
            
            k
          
        
      
      =
      
        [
        
          
            
              1
            
            
              t
            
            
              
                
                  t
                  2
                
                /
                2
              
            
            
              
                
                  t
                  3
                
                ⁢
                
                  /
                
                ⁢
                6
              
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              1
            
            
              t
            
            
              
                
                  t
                  2
                
                /
                2
              
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              1
            
            
              t
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
            
              
                
                  t
                  2
                
                /
                2
              
            
            
              
                
                  t
                  3
                
                /
                6
              
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
            
              
                
                  t
                  2
                
                /
                2
              
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
            
              
                
                  t
                  2
                
                /
                2
              
            
            
              
                
                  t
                  3
                
                /
                6
              
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
            
              
                
                  t
                  2
                
                /
                2
              
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
            
              t
            
          
          
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              0
            
            
              1
            
          
        
        ]
      
    
  




where

{circumflex over (x)}k=[xk {dot over (x)}k {umlaut over (x)}k  yk {dot over (y)}k ÿk  zk żk {umlaut over (z)}k ].

Filter Update with Single 2D Observations
Let p be the pointing vector for a 2D observation. Then
 p=x−v 
where x is the current target position and v is the position of the sensor. Expressing the observation in polar coordinates we have




  z
  =
  
    
      (
      
        
          
            θ
          
        
        
          
            φ
          
        
      
      )
    
    =
    
      (
      
        
          
            
              
                tan
                
                  -
                  1
                
              
              ⁡
              
                (
                
                  
                    p
                    y
                  
                  
                    p
                    x
                  
                
                )
              
            
          
        
        
          
            
              
                tan
                
                  -
                  1
                
              
              ⁡
              
                (
                
                  
                    p
                    z
                  
                  
                    
                      (
                      
                        
                          p
                          x
                          2
                        
                        +
                        
                          p
                          y
                          2
                        
                      
                      )
                    
                    
                      1
                      /
                      2
                    
                  
                
                )
              
            
          
        
      
      )
    
  




Since x is independent of v we have





  H
  =
  
    
      
        ∂
        h
      
      
        ∂
        x
      
    
    =
    
      
        
          ∂
          h
        
        
          ∂
          p
        
      
      =
      
        
          ∂
          z
        
        
          ∂
          p
        
      
    
  




Thus, for a 9-State Kalman Filter we have





  
    H
    k
  
  =
  
    [
    
      
        
          
            
              ∂
              θ
            
            
              ∂
              
                p
                x
              
            
          
        
        
          0
        
        
          0
        
        
          
            
              ∂
              θ
            
            
              ∂
              
                p
                y
              
            
          
        
        
          0
        
        
          0
        
        
          
            
              ∂
              θ
            
            
              ∂
              
                p
                z
              
            
          
        
        
          0
        
        
          0
        
      
      
        
          
            
              ∂
              φ
            
            
              ∂
              
                p
                x
              
            
          
        
        
          0
        
        
          0
        
        
          
            
              ∂
              φ
            
            
              ∂
              
                p
                y
              
            
          
        
        
          0
        
        
          0
        
        
          
            
              ∂
              φ
            
            
              ∂
              
                p
                z
              
            
          
        
        
          0
        
        
          0
        
      
    
    ]
  




and for a 12-State Kalman Filter we have





  
    H
    k
  
  =
  
    [
    
      
        
          
            
              ∂
              θ
            
            
              ∂
              
                p
                x
              
            
          
        
        
          0
        
        
          0
        
        
          0
        
        
          
            
              ∂
              θ
            
            
              ∂
              
                p
                y
              
            
          
        
        
          0
        
        
          0
        
        
          0
        
        
          
            
              ∂
              θ
            
            
              ∂
              
                p
                z
              
            
          
        
        
          0
        
        
          0
        
        
          0
        
      
      
        
          
            
              ∂
              φ
            
            
              ∂
              
                p
                x
              
            
          
        
        
          0
        
        
          0
        
        
          0
        
        
          
            
              ∂
              φ
            
            
              ∂
              
                p
                y
              
            
          
        
        
          0
        
        
          0
        
        
          0
        
        
          
            
              ∂
              φ
            
            
              ∂
              
                p
                z
              
            
          
        
        
          0
        
        
          0
        
        
          0
        
      
    
    ]
  




where




  
    
      
        ∂
        θ
      
      
        ∂
        
          p
          x
        
      
    
    =
    
      -
      
        
          p
          y
        
        
          
            (
            
              
                p
                x
                2
              
              +
              
                p
                y
                2
              
            
            )
          
          
            1
            /
            2
          
        
      
    
  




  
    
      
        ∂
        θ
      
      
        ∂
        
          p
          y
        
      
    
    =
    
      
        p
        x
      
      
        
          (
          
            
              p
              x
              2
            
            +
            
              p
              y
              2
            
          
          )
        
        
          1
          /
          2
        
      
    
  




  
    
      
        ∂
        θ
      
      
        ∂
        
          p
          z
        
      
    
    =
    0
  




  
    
      
        ∂
        φ
      
      
        ∂
        
          p
          x
        
      
    
    =
    
      -
      
        
          
            p
            x
          
          ⁢
          
            p
            z
          
        
        
          
            
              (
              
                
                  p
                  x
                  2
                
                +
                
                  p
                  y
                  2
                
              
              )
            
            
              1
              /
              2
            
          
          ⁢
          
            (
            
              
                p
                x
                2
              
              +
              
                p
                y
                2
              
              +
              
                p
                z
                2
              
            
            )
          
        
      
    
  




  
    
      
        ∂
        φ
      
      
        ∂
        
          p
          y
        
      
    
    =
    
      -
      
        
          
            p
            y
          
          ⁢
          
            p
            z
          
        
        
          
            
              (
              
                
                  p
                  x
                  2
                
                +
                
                  p
                  y
                  2
                
              
              )
            
            
              1
              /
              2
            
          
          ⁢
          
            (
            
              
                p
                x
                2
              
              +
              
                p
                y
                2
              
              +
              
                p
                z
                2
              
            
            )
          
        
      
    
  




  
    
      
        ∂
        φ
      
      
        ∂
        
          p
          z
        
      
    
    =
    
      
        
          (
          
            
              p
              x
              2
            
            +
            
              p
              y
              2
            
          
          )
        
        
          1
          /
          2
        
      
      
        (
        
          
            p
            x
            2
          
          +
          
            p
            y
            2
          
          +
          
            p
            z
            2
          
        
        )
      
    
  




  
    
      
        ∂
        
          h
          
            k
            ,
            i
          
        
      
      
        ∂
        
          x
          
            k
            ,
            i
          
        
      
    
    =
    1
  




  
    
      
        ∂
        
          h
          
            k
            ,
            i
          
        
      
      
        ∂
        
          
            x
            .
          
          
            k
            ,
            i
          
        
      
    
    =
    0
  




  
    
      
        ∂
        
          h
          
            k
            ,
            i
          
        
      
      
        ∂
        
          
            x
             ̈
          
          
            k
            ,
            i
          
        
      
    
    =
    0
  




  
    
      
        ∂
        
          h
          
            k
            ,
            i
          
        
      
      
        ∂
        
          
            x
            ⃛
          
          
            k
            ,
            i
          
        
      
    
    =
    0
  



where i is one of x, y, z.

Thus for a 9 State Kalman Filter it can be seen that




  
    H
    k
  
  =
  
    
      [
      
        
          
            1
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            1
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            1
          
          
            0
          
          
            0
          
        
      
      ]
    
    .
  




For a 12 State Kalman Filter it can be seen that





  
    H
    k
  
  =
  
    
      [
      
        
          
            1
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            1
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            1
          
          
            0
          
          
            0
          
          
            0
          
        
      
      ]
    
    .
  




Synthesized Observations From Fit
Simple 2 degree polynomial fits may be advantageously used to interpolate 2D observations in order to synthesize observations from multiple sensors at a common time. An adaptive fit window may be defined by

    
    
        requiring a minimum number of points,
        setting a maximum number of points,
        setting a maximum fit time
    
    


A windowing scheme may attempt to select points centered about the time of the synthesized point. However, this will not be possible if the time of interest is near an end of the observation set. The random error component for the synthesized point is replaced by the fit error. An observation frequency for the synthesized points is also specified. When many points are synthesized from observation from the same two sensors, it was found the Kalman filter is blind to the constant bias errors and converged more quickly if the measurement error was set only to the fit error without the bias component.
Weighted Least Squares for Triangulation
Weighted least squares approaches have been found to be advantageous in multiple-image data point triangulation. In this section of the disclosure, a summary of the weighted least squares formulation is provided followed by application to the triangulation points for multiple sensors seeing the same.
Weighted Least Squares Formulation
Assuming

l={tilde over (l)}+e, 

where l is a vector of n unperturbed observations, e is a vector of a priori unknown residuals and the actual observations {tilde over (l)} are normally distributed observations with

{tilde over (l)} ̃N({tilde over (l)},Σll),

where l is the mean of the distribution and Σll denotes the covariance matrix. Let x be a vector of u model parameters. The maximum likelihood estimators for model parameters are found by solving the nonlinear equality constrained problem

minimize└Θ=eTQu−1e┘ subject to f(x,1)=0

where Qu is the cofactor matrix found from normalization Σu=σ02Qu and σ02 is the a priori standard deviation of a unit weight observation. The inverted cofactor matrix can be interpreted as a weight matrix. Linearization of the problem allows an iterative solution given an initial guess of the model parameters {circumflex over (x)}(0) and with a first estimate Î(0)=Ĩ for the unperturbed observations. The iteration is stopped if the improvement Δ{circumflex over (x)}(s)={circumflex over (x)}(s+1)−{circumflex over (x)}(s) at step s is smaller than a specified threshold. The estimates for the unperturbed observations are updated at each step by ΔÎ(s)=Î(s+1)−Î(s). When stopping the iteration at step s*, the final estimates of the unknowns are Î(s*+1) and {circumflex over (x)}(s*+1). A final estimate for the residuals e is obtained through e=Î(s*+1)−{tilde over (l)}.

A linearized form of the constraint functions is

AΔx+BΔl+w=0,

where A=∂f/∂x is an r×u matrix and B=∂f/∂I is an r×n and w=−f({circumflex over (x)}(s),Î(s)) is a vector of r function residuals.

The solutions for  and  are

=(ATQww−1A)−1ATQww−1w, 

and

=QllBTQww−1(−A(ATQww−1A)−1ATQww−1)w, 

where Qww=BQllBT simplifies the notation and can be interpreted as the cofactor matrix of the function residuals. The cofactors Qww can be converted to covariances Σww by multiplying Qww with





  
    
      
        σ
        0
        2
      
      ^
    
    =
    
      
        
          
            e
            T
          
          ^
        
        ⁢
        
          Q
          ll
          
            -
            1
          
        
        ⁢
        
          e
          ^
        
      
      v
    
  
  ,




where v is the number of degrees of freedom of the estimation problem. If the data and the model agree, {circumflex over (σ)}02 is an unbiased estimator of σ02. It is noted that multiplying Qww with σ02 instead of {circumflex over (σ)}02 results in a priori covariances. Thus, the solutions for  and  can be written

=(ATΣww−1A)−1ATΣww−1w, 

and

=ΣllBTΣww−1(−A(ATΣww−1A)−1ATΣww−1)w, 

where Σww=BΣllBT again simplifies the notation and can be interpreted as the a priori covariance matrix of the function residuals. In practice, Î(s+1) can often be calculated from a sensor model using the new value {circumflex over (x)}(s+1), removing the need to use the approximate  above.

Propagation of the cofactors of the function residuals into the spaces of the unknowns yields:

Q=(ATQww−1A)−1=(AT(BQllBT)−1A)−1,

and

Q=QllBTQww−1BQll−QllBTQww−1A(ATQww−1A)−1ATQww−1BQll.

Now




  
    Q
    
      
        x
        ^
      
      ⁢
      
        x
        ^
      
    
  
  ≈
  
    
      Q
      
        Δ
        ⁢
        
          x
          ^
        
        ⁢
        Δ
        ⁢
        
          x
          ^
        
      
    
    ⁢
    
      
    
    ⁢
    and
    ⁢
    
      
    
    ⁢
    
      Q
      
        
          e
          ^
        
        ⁢
        
          e
          ^
        
      
    
  
  ≈
  
    
      Q
      
        
          
            Δ
            ⁢
            
              
            
            ⁢
            1
          
          ^
        
        ⁢
        
          
            Δ
            ⁢
            
              
            
            ⁢
            1
          
          ^
        
      
    
    .
  




The covariances are obtained by multiplying by σ02 or {circumflex over (σ)}02. So the a priori covariances are





  
    
      ∑
      
        
          X
          ^
        
        ⁢
        
          X
          ^
        
      
    
    ⁢
    
      =
      
        
          
            (
            
              
                A
                T
              
              ⁢
              
                
                  ∑
                  ww
                  
                    -
                    1
                  
                
                ⁢
                A
              
            
            )
          
          
            -
            1
          
        
        =
        
          
            (
            
              
                
                  
                    A
                    T
                  
                  ⁡
                  
                    (
                    
                      B
                      ⁢
                      
                        
                          ∑
                          ll
                        
                        ⁢
                        
                          B
                          T
                        
                      
                    
                    )
                  
                
                
                  -
                  1
                
              
              ⁢
              A
            
            )
          
          
            -
            1
          
        
      
    
  
  ,




and




  
    
      ∑
      
        
          e
          ^
        
        ⁢
        
          e
          ^
        
      
    
    ⁢
    
      =
      
        
          ∑
          ll
        
        ⁢
        
          
            B
            T
          
          ⁢
          
            
              ∑
              ww
              
                -
                1
              
            
            ⁢
            
              B
              ⁢
              
                
              
              ⁢
              
                
                  ∑
                  ll
                
                ⁢
                
                  -
                  
                    
                      ∑
                      ll
                    
                    ⁢
                    
                      
                        B
                        T
                      
                      ⁢
                      
                        
                          ∑
                          ww
                          
                            -
                            1
                          
                        
                        ⁢
                        
                          
                            
                              A
                              ⁡
                              
                                (
                                
                                  
                                    A
                                    T
                                  
                                  ⁢
                                  
                                    
                                      ∑
                                      ww
                                      
                                        -
                                        1
                                      
                                    
                                    ⁢
                                    A
                                  
                                
                                )
                              
                            
                            
                              -
                              1
                            
                          
                          ⁢
                          
                            A
                            T
                          
                          ⁢
                          
                            
                              ∑
                              ww
                              
                                -
                                1
                              
                            
                            ⁢
                            
                              B
                              ⁢
                              
                                
                              
                              ⁢
                              Σ
                            
                          
                        
                      
                    
                  
                
              
            
          
        
      
    
  




  
    
      ∑
      
        
          e
          ^
        
        ⁢
        
          e
          ^
        
      
    
    ⁢
    
      =
      
        
          ∑
          ll
        
        ⁢
        
          
            B
            T
          
          ⁢
          
            
              ∑
              ww
              
                -
                1
              
            
            ⁢
            
              B
              ⁢
              
                
              
              ⁢
              
                
                  ∑
                  ll
                
                ⁢
                
                  -
                  
                    
                      ∑
                      ll
                    
                    ⁢
                    
                      
                        B
                        T
                      
                      ⁢
                      
                        
                          ∑
                          ww
                          
                            -
                            1
                          
                        
                        ⁢
                        
                          A
                          ⁢
                          
                            
                          
                          ⁢
                          
                            Σ
                            
                              
                                x
                                ^
                              
                              ⁢
                              
                                x
                                ^
                              
                            
                          
                          ⁢
                          
                            A
                            T
                          
                          ⁢
                          
                            
                              ∑
                              ww
                              
                                -
                                1
                              
                            
                            ⁢
                            
                              B
                              ⁢
                              
                                
                              
                              ⁢
                              
                                
                                  ∑
                                  ll
                                
                                .
                              
                            
                          
                        
                      
                    
                  
                
              
            
          
        
      
    
  



The standardized residual for use in test statistics is





  
    
      e
      _
    
    i
  
  =
  
    
      
        
          e
          ^
        
        i
      
      
        
          ∑
          
            
              e
              ^
            
            ⁢
            
              e
              ^
            
            ⁢
            
              
            
            ⁢
            ii
          
        
      
    
    ∼
    
      
        N
        ⁡
        
          (
          
            0
            ,
            1
          
          )
        
      
      .
    
  




Where





  
    ∑
    
      
        e
        ^
      
      ⁢
      
        e
        ^
      
      ⁢
      ii
    
  
  ⁢
  
    
  




is the ith diagonal element of





  
    ∑
    
      
        e
        ^
      
      ⁢
      
        e
        ^
      
    
  
  ⁢
  
    
  
  .




It may be assumed that the standardized residuals are distributed normally.

Application to Events Seen by Multiple Pointing Sensors
In the current embodiment, the data associated with a line of sight Li are sensor position vi, sensor pointing vector pi, and ei. The 2D error ellipse perpendicular to the pointing vector is expressed in terms of major axis ai, minor axis bi, and orientation angle θi. This allows a simple formulation of the weighted least square approach to find the triangulation point (x≡T) in terms of these measurements that form m sensors minimizes




  Θ
  =
  
    
      
        e
        T
      
      ⁢
      
        Q
        ll
        
          -
          1
        
      
      ⁢
      e
    
    =
    
      
        
          σ
          0
          2
        
        ⁢
        
          e
          T
        
        ⁢
        
          
            ∑
            ll
            
              -
              1
            
          
          ⁢
          
            
          
          ⁢
          e
        
      
      =
      
        
          σ
          0
          2
        
        ⁢
        
          
            ∑
            i
          
          ⁢
          
            
          
          ⁢
          
            (
            
              
                
                  (
                  
                    
                      γ
                      ai
                    
                    
                      a
                      i
                    
                  
                  )
                
                2
              
              +
              
                
                  (
                  
                    
                      γ
                      bi
                    
                    
                      b
                      i
                    
                  
                  )
                
                2
              
            
            )
          
        
      
    
  




where γai is the angular error in the ai direction for sensor i, γbi is the angular error in the bi direction for sensor i and





  
    ∑
    ll
  
  ⁢
  
    
  
  ⁢
  
    =
    
      [
      
        
          
            
              a
              1
              2
            
          
          
            0
          
          
            ...
          
          
            0
          
          
            0
          
        
        
          
            0
          
          
            
              b
              1
              2
            
          
          
            ...
          
          
            0
          
          
            0
          
        
        
          
            ...
          
          
            ...
          
          
            ...
          
          
            ...
          
          
            ...
          
        
        
          
            0
          
          
            0
          
          
            ...
          
          
            
              a
              m
              2
            
          
          
            0
          
        
        
          
            0
          
          
            0
          
          
            ...
          
          
            0
          
          
            
              b
              m
              2
            
          
        
      
      ]
    
  




is the 2m×2m covariance matrix. Then there are two condition equations for each sensor:

Fai=γai=gai(vi,pi,ei,T)=0

Fbi=γbi=gbi(vi,pi,ei,T)=0

where gai calculates γai using vi,pi,ei and an estimate of the triangulation point, T, and gbi calculates γai using vi,pi,ei and an estimate of T.

Now

{circumflex over (p)}i−{tilde over (p)}i={circumflex over (γ)}aiuai+{circumflex over (γ)}biubi 

where uai and ubi are the unit vectors in the directions of the ai and bi axes of ei. So the condition functions can be determined from

gai(vi,pi,ei,T)=gai(vi,{tilde over (p)}i,γai,γbi,ei,T)

gbi(vi,pi,ei,T)=gbi(vi,{tilde over (p)}i,γai,γbi,ei,T)

Leading to the identification





  l
  ≡
  
    [
    
      
        
          
            γ
            
              a
              ⁢
              
                
              
              ⁢
              1
            
          
        
      
      
        
          
            γ
            
              b
              ⁢
              
                
              
              ⁢
              1
            
          
        
      
      
        
          ...
        
      
      
        
          ...
        
      
      
        
          
            γ
            am
          
        
      
      
        
          
            γ
            bm
          
        
      
    
    ]
  




and





  B
  =
  
    
      
        ∂
        f
      
      
        ∂
        l
      
    
    =
    
      [
      
        
          
            
              
                ∂
                
                  f
                  1
                
              
              
                ∂
                
                  l
                  1
                
              
            
          
          
            ...
          
          
            0
          
        
        
          
            ...
          
          
            ...
          
          
            ...
          
        
        
          
            0
          
          
            ...
          
          
            
              
                ∂
                
                  f
                  m
                
              
              
                ∂
                
                  l
                  m
                
              
            
          
        
      
      ]
    
  




where





  
    
      ∂
      
        f
        i
      
    
    
      ∂
      
        l
        i
      
    
  
  =
  
    
      [
      
        
          
            
              
                ∂
                
                  g
                  ai
                
              
              
                ∂
                
                  γ
                  ai
                
              
            
          
          
            
              
                ∂
                
                  g
                  ai
                
              
              
                ∂
                
                  γ
                  bi
                
              
            
          
        
        
          
            
              
                ∂
                
                  g
                  bi
                
              
              
                ∂
                
                  γ
                  ai
                
              
            
          
          
            
              
                ∂
                
                  g
                  bi
                
              
              
                ∂
                
                  γ
                  bi
                
              
            
          
        
      
      ]
    
    .
  




Since gai calculates the current γai and gbi calculates the current γbi, which are assumed to be independent errors ∂fi/∂li, should be the identity matrix





  
    
      ∂
      
        f
        i
      
    
    
      ∂
      
        l
        i
      
    
  
  ≈
  
    
      [
      
        
          
            1
          
          
            0
          
        
        
          
            0
          
          
            1
          
        
      
      ]
    
    .
  




Now x≡T so




  A
  =
  
    
      
        ∂
        f
      
      
        ∂
        x
      
    
    =
    
      [
      
        
          
            
              
                ∂
                
                  g
                  
                    a
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  x
                
              
            
          
          
            
              
                ∂
                
                  g
                  
                    a
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  y
                
              
            
          
          
            
              
                ∂
                
                  g
                  
                    a
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  z
                
              
            
          
        
        
          
            
              
                ∂
                
                  g
                  
                    b
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  x
                
              
            
          
          
            
              
                ∂
                
                  g
                  
                    b
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  y
                
              
            
          
          
            
              
                ∂
                
                  g
                  
                    b
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              
                ∂
                
                  T
                  z
                
              
            
          
        
        
          
            ...
          
          
            ...
          
          
            ...
          
        
        
          
            ...
          
          
            ...
          
          
            ...
          
        
        
          
            
              
                ∂
                
                  g
                  am
                
              
              
                ∂
                
                  T
                  x
                
              
            
          
          
            
              
                ∂
                
                  g
                  am
                
              
              
                ∂
                
                  T
                  y
                
              
            
          
          
            
              
                ∂
                
                  g
                  am
                
              
              
                ∂
                
                  T
                  z
                
              
            
          
        
        
          
            
              
                ∂
                
                  g
                  bm
                
              
              
                ∂
                
                  T
                  x
                
              
            
          
          
            
              
                ∂
                
                  g
                  bm
                
              
              
                ∂
                
                  T
                  y
                
              
            
          
          
            
              
                ∂
                
                  g
                  bm
                
              
              
                ∂
                
                  T
                  z
                
              
            
          
        
      
      ]
    
  




Finally





  w
  =
  
    
      -
      f
    
    =
    
      
        [
        
          
            
              
                -
                
                  F
                  
                    a
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
            
          
          
            
              
                -
                
                  F
                  
                    b
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
            
          
          
            
              ...
            
          
          
            
              ...
            
          
          
            
              
                -
                
                  F
                  am
                
              
            
          
          
            
              
                -
                
                  F
                  bm
                
              
            
          
        
        ]
      
      .
    
  




At each iteration step, s, the current value of pi and the new value of T are used to calculate γai and γbi. The angle between the new and previous pi is the current angular error and is projected onto the ai and bi axes of the error ellipse to obtain γai and γbi. The new pointing vector at the next step can be found from

{circumflex over (T)}(s+1)=vi+{circumflex over (t)}(s+1){circumflex over (p)}i(s+1)

giving





  
    
      
        p
        ^
      
      i
    
    ⁡
    
      (
      
        s
        +
        1
      
      )
    
  
  =
  
    
      
        
          T
          ^
        
        ⁡
        
          (
          
            s
            +
            1
          
          )
        
      
      -
      
        v
        i
      
    
    
      
      
        
          
            T
            ^
          
          ⁡
          
            (
            
              s
              +
              1
            
            )
          
        
        -
        
          v
          i
        
      
      
    
  




Now,

{circumflex over (p)}i(s+1)•{circumflex over (p)}i(s)=cos {circumflex over (γ)}i(s+1)

{circumflex over (γ)}i(s+1)=cos−1({circumflex over (p)}i(s+1)•{circumflex over (p)}i(s))

where {circumflex over (γ)}i is an estimate of the angular error for the current {circumflex over (T)}. Define




  
    
      
        
          u
          ^
        
        
          γ
          ⁢
          
            
          
          ⁢
          i
        
      
      ⁡
      
        (
        
          s
          +
          1
        
        )
      
    
    =
    
      
        
          
            
              p
              ^
            
            i
          
          ⁡
          
            (
            
              s
              +
              1
            
            )
          
        
        -
        
          
            
              p
              ^
            
            i
          
          ⁡
          
            (
            s
            )
          
        
      
      
        
        
          
            
              
                p
                ^
              
              i
            
            ⁡
            
              (
              
                s
                +
                1
              
              )
            
          
          -
          
            
              
                p
                ^
              
              i
            
            ⁡
            
              (
              s
              )
            
          
        
        
      
    
  




  
    
      
        
          ρ
          ^
        
        i
      
      ⁡
      
        (
        
          s
          +
          1
        
        )
      
    
    =
    
      
        cos
        
          -
          1
        
      
      ⁡
      
        (
        
          
            
              
                u
                ^
              
              
                γ
                ⁢
                
                  
                
                ⁢
                i
              
            
            ⁡
            
              (
              
                s
                +
                1
              
              )
            
          
          ·
          
            u
            ai
          
        
        )
      
    
  



where ûγi is the unit vector in the direction of {circumflex over (γ)}i, uai is the unit vector in the direction of major axis of the error ellipse ei, and {circumflex over (ρ)}i is the angle between {circumflex over (γ)}i and the major axis of the error ellipse. Then

Fai(s+1)={circumflex over (γ)}ai(s+1)={circumflex over (γ)}i(s+1)cos ρi(s+1)

Fbi(s+1)={circumflex over (γ)}bi(s+1)={circumflex over (γ)}i(s+1)sin ρi(s+1)

Recall that after the final iteration the final residuals are

e={circumflex over (l)}(s*+1)−{tilde over (l)}. 

As before

{circumflex over (p)}i(s*+1)•{tilde over (p)}i=cos i 





  
    
      γ
      +
    
    i
  
  =
  
    
      cos
      
        -
        1
      
    
    ⁡
    
      (
      
        
          
            
              p
              ^
            
            i
          
          ⁡
          
            (
            
              
                s
                *
              
              +
              1
            
            )
          
        
        ·
        
          
            p
            ~
          
          i
        
      
      )
    
  




where i is an estimate of the angular error for the final estimate of the triangulation point . (U) Define




  
    
      
        u
        +
      
      
        γ
        ⁢
        
          
        
        ⁢
        i
      
    
    =
    
      
        
          
            
              p
              ^
            
            i
          
          ⁡
          
            (
            
              
                s
                *
              
              +
              1
            
            )
          
        
        -
        
          
            p
            ~
          
          i
        
      
      
        
        
          
            
              
                p
                ^
              
              i
            
            ⁡
            
              (
              
                
                  s
                  *
                
                +
                1
              
              )
            
          
          -
          
            
              p
              ~
            
            i
          
        
        
      
    
  




  
    
      
        ρ
        +
      
      i
    
    =
    
      
        cos
        
          -
          1
        
      
      ⁡
      
        (
        
          
            
              u
              ′
            
            
              γ
              ⁢
              
                
              
              ⁢
              i
            
          
          ·
          
            u
            ai
          
        
        )
      
    
  



where i is the unit vector in the direction of i, and i is the angle between i and the major axis of the error ellipse. Then




  
    
      
        γ
        +
      
      ai
    
    =
    
      
        
          γ
          +
        
        i
      
      ⁢
      cos
      ⁢
      
        
      
      ⁢
      
        
          ρ
          +
        
        i
      
    
  




  
    
      
        γ
        +
      
      bi
    
    =
    
      
        
          γ
          +
        
        i
      
      ⁢
      sin
      ⁢
      
        
      
      ⁢
      
        
          ρ
          +
        
        i
      
    
  



The standardized residuals are



  
    
      
        γ
        -
      
      ai
    
    =
    
      
        
          
            γ
            +
          
          ai
        
        
          
            
              ∑
              
                
                  e
                  ^
                
                ⁢
                
                  e
                  ^
                
                ⁢
                ai
              
            
            ⁢
            
              
            
          
        
      
      ∼
      
        N
        ⁡
        
          (
          
            0
            ,
            1
          
          )
        
      
    
  




  
    
      
        γ
        -
      
      bi
    
    =
    
      
        
          
            γ
            +
          
          bi
        
        
          
            
              ∑
              
                
                  e
                  ^
                
                ⁢
                
                  e
                  ^
                
                ⁢
                bi
              
            
            ⁢
            
              
            
          
        
      
      ∼
      
        N
        ⁡
        
          (
          
            0
            ,
            1
          
          )
        
      
    
  



The standardized residuals can be used as gates to perform preliminary association of observations from different 2D sensors.

Association of 2D Tracks from Different Sensors
2D tracks for pointing sensor provide no range information and therefore must be associated with another 2D track from another sensor to determine where the lines of sight intersect. This intersection can provide an estimate of the position of the observation source position. Pointing errors cause the lines of sight to miss intersection. The miss distance can be converted into a confidence that the observations are consistent with being caused by the same source. FIG. 4 illustrates an exemplary general geometry under one embodiment. Here, Djk is shortest distance between lines of sight, eni is angular error, ti is range from sensor to target, T is target position, pi is sensor pointing vector, vi is sensor position vector. For small angular errors, we have:

Djk=|tjenj−tkenk|

Letting

σDjk2=(tjσnj)2+(tkσnk)2 

eni ̃N(0,σni2)

where σni is projection of error ellipse onto line between lines of sight. Then the normalized distance between the line of sight is

{tilde over (D)}jk=Djk/σDjk ̃|N(0,1)|

This normalized distance allows a first order confidence based assessment of whether the two lines of sight are from the same source. This is used as a preliminary screen for association of the observations (FIG. 4). The final assessment of association is based on the standardized residual of the triangulation of the two lines of sight to find the target position.
Track Initiation and Extension
Observation sequences from sensors are considered in one embodiment for track initiation when they have at least a user specified number of observations (e.g., 5-10) or a user specified duration (e.g., 4-8 seconds). 3D observation sequences are used directly to initiate a 3D track with the EKF.
A 2D sequence is checked for association with 2D sequences from different sensors that overlap in time with it. If the time overlap is short, the association calculation is performed for the midpoint time of the overlap. If the overlap is long enough (a user specified duration), the association calculation is performed twice: at 0.1 of the overlap and at 0.9 of the overlap. The confidence values are then fused to make the determination of association. Since the observations from different sensors are not synchronized, fits are used to synthesize observations at the time of the association evaluations.
A 3D track hypothesis is initiated for each pair of observation sequences that pass the association test. To initiate a track from associated 2D sequences, a sequence of 3D points is synthesized by triangulation of 2D observations synthesized using fits of the 2D observation sequences from the different sensors. The frequency of synthesized 3D observations and the length of the track initiation sequence are user specified. Pruning methods for eliminating 3D track hypotheses are described in further detail below.
After track initiation, the track can be extended using the 2D observations from the individual sensors one at a time or by continuing the triangulation scheme used to initiate the track. Normally, for the sensors of current interest, the triangulation approach to extending the track leads to smaller velocity and position errors in the estimated track. This is due to the fact that the fitting of observation sequences to synthesize points from the sensors at a common time replaces the random portion of the pointing errors with the smaller fit error.
Association of 2D and 3D Tracks to 3D Fusion Tracks
A track sequences from a sensor can be associated with an existing 3D fusion track if the 3D fusion track does not yet have a contributing track sequence from that sensor. This association is accessed using the normalized square residual of a 2D or 3D observation when adding it to the EKF for the 3D fusion track. Each passing 3D Fusion track is branched into two track hypotheses—one with the sequence observations added and one without.
When there are three 2D sensors that see a target, association calculations can generate track hypotheses for the three possible pairs of sensors and the branch 3D track that contains all three 2D tracks. Logic is used to identify this case and keep only the 3D track that incorporates all three 2D tracks.
Closely Spaced Objects
Closely spaced objects may present challenges to a 3D fusion tracker. The input tracks from individual sensors may pass association gates for multiple tracks from the other sensors and selecting the right association can be difficult. For example, two targets of the same type may stay close together for an extended time so that the gating calculations cannot reveal which of the associations are correct. Furthermore, resolution differences may cause one sensor to not be able to resolve two targets into separate tracks, while another sensor reports two tracks. Similarly, geometry may cause one sensor to resolve multiple targets into separate tracks well after another similar sensor is able to resolve them.
Intermittent Detection
Tracking may be affected by the sensitivity of contributing sensors. The delectability of the target varies over the track and a sensor's ability to detect the target may be intermittent or very different from that of other contributing sensors. This can lead to disconnected track fragments from contributing sensors or time gaps in the reported observations for a single track. The fusion tracker should be able to deal with various combinations of these situations.
When two 2D sensors are contributing to a 3D track and one of them has a gap in detections, updates of the track are due to the other sensor's observations which provide no range information. The 3D track estimate can accumulate large errors along the line of sight of the contributing sensor. When the other sensor starts adding observation again, the 3D track estimate can require a large correction which can make the track estimate experience a large error transient as it re-stabilizes.
Likelihood Pruning
Multi-hypothesis tracking may be employed for single sensor tracks. In the case of single sensors, the bias is the same for each track hypothesis and therefore does not affect the scoring of the hypotheses using likelihood arguments. In the case of multiple sensor miss-associations, the difference in the scores for one candidate association and another cannot be isolated from the bias. The low score for one hypothesis may be due to its non-physical nature or the fact that the bias errors work against it.
Bias Consistency Pruning
If boresites for the sensors are small enough, then it would be expected that the net bias error effect on the line of site miss distance would be the same for all targets seen by both sensors in the overlapping area of their boresites. In this case, one can invoke a bias consistency technique. The miss distance would be distributed about the constant net bias error according to a variance that reflects the variance in the zero sum net random errors. In tests, it was found that if there are only a few closely spaced targets among a large number of targets in the overlapping boresites, outliers could be reliably rejected as false associations if their miss distance was not consistent with a distribution about the mean miss distance due to the random errors. However, if there are a large percentage of closely spaced objects or the random errors are large with respect to the bias errors, outlier selection is not reliable and the mean miss distance due to net bias errors cannot be determined.
Track Feature Assessment
Feature based classification may be advantageous in target recognition applications and can play a role the pruning of track hypotheses in 3D track fusion applications. For example, maximum acceleration and velocity might be specified by target type, and miss-association of 2D tracks from different sensors can lead to negative calculated altitudes, which can be rejected.
3D Fusion Tracker Process Flow
After initialization, the basic flow for the 3D Fusion Tracker is illustrated in the exemplary embodiment of FIG. 5 and comprises the following nested loop:

    
    
        1) Loop processing sensor observation sets (600)
        
            a) Wait for next observation set (601)
            b) Sort new observations by time (601)
            c) Loop processing single observations (602)
            
                i) Get next observation (603)
                ii) Initiate tracks from ready observation sequences (604)
                iii) Associate sequences of observations with existing 3D tracks (605)
                iv) Add observation to all track branches that it is associated with (606)
                v) Prune track hypotheses (607)
                vi) Go to top of loop
            
            
            d) Report current tracks (608)
            e) Terminate stale tracks (609)
            f) Go to top of loop
        
        
    
    


Various functions described herein can be implemented in hardware, software, or any combination thereof. If implemented in software, the functions can be stored on or transmitted over as one or more instructions or code on a computer-readable medium. Computer-readable media includes computer-readable storage media. A computer-readable storage media can be any available storage media that can be accessed by a computer. By way of example, and not limitation, such computer-readable storage media can comprise RAM, ROM, EEPROM, CD-ROM or other optical disk storage, magnetic disk storage or other magnetic storage devices, or any other medium that can be used to carry or store desired program code in the form of instructions or data structures and that can be accessed by a computer. Disk and disc, as used herein, include compact disc (CD), laser disc, optical disc, digital versatile disc (DVD), floppy disk, and Blu-ray disc (BD), where disks usually reproduce data magnetically and discs usually reproduce data optically with lasers. Further, a propagated signal is not included within the scope of computer-readable storage media. Computer-readable media also includes communication media including any medium that facilitates transfer of a computer program from one place to another. A connection, for instance, can be a communication medium. For example, if the software is transmitted from a website, server, or other remote source using a coaxial cable, fiber optic cable, twisted pair, digital subscriber line (DSL), or wireless technologies such as infrared, radio, and microwave, then the coaxial cable, fiber optic cable, twisted pair, DSL, or wireless technologies such as infrared, radio and microwave are included in the definition of communication medium. Combinations of the above should also be included within the scope of computer-readable media.
Alternatively, or in addition, the functionally described herein can be performed, at least in part, by one or more hardware logic components. For example, and without limitation, illustrative types of hardware logic components that can be used include Field-programmable Gate Arrays (FPGAs), Program-specific Integrated Circuits (ASICs), Program-specific Standard Products (ASSPs), System-on-a-chip systems (SOCs), Complex Programmable Logic Devices (CPLDs), etc.
What has been described above includes examples of one or more embodiments. It is, of course, not possible to describe every conceivable modification and alteration of the above devices or methodologies for purposes of describing the aforementioned aspects, but one of ordinary skill in the art can recognize that many further modifications and permutations of various aspects are possible. Accordingly, the described aspects are intended to embrace all such alterations, modifications, and variations that fall within the spirit and scope of the appended claims. Furthermore, to the extent that the term “includes” is used in either the details description or the claims, such term is intended to be inclusive in a manner similar to the term “comprising” as “comprising” is interpreted when employed as a transitional word in a claim.
It can also be seen that various features are grouped together in individual embodiments for the purpose of streamlining the disclosure. This method of disclosure is not to be interpreted as reflecting an intention that the claimed embodiments require more features than are expressly recited in each claim. Rather, as the following claims reflect, inventive subject matter lies in less than all features of a single disclosed embodiment. Thus the following claims are hereby incorporated into the Detailed Description, with each claim standing on its own as a separate embodiment.

## Claims

1. A processor-based method for tracking at least one object utilizing a plurality of sensors, the processor-based method comprising:
receiving a series of observation data relative to the at least one object over a time base for each of the plurality of sensors, the observation data comprising sensor position data, pointing vector data and observation error data;
determining a triangulation point in a processor using a magnitude (D.jk.) of a shortest line connecting a line of sight value (M.jk.) from each of the series of observation data from each of the plurality of sensors to the at least one object; and
performing correlation processing on the observation data and triangulation point in the processor to determine if at least two of the plurality of sensors are tracking the same object.

2. The processor-based method of claim 1, wherein the magnitude is determined from a measure of observation error data perpendicular to the line of sight.

3. The processor-based method of claim 1, wherein the magnitude is determined from an approximation of angular errors (γaj,γbj) of the observation data perpendicular to an epipolar plane.

4. The processor-based method of claim 1, wherein determining the triangulation point comprises weighted least squares triangulation.

5. The processor-based method of claim 4, further comprising the step of generating a confidence value that the same object is being tracked, wherein the confidence value is derived from one or more standardized residuals from the weighted least squares triangulation.

6. The processor-based method of claim 1, wherein the correlation processing comprises normalizing each line of sight value.

7. The processor-based method of claim 6, wherein the correlation processing further comprises an iteration of least squares triangulation for each normalized line of sight value.

8. The processor-based method of claim 1, further comprising the step of applying a polynomial fit to at least some of the series of observation data occurring at a common time along the time base.

9. The processor-based method of claim 1, wherein determining the triangulation point comprises the step of applying a fit to smooth at least some of the series of observation data occurring at a common time base to reduce error in velocity estimation.

10. A system for tracking at least one object, the system comprising:
a plurality of pointing sensors; and
a tracking system, operatively coupled to the plurality of pointing sensors, wherein the tracking system is configured to receive a series of observation data relative to the at least one object over a time base for each of the plurality of pointing sensors, the observation data comprising sensor position data, pointing vector data and observation error data,
the tracking system being further configured to determine a triangulation point using a magnitude (D.jk.) of a shortest line connecting a line of sight value (M.jk.) from each of the series of observation data from each of the plurality of sensors to the at least one object, and perform correlation processing on the observation data and triangulation point to determine if at least two of the plurality of sensors are tracking the same object.

11. The system of claim 10, wherein the tracking system is configured to determine the magnitude from a measure of observation error data perpendicular to the line of sight.

12. The system of claim 10, wherein the tracking system is configured to determine the magnitude from an approximation of angular errors (γaj,γbj) of the observation data perpendicular to an epipolar plane.

13. The system of claim 10, wherein the triangulation point is determined at least in part from weighted least squares triangulation.

14. The system of claim 13, wherein the tracking system is configured to generate a confidence value that the same object is being tracked, wherein the confidence value is derived from one or more standardized residuals from the weighted least squares triangulation.

15. The system of claim 10, wherein the system is configured to perform correlation processing comprising normalization of each line of sight value.

16. The system of claim 15, wherein the correlation processing further comprises an iteration of least squares triangulation for each normalized line of sight value.

17. The system of claim 10, wherein the tracking system is further configured to apply a polynomial fit to at least some of the series of observation data occurring at a common time along the time base.

18. The system of claim 10, wherein the tracking system is further configured to determine the triangulation point by applying a fit to smooth at least some of the series of observation data occurring at a common time base to reduce error in velocity estimation.