Abstract:
A method for GPS navigation which uses an interacting multiple-model (IMM) estimator with a probabilistic data association filter (PDAF) improves navigation performance. The method includes (a) providing two or more models of GPS navigation, with each model characterized by a model state vector which is updated periodically, (b) providing for each model a corresponding filter for deriving, for each period, a current value for the corresponding model state vector based on current measurements made on parameters affecting the corresponding state vector; and (c) applying an interacting multiple model (IMM) estimator to provide, for each period, a current value for a system state vector using the current values of the model state vectors for that period and their corresponding filters. Each model state vector may include one or more of the following: variables: 3-dimensional position, 3-dimensional velocity, satellite clock bias, satellite clock drifts and one or more other satellite parameters. The current value of the system state vector may be a weighted average of the current values of the model state vectors, where the weights are a set of mode probabilities. In addition, one or more of the filters is a probabilitic data association filter (PDAF).

Description:
BACKGROUND OF THE INVENTION 
       [0001]    1. Field of the Invention 
         [0002]    The present invention relates to processing of global positioning systems (GPS) signals. In particular, the present invention relates to processing GPS signals in the presence of dynamically changing, false observations and multipath signal interference. 
         [0003]    2. Discussion of the Related Art 
         [0004]    Conventional GPS navigation may use a Kalman filter to improve accuracy in position determination using pseudorange and other measurements. This approach is based on a single mathematical model of the GPS receiver and its environment. Often, such a model assumes both a relatively constant velocity for the GPS receiver, and a static measurement environment. In practice, such assumptions are often found to be invalid. For example, the GPS receiver may be moving at constant velocity only for very limited time periods, with time periods of acceleration and deceleration in between. A GPS receiver also often changes its direction of travel. The appropriate model for each of these circumstances may be different. Transitions from one model to another model are also important. Thus, navigation based on a single Kalman filter is at best a compromise, and results in poor navigation performance. 
         [0005]    In conventional GPS processing, multipath effects (i.e., the super-positioning of the direct line-of-sight signal path and indirect signal paths) are often present, especially in an urban canyon environment. From the Kalman&#39;s filter point of view, multipath effects affect the statistics in the measurement environment, thereby changes measurement accuracy (e.g., increasing measurement noise variance). Multi-path effects are typically time-varying, intermitten and random. In such an environment, a static multipath model is inappropriate. In the presence of multipath effects, the measurement error is likely positively biased, with an increased variance relative to the case in which multipath effects are not present. A Kalman filter designed for an environment without multipath effects does not perform well in the presence of multipath effects. 
         [0006]    Further, multipath effects and other acquisition errors often cause severe noise in the measured GPS parameters, rendering such measurements unsuitable for use in the Kalman filter. These invalid measurements may lead very large navigation error, thus significantly degrading the Kalman filter&#39;s performance. A complicated filtering of the measured parameters and noise-rejection before the measurements can be used in a Kalman filter. However, proper criteria for accepting or rejecting the measurements are not always clear or known, so that a proper measurement may be improperly rejected, and a false measurement may be improperly accepted. Even with good acceptance and rejection criteria, different weights should be given to navigation data under different collection conditions. For example, one measurement found barely acceptable should not be given the same weight as a measurement found well within the region of acceptance. 
       SUMMARY OF THE INVENTION 
       [0007]    According to one embodiment of the present invention, a method for GPS navigation which uses an interacting multiple-model (IMM) estimator with a probabilistic data association filter (PDAF) improves navigation performance. 
         [0008]    In one embodiment of the present invention, the method includes (a) providing two or more models of GPS navigation, with each model characterized by a model state vector which is updated periodically, (b) providing for each model a corresponding filter for deriving, for each period, a current value for the corresponding model state vector based on current measurements made on parameters affecting the corresponding state vector; and (c) applying an interacting multiple model (IMM) estimator to provide, for each period, a current value for a system state vector using the current values of the model state vectors for that period and their corresponding filters. Each model state vector may include one or more of the following: variables: 3-dimensional position, 3-dimensional velocity, clock bias, clock drifts and one or more other satellite parameters. The current value of the system state vector may be a weighted average of the current values of the model state vectors, where the weights are a set of mode probabilities. 
         [0009]    According to one embodiment of the present invention, the IMM estimator calculates mixing probabilities for each model based on previous measurements made on the parameters. In each cycle, prior to deriving the current values of the model state vectors in the filters, the IMM estimator uses the mixing probabilities to calculate an initial value for each model state vector by weighting the current model state vectors of the immediately previous cycle. Using the current values of the model state vectors derived by the filters, the IMM estimator calculates the mixing probabilities for use in the next cycle, and the mode probabilities for providing the current value of the system state vector. 
         [0010]    According to another aspect of the present invention, one or more of the filters is a probabilitic data association filter (PDAF). The PDAF provides (i) a predicted value for the corresponding model state vector based on internal parameters of the model, and (ii) the current value of the model state vector by updating the predicted value of the model state vector, based on the current measurements made. The PDAF may also apply a statistical difference between a correct observation and a false observation to the current measurements made to derive the current value of the model state vector. The statistical difference may be expressed by a first probability density function for a correct observation, and a second probability density function for a false observation. The probability density functions may include a function of a distance in standard deviations between one of the current measurements and its nominal value. Alternatively, the probability density functions may each be a function of a measured value for the parameters affecting the model state vector. Possible probability density functions for correct observation include a Gaussian distribution, a Rayleigh distribution or an exponential distribution. Other possible probability density functions include a uniform distribution, a linear distribution, an exponential distribution, or a combination of a uniform distribution and a linear distribution. 
         [0011]    The present invention is applicable to models pertaining to multipath effects, environments with low signal-to-noise ratios, a GPS receiver moving at constant velocity, or a GPS receiver having motions that involve non-constant velocities (e.g., acceleration, deceleration, or making turns). 
         [0012]    The present invention is better understood upon consideration of the detailed description below in conjunction with the accompanying drawings. 
     
    
     
       BRIEF DESCRIPTION OF DRAWINGS 
         [0013]      FIG. 1  shows an IMM estimator with a PDAF (i.e., IMM-PDAF  100 ), according to one embodiment of the present invention. 
           [0014]      FIG. 2  shows a Gaussian probability density function  202 , suitable for modeling correct observations, and a uniform probability density function  201 , suitable for modeling false observations, according to one embodiment of the present invention. 
           [0015]      FIG. 3  shows a linear probability density function  302 , suitable for modeling amplitudes that are correct observations, and an exponential probability density function  301 , suitable for modeling amplitudes of false observations, according to one embodiment of the present invention. 
           [0016]      FIG. 4  illustrates PDF  401 , which models false observations due to signal multipaths, and Gaussian PDF  402 , which models correct observations, according to one embodiment of the present invention. 
       
    
    
     DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS 
       [0017]    An interacting multiple-model (IMM) estimator uses several filters, rather than a single Kalman filter. According to one embodiment, an IMM estimator may be designed specifically for specific navigation circumstances. The final output of an IMM estimator is statistically combined from the output data of the multiple filters. In an IMM estimator, information may be exchanged between different filters, weights for the component filters may be dynamically calculated, giving appropriate weights to the component filters according to the prevailing navigation circumstances. Therefore, the final output of an IMM follows the best model. For each component filter of an IMM estimator, a probabilistic data association filter (PDAF) may be used, in place of a single Kalman filter. The PDAF is an extension of a standard Kalman filter, with the ability to handle false or multiple observations probabilistically. 
         [0018]      FIG. 1  shows an IMM estimator with a PDAF (i.e., IMM-PDAF  100 ), according to one embodiment of the present invention. As shown in  FIG. 1 , IMM-PDAF  100  includes an interacting/mixing filter  101 , which receives updated state vectors {circumflex over (X)} 1 (k−1|k−1) and {circumflex over (X)} 2 (k−1|k−1) at time k−1 for models M 1  and M 2  respectively, together with corresponding error variance P 1 (k−1|k−1) and P 2  (k−1|k−1). In a GPS application, for example, state vectors {circumflex over (X)} 1  and {circumflex over (X)} 2  may each be a state vector having elements representing, 3-dimensional position, 3-dimensional velocity, clock bias, clock drifts and other satellite parameters: 
         [0000]        {circumflex over (X)}=[x {dot over (x)} y {dot over (y)} z ż cΔt cΔi SV   bias1    . . . SV   biasN ] T    
         [0019]    Models M 1  and M 2  may be, for example, models where multi-path effects dominate, or models where low signal-to-noise ratio (SNR) conditions dominate, respectively. 
         [0020]    Interacting/mixing filter  101  receives also mixing probabilities μ i|j (k−1|k−1) to provide mixing initial conditions {circumflex over (X)} 01 (k−1|k−1) and {circumflex over (X)} 02  (k−1|k−1). Mixing probabilities μ i|j (k−1|k−1) represent the probabilities that model M j  was in effect at time k−1 given that model M i  is in effect at time k and measurements z(k−1) were observed up to time k−1. 
         [0021]    In one embodiment, mixing probabilities μ i|j (k−1|k−1) may be obtained from mode probabilities μ i (k−1) and mode transition probabilities p ij : 
         [0000]    
       
         
           
             
               
                 μ 
                 
                   i 
                   | 
                   j 
                 
               
                
               
                 ( 
                 
                   
                     k 
                     - 
                     1 
                   
                   | 
                   
                     k 
                     - 
                     1 
                   
                 
                 ) 
               
             
             = 
             
               
                 1 
                 
                   
                     c 
                     _ 
                   
                   j 
                 
               
                
               
                 p 
                 ij 
               
                
               
                 
                   μ 
                   i 
                 
                  
                 
                   ( 
                   
                     k 
                     - 
                     1 
                   
                   ) 
                 
               
             
           
         
       
     
         [0022]    where 
         [0000]    
       
         
           
             
               c 
               _ 
             
             = 
             
               
                 ∑ 
                 
                   i 
                   = 
                   1 
                 
                 r 
               
                
               
                   
               
                
               
                 
                   p 
                   ij 
                 
                  
                 
                   
                     μ 
                     i 
                   
                    
                   
                     ( 
                     
                       k 
                       - 
                       1 
                     
                     ) 
                   
                 
               
             
           
         
       
     
         [0000]    is the normalizing factor for the r models. 
         [0023]    The mixing initial conditions {circumflex over (X)} 01 (k−1|k−1) and {circumflex over (X)} 02 (k−1|k−1) are fed respectively into probability data association filters (PDAF)  102  and  103  representing models M 1  and M 2 , each receiving measurements z(k) observed up to time k. PDAFs  102  and  103  may each be, for example, a Kalman filter-like filter, providing for time k updated state vectors {circumflex over (X)} 1 (k|k) and {circumflex over (X)} 2 (k|k), and corresponding error variance P 1 (k|k) and P 2 (k|k), respectively. In addition, PDAFs  102  and  103  provide likelihood functions Λ 1  and Λ 2 . Likelihood function Λ j  representing the conditional probabilities for observing measurements z(k) up to time k, given model M j  is in effect at time k and measurements z(k−1) up to time k−1. 
         [0024]    The likelihood functions are used to derive the mode probabilities μ i (k−1) and the mixing probabilities μ i|j (k−1|k−1). In one embodiment, mode probabilities μ i (k−1) are given by: 
         [0000]    
       
         
           
             
               
                 μ 
                 i 
               
                
               
                 ( 
                 k 
                 ) 
               
             
             = 
             
               
                 1 
                 c 
               
                
               
                 
                   Λ 
                   i 
                 
                  
                 
                   ( 
                   k 
                   ) 
                 
               
                
               
                 c 
                 _ 
               
             
           
         
       
     
         [0025]    where 
         [0000]    
       
         
           
             c 
             = 
             
               
                 ∑ 
                 
                   i 
                   = 
                   1 
                 
                 r 
               
                
               
                   
               
                
               
                 
                   
                     Λ 
                     i 
                   
                    
                   
                     ( 
                     k 
                     ) 
                   
                 
                  
                 
                   c 
                   _ 
                 
               
             
           
         
       
     
         [0000]    is a normalization factor. 
         [0026]    The updated combined state vector {circumflex over (X)}(k|k) at time k and its corresponding error variance P(k|k) are output. Note that updated combined state vector {circumflex over (X)}(k|k) and its corresponding error variance P(k|k) are not used in the recursive algorithm. In one embodiment, the combined state vector {circumflex over (X)}(k|k) at time k its corresponding error variance P(k|k) may be given by: 
         [0000]    
       
         
           
             
                 
             
              
             
               
                 
                   X 
                   ^ 
                 
                  
                 
                   ( 
                   
                     k 
                     | 
                     k 
                   
                   ) 
                 
               
               = 
               
                 
                   ∑ 
                   
                     j 
                     = 
                     1 
                   
                   r 
                 
                  
                 
                     
                 
                  
                 
                   
                     
                       
                         X 
                         ^ 
                       
                       j 
                     
                      
                     
                       ( 
                       
                         k 
                         | 
                         k 
                       
                       ) 
                     
                   
                    
                   
                     
                       μ 
                       j 
                     
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                 
               
             
           
         
       
       
         
           
             
               P 
                
               
                 ( 
                 
                   k 
                   | 
                   k 
                 
                 ) 
               
             
             = 
             
               
                 ∑ 
                 
                   j 
                   = 
                   1 
                 
                 r 
               
                
               
                   
               
                
               
                 
                   
                     μ 
                     j 
                   
                    
                   
                     ( 
                     k 
                     ) 
                   
                 
                  
                 
                   [ 
                   
                     
                       
                         P 
                         j 
                       
                        
                       
                         ( 
                         
                           k 
                           | 
                           k 
                         
                         ) 
                       
                     
                     + 
                     
                       
                         [ 
                         
                           
                             
                               
                                 X 
                                 ^ 
                               
                               j 
                             
                              
                             
                               ( 
                               
                                 k 
                                 | 
                                 k 
                               
                               ) 
                             
                           
                           - 
                           
                             
                               X 
                               ^ 
                             
                              
                             
                               ( 
                               
                                 k 
                                 | 
                                 k 
                               
                               ) 
                             
                           
                         
                         ] 
                       
                       * 
                       
                         
                           [ 
                           
                             
                               
                                 
                                   X 
                                   ^ 
                                 
                                 j 
                               
                                
                               
                                 ( 
                                 
                                   k 
                                   | 
                                   k 
                                 
                                 ) 
                               
                             
                             - 
                             
                               
                                 X 
                                 ^ 
                               
                                
                               
                                 ( 
                                 
                                   k 
                                   | 
                                   k 
                                 
                                 ) 
                               
                             
                           
                           ] 
                         
                         T 
                       
                     
                   
                   ] 
                 
               
             
           
         
       
     
         [0027]    PDAFs  102  and  103  may each have the following prediction equations for the state vector, the associated error covariance P 0j , a measurement estimate and the innovation covariance, respectively, as in a standard Kalman filter: 
         [0000]        {circumflex over (X)}   0j ( k|k− 1)= F   j ( k ) {circumflex over (X)}   0j ( k− 1 |k− 1) 
         [0000]        P   0j ( k|k− 1)= F   j ( k ) P   0j ( k− 1 |k− 1) F   j ( k ) T   +Q   i (k−1) 
         [0000]        {circumflex over (z)} ( k|k− 1)= H   j ( k ) {circumflex over (X)}   0j ( k|k− 1) 
         [0000]        S   j ( k )= H   j ( k ) P   0j ( k|k− 1) H   j ( k ) T   +R   j ( k ) 
         [0028]    where F j , Q j , H j , and R j  are the state transition model, the noise covariance matrix, the measurement model, and the residue covariance matrix for model M j , respectively. 
         [0029]    The updated state vectors {circumflex over (X)} j (k|k) and its error covariance P j (k|k) for model M j  may be obtained using measurement z(k) according to the following updating equations: 
         [0000]        {circumflex over (X)}   j ( k|k )= {circumflex over (X)}   0j ( k−k− 1)+ W   j ( k )[ z ( k )− {circumflex over (z)} ( k|k− 1)] 
         [0000]        P   j ( k|k )= P   0j ( k|k− 1)− W   j ( k ) S   j ( k ) W   j ( k ) T    
         [0030]    where W j  is the gain of the filter. Filter gain W j  may be given by: 
         [0000]        W   j ( k )= P   0j ( k|k− 1) H   j ( k ) T   [S   j ( k )] −1    
         [0031]    The algorithm described above may be used to probabilistically distinguish between a correct observation and a false observation, using the different statistical characteristics between the two types of observations. For example, correct observations may be modeled by a Gaussian probability density function, while false observations may be better modeled by a uniform distribution.  FIG. 2  shows a Gaussian probability density function  202 , suitable for modeling correct observations, and a uniform probability density function  201 , suitable for modeling false observations. Each PDAF makes “soft” decisions, rather than black-and-white “hard” decision. A reasonable soft decision takes advantage of more information, as a hard decision is merely a special case of a soft decision. 
         [0032]    In one embodiment, false or invalid measurements are represented by uniform distribution  201  of  FIG. 2 , and correct or valid measurements are represented by Gaussian distribution  202 , respectively. In that embodiment, the residual of observation exceeds a predetermined value of the innovation variance (e.g., five standard deviations, or “5-sigma”), the probability that the observation is invalid is assigned the probability of 1. That is, 
         [0000]      [ z ( k )− z ( k|k− 1)] T   S ( k ) −1   [z ( k )− z ( k|k− 1)]≦γ 
         [0033]    where γ is the gate threshold (for 5-sigma, γ is 25). 
         [0034]    The probability density function (PDF) for a correct observation is given by: 
         [0000]        P   D   P   G   −1   N[z ( k )| z ( k|k− 1), S ( k )]= P   D   P   G   −1   N[z ( k )− z ( k|k− 1)|0 ,S ( k )] 
         [0035]    where P D  is the probability of detection, P G   −1  is the normalization factor to make the Gaussian PDF unity in the validation area. 
         [0036]    Similarly, because an invalid observation is assumed to have a uniform distribution 1 , the PDF is given by:  1 In other embodiments, other PDFs may be used 
         [0000]    
       
         
           
             
               ( 
               
                 1 
                 - 
                 
                   P 
                   D 
                 
               
               ) 
             
              
             
               1 
               
                 2 
                  
                 
                   
                      
                     
                       γ 
                        
                       
                           
                       
                        
                       
                         S 
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                     
                      
                   
                   
                     1 
                     / 
                     2 
                   
                 
               
             
           
         
       
     
         [0037]    Therefore, the association probabilities β 1 (k) and β 0 (k) are the probabilities that an observation is valid or invalid, respectively. According to the Total Probability Theorem, association probabilities β 1 (k) and β 0 (k) are given by: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       β 
                       1 
                     
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                   = 
                     
                    
                   
                     
                       
                         P 
                         D 
                       
                        
                       
                         P 
                         G 
                         
                           - 
                           1 
                         
                       
                        
                       
                         1 
                         
                           
                              
                             
                               2 
                                
                               π 
                                
                               
                                   
                               
                                
                               
                                 S 
                                  
                                 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                             
                              
                           
                           
                             1 
                             / 
                             2 
                           
                         
                       
                        
                       
                          
                         
                           
                             - 
                             
                               1 
                               2 
                             
                           
                            
                           
                             
                               v 
                                
                               
                                 ( 
                                 k 
                                 ) 
                               
                             
                             ′ 
                           
                            
                           
                             
                               S 
                                
                               
                                 ( 
                                 k 
                                 ) 
                               
                             
                             
                               - 
                               1 
                             
                           
                            
                           
                             v 
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                         
                       
                     
                     
                       
                         
                           ( 
                           
                             1 
                             - 
                             
                               P 
                               D 
                             
                           
                           ) 
                         
                          
                         
                           1 
                           
                             2 
                              
                             
                               
                                  
                                 
                                   γ 
                                    
                                   
                                       
                                   
                                    
                                   
                                     S 
                                      
                                     
                                       ( 
                                       k 
                                       ) 
                                     
                                   
                                 
                                  
                               
                               
                                 1 
                                 / 
                                 2 
                               
                             
                           
                         
                       
                       + 
                       
                         
                           P 
                           D 
                         
                          
                         
                           P 
                           G 
                           
                             - 
                             1 
                           
                         
                          
                         
                           1 
                           
                             
                                
                               
                                 2 
                                  
                                 π 
                                  
                                 
                                     
                                 
                                  
                                 
                                   S 
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                               
                                
                             
                             
                               1 
                               / 
                               2 
                             
                           
                         
                          
                         
                            
                           
                             
                               - 
                               
                                 1 
                                 2 
                               
                             
                              
                             
                               
                                 v 
                                  
                                 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               ′ 
                             
                              
                             
                               
                                 S 
                                  
                                 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               
                                 - 
                                 1 
                               
                             
                              
                             
                               v 
                                
                               
                                 ( 
                                 k 
                                 ) 
                               
                             
                           
                         
                       
                     
                   
                 
               
             
             
               
                 
                   = 
                     
                    
                   
                     
                        
                       
                         
                           - 
                           
                             1 
                             2 
                           
                         
                          
                         
                           
                             v 
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                           ′ 
                         
                          
                         
                           
                             S 
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                           
                             - 
                             1 
                           
                         
                          
                         
                           v 
                            
                           
                             ( 
                             k 
                             ) 
                           
                         
                       
                     
                     
                       
                         
                           
                             ( 
                             
                               
                                 2 
                                  
                                 π 
                               
                               γ 
                             
                             ) 
                           
                           
                             1 
                             2 
                           
                         
                          
                         
                           1 
                           2 
                         
                          
                         
                           
                             
                               ( 
                               
                                 1 
                                 - 
                                 
                                   P 
                                   D 
                                 
                               
                               ) 
                             
                              
                             
                               P 
                               G 
                               
                                 - 
                                 1 
                               
                             
                           
                           
                             P 
                             D 
                           
                         
                       
                       + 
                       
                          
                         
                           
                             - 
                             
                               1 
                               2 
                             
                           
                            
                           
                             
                               v 
                                
                               
                                 ( 
                                 k 
                                 ) 
                               
                             
                             ′ 
                           
                            
                           
                             
                               S 
                                
                               
                                 ( 
                                 k 
                                 ) 
                               
                             
                             
                               - 
                               1 
                             
                           
                            
                           
                             v 
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                         
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
               
                 β 
                 0 
               
                
               
                 ( 
                 k 
                 ) 
               
             
             = 
             
               
                 
                   
                     ( 
                     
                       
                         2 
                          
                         π 
                       
                       γ 
                     
                     ) 
                   
                   
                     1 
                     2 
                   
                 
                  
                 
                   1 
                   2 
                 
                  
                 
                   
                     
                       ( 
                       
                         1 
                         - 
                         
                           P 
                           D 
                         
                       
                       ) 
                     
                      
                     
                       P 
                       G 
                       
                         - 
                         1 
                       
                     
                   
                   
                     P 
                     D 
                   
                 
               
               
                 
                   
                     
                       ( 
                       
                         
                           2 
                            
                           π 
                         
                         γ 
                       
                       ) 
                     
                     
                       1 
                       2 
                     
                   
                    
                   
                     1 
                     2 
                   
                    
                   
                     
                       
                         ( 
                         
                           1 
                           - 
                           
                             P 
                             D 
                           
                         
                         ) 
                       
                        
                       
                         P 
                         G 
                         
                           - 
                           1 
                         
                       
                     
                     
                       P 
                       D 
                     
                   
                 
                 + 
                 
                    
                   
                     
                       - 
                       
                         1 
                         2 
                       
                     
                      
                     
                       
                         v 
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                       ′ 
                     
                      
                     
                       
                         S 
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                       
                         - 
                         1 
                       
                     
                      
                     
                       v 
                        
                       
                         ( 
                         k 
                         ) 
                       
                     
                   
                 
               
             
           
         
       
       
         
           Or 
         
       
       
         
           
             
               
                 β 
                 i 
               
                
               
                 ( 
                 k 
                 ) 
               
             
             = 
             
               { 
               
                 
                   
                     
                       
                         
                           b 
                           
                             b 
                             + 
                             
                               e 
                               1 
                             
                           
                         
                       
                       
                         
                           
                             i 
                             = 
                             0 
                           
                           , 
                           
                             invalid 
                              
                             
                                 
                             
                              
                             observation 
                           
                         
                       
                     
                     
                       
                         
                           
                             e 
                             1 
                           
                           
                             b 
                             + 
                             
                               e 
                               1 
                             
                           
                         
                       
                       
                         
                           
                             i 
                             = 
                             1 
                           
                           , 
                           
                             valid 
                              
                             
                                 
                             
                              
                             observation 
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   where 
                    
                   
                     
 
                   
                    
                   
                     e 
                     1 
                   
                 
                 = 
                 
                   
                     
                        
                       
                         
                           - 
                           
                             1 
                             2 
                           
                         
                          
                         
                           
                             v 
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                           ′ 
                         
                          
                         
                           
                             S 
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                           
                             - 
                             1 
                           
                         
                          
                         
                           v 
                            
                           
                             ( 
                             k 
                             ) 
                           
                         
                       
                     
                      
                     
                       
 
                     
                      
                     b 
                   
                   = 
                   
                     
                       
                         ( 
                         
                           
                             2 
                              
                             π 
                           
                           γ 
                         
                         ) 
                       
                       
                         1 
                         2 
                       
                     
                      
                     
                       1 
                       2 
                     
                      
                     
                       
                         
                           ( 
                           
                             1 
                             - 
                             
                               P 
                               D 
                             
                           
                           ) 
                         
                          
                         
                           P 
                           G 
                           
                             - 
                             1 
                           
                         
                       
                       
                         P 
                         D 
                       
                     
                   
                 
               
             
           
         
       
     
         [0038]    Consequently, the output state vector for model M i  is given by: 
         [0000]    
       
         
           
             
               
                 
                   X 
                   ^ 
                 
                 i 
               
                
               
                 ( 
                 
                   k 
                   | 
                   k 
                 
                 ) 
               
             
             = 
             
               
                 ∑ 
                 
                   j 
                   = 
                   0 
                 
                 1 
               
                
               
                   
               
                
               
                 
                   
                     
                       X 
                       ^ 
                     
                     j 
                     i 
                   
                    
                   
                     ( 
                     
                       k 
                       | 
                       k 
                     
                     ) 
                   
                 
                  
                 
                   
                     β 
                     j 
                   
                    
                   
                     ( 
                     k 
                     ) 
                   
                 
               
             
           
         
       
     
         [0039]    For a false observation, the state vector is not updated: 
         [0000]        {circumflex over (X)}   0   i ( k|k )= {circumflex over (X)}   0i ( k|k− 1) 
         [0040]    For a valid observation, the state vector is updated in the same manner as a Kalman filter: 
         [0000]        {circumflex over (X)}   1   i ( k|k )= {circumflex over (X)}   0i ( k|k− 1)+ W ( k )[ z ( k )− z ( k|k− 1)] 
         [0041]    where gain W(k) is given by: 
         [0000]        W ( k )= P   02 ( k|k− 1) H ( k ) T   S ( k ) −1 , as in a Kalman filter. 
         [0042]    Therefore updated state vector {circumflex over (X)} i (k|k) and error variance P i (k|k) are given by: 
         [0000]        {circumflex over (X)}   i ( k|k )= {circumflex over (X)}   0i ( k|k− 1)+ W ( k )β 1 ( k )[ z ( k )− z ( k|k− 1)] 
         [0000]        P   i ( k|k )=β 0 ( k ) P   i ( k|k− 1)+(1−β 0 ( k )) P   c ( k|k )+ {tilde over (P)} ( k ) 
         [0000]      where 
         [0000]        P   c ( k|k )= P   0i ( k|k− 1)− W ( k ) S ( k ) W ( k ) T    
         [0000]        {tilde over (P)} ( k )= W ( k )[β 1 ( k ) v ( k ) v ( k ) T −β 1 ( k ) v ( k ) v ( k ) T β 1 ( k )] W ( k ) T    
         [0043]    Overall, the PDAF is very similar to the standard Kalman filter. The PDAF is very simple, easy to understand, and also very computational efficient, with about 50% than a Kalman filter. Note that the PDAF algorithm can also handle multiple observations. Assuming there are m observations in the validation region. The conventional Kalman filter can only choose one as the correct measurement, while the PDAF can probabilistically associate all measurements to the filter. Besides this, the PDAF also considered the probability that all these m observations are false (i.e., the PDAF handles m+1 hypotheses). In this case, the following equations may be used for model M i : 
         [0000]    
       
         
           
             
                 
             
              
             
               
                 
                   
                     X 
                     ⋒ 
                   
                   i 
                 
                  
                 
                   ( 
                   
                     k 
                     | 
                     k 
                   
                   ) 
                 
               
               = 
               
                 
                   
                     
                       X 
                       ⋒ 
                     
                     i 
                   
                    
                   
                     ( 
                     
                       k 
                       | 
                       
                         k 
                         - 
                         1 
                       
                     
                     ) 
                   
                 
                 + 
                 
                   
                     W 
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                    
                   
                     v 
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
              
             
               
                 v 
                  
                 
                   ( 
                   k 
                   ) 
                 
               
               = 
               
                 
                   ∑ 
                   
                     i 
                     = 
                     1 
                   
                   m 
                 
                  
                 
                     
                 
                  
                 
                   
                     
                       β 
                       i 
                     
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                    
                   
                     [ 
                     
                       
                         
                           z 
                           i 
                         
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                       - 
                       
                         z 
                          
                         
                           ( 
                           
                             k 
                             | 
                             
                               k 
                               - 
                               1 
                             
                           
                           ) 
                         
                       
                     
                     ] 
                   
                 
               
             
           
         
       
       
         
           
             
               
                 P 
                 ~ 
               
                
               
                 ( 
                 k 
                 ) 
               
             
             = 
             
               
                 
                   W 
                    
                   
                     ( 
                     k 
                     ) 
                   
                 
                  
                 
                   [ 
                   
                     
                       ∑ 
                       
                         i 
                         = 
                         1 
                       
                       m 
                     
                      
                     
                         
                     
                      
                     
                       [ 
                       
                         
                           
                             
                               β 
                               i 
                             
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                            
                           
                             
                               v 
                               i 
                             
                              
                             
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                           
                            
                           
                             
                               
                                 v 
                                 i 
                               
                                
                               
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                             T 
                           
                         
                         - 
                         
                           
                             v 
                              
                             
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                           
                            
                           
                             
                               v 
                                
                               
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                             T 
                           
                         
                       
                       ] 
                     
                   
                   ] 
                 
               
                
               
                 
                   W 
                    
                   
                     ( 
                     k 
                     ) 
                   
                 
                 T 
               
             
           
         
       
       
         
           
             
                 
             
              
             
               
                 
                   β 
                   i 
                 
                  
                 
                   ( 
                   k 
                   ) 
                 
               
               = 
               
                 { 
                 
                   
                     
                       
                         
                           e 
                           i 
                         
                         
                           b 
                           + 
                           
                             
                               ∑ 
                               
                                 i 
                                 = 
                                 1 
                               
                               m 
                             
                              
                             
                                 
                             
                              
                             
                               e 
                               i 
                             
                           
                         
                       
                     
                     
                       
                         
                           i 
                           = 
                           1 
                         
                         , 
                         2 
                         , 
                         
                           … 
                            
                           
                               
                           
                            
                           m 
                         
                       
                     
                   
                   
                     
                       
                         b 
                         
                           b 
                           + 
                           
                             
                               ∑ 
                               
                                 i 
                                 = 
                                 1 
                               
                               m 
                             
                              
                             
                                 
                             
                              
                             
                               e 
                               i 
                             
                           
                         
                       
                     
                     
                       
                         
                           i 
                           = 
                           0 
                         
                         , 
                         false 
                       
                     
                   
                 
               
             
           
         
       
     
         [0044]    In this multiple observation case (or other very low SNR scenarios), the PDAF algorithm out-performs the Kalman filter significantly. In one embodiment, six observations were made, so that seven hypotheses were tested (i.e., one of the six observations are correct or all of the observations are false). Assuming that the probability density function of a correct measurement is provided by a Gaussian distribution, and assuming that, when the residual of observation exceeds a predetermined value of the innovation variance (e.g., five standard deviations, or “5-sigma”), the probability that the observation is invalid is assigned the probability of 1. That is, 
         [0000]      [ z ( k )− z ( k|k− 1)] T   S ( k ) −1   [z ( k )− z ( k|k− 1)]≦γ 
         [0045]    where γ is the gate threshold (for 5-sigma, γ is 25). 
         [0046]    The probability density function (PDF) for a correct observation is given by: 
         [0000]        P   D   P   G   P   G   −1   N[z ( k )| z ( k|k− 1), S ( k )]= P   D   P   G   P   G   −1   N[z ( k )− z ( k|k− 1)|0 ,S ( k )] 
         [0047]    where P D  is the probability of detection, P G  is the Gaussian cumulative distribution function (CDF) within the 5-sigma region, P G   −1  is the normalization factor to make the Gaussian PDF unity in the validation area. Let σ≡|S(k)| 1/2 , the PDF for the correct association for measurement i is given by 
         [0000]    
       
         
           
             
               
                 P 
                 D 
               
                
               
                  
                 
                   
                     - 
                     
                       
                         [ 
                         
                           
                             
                               z 
                               i 
                             
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                           - 
                           
                             z 
                              
                             
                               ( 
                               
                                 k 
                                 | 
                                 
                                   k 
                                   - 
                                   1 
                                 
                               
                               ) 
                             
                           
                         
                         ] 
                       
                       2 
                     
                   
                   
                     2 
                      
                     
                       σ 
                       2 
                     
                   
                 
               
             
             
               
                 
                   2 
                    
                   π 
                 
               
                
               σ 
             
           
         
       
     
         [0048]    Assuming that an invalid observation has a uniform distribution 2 , the PDF for a false observation is given by:  2 In other embodiments, other PDFs may be used 
         [0000]    
       
         
           
             
               
                 ( 
                 
                   1 
                   - 
                   
                     
                       P 
                       D 
                     
                      
                     
                       P 
                       G 
                     
                   
                 
                 ) 
               
                
               
                 1 
                 
                   2 
                    
                   
                     
                        
                       
                         γ 
                          
                         
                             
                         
                          
                         
                           S 
                            
                           
                             ( 
                             k 
                             ) 
                           
                         
                       
                        
                     
                     
                       1 
                       / 
                       2 
                     
                   
                 
               
             
             = 
             
               
                 ( 
                 
                   1 
                   - 
                   
                     
                       P 
                       D 
                     
                      
                     
                       P 
                       G 
                     
                   
                 
                 ) 
               
               
                 10 
                  
                 
                     
                 
                  
                 σ 
               
             
           
         
       
     
         [0049]    Therefore, the association probabilities β i (k) and β 0 (k) are the probabilities that that i th  observation is valid and none of the observations are valid, respectively. According to the Total Probability Theorem, association probabilities β 2 (k) and β 1 (k) are given by: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       β 
                       i 
                     
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                   = 
                     
                    
                   
                     
                       
                         
                           
                             
                               P 
                               D 
                             
                              
                             
                                
                               
                                 
                                   - 
                                   
                                     
                                       [ 
                                       
                                         
                                           
                                             z 
                                             i 
                                           
                                            
                                           
                                             ( 
                                             k 
                                             ) 
                                           
                                         
                                         - 
                                         
                                           z 
                                            
                                           
                                             ( 
                                             
                                               k 
                                               | 
                                               
                                                 k 
                                                 - 
                                                 1 
                                               
                                             
                                             ) 
                                           
                                         
                                       
                                       ] 
                                     
                                     2 
                                   
                                 
                                 
                                   2 
                                    
                                   
                                     σ 
                                     2 
                                   
                                 
                               
                             
                           
                           
                             
                               
                                 2 
                                  
                                 π 
                               
                             
                              
                             σ 
                           
                         
                          
                         
                           [ 
                           
                             1 
                             
                               10 
                                
                               σ 
                             
                           
                           ] 
                         
                       
                       5 
                     
                     
                       
                         
                           
                             ( 
                             
                               1 
                               - 
                               
                                 
                                   P 
                                   D 
                                 
                                  
                                 
                                   P 
                                   G 
                                 
                               
                             
                             ) 
                           
                            
                           
                             [ 
                             
                               1 
                               
                                 10 
                                  
                                 σ 
                               
                             
                             ] 
                           
                         
                         6 
                       
                       + 
                       
                         
                           ∑ 
                           
                             j 
                             = 
                             1 
                           
                           6 
                         
                          
                         
                             
                         
                          
                         
                           
                             
                               
                                 
                                   P 
                                   D 
                                 
                                  
                                 
                                    
                                   
                                     
                                       - 
                                       
                                         
                                           [ 
                                           
                                             
                                               
                                                 z 
                                                 j 
                                               
                                                
                                               
                                                 ( 
                                                 k 
                                                 ) 
                                               
                                             
                                             - 
                                             
                                               z 
                                                
                                               
                                                 ( 
                                                 
                                                   k 
                                                   | 
                                                   
                                                     k 
                                                     - 
                                                     1 
                                                   
                                                 
                                                 ) 
                                               
                                             
                                           
                                           ] 
                                         
                                         2 
                                       
                                     
                                     
                                       2 
                                        
                                       
                                         σ 
                                         2 
                                       
                                     
                                   
                                 
                               
                               
                                 
                                   
                                     2 
                                      
                                     π 
                                   
                                 
                                  
                                 σ 
                               
                             
                              
                             
                               [ 
                               
                                 1 
                                 
                                   10 
                                    
                                   
                                       
                                   
                                    
                                   σ 
                                 
                               
                               ] 
                             
                           
                           5 
                         
                       
                     
                   
                 
               
             
             
               
                 
                   = 
                     
                    
                   
                     
                        
                       
                         
                           - 
                           
                             
                               [ 
                               
                                 
                                   
                                     z 
                                     i 
                                   
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                                 - 
                                 
                                   z 
                                    
                                   
                                     ( 
                                     
                                       k 
                                       | 
                                       
                                         k 
                                         - 
                                         1 
                                       
                                     
                                     ) 
                                   
                                 
                               
                               ] 
                             
                             2 
                           
                         
                         
                           2 
                            
                           
                             σ 
                             2 
                           
                         
                       
                     
                     
                       
                         
                           
                             
                               2 
                                
                               π 
                             
                           
                           10 
                         
                          
                         
                           
                             ( 
                             
                               1 
                               - 
                               
                                 
                                   P 
                                   D 
                                 
                                  
                                 
                                   P 
                                   G 
                                 
                               
                             
                             ) 
                           
                           
                             P 
                             D 
                           
                         
                       
                       + 
                       
                         
                           ∑ 
                           
                             j 
                             = 
                             1 
                           
                           6 
                         
                          
                         
                             
                         
                          
                         
                            
                           
                             
                               - 
                               
                                 
                                   [ 
                                   
                                     
                                       
                                         z 
                                         j 
                                       
                                        
                                       
                                         ( 
                                         k 
                                         ) 
                                       
                                     
                                     - 
                                     
                                       z 
                                        
                                       
                                         ( 
                                         
                                           k 
                                           | 
                                           
                                             k 
                                             - 
                                             1 
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                   ] 
                                 
                                 2 
                               
                             
                             
                               2 
                                
                               
                                 σ 
                                 2 
                               
                             
                           
                         
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
               
                 β 
                 0 
               
                
               
                 ( 
                 k 
                 ) 
               
             
             = 
             
               
                 
                   
                     
                       2 
                        
                       π 
                     
                   
                   10 
                 
                  
                 
                   
                     ( 
                     
                       1 
                       - 
                       
                         
                           P 
                           D 
                         
                          
                         
                           P 
                           G 
                         
                       
                     
                     ) 
                   
                   
                     P 
                     D 
                   
                 
               
               
                 
                   
                     
                       
                         2 
                          
                         π 
                       
                     
                     10 
                   
                    
                   
                     
                       ( 
                       
                         1 
                         - 
                         
                           
                             P 
                             D 
                           
                            
                           
                             P 
                             G 
                           
                         
                       
                       ) 
                     
                     
                       P 
                       D 
                     
                   
                 
                 + 
                 
                   
                     ∑ 
                     
                       j 
                       = 
                       1 
                     
                     6 
                   
                    
                   
                       
                   
                    
                   
                      
                     
                       
                         - 
                         
                           
                             [ 
                             
                               
                                 
                                   z 
                                   j 
                                 
                                  
                                 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               - 
                               
                                 z 
                                  
                                 
                                   ( 
                                   
                                     k 
                                     | 
                                     
                                       k 
                                       - 
                                       1 
                                     
                                   
                                   ) 
                                 
                               
                             
                             ] 
                           
                           2 
                         
                       
                       
                         2 
                          
                         
                           σ 
                           2 
                         
                       
                     
                   
                 
               
             
           
         
       
       
         
           Or 
         
       
       
         
           
             
               
                 β 
                 i 
               
                
               
                 ( 
                 k 
                 ) 
               
             
             = 
             
               { 
               
                 
                   
                     
                       
                         
                           
                             e 
                             i 
                           
                           
                             b 
                             + 
                             
                               
                                 ∑ 
                                 
                                   i 
                                   = 
                                   1 
                                 
                                 m 
                               
                                
                               
                                   
                               
                                
                               
                                 e 
                                 i 
                               
                             
                           
                         
                       
                       
                         
                           
                             i 
                             = 
                             1 
                           
                           , 
                           2 
                           , 
                           
                             … 
                              
                             
                                 
                             
                              
                             m 
                           
                         
                       
                     
                     
                       
                         
                           b 
                           
                             b 
                             + 
                             
                               
                                 ∑ 
                                 
                                   i 
                                   = 
                                   1 
                                 
                                 m 
                               
                                
                               
                                   
                               
                                
                               
                                 e 
                                 i 
                               
                             
                           
                         
                       
                       
                         
                           
                             i 
                             = 
                             0 
                           
                           , 
                           false 
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   where 
                    
                   
                     
 
                   
                    
                   
                     e 
                     1 
                   
                 
                 = 
                 
                   
                     
                        
                       
                         
                           - 
                           
                             
                               [ 
                               
                                 
                                   
                                     z 
                                     i 
                                   
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                                 - 
                                 
                                   z 
                                    
                                   
                                     ( 
                                     
                                       k 
                                       | 
                                       
                                         k 
                                         - 
                                         1 
                                       
                                     
                                     ) 
                                   
                                 
                               
                               ] 
                             
                             2 
                           
                         
                         
                           2 
                            
                           
                             σ 
                             2 
                           
                         
                       
                     
                      
                     
                         
                     
                      
                     b 
                   
                   = 
                   
                     
                       
                         
                           2 
                            
                           π 
                         
                       
                       10 
                     
                      
                     
                       
                         ( 
                         
                           1 
                           - 
                           
                             
                               P 
                               D 
                             
                              
                             
                               P 
                               G 
                             
                           
                         
                         ) 
                       
                       
                         P 
                         D 
                       
                     
                   
                 
               
             
           
         
       
     
         [0050]    Of course, the state and covariance updates follow from the general equations discussed above, for model M i : 
         [0051]    For a false observation, the state vector is not updated: 
         [0000]        {circumflex over (X)}   0   i ( k|k )= {circumflex over (X)}   0i ( k|k− 1) 
         [0052]    If the j th  observation is valid, the state vector is updated in the same manner as a Kalman filter: 
         [0000]        {circumflex over (X)}   j   i ( k|k )= {circumflex over (X)}   0i ( k|k− 1)+ W ( k )[ z ( k )− z ( k|k− 1)] 
         [0053]    where gain W(k) is given by: 
         [0000]        W ( k )= P   0i ( k|k− 1) H ( k ) T   S ( k ) −1 , as in a Kalman filter. 
         [0054]    Therefore updated state vector {circumflex over (X)} i (k|k) and error variance P i (k|k) are given by: 
         [0000]    
       
         
           
             
                 
             
              
             
               
                 
                   
                     X 
                     ^ 
                   
                   i 
                 
                  
                 
                   ( 
                   
                     k 
                     | 
                     k 
                   
                   ) 
                 
               
               = 
               
                 
                   
                     
                       X 
                       ^ 
                     
                     
                       0 
                        
                       i 
                     
                   
                    
                   
                     ( 
                     
                       k 
                       | 
                       
                         k 
                         - 
                         1 
                       
                     
                     ) 
                   
                 
                 + 
                 
                   
                     W 
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                    
                   
                     
                       ∑ 
                       
                         j 
                         = 
                         1 
                       
                       6 
                     
                      
                     
                         
                     
                      
                     
                       
                         β 
                         j 
                       
                        
                       
                         [ 
                         
                           
                             z 
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                           - 
                           
                             z 
                              
                             
                               ( 
                               
                                 k 
                                 | 
                                 
                                   k 
                                   - 
                                   1 
                                 
                               
                               ) 
                             
                           
                         
                         ] 
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
              
             
               
                 
                   P 
                   i 
                 
                  
                 
                   ( 
                   
                     k 
                     | 
                     k 
                   
                   ) 
                 
               
               = 
               
                 
                   
                     
                       β 
                       0 
                     
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                    
                   
                     
                       P 
                       i 
                     
                      
                     
                       ( 
                       
                         k 
                         | 
                         
                           k 
                           - 
                           1 
                         
                       
                       ) 
                     
                   
                 
                 + 
                 
                   
                     ( 
                     
                       1 
                       - 
                       
                         
                           β 
                           0 
                         
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                     
                     ) 
                   
                    
                   
                     
                       P 
                       c 
                     
                      
                     
                       ( 
                       
                         k 
                         | 
                         k 
                       
                       ) 
                     
                   
                 
                 + 
                 
                   
                     P 
                     ~ 
                   
                    
                   
                     ( 
                     k 
                     ) 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
              
             where 
           
         
       
       
         
           
             
                 
             
              
             
               
                 
                   P 
                   c 
                 
                  
                 
                   ( 
                   
                     k 
                     | 
                     k 
                   
                   ) 
                 
               
               = 
               
                 
                   
                     P 
                     
                       0 
                        
                       i 
                     
                   
                    
                   
                     ( 
                     
                       k 
                       | 
                       
                         k 
                         - 
                         1 
                       
                     
                     ) 
                   
                 
                 - 
                 
                   
                     W 
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                    
                   
                     S 
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                    
                   
                     
                       W 
                        
                       
                         ( 
                         k 
                         ) 
                       
                     
                     T 
                   
                 
               
             
           
         
       
       
         
           
             
               
                 P 
                 ~ 
               
                
               
                 ( 
                 k 
                 ) 
               
             
             = 
             
               
                 
                   W 
                    
                   
                     ( 
                     k 
                     ) 
                   
                 
                  
                 
                   [ 
                   
                     
                       ∑ 
                       
                         i 
                         = 
                         1 
                       
                       6 
                     
                      
                     
                         
                     
                      
                     
                       [ 
                       
                         
                           
                             
                               β 
                               i 
                             
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                            
                           
                             
                               v 
                               i 
                             
                              
                             
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                           
                            
                           
                             
                               
                                 v 
                                 i 
                               
                                
                               
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                             T 
                           
                         
                         - 
                         
                           
                             v 
                              
                             
                               ( 
                               
                                 k 
                                 - 
                                 1 
                               
                               ) 
                             
                           
                            
                           
                             
                               v 
                                
                               
                                 ( 
                                 
                                   k 
                                   - 
                                   1 
                                 
                                 ) 
                               
                             
                             T 
                           
                         
                       
                       ] 
                     
                   
                   ] 
                 
               
                
               
                 
                   W 
                    
                   
                     ( 
                     k 
                     ) 
                   
                 
                 T 
               
             
           
         
       
       
         
           
             
                 
             
              
             
               
                 v 
                  
                 
                   ( 
                   k 
                   ) 
                 
               
               = 
               
                 
                   ∑ 
                   
                     i 
                     = 
                     1 
                   
                   6 
                 
                  
                 
                     
                 
                  
                 
                   
                     
                       β 
                       i 
                     
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                    
                   
                     [ 
                     
                       
                         
                           z 
                           i 
                         
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                       - 
                       
                         z 
                          
                         
                           ( 
                           
                             k 
                             | 
                             
                               k 
                               - 
                               1 
                             
                           
                           ) 
                         
                       
                     
                     ] 
                   
                 
               
             
           
         
       
     
         [0055]    In one embodiment of the present invention, measured SNR information may be used in the PDAF. In that embodiment, the measured amplitude information (CN0 or other amplitude information) can also be used in PDAF. That is, an observed high SNR is more likely come from a correct measurement. There is some statistical difference in the amplitude between correct and false observations.  FIG. 3  shows a linear probability density function  302 , suitable for modeling amplitudes that are valid observations, and an exponential probability density function  301 , suitable for modeling amplitudes of false observations, according to one embodiment of the present invention. 
         [0056]    If the measured amplitude is a i (k), the PDF of the amplitude due to noise may be modeled by a Rayleigh distribution or an exponential distribution (the latter distribution is illustributed by PDF  301  of  FIG. 3 ), 
         [0000]    
       
         
           
             
               
                 
                   p 
                   0 
                 
                  
                 
                   ( 
                   a 
                   ) 
                 
               
               = 
               
                 
                   a 
                   β 
                 
                  
                 
                    
                   
                     - 
                     
                       
                         a 
                         2 
                       
                       
                         2 
                          
                         β 
                       
                     
                   
                 
               
             
             , 
             
               a 
               ≥ 
               0 
             
           
         
       
       
         
           or 
         
       
       
         
           
             
               
                 
                   p 
                   0 
                 
                  
                 
                   ( 
                   a 
                   ) 
                 
               
               = 
               
                 
                   a 
                   β 
                 
                  
                 
                    
                   
                     - 
                     
                       a 
                       β 
                     
                   
                 
               
             
             , 
             
               a 
               ≥ 
               0 
             
           
         
       
     
         [0057]    However, the PDF of the amplitude of a correct observation may be modeled with Rayleigh distribution or a simple linear between certain ranges (the latter distribution is illustributed by PDF  302  of  FIG. 3 ), 
         [0000]    
       
         
           
             
               
                 
                   p 
                   1 
                 
                  
                 
                   ( 
                   a 
                   ) 
                 
               
               = 
               
                 
                   a 
                   
                     β 
                     + 
                     d 
                   
                 
                  
                 
                    
                   
                     - 
                     
                       
                         a 
                         2 
                       
                       
                         2 
                          
                         
                           ( 
                           
                             β 
                             + 
                             d 
                           
                           ) 
                         
                       
                     
                   
                 
               
             
             , 
             
               a 
               ≥ 
               0 
             
           
         
       
       
         
           or 
         
       
       
         
           
             
               
                 p 
                 1 
               
                
               
                 ( 
                 a 
                 ) 
               
             
             = 
             
               
                 2 
                 
                   L 
                   2 
                 
               
                
               a 
             
           
         
       
     
         [0058]    With the amplitude information, the mode probabilities in PDAF are almost in the same form as described above, with only some minor modifications that incorporate the amplitude information. For general multiple observation case, 
         [0000]    
       
         
           
             
               
                 β 
                 i 
               
                
               
                 ( 
                 k 
                 ) 
               
             
             = 
             
               { 
               
                 
                   
                     
                       
                         
                           
                             e 
                             i 
                           
                           
                             b 
                             + 
                             
                               
                                 ∑ 
                                 
                                   i 
                                   = 
                                   1 
                                 
                                 m 
                               
                                
                               
                                   
                               
                                
                               
                                 e 
                                 i 
                               
                             
                           
                         
                       
                       
                         
                           
                             i 
                             = 
                             1 
                           
                           , 
                           2 
                           , 
                           
                             … 
                              
                             
                                 
                             
                              
                             m 
                           
                         
                       
                     
                     
                       
                         
                           b 
                           
                             b 
                             + 
                             
                               
                                 ∑ 
                                 
                                   i 
                                   = 
                                   1 
                                 
                                 m 
                               
                                
                               
                                   
                               
                                
                               
                                 e 
                                 i 
                               
                             
                           
                         
                       
                       
                         
                           
                             i 
                             = 
                             0 
                           
                           , 
                           false 
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   Where 
                    
                   
                     
 
                   
                    
                   
                     
                       e 
                       i 
                     
                      
                     
                       ( 
                       k 
                       ) 
                     
                   
                 
                 = 
                 
                   
                      
                     
                       
                         - 
                         
                           1 
                           2 
                         
                       
                        
                       
                         
                           v 
                            
                           
                             ( 
                             k 
                             ) 
                           
                         
                         ′ 
                       
                        
                       
                         
                           S 
                            
                           
                             ( 
                             k 
                             ) 
                           
                         
                         
                           - 
                           1 
                         
                       
                        
                       
                         v 
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                     
                   
                    
                   
                     
                       
                         p 
                         1 
                       
                        
                       
                         ( 
                         
                           a 
                           i 
                         
                         ) 
                       
                     
                     
                       
                         p 
                         0 
                       
                        
                       
                         ( 
                         
                           a 
                           i 
                         
                         ) 
                       
                     
                   
                 
               
             
           
         
       
     
         [0059]    Any other parameter a i  can be associated in the likelihood ratio 
         [0000]    
       
         
           
             
               
                 p 
                 1 
               
                
               
                 ( 
                 
                   a 
                   i 
                 
                 ) 
               
             
             
               
                 p 
                 0 
               
                
               
                 ( 
                 
                   a 
                   i 
                 
                 ) 
               
             
           
         
       
     
         [0000]    to differentiate the correct observation from the false one. 
         [0060]    According to another embodiment of the present invention, false measurements due to multipath may be rejected by correctly modeling the multipath-based false observations. Observation residuals due to multipath are more positive than normal observation residuals, the distribution of a false observation can thus be modeled, for example, to be uniformly distributed on the negative side and linearly increasing on the positive side. Such a distribution is illustrated in  FIG. 4  by PDF  401 , for example. If the residual is negative with probability of 20% and positive with probability of 80%, the PDF for false observations may be provided by: 
         [0000]    
       
         
           
             
               p 
               0 
             
             = 
             
               { 
               
                 
                   
                     
                       1 
                       
                         5 
                          
                         
                           
                              
                             
                               γ 
                                
                               
                                   
                               
                                
                               
                                 S 
                                  
                                 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                             
                              
                           
                           
                             1 
                             / 
                             2 
                           
                         
                       
                     
                   
                   
                     
                       
                         v 
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                       &lt; 
                       0 
                     
                   
                 
                 
                   
                     
                       
                         1 
                         
                           5 
                            
                           
                             
                                
                               
                                 γ 
                                  
                                 
                                     
                                 
                                  
                                 
                                   S 
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                               
                                
                             
                             
                               1 
                               / 
                               2 
                             
                           
                         
                       
                       + 
                       
                         
                           6 
                           5 
                         
                          
                         
                           
                             v 
                              
                             
                               ( 
                               k 
                               ) 
                             
                           
                           
                              
                             
                               γ 
                                
                               
                                   
                               
                                
                               
                                 S 
                                  
                                 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                             
                              
                           
                         
                       
                     
                   
                   
                     
                       
                         v 
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                       ≥ 
                       0 
                     
                   
                 
               
             
           
         
       
     
         [0061]    Thus, when the GPS receiver is not sufficiently modeled by a single model, the IMM estimator allows the GPS receiver to be modeled by multiple models simultaneously. In addition, when there is a probability that some false observations may be incorporated in the GPS navigation calculations, a PDAF improves over a conventional Kalman filter. A possible application of an IMM estimator and PDAFs in GPS navigation is a dynamically changing environment in which a constant velocity model accurately describes the receiver&#39;s motion only a portion of the time, while one or more other models incorporating accelerations (e.g., acceleration, deceleration and turns). 
         [0062]    Another example for the IMM estimator is a multipath environment in which the IMM estimator uses one model which does not incorporate multipath effects, and another model which incorporates multipath effects. When multipath effects are not present, the first model dominates and provides the navigation output most of the time. When multipath effects are present, the IMM estimator gives greater weight to the multipath model which has a large measurement variance, and may incorporate a multipath false measurement model in a PDAF. Conventional methods can not handle such a situation because multipath effects appear almost randomly. 
         [0063]    Yet another example for an IMM estimator and PDAF filters is an environment with very low SNR and with false observations. When false observations are frequent due to a very low SNR, multipath effects, or other effects in a hostile environment to GPS navigation, the PDAF algorithm with some proper statistics models out perform conventional Kalman filters. 
         [0064]    The above detailed description is provided to illustrate specific embodiments of the present invention and is not intended to be limiting. Numerous variations and modifications within the scope of the present invention are possible. The present invention is set forth in the following claims.