PATENT ABSTRACT
This invention is related to a Video-Doppler-Radar Traffic Surveillance System comprising of multiple Doppler radars and video cameras, circuitry for processing radar and video signals, and data recording and displaying devices. Although the system is mainly designed for roadside traffic surveillance, it can be used in different applications, such as mounted on a host vehicle or on a UAV. The system will provide continuous surveillance of all incoming and leaving traffic.

PATENT DESCRIPTION
TECHNICAL FIELD 
       [0001]    The invention relates to a video-Doppler-radar (Vidar) traffic surveillance system. 
       BACKGROUND OF THE INVENTION 
       [0002]    (1) Doppler Radar Based Traffic Surveillance Systems: A traditional radar based traffic surveillance system uses a Doppler radar for vehicle speed monitoring which measures a vehicle speed at line-of-sight (LOS). In  FIG. 1 , the speed of an approaching (or a leaving) vehicle is calculated in terms of Doppler frequency f D  by 
         [0000]    
       
         
           
             
               
                 
                   
                     v 
                     t 
                   
                   = 
                   
                     
                       f 
                       D 
                     
                     
                       K 
                        
                       
                           
                       
                        
                       
                         cos 
                          
                         
                           ( 
                           
                             φ 
                             t 
                           
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   1 
                   ) 
                 
               
             
           
         
       
     
         [0003]    where K is a Doppler frequency conversion constant. Although a Doppler radar based system has an advantage of a long detection range, there are several difficulties associated with the traditional radar based system, including (1) the Doppler radar beam angle is too large to precisely locate vehicles within the radar beam; (2) the angle between the vehicle moving direction and the LOS, φ t , is unknown and therefore, needs to be small enough for a reasonable speed estimation accuracy; (3) since all velocity vectors on the equal-Doppler cone in  FIG. 1  will generate a same speed, the Doppler radar cannot differentiate the vehicles with a same speed but different directions defined by the same equal-Doppler cone. Therefore, no precise target location information can be derived in a traditional Doppler radar based traffic surveillance system. 
         [0004]    (2) Video Camera Based Traffic Surveillance Systems: 
         [0005]    A video camera based traffic surveillance system uses a video camera to capture a traffic scene and relies on computer vision techniques to indirectly calculate vehicle speeds. Precise vehicle locations can be identified. However, since no direct speed measurements are available and the camera has a finite number of pixels, the video camera based traffic surveillance system can be used only in a short distance application. 
         [0006]    This invention combines the both Doppler radar based system and the video based system into a unique traffic surveillance system to preserve the advantages of both systems and overcome the shortcomings of both systems. 
       SUMMARY 
       [0007]    A video-Doppler-radar (Vidar) traffic surveillance system to monitor traffic may include a first movable Doppler radar to generate a first radar beam along the direction of a first motion ray, a second movable Doppler radar to generate a second radar beam along the direction of a second motion ray, a third fixed Doppler radar to generate a third radar beam along a direction ray, a video camera to serve as an information fusion platform by intersecting the first and second radar motion rays with the camera virtual image plane, a data processing device to process Doppler radar and video information, a tracking device to continuously point the surveillance system to the moving vehicle, and a recording device to continuously record the complete information of the moving vehicle. 
         [0008]    The surveillance system may register the first movable radar and the second movable radar with the video camera by locating the intersections of the first and second movable radar motion rays with the video camera virtual image plane. 
         [0009]    The surveillance system may locate a moving vehicle on the virtual image plane by intersecting two Doppler circles on the virtual image plane. 
         [0010]    The surveillance system may find 3D lines linking Doppler circle intersections to the moving vehicle. 
         [0011]    The surveillance system may locate the moving vehicle in 3D space by using three 3D lines from three frames. 
         [0012]    The surveillance system may establish moving vehicle models for forming the moving vehicle trajectory. 
         [0013]    The surveillance system may find the moving vehicle speed by using Doppler signal from the fixed radar over three frames. 
         [0014]    The surveillance system may find the complete vehicle state information, position and velocity, by jointly using three radars and video camera. 
         [0015]    The surveillance system may track the moving vehicle by continuously pointing to the vehicle using the vehicle location on the virtual image plane. 
         [0016]    The surveillance system may record the moving vehicle state information onto a recording device. 
     
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         [0017]    The invention may be understood by reference to the following description taken in conjunction with the accompanying drawings, in which, like reference numerals identify like elements, and in which: 
           [0018]      FIG. 1  illustrates the speed measurement of an approaching vehicle and a leaving vehicle with a Doppler radar; 
           [0019]      FIG. 2  illustrates the operational setup of the surveillance system; 
           [0020]      FIG. 3  illustrates the lay out of the surveillance system; 
           [0021]      FIG. 4  illustrates the functional flow chart of the surveillance system; and 
           [0022]      FIG. 5  illustrates registration of the first and second movable Doppler radars with the video camera. 
       
    
    
     DETAILED DESCRIPTION 
       [0023]    While the term “traffic surveillance” is used herein, it may also refer to other traffic applications, such as “traffic monitoring”, etc. The term “video” may refer to “any image sequences” which may be generated by electro-optical or thermal or hyper-spectral devices. The invention discussed here may be applied to the case of multiple video cameras and more than three radars. 
         [0024]    A video-Doppler-radar (Vidar) traffic surveillance system is shown in  FIG. 2  where  1 —the sensor system which may include a sensor suite/recording device or apparatus,  2 —a target tracking device,  3 —the camera virtual image plane of the video camera  14 ,  4 —a first moving Doppler radar motion ray,  5 —a second moving Doppler radar motion ray,  6 —a radar direction ray connecting the sensor apparatus  1  to a moving vehicle  10 ,  7 —the intersection of the first Doppler radar motion ray  4  with the virtual image plane  3 ,  8 —the intersection of the second Doppler radar motion ray  5  with the virtual image plane  3 ,  9 —the intersection of a ray connecting the sensor apparatus  1  and the moving vehicle  10 , and  10  a moving vehicle. 
         [0025]      FIG. 3  shows the layout of the sensor apparatus  1  where  11 —a first moving Doppler radar,  12 —a second moving Doppler radar,  13 —a fixed or stationary Doppler radar,  14 —a fixed or stationary video camera,  15 —a data processing device, such as a computer, laptop, personal computer, PDA or other such device, and  16 —data recording device, such as a hard drive, a flash drive or other such device. 
         [0026]    The functional flow chart of the system is shown in  FIG. 4 . In the following, we will describe the functional blocks. 
         [0027]    1. Register Doppler Radars and Video Camera 
         [0028]    The first and second Doppler radars  11 , 12  in the sensor apparatus  1  may be extended or retracted or moved side to side as illustrated in steps  100 ,  101 ,  103  by a motor (not shown) which may be a DC or stepper motor or other movement device and may be moved on sliding tracks (not shown). An optical encoder (not shown) may be mounted on the shaft of the motor, so the sliding speeds of the Doppler radars (ν r     1    and ν r     2    in  FIG. 3 ) may be predetermined. The sliding track orientation angles (θ r     1    and θ r     2    in  FIG. 3 ) may be predetermined. Using a calibration method, the intersections (C 1  and C 2  in  FIG. 2 ) of the first and second motion rays  4 ,  5  with the virtual image planes  3  may be predetermined. Note, this registration method can be applied to a plurality of Doppler radars and cameras. 
         [0029]    It can be seen in  FIG. 5 , showing the registration of the first and second moving Doppler radars  11 ,  12  with the video camera  14 , with the determination of C 1  and C 2  that the first and second moving Doppler radars  11 ,  12  may be substantially precisely registered with the video camera  14 . The locations of substantially equal-Doppler cones of each of the radars  11 ,  12  may be determined on the camera&#39;s virtual image plane  3 , so that the physical information from the moving vehicle  10  may be calculated from both Doppler and video signals from the first moving radar  11 , the second moving radar  12 , a stationary Doppler radar  13  and the video camera  14 . The computing device  15  may accept inputs from the above described elements and may perform the following calculations. 
         [0030]    2. Calculate Doppler Frequency of the Moving Vehicle for the k th Frame 
         [0031]    Assume the current time is the time of the k th video image frame, i.e., t=k in steps  105 ,  106 ,  107 . The Doppler frequencies of the moving vehicle  p   10  induced by both moving Doppler radars may be given by 
         [0000]        f   D     k     1   =K   1 [ν t     k    cos(φ t     k   )+ν r     1k    cos(θ r1     k   )]  (4)
 
         [0000]      and 
         [0000]        f   D     k     2   =K   2 [ν t     k    cos(φ t     k   )+ν r     2k    cos(θ r2     k   )].   (5)
 
         [0032]    where K 1  and K 2  may be Doppler conversion constants for the first and second moving Doppler radar ( 11  and  12  in  FIG. 3 ), and θ r1     k   , θ r2     k    and φ t     k    are depicted in  FIG. 3  with an additional time index k. A fixed Doppler radar  13  may be used to sense the moving vehicle motion 
         [0000]        f   D     k     3   =K   3 ν t     k    cos(φ t     k   )   (6)
 
         [0033]    where K 3  is the Doppler conversion constant for the fixed Doppler radar ( 13  in  FIG. 3 ). Doppler frequencies described by Eqs. (4), (5) and (6) may be obtained at (k+1)th and (k+2)th frames, as in steps  113 ,  114 ,  115 ,  120 ,  121 , and  122 . 
         [0034]    3. Calculate Doppler Difference, Cone Angle and Circle for the k th Frame 
         [0035]    In steps  109 ,  110 , since all three radars  11 , 12 , 13  may be located together and assuming that the distance from the sensor suite to the moving vehicle  10  may be much larger than the distance between radars  11 , 12 , 13 , the following Doppler differences may be 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           k 
                         
                         13 
                       
                     
                     = 
                     
                       
                         
                           
                             f 
                             
                               D 
                               k 
                             
                             1 
                           
                           
                             K 
                             1 
                           
                         
                         - 
                         
                           
                             f 
                             
                               D 
                               k 
                             
                             3 
                           
                           
                             K 
                             3 
                           
                         
                       
                       = 
                       
                         
                           v 
                           
                             r 
                             
                               1 
                                
                               
                                   
                               
                                
                               k 
                             
                           
                         
                          
                         
                           cos 
                            
                           
                             ( 
                             
                               θ 
                               
                                 r 
                                 
                                   1 
                                    
                                   
                                       
                                   
                                    
                                   k 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   7 
                   ) 
                 
               
             
             
               
                 
                   
                     Δ 
                      
                     
                         
                     
                      
                     
                       f 
                       
                         D 
                         k 
                       
                       23 
                     
                   
                   = 
                   
                     
                       
                         
                           f 
                           
                             D 
                             k 
                           
                           2 
                         
                         
                           K 
                           2 
                         
                       
                       - 
                       
                         
                           f 
                           
                             D 
                             k 
                           
                           3 
                         
                         
                           K 
                           3 
                         
                       
                     
                     = 
                     
                       
                         v 
                         
                           r 
                           
                             2 
                              
                             
                                 
                             
                              
                             k 
                           
                         
                       
                        
                       
                         cos 
                          
                         
                           ( 
                           
                             θ 
                             
                               r 
                               
                                 2 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   8 
                   ) 
                 
               
             
           
         
       
     
         [0036]    where the impact of the moving vehicle may have been removed. Eqs. (7) and (8) may actually recover the substantially independent motion Doppler signals of the first and second moving Doppler radars  11 ,  12 , except for the conversion constants. The Doppler differences in Eqs. (7) and (8) are the ones for the kth frame. 
         [0037]    From Eqs. (7) and (8), since ν r     1k    and ν r     2k    are known from calibration, Doppler cone angles at t=k may be calculated as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         θ 
                         ^ 
                       
                       
                         r 
                         
                           1 
                            
                           
                               
                           
                            
                           k 
                         
                       
                     
                     = 
                     
                       
                         cos 
                         
                           - 
                           1 
                         
                       
                        
                       
                         ( 
                         
                           
                             Δ 
                              
                             
                                 
                             
                              
                             
                               f 
                               
                                 D 
                                 k 
                               
                               13 
                             
                           
                           
                             v 
                             
                               r 
                               
                                 1 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                         
                         ) 
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   9 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       θ 
                       ^ 
                     
                     
                       r 
                       
                         2 
                          
                         
                             
                         
                          
                         k 
                       
                     
                   
                   = 
                   
                     
                       
                         cos 
                         
                           - 
                           1 
                         
                       
                        
                       
                         ( 
                         
                           
                             Δ 
                              
                             
                                 
                             
                              
                             
                               f 
                               
                                 D 
                                 k 
                               
                               23 
                             
                           
                           
                             v 
                             
                               r 
                               
                                 2 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                         
                         ) 
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   10 
                   ) 
                 
               
             
           
         
       
     
         [0038]    Using Doppler cone angles in Eqs. (9) and (10), Doppler circles 1  may be constructed on the virtual image plane  3 , as shown in  FIG. 5 . The intersections of the Doppler circles specified by {circumflex over (θ)} r     1k    and {circumflex over (θ)} r     2k    may effectively locate the vehicle q on the image plane, as shown in  FIG. 5 . The ghost intersection point, q′, may be easily removed with some physical constraints. Doppler differences, cone angles and circles defined by Eqs. (7), (8), (9) and (10) may be obtained at (k+1)th and (k+2)th frames, as in steps  116 ,  117 ,  123 , and  124 .  1  Precisely speaking, these may be ellipses. Due to a small angle between radar motion vectors, the ellipses may be well approximated as circles. 
         [0039]    4. Calculate 3D Lines from Doppler Radar to Vehicle 
         [0040]    In step  111 , assume the vehicle location is  X   t     k   =[x t ,y t ,z t ] k  and moving Doppler motion vectors are  ν   r     1k   =[ν r     1x   ,ν r     1y   ,ν r     1z   ] k  and  ν   r     2k   =[ν r     2x   ,ν r     2y   ,ν r     2z   ] k . At t=k, we may have 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       θ 
                       
                         r 
                         
                           1 
                            
                           
                               
                           
                            
                           k 
                         
                       
                     
                     = 
                     
                       
                         cos 
                         
                           - 
                           1 
                         
                       
                        
                       
                         
                           
                             
                               X 
                               _ 
                             
                             
                               t 
                               k 
                             
                           
                           · 
                           
                             
                               v 
                               _ 
                             
                             
                               r 
                               
                                 1 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                         
                         
                           
                              
                             
                               
                                 X 
                                 _ 
                               
                               
                                 t 
                                 k 
                               
                             
                              
                           
                            
                           
                              
                             
                               
                                 v 
                                 _ 
                               
                               
                                 r 
                                 
                                   1 
                                    
                                   
                                       
                                   
                                    
                                   k 
                                 
                               
                             
                              
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   9 
                   ) 
                 
               
             
             
               
                 
                   
                     θ 
                     
                       r 
                       
                         2 
                          
                         
                             
                         
                          
                         k 
                       
                     
                   
                   = 
                   
                     
                       cos 
                       
                         - 
                         1 
                       
                     
                      
                     
                       
                         
                           
                             
                               X 
                               _ 
                             
                             
                               t 
                               k 
                             
                           
                           · 
                           
                             
                               v 
                               _ 
                             
                             
                               r 
                               
                                 2 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                         
                         
                           
                              
                             
                               
                                 X 
                                 _ 
                               
                               
                                 t 
                                 k 
                               
                             
                              
                           
                            
                           
                              
                             
                               
                                 v 
                                 _ 
                               
                               
                                 r 
                                 
                                   2 
                                    
                                   
                                       
                                   
                                    
                                   k 
                                 
                               
                             
                              
                           
                         
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   10 
                   ) 
                 
               
             
           
         
       
     
         [0041]    The Doppler differences may then be calculated as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             Δ 
                              
                             
                                 
                             
                              
                             
                               f 
                               
                                 D 
                                 k 
                               
                               13 
                             
                           
                           = 
                           
                             
                               v 
                               
                                 r 
                                 
                                   1 
                                    
                                   
                                       
                                   
                                    
                                   k 
                                 
                               
                             
                              
                             
                               cos 
                                
                               
                                 ( 
                                 
                                   θ 
                                   
                                     r 
                                     
                                       1 
                                        
                                       
                                           
                                       
                                        
                                       k 
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                         
                       
                     
                     
                       
                         
                           = 
                           
                             
                               
                                 
                                   X 
                                   _ 
                                 
                                 
                                   t 
                                   k 
                                 
                               
                               · 
                               
                                 
                                   v 
                                   _ 
                                 
                                 
                                   r 
                                   
                                     1 
                                      
                                     
                                         
                                     
                                      
                                     k 
                                   
                                 
                               
                             
                             
                                
                               
                                 
                                   X 
                                   _ 
                                 
                                 
                                   t 
                                   k 
                                 
                               
                                
                             
                           
                         
                       
                     
                     
                       
                         
                           = 
                           
                             
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       
                                         1 
                                          
                                         xk 
                                       
                                        
                                       
                                           
                                       
                                     
                                   
                                 
                                  
                                 
                                   x 
                                   
                                     t 
                                     k 
                                   
                                 
                               
                               + 
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       1 
                                        
                                       yk 
                                     
                                   
                                 
                                  
                                 
                                   y 
                                   
                                     t 
                                     k 
                                   
                                 
                               
                               + 
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       1 
                                        
                                       zk 
                                     
                                   
                                 
                                  
                                 
                                   z 
                                   
                                     t 
                                     k 
                                   
                                 
                               
                             
                             
                               
                                 
                                   x 
                                   
                                     t 
                                     k 
                                   
                                   2 
                                 
                                 + 
                                 
                                   y 
                                   
                                     t 
                                     k 
                                   
                                   2 
                                 
                                 + 
                                 
                                   z 
                                   
                                     t 
                                     k 
                                   
                                   2 
                                 
                               
                             
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   
                     
                       
                         ( 
                         11 
                         ) 
                       
                     
                   
                   
                     
                       
                         ( 
                         12 
                         ) 
                       
                     
                   
                   
                     
                       
                         ( 
                         13 
                         ) 
                       
                     
                   
                 
               
             
             
               
                 
                   
                     
                       
                         
                           Δ 
                            
                           
                               
                           
                            
                           
                             f 
                             
                               D 
                               k 
                             
                             23 
                           
                         
                         = 
                         
                           
                             v 
                             
                               r 
                               
                                 2 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                            
                           
                             cos 
                              
                             
                               ( 
                               
                                 θ 
                                 
                                   r 
                                   
                                     2 
                                      
                                     
                                         
                                     
                                      
                                     k 
                                   
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       
                                         2 
                                          
                                         xk 
                                       
                                        
                                       
                                           
                                       
                                     
                                   
                                 
                                  
                                 
                                   x 
                                   
                                     t 
                                     k 
                                   
                                 
                               
                               + 
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       2 
                                        
                                       yk 
                                     
                                   
                                 
                                  
                                 
                                   y 
                                   
                                     t 
                                     k 
                                   
                                 
                               
                               + 
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       2 
                                        
                                       zk 
                                     
                                   
                                 
                                  
                                 
                                   z 
                                   
                                     t 
                                     k 
                                   
                                 
                               
                             
                             
                               
                                 
                                   x 
                                   
                                     t 
                                     k 
                                   
                                   2 
                                 
                                 + 
                                 
                                   y 
                                   
                                     t 
                                     k 
                                   
                                   2 
                                 
                                 + 
                                 
                                   z 
                                   
                                     t 
                                     k 
                                   
                                   2 
                                 
                               
                             
                           
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   
                     
                       
                         ( 
                         14 
                         ) 
                       
                     
                   
                   
                     
                       
                         ( 
                         15 
                         ) 
                       
                     
                   
                 
               
             
           
         
       
     
         [0042]    Eqs. (13) and (15) may describe two cones with central axes being OC 1  and OC 2 , where O is the joint of the two cone tips. The ratio of Doppler differences may define a 3D line passing though O, q k  and  X   t     k      
         [0000]    
       
         
           
             
               
                 
                   
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           k 
                         
                         13 
                       
                     
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           k 
                         
                         23 
                       
                     
                   
                   = 
                   
                     
                       
                         
                           
                             v 
                             
                               r 
                               
                                 
                                   1 
                                    
                                   xk 
                                 
                                  
                                 
                                     
                                 
                               
                             
                           
                            
                           
                             x 
                             
                               t 
                               k 
                             
                           
                         
                         + 
                         
                           
                             v 
                             
                               r 
                               
                                 1 
                                  
                                 yk 
                               
                             
                           
                            
                           
                             y 
                             
                               t 
                               k 
                             
                           
                         
                         + 
                         
                           
                             v 
                             
                               r 
                               
                                 1 
                                  
                                 zk 
                               
                             
                           
                            
                           
                             z 
                             
                               t 
                               k 
                             
                           
                         
                       
                       
                         
                           
                             v 
                             
                               r 
                               
                                 
                                   2 
                                    
                                   xk 
                                 
                                  
                                 
                                     
                                 
                               
                             
                           
                            
                           
                             x 
                             
                               t 
                               k 
                             
                           
                         
                         + 
                         
                           
                             v 
                             
                               r 
                               
                                 2 
                                  
                                 yk 
                               
                             
                           
                            
                           
                             y 
                             
                               t 
                               k 
                             
                           
                         
                         + 
                         
                           
                             v 
                             
                               r 
                               
                                 2 
                                  
                                 zk 
                               
                             
                           
                            
                           
                             z 
                             
                               t 
                               k 
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   16 
                   ) 
                 
               
             
           
         
       
     
         [0043]    At t=k+1 and t=k+2, the ratios of Doppler differences may become, as in steps  118  and  125 , 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         Δ 
                          
                         
                             
                         
                          
                         
                           f 
                           
                             D 
                             
                               ( 
                               
                                 k 
                                 + 
                                 1 
                               
                               ) 
                             
                           
                           13 
                         
                       
                       
                         Δ 
                          
                         
                             
                         
                          
                         
                           f 
                           
                             D 
                             
                               ( 
                               
                                 k 
                                 + 
                                 1 
                               
                               ) 
                             
                           
                           23 
                         
                       
                     
                     = 
                     
                       
                         
                           
                             v 
                             
                               r 
                               
                                 
                                   1 
                                    
                                   
                                     x 
                                      
                                     
                                       ( 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                       ) 
                                     
                                   
                                 
                                  
                                 
                                     
                                 
                               
                             
                           
                            
                           
                             x 
                             
                               t 
                               
                                 ( 
                                 
                                   k 
                                   + 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                         + 
                         
                           
                             v 
                             
                               r 
                               
                                 1 
                                  
                                 
                                   y 
                                    
                                   
                                     ( 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     ) 
                                   
                                 
                               
                             
                           
                            
                           
                             y 
                             
                               t 
                               
                                 ( 
                                 
                                   k 
                                   + 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                         + 
                         
                           
                             v 
                             
                               r 
                               
                                 1 
                                  
                                 
                                   z 
                                    
                                   
                                     ( 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     ) 
                                   
                                 
                               
                             
                           
                            
                           
                             z 
                             
                               t 
                               
                                 ( 
                                 
                                   k 
                                   + 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           
                             v 
                             
                               r 
                               
                                 
                                   2 
                                    
                                   
                                     x 
                                      
                                     
                                       ( 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                       ) 
                                     
                                   
                                 
                                  
                                 
                                     
                                 
                               
                             
                           
                            
                           
                             x 
                             
                               t 
                               
                                 ( 
                                 
                                   k 
                                   + 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                         + 
                         
                           
                             v 
                             
                               r 
                               
                                 2 
                                  
                                 
                                   y 
                                    
                                   
                                     ( 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     ) 
                                   
                                 
                               
                             
                           
                            
                           
                             y 
                             
                               t 
                               
                                 ( 
                                 
                                   k 
                                   + 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                         + 
                         
                           
                             v 
                             
                               r 
                               
                                 2 
                                  
                                 
                                   z 
                                    
                                   
                                     ( 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     ) 
                                   
                                 
                               
                             
                           
                            
                           
                             z 
                             
                               t 
                               
                                 ( 
                                 
                                   k 
                                   + 
                                   1 
                                 
                                 ) 
                               
                             
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   17 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           
                             ( 
                             
                               k 
                               + 
                               2 
                             
                             ) 
                           
                         
                         13 
                       
                     
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           
                             ( 
                             
                               k 
                               + 
                               2 
                             
                             ) 
                           
                         
                         23 
                       
                     
                   
                   = 
                   
                     
                       
                         
                           v 
                           
                             r 
                             
                               
                                 1 
                                  
                                 
                                   x 
                                    
                                   
                                     ( 
                                     
                                       k 
                                       + 
                                       2 
                                     
                                     ) 
                                   
                                 
                               
                                
                               
                                   
                               
                             
                           
                         
                          
                         
                           x 
                           
                             t 
                             
                               ( 
                               
                                 k 
                                 + 
                                 2 
                               
                               ) 
                             
                           
                         
                       
                       + 
                       
                         
                           v 
                           
                             r 
                             
                               1 
                                
                               
                                 y 
                                  
                                 
                                   ( 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                   ) 
                                 
                               
                             
                           
                         
                          
                         
                           y 
                           
                             t 
                             
                               ( 
                               
                                 k 
                                 + 
                                 2 
                               
                               ) 
                             
                           
                         
                       
                       + 
                       
                         
                           v 
                           
                             r 
                             
                               1 
                                
                               
                                 z 
                                  
                                 
                                   ( 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                   ) 
                                 
                               
                             
                           
                         
                          
                         
                           z 
                           
                             t 
                             
                               ( 
                               
                                 k 
                                 + 
                                 2 
                               
                               ) 
                             
                           
                         
                       
                     
                     
                       
                         
                           v 
                           
                             r 
                             
                               
                                 2 
                                  
                                 
                                   x 
                                    
                                   
                                     ( 
                                     
                                       k 
                                       + 
                                       2 
                                     
                                     ) 
                                   
                                 
                               
                                
                               
                                   
                               
                             
                           
                         
                          
                         
                           x 
                           
                             t 
                             
                               ( 
                               
                                 k 
                                 + 
                                 2 
                               
                               ) 
                             
                           
                         
                       
                       + 
                       
                         
                           v 
                           
                             r 
                             
                               2 
                                
                               
                                 y 
                                  
                                 
                                   ( 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                   ) 
                                 
                               
                             
                           
                         
                          
                         
                           y 
                           
                             t 
                             
                               ( 
                               
                                 k 
                                 + 
                                 2 
                               
                               ) 
                             
                           
                         
                       
                       + 
                       
                         
                           v 
                           
                             r 
                             
                               2 
                                
                               
                                 z 
                                  
                                 
                                   ( 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                   ) 
                                 
                               
                             
                           
                         
                          
                         
                           z 
                           
                             t 
                             
                               ( 
                               
                                 k 
                                 + 
                                 2 
                               
                               ) 
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   18 
                   ) 
                 
               
             
           
         
       
     
         [0044]    which may describe two more 3D lines passing through O, q k+1  and  X   t     k+1   , and O, q k+2  and  X   t     k−2   . 
         [0045]    5. Target Kinematic and Measurement Modeling 
         [0046]    We may need to connect three frames positional information together. In steps  104  and  108 , let&#39;s consider a deterministic modeling case first. Assume the vehicle kinematics satisfy a constant velocity (CV) model 
         [0047]    or 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       [ 
                       
                         
                           
                             
                               X 
                               _ 
                             
                           
                         
                         
                           
                             
                               
                                 X 
                                 . 
                               
                               _ 
                             
                           
                         
                       
                       ] 
                     
                     
                       k 
                       + 
                       1 
                     
                   
                   = 
                   
                     
                       
                         [ 
                         
                           
                             
                               I 
                             
                             
                               T 
                             
                           
                           
                             
                               0 
                             
                             
                               I 
                             
                           
                         
                         ] 
                       
                        
                       
                         [ 
                         
                           
                             
                               
                                 X 
                                 _ 
                               
                             
                           
                           
                             
                               
                                 
                                   X 
                                   . 
                                 
                                 _ 
                               
                             
                           
                         
                         ] 
                       
                     
                     k 
                   
                 
               
               
                 
                   ( 
                   19 
                   ) 
                 
               
             
             
               
                 
                   
                     x 
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           1 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       x 
                       
                         t 
                         k 
                       
                     
                     + 
                     
                       T 
                        
                       
                         
                           x 
                           . 
                         
                         
                           t 
                           k 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   20 
                   ) 
                 
               
             
             
               
                 
                   
                     y 
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           1 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       y 
                       
                         t 
                         k 
                       
                     
                     + 
                     
                       T 
                        
                       
                         
                           y 
                           . 
                         
                         
                           t 
                           k 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   21 
                   ) 
                 
               
             
             
               
                 
                   
                     z 
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           1 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       z 
                       
                         t 
                         k 
                       
                     
                     + 
                     
                       T 
                        
                       
                         
                           z 
                           . 
                         
                         
                           t 
                           k 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   22 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       x 
                       . 
                     
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           1 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       x 
                       . 
                     
                     
                       t 
                       k 
                     
                   
                 
               
               
                 
                   ( 
                   23 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       y 
                       . 
                     
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           1 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       y 
                       . 
                     
                     
                       t 
                       k 
                     
                   
                 
               
               
                 
                   ( 
                   24 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       z 
                       . 
                     
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           1 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       z 
                       . 
                     
                     
                       t 
                       k 
                     
                   
                 
               
               
                 
                   ( 
                   25 
                   ) 
                 
               
             
             
               
                 
                   
                     x 
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           2 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       x 
                       
                         t 
                         k 
                       
                     
                     + 
                     
                       2 
                        
                       T 
                        
                       
                         
                           x 
                           . 
                         
                         
                           t 
                           k 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   26 
                   ) 
                 
               
             
             
               
                 
                   
                     y 
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           2 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       y 
                       
                         t 
                         k 
                       
                     
                     + 
                     
                       2 
                        
                       T 
                        
                       
                         
                           y 
                           . 
                         
                         
                           t 
                           k 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   27 
                   ) 
                 
               
             
             
               
                 
                   
                     z 
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           2 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       z 
                       
                         t 
                         k 
                       
                     
                     + 
                     
                       2 
                        
                       T 
                        
                       
                         
                           z 
                           . 
                         
                         
                           t 
                           k 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   28 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       x 
                       . 
                     
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           2 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       x 
                       . 
                     
                     
                       t 
                       k 
                     
                   
                 
               
               
                 
                   ( 
                   29 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       y 
                       . 
                     
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           2 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       y 
                       . 
                     
                     
                       t 
                       k 
                     
                   
                 
               
               
                 
                   ( 
                   30 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       z 
                       . 
                     
                     
                       t 
                       
                         ( 
                         
                           k 
                           + 
                           2 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     
                       
                         z 
                         . 
                       
                       
                         t 
                         k 
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   31 
                   ) 
                 
               
             
           
         
       
     
         [0048]    So, if we know {dot over (x)} t     k   , {dot over (y)} t     k    and ż t     k   , we may easily connect three frame information. The fixed Doppler radar may provide the vehicle velocity magnitude information, and we may know the LOS direction angles from the moving Doppler radars. Assume that the vectors from O to q k , q k+1  and q k+2  are  Oq   k =[u k ,v k ,f],  Oq   k+1 =[u k−1 ,v k+1 ,f], and  Oq   k+2 =[u k−2 ,v k+2 ,f] where f is the focal length. The fixed Doppler radar measurement at t=k may be 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           f 
                           
                             D 
                             k 
                           
                           3 
                         
                         = 
                         
                           
                             K 
                             3 
                           
                            
                           
                             v 
                             
                               t 
                               k 
                             
                           
                            
                           
                             
                               
                                 - 
                                 
                                   
                                     Oq 
                                     _ 
                                   
                                   k 
                                 
                               
                               · 
                               
                                 
                                   
                                     X 
                                     . 
                                   
                                   _ 
                                 
                                 k 
                               
                             
                             
                               
                                  
                                 
                                   
                                     Oq 
                                     _ 
                                   
                                   k 
                                 
                                  
                               
                                
                               
                                  
                                 
                                   
                                     
                                       X 
                                       . 
                                     
                                     _ 
                                   
                                   k 
                                 
                                  
                               
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             K 
                             3 
                           
                            
                           
                             
                               
                                 - 
                                 
                                   
                                     Oq 
                                     _ 
                                   
                                   k 
                                 
                               
                               · 
                               
                                 
                                   
                                     X 
                                     . 
                                   
                                   _ 
                                 
                                 k 
                               
                             
                             
                                
                               
                                 
                                   Oq 
                                   _ 
                                 
                                 k 
                               
                                
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             K 
                             3 
                           
                            
                           
                             
                               
                                 
                                   
                                     u 
                                     k 
                                   
                                    
                                   
                                     
                                       x 
                                       . 
                                     
                                     
                                       t 
                                       k 
                                     
                                   
                                 
                                 + 
                                 
                                   
                                     v 
                                     k 
                                   
                                    
                                   
                                     
                                       y 
                                       . 
                                     
                                     
                                       t 
                                       k 
                                     
                                   
                                 
                                 + 
                                 
                                   f 
                                    
                                   
                                     
                                       z 
                                       . 
                                     
                                     
                                       t 
                                       k 
                                     
                                   
                                 
                               
                               
                                 
                                   
                                     u 
                                     k 
                                     2 
                                   
                                   + 
                                   
                                     v 
                                     k 
                                     2 
                                   
                                   + 
                                   
                                     f 
                                     2 
                                   
                                 
                               
                             
                             . 
                           
                         
                       
                     
                   
                 
               
               
                 
                   
                     
                       
                         ( 
                         32 
                         ) 
                       
                     
                   
                   
                     
                       
                           
                       
                     
                   
                   
                     
                       
                           
                       
                     
                   
                   
                     
                       
                         ( 
                         33 
                         ) 
                       
                     
                   
                   
                     
                       
                           
                       
                     
                   
                   
                     
                       
                         ( 
                         34 
                         ) 
                       
                     
                   
                   
                     
                       
                         ( 
                         34 
                         ) 
                       
                     
                   
                   
                     
                       
                         ( 
                         34 
                         ) 
                       
                     
                   
                 
               
             
           
         
       
     
         [0049]    At t=k+1 and t=k+2 moments, we may have 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             f 
                             
                               D 
                               
                                 k 
                                 + 
                                 1 
                               
                             
                             3 
                           
                           = 
                           
                             
                               K 
                               3 
                             
                              
                             
                               
                                 
                                   
                                     u 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                   
                                    
                                   
                                     
                                       x 
                                       . 
                                     
                                     
                                       t 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                 + 
                                 
                                   
                                     v 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                   
                                    
                                   
                                     
                                       y 
                                       . 
                                     
                                     
                                       t 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                 + 
                                 
                                   f 
                                    
                                   
                                     
                                       z 
                                       . 
                                     
                                     
                                       t 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                               
                               
                                 
                                   
                                     u 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     2 
                                   
                                   + 
                                   
                                     v 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     2 
                                   
                                   + 
                                   
                                     f 
                                     2 
                                   
                                 
                               
                             
                           
                         
                       
                     
                     
                       
                         
                           = 
                           
                             
                               K 
                               3 
                             
                              
                             
                               
                                 
                                   
                                     u 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                   
                                    
                                   
                                     
                                       x 
                                       . 
                                     
                                     
                                       t 
                                       k 
                                     
                                   
                                 
                                 + 
                                 
                                   
                                     v 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                   
                                    
                                   
                                     
                                       y 
                                       . 
                                     
                                     
                                       t 
                                       k 
                                     
                                   
                                 
                                 + 
                                 
                                   f 
                                    
                                   
                                     
                                       z 
                                       
                                         t 
                                         k 
                                       
                                     
                                     . 
                                   
                                 
                               
                               
                                 
                                   
                                     u 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     2 
                                   
                                   + 
                                   
                                     v 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     2 
                                   
                                   + 
                                   
                                     f 
                                     2 
                                   
                                 
                               
                             
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   
                     
                       
                         ( 
                         35 
                         ) 
                       
                     
                   
                   
                     
                       
                           
                       
                     
                   
                   
                     
                       
                           
                       
                     
                   
                   
                     
                       
                         ( 
                         36 
                         ) 
                       
                     
                   
                   
                     
                       
                           
                       
                     
                   
                 
               
             
             
               
                 
                   
                     
                       
                         
                           f 
                           
                             D 
                             
                               k 
                               + 
                               2 
                             
                           
                           3 
                         
                         = 
                         
                           
                             K 
                             3 
                           
                            
                           
                             
                               
                                 
                                   u 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                 
                                  
                                 
                                   
                                     x 
                                     . 
                                   
                                   
                                     t 
                                     
                                       k 
                                       + 
                                       2 
                                     
                                   
                                 
                               
                               + 
                               
                                 
                                   v 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                 
                                  
                                 
                                   
                                     y 
                                     . 
                                   
                                   
                                     t 
                                     
                                       k 
                                       + 
                                       2 
                                     
                                   
                                 
                               
                               + 
                               
                                 f 
                                  
                                 
                                   
                                     z 
                                     . 
                                   
                                   
                                     t 
                                     
                                       k 
                                       + 
                                       2 
                                     
                                   
                                 
                               
                             
                             
                               
                                 
                                   u 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                   2 
                                 
                                 + 
                                 
                                   v 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                   2 
                                 
                                 + 
                                 
                                   f 
                                   2 
                                 
                               
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             K 
                             3 
                           
                            
                           
                             
                               
                                 
                                   u 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                 
                                  
                                 
                                   
                                     x 
                                     . 
                                   
                                   
                                     t 
                                     k 
                                   
                                 
                               
                               + 
                               
                                 
                                   v 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                 
                                  
                                 
                                   
                                     y 
                                     . 
                                   
                                   
                                     t 
                                     k 
                                   
                                 
                               
                               + 
                               
                                 f 
                                  
                                 
                                   
                                     z 
                                     . 
                                   
                                   
                                     t 
                                     k 
                                   
                                 
                               
                             
                             
                               
                                 
                                   u 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                   2 
                                 
                                 + 
                                 
                                   v 
                                   
                                     k 
                                     + 
                                     2 
                                   
                                   2 
                                 
                                 + 
                                 
                                   f 
                                   2 
                                 
                               
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   
                     
                       
                         ( 
                         37 
                         ) 
                       
                     
                   
                   
                     
                       
                           
                       
                     
                   
                   
                     
                       
                           
                       
                     
                   
                   
                     
                       
                         ( 
                         38 
                         ) 
                       
                     
                   
                   
                     
                       
                           
                       
                     
                   
                 
               
             
           
         
       
     
         [0050]    Eqs. (17) and (18) are rewritten as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         f 
                         
                           D 
                           
                             ( 
                             
                               k 
                               + 
                               1 
                             
                             ) 
                           
                         
                         13 
                       
                       
                         f 
                         
                           D 
                           
                             ( 
                             
                               k 
                               + 
                               1 
                             
                             ) 
                           
                         
                         23 
                       
                     
                     = 
                     
                       
                         
                           
                             
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       
                                         1 
                                          
                                         
                                           x 
                                            
                                           
                                             ( 
                                             
                                               k 
                                               + 
                                               1 
                                             
                                             ) 
                                           
                                         
                                       
                                        
                                       
                                           
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       x 
                                       
                                         t 
                                         k 
                                       
                                     
                                     + 
                                     
                                       T 
                                        
                                       
                                         
                                           x 
                                           . 
                                         
                                         
                                           t 
                                           k 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                             
                           
                         
                         
                           
                             
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       1 
                                        
                                       
                                         y 
                                          
                                         
                                           ( 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       y 
                                       
                                         t 
                                         k 
                                       
                                     
                                     + 
                                     
                                       T 
                                        
                                       
                                         
                                           y 
                                           . 
                                         
                                         
                                           t 
                                           k 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       1 
                                        
                                       
                                         z 
                                          
                                         
                                           ( 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       z 
                                       
                                         t 
                                         k 
                                       
                                     
                                     + 
                                     
                                       T 
                                        
                                       
                                         
                                           z 
                                           . 
                                         
                                         
                                           t 
                                           k 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       
                                         2 
                                          
                                         
                                           x 
                                            
                                           
                                             ( 
                                             
                                               k 
                                               + 
                                               1 
                                             
                                             ) 
                                           
                                         
                                       
                                        
                                       
                                           
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       x 
                                       
                                         t 
                                         k 
                                       
                                     
                                     + 
                                     
                                       T 
                                        
                                       
                                         o 
                                         . 
                                       
                                        
                                       
                                         tx 
                                         
                                           
                                               
                                           
                                           
                                             t 
                                             k 
                                           
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                             
                           
                         
                         
                           
                             
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       2 
                                        
                                       
                                         y 
                                          
                                         
                                           ( 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       y 
                                       
                                         t 
                                         k 
                                       
                                     
                                     + 
                                     
                                       T 
                                        
                                       
                                         
                                           y 
                                           . 
                                         
                                         
                                           
                                               
                                           
                                           
                                             t 
                                             k 
                                           
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   v 
                                   
                                     r 
                                     
                                       2 
                                        
                                       
                                         z 
                                          
                                         
                                           ( 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       z 
                                       
                                         t 
                                         k 
                                       
                                     
                                     + 
                                     
                                       T 
                                        
                                       
                                         
                                           z 
                                           . 
                                         
                                         
                                           
                                               
                                           
                                           
                                             t 
                                             k 
                                           
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                             
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   39 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       f 
                       
                         D 
                         
                           ( 
                           
                             k 
                             + 
                             2 
                           
                           ) 
                         
                       
                       13 
                     
                     
                       f 
                       
                         D 
                         
                           ( 
                           
                             k 
                             + 
                             2 
                           
                           ) 
                         
                       
                       23 
                     
                   
                   = 
                   
                     
                       
                         
                           
                             
                               
                                 v 
                                 
                                   r 
                                   
                                     
                                       1 
                                        
                                       
                                         x 
                                          
                                         
                                           ( 
                                           
                                             k 
                                             + 
                                             2 
                                           
                                           ) 
                                         
                                       
                                     
                                      
                                     
                                         
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     x 
                                     
                                       t 
                                       k 
                                     
                                   
                                   + 
                                   
                                     2 
                                      
                                     T 
                                      
                                     
                                       
                                         x 
                                         . 
                                       
                                       
                                         t 
                                         k 
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                             + 
                           
                         
                       
                       
                         
                           
                             
                               
                                 v 
                                 
                                   r 
                                   
                                     1 
                                      
                                     
                                       y 
                                        
                                       
                                         ( 
                                         
                                           k 
                                           + 
                                           2 
                                         
                                         ) 
                                       
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     y 
                                     
                                       t 
                                       k 
                                     
                                   
                                   + 
                                   
                                     2 
                                      
                                     T 
                                      
                                     
                                       
                                         y 
                                         . 
                                       
                                       
                                         t 
                                         k 
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                             + 
                             
                               
                                 v 
                                 
                                   r 
                                   
                                     1 
                                      
                                     
                                       z 
                                        
                                       
                                         ( 
                                         
                                           k 
                                           + 
                                           2 
                                         
                                         ) 
                                       
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     z 
                                     
                                       t 
                                       k 
                                     
                                   
                                   + 
                                   
                                     2 
                                      
                                     T 
                                      
                                     
                                       
                                         z 
                                         . 
                                       
                                       
                                         t 
                                         k 
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                         
                       
                     
                     
                       
                         
                           
                             
                               
                                 v 
                                 
                                   r 
                                   
                                     
                                       2 
                                        
                                       
                                         x 
                                          
                                         
                                           ( 
                                           
                                             k 
                                             + 
                                             2 
                                           
                                           ) 
                                         
                                       
                                     
                                      
                                     
                                         
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     x 
                                     
                                       t 
                                       k 
                                     
                                   
                                   + 
                                   
                                     2 
                                      
                                     T 
                                      
                                     
                                       
                                         x 
                                         . 
                                       
                                       
                                         
                                             
                                         
                                         
                                           t 
                                           k 
                                         
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                             + 
                           
                         
                       
                       
                         
                           
                             
                               
                                 v 
                                 
                                   r 
                                   
                                     2 
                                      
                                     
                                       y 
                                        
                                       
                                         ( 
                                         
                                           k 
                                           + 
                                           2 
                                         
                                         ) 
                                       
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     y 
                                     
                                       t 
                                       k 
                                     
                                   
                                   + 
                                   
                                     2 
                                      
                                     T 
                                      
                                     
                                       
                                         y 
                                         . 
                                       
                                       
                                         
                                             
                                         
                                         
                                           t 
                                           k 
                                         
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                             + 
                             
                               
                                 v 
                                 
                                   r 
                                   
                                     2 
                                      
                                     
                                       z 
                                        
                                       
                                         ( 
                                         
                                           k 
                                           + 
                                           2 
                                         
                                         ) 
                                       
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     z 
                                     
                                       t 
                                       k 
                                     
                                   
                                   + 
                                   
                                     2 
                                      
                                     T 
                                      
                                     
                                       
                                         z 
                                         . 
                                       
                                       
                                         
                                             
                                         
                                         
                                           t 
                                           k 
                                         
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   40 
                   ) 
                 
               
             
           
         
       
     
         [0051]    Solving Eqs. (16), (34), (36), (38), (39) and (40) simultaneously may give us the positional and velocity information, [x t     k   ,y t     k   ,z t     k   ,{dot over (x)} t     k   ,{dot over (y)} t     k   , ż t     k   ], completely with the constraint of Eq. (19). Theoretically, we may calculate the velocity of a target with any heading angle, φ! 
         [0052]    We now consider a stochastic modeling case. Assume the vehicle kinematics satisfy a stochastic CV model 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         [ 
                         
                           
                             X 
                             _ 
                           
                           
                             
                               X 
                               _ 
                             
                             . 
                           
                         
                         ] 
                       
                       
                         k 
                         + 
                         1 
                       
                     
                     = 
                     
                       
                         
                           
                             [ 
                             
                               
                                 
                                   I 
                                 
                                 
                                   
                                     I 
                                      
                                     
                                         
                                     
                                      
                                     T 
                                   
                                 
                               
                               
                                 
                                   0 
                                 
                                 
                                   I 
                                 
                               
                             
                             ] 
                           
                            
                           
                             [ 
                             
                               
                                 X 
                                 _ 
                               
                               
                                 
                                   X 
                                   _ 
                                 
                                 . 
                               
                             
                             ] 
                           
                         
                         k 
                       
                       + 
                       
                         
                           [ 
                           
                             
                               
                                 
                                   
                                     1 
                                     2 
                                   
                                    
                                   I 
                                    
                                   
                                       
                                   
                                    
                                   
                                     T 
                                     2 
                                   
                                 
                               
                             
                             
                               
                                 I 
                               
                             
                           
                           ] 
                         
                          
                         
                           
                             ρ 
                             _ 
                           
                           k 
                         
                       
                     
                   
                   , 
                   
                     
                       
                         ρ 
                         _ 
                       
                       k 
                     
                      
                     • 
                      
                     
                         
                     
                      
                     
                       
                         N 
                          
                         
                           ( 
                           
                             
                               0 
                               _ 
                             
                             , 
                             
                               Q 
                               k 
                             
                           
                           ) 
                         
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   41 
                   ) 
                 
               
             
           
         
       
     
         [0053]    From Eq. (16), the positional measurement equation may be 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         0 
                         = 
                           
                          
                         
                           
                             
                               ( 
                               
                                 
                                   
                                     
                                       Δ 
                                        
                                       
                                           
                                       
                                        
                                       
                                         f 
                                         
                                           D 
                                           k 
                                         
                                         13 
                                       
                                     
                                     
                                       Δ 
                                        
                                       
                                           
                                       
                                        
                                       
                                         f 
                                         
                                           D 
                                           k 
                                         
                                         23 
                                       
                                     
                                   
                                    
                                   
                                     v 
                                     
                                       r 
                                       
                                         2 
                                          
                                         
                                             
                                         
                                          
                                         xk 
                                       
                                     
                                   
                                 
                                 - 
                                 
                                   v 
                                   
                                     r 
                                     
                                       1 
                                        
                                       
                                           
                                       
                                        
                                       xk 
                                     
                                   
                                 
                               
                               ) 
                             
                              
                             
                               x 
                               
                                 t 
                                 k 
                               
                             
                           
                           + 
                           
                             
                               ( 
                               
                                 
                                   
                                     
                                       Δ 
                                        
                                       
                                           
                                       
                                        
                                       
                                         f 
                                         
                                           D 
                                           k 
                                         
                                         13 
                                       
                                     
                                     
                                       Δ 
                                        
                                       
                                           
                                       
                                        
                                       
                                         f 
                                         
                                           D 
                                           k 
                                         
                                         23 
                                       
                                     
                                   
                                    
                                   
                                     v 
                                     
                                       r 
                                       
                                         2 
                                          
                                         
                                             
                                         
                                          
                                         yk 
                                       
                                     
                                   
                                 
                                 - 
                                 
                                   v 
                                   
                                     r 
                                     
                                       1 
                                        
                                       
                                           
                                       
                                        
                                       yk 
                                     
                                   
                                 
                               
                               ) 
                             
                              
                             
                               y 
                               
                                 t 
                                 k 
                               
                             
                           
                           + 
                         
                       
                     
                   
                   
                     
                       
                           
                          
                         
                           
                             ( 
                             
                               
                                 
                                   
                                     Δ 
                                      
                                     
                                         
                                     
                                      
                                     
                                       f 
                                       
                                         D 
                                         k 
                                       
                                       13 
                                     
                                   
                                   
                                     Δ 
                                      
                                     
                                         
                                     
                                      
                                     
                                       f 
                                       
                                         D 
                                         k 
                                       
                                       23 
                                     
                                   
                                 
                                  
                                 
                                   v 
                                   
                                     r 
                                     
                                       2 
                                        
                                       
                                           
                                       
                                        
                                       zk 
                                     
                                   
                                 
                               
                               - 
                               
                                 v 
                                 
                                   r 
                                   
                                     1 
                                      
                                     
                                         
                                     
                                      
                                     zk 
                                   
                                 
                               
                             
                             ) 
                           
                            
                           
                             z 
                             
                               t 
                               k 
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                          
                         
                           [ 
                           
                             
                               
                                 
                                   
                                     Δ 
                                      
                                     
                                         
                                     
                                      
                                     
                                       f 
                                       
                                         D 
                                         k 
                                       
                                       13 
                                     
                                   
                                   
                                     Δ 
                                      
                                     
                                         
                                     
                                      
                                     
                                       f 
                                       
                                         D 
                                         k 
                                       
                                       23 
                                     
                                   
                                 
                                  
                                 
                                   v 
                                   
                                     r 
                                     
                                       2 
                                        
                                       
                                           
                                       
                                        
                                       xk 
                                     
                                   
                                 
                               
                               - 
                               
                                 v 
                                 
                                   r 
                                   
                                     1 
                                      
                                     
                                         
                                     
                                      
                                     xk 
                                   
                                 
                               
                             
                             , 
                             
                               
                                 
                                   
                                     Δ 
                                      
                                     
                                         
                                     
                                      
                                     
                                       f 
                                       
                                         D 
                                         k 
                                       
                                       13 
                                     
                                   
                                   
                                     Δ 
                                      
                                     
                                         
                                     
                                      
                                     
                                       f 
                                       
                                         D 
                                         k 
                                       
                                       23 
                                     
                                   
                                 
                                  
                                 
                                   v 
                                   
                                     r 
                                     
                                       2 
                                        
                                       
                                           
                                       
                                        
                                       xk 
                                     
                                   
                                 
                               
                               - 
                               
                                 v 
                                 
                                   r 
                                   
                                     1 
                                      
                                     
                                         
                                     
                                      
                                     xk 
                                   
                                 
                               
                             
                             , 
                             
                               
                                 
                                   
                                     Δ 
                                      
                                     
                                         
                                     
                                      
                                     
                                       f 
                                       
                                         D 
                                         k 
                                       
                                       13 
                                     
                                   
                                   
                                     Δ 
                                      
                                     
                                         
                                     
                                      
                                     
                                       f 
                                       
                                         D 
                                         k 
                                       
                                       23 
                                     
                                   
                                 
                                  
                                 
                                   v 
                                   
                                     r 
                                     
                                       2 
                                        
                                       
                                           
                                       
                                        
                                       xk 
                                     
                                   
                                 
                               
                               - 
                               
                                 v 
                                 
                                   r 
                                   
                                     1 
                                      
                                     
                                         
                                     
                                      
                                     xk 
                                   
                                 
                               
                             
                           
                           ] 
                         
                       
                     
                   
                   
                     
                       
                           
                          
                         
                           
                             
                               
                                 X 
                                 _ 
                               
                               k 
                             
                             + 
                             
                               
                                 γ 
                                 _ 
                               
                               
                                 x 
                                 k 
                               
                             
                           
                           , 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   42 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       γ 
                       _ 
                     
                     
                       x 
                       k 
                     
                   
                    
                   • 
                    
                   
                       
                   
                    
                   
                     
                       N 
                        
                       
                         ( 
                         
                           
                             0 
                             _ 
                           
                           , 
                           
                             R 
                             
                               x 
                               k 
                             
                           
                         
                         ) 
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   43 
                   ) 
                 
               
             
           
         
       
     
         [0054]    The velocity measurement equation may be established from Eq. (34) as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             f 
                             
                               D 
                               k 
                             
                             3 
                           
                           = 
                           
                             
                               
                                 
                                   u 
                                   _ 
                                 
                                 k 
                               
                                
                               
                                 
                                   x 
                                   . 
                                 
                                 
                                   t 
                                   k 
                                 
                               
                             
                             + 
                             
                               
                                 
                                   v 
                                   _ 
                                 
                                 k 
                               
                                
                               
                                 
                                   y 
                                   . 
                                 
                                 
                                   t 
                                   k 
                                 
                               
                             
                             + 
                             
                               
                                 f 
                                 _ 
                               
                                
                               
                                 
                                   z 
                                   . 
                                 
                                 
                                   t 
                                   k 
                                 
                               
                             
                             + 
                             
                               
                                 γ 
                                 _ 
                               
                               
                                 
                                   x 
                                   . 
                                 
                                 k 
                               
                             
                           
                         
                       
                     
                     
                       
                         
                           = 
                           
                             
                               
                                 [ 
                                 
                                   
                                     
                                       u 
                                       _ 
                                     
                                     k 
                                   
                                   , 
                                   
                                     
                                       v 
                                       _ 
                                     
                                     k 
                                   
                                   , 
                                   
                                     f 
                                     _ 
                                   
                                 
                                 ] 
                               
                                
                               
                                 
                                   
                                     X 
                                     _ 
                                   
                                   . 
                                 
                                 k 
                               
                             
                             + 
                             
                               
                                 γ 
                                 _ 
                               
                               
                                 
                                   x 
                                   . 
                                 
                                 k 
                               
                             
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   where 
                 
               
               
                 
                   
                     
                       
                         ( 
                         44 
                         ) 
                       
                     
                   
                   
                     
                       
                         ( 
                         45 
                         ) 
                       
                     
                   
                 
               
             
             
               
                 
                   
                     
                       
                         
                           u 
                           _ 
                         
                         k 
                       
                       = 
                       
                         
                           K 
                           3 
                         
                          
                         
                           
                             u 
                             k 
                           
                           
                             
                               
                                 u 
                                 k 
                                 2 
                               
                               + 
                               
                                 v 
                                 k 
                                 2 
                               
                               + 
                               
                                 f 
                                 2 
                               
                             
                           
                         
                       
                     
                     , 
                     
                       
 
                     
                      
                     
                       
                         
                           v 
                           _ 
                         
                         k 
                       
                       = 
                       
                         
                           K 
                           3 
                         
                          
                         
                           
                             v 
                             k 
                           
                           
                             
                               
                                 u 
                                 k 
                                 2 
                               
                               + 
                               
                                 v 
                                 k 
                                 2 
                               
                               + 
                               
                                 f 
                                 2 
                               
                             
                           
                         
                          
                         
                             
                         
                          
                         and 
                       
                     
                   
                    
                   
                     
 
                   
                    
                   
                     
                       f 
                       _ 
                     
                     = 
                     
                       
                         K 
                         3 
                       
                        
                       
                         
                           f 
                           
                             
                               
                                 u 
                                 k 
                                 2 
                               
                               + 
                               
                                 v 
                                 k 
                                 2 
                               
                               + 
                               
                                 f 
                                 2 
                               
                             
                           
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   46 
                   ) 
                 
               
             
           
         
       
     
         [0055]    Eqs. (41), (43) and (45) may form a stochastic system for the vehicle and a Kalman filter may be used to estimate the position and velocity of the vehicle. Minimum three scans may be needed to converge.