Patent ID: 11927446
Assignee: SOUTHEAST UNIVERSITY
Field: Measurement (Instruments)
Classification: CPC G  B | IPC B  G

Claim 2:
3. An up floating error correction method for a navigation and positioning system for an underwater glider, comprising the following steps:
(1) determining whether an underwater glider needs to perform an up floating error correction operation or not, and when a heading variation is large, a velocity variation is large, or manually determining that a navigation and positioning error of the underwater glider is large, floating, by the underwater glider, up to a water surface, to perform the up floating error correction operation;
(2) receiving, after the underwater glider floats up to the water surface, a longitude and latitude signal, an altitude signal, and a triaxial velocity signal from a global positioning system (GPS) receiving module, and a triaxial acceleration signal and a triaxial angular velocity signal from an inertial measurement unit (IMU), and applying an H∞ Kalman filter algorithm based on an adaptive multiple fading factor to an integrated navigation system for a correction of GPS(global positioning system)/inertial navigation system (INS) location and velocity errors, to perform a data fusion; and
(3) reducing, through step (2), velocity and location errors of the integrated navigation system gradually to approach zero, and if the velocity and location errors are smaller than specified thresholds, indicating that the up floating error correction is completed, switching, by the underwater glider, to an underwater working state,
wherein a state prediction covariance matrix of the H∞ Kalman filter algorithm based on the adaptive multiple fading factor is Pk|k-1=SkΦk|k-1Pk-1Φk|5−1TSkT+Qk-1,
an H∞ filter gain matrix is Kk=Pk|k-1HkT(HkPk|k-1HkT+I)−1 and an H∞ filter state optimal covariance matrix is, P
    k
   
   =
   
    
     P
     
      k
      |
      
       k
       -
       1
      
     
    
    -
    
     
      Φ
      
       k
       |
       
        k
        -
        1
       
      
     
     ⁢
     
      
       P
       
        k
        |
        
         k
         -
         1
        
       
      
      [
      
       
        
         
          H
          k
          T
         
        
        
         
          L
          k
          T
         
        
       
      
      ]
     
     ⁢
     
      
       R
       
        e
        ,
        k
       
       
        -
        1
       
      
      [
      
       
        
         
          H
          k
         
        
       
       
        
         
          L
          k
         
        
       
      
      ]
     
     ⁢
     
      P
      
       k
       |
       
        k
        -
        1
       
      
     
     ⁢
     
      Φ
      
       k
       |
       
        k
        -
        1
       
      
      T
     
    
   
  
  ,, wherein k and k-1 represent a current moment and a previous moment respectively, Φk|k-1 is a state-transition matrix, Hk is an observation matrix, Qk-1 is a system noise covariance, Sk is an adaptive multiple fading factor matrix, Lk is an estimate of a linear combination of system state variables, I is a unit matrix,, R
    
     e
     ,
     k
    
   
   =
   
    
     [
     
      
       
        I
       
       
        0
       
      
      
       
        0
       
       
        
         
          -
          
           γ
           2
          
         
         ⁢
         I
        
       
      
     
     ]
    
    +
    
     
      [
      
       
        
         
          H
          k
         
        
       
       
        
         
          L
          k
         
        
       
      
      ]
     
     ⁢
     
      
       P
       
        k
        |
        
         k
         -
         1
        
       
      
      [
      
       
        
         
          H
          k
          T
         
        
        
         
          L
          k
          T
         
        
       
      
      ]
     
    
   
  
  ,, and γ is an adaptive threshold,
wherein the adaptive multiple fading factor matrix Sk=diag (s1, s2, s3 . . . , sn) is calculated by using the following formula:, s
   i
  
  =
  
   {
   
    
     
      
       
        
         max
         ⁡
         (
         
          1
          ,
          
           
            
             
              
               [
               
                
                 υ
                 i
                
                (
                k
                )
               
               ]
              
              2
             
             
              
               λ
               i
               2
              
              ⁢
              
               
                j
                ii
               
               (
               k
               )
              
              ⁢
              
               ε
               i
              
             
            
            -
            
             
              
               b
               ii
              
              (
              k
              )
             
             
              
               j
               ii
              
              (
              k
              )
             
            
           
          
         
         )
        
        ,
       
      
      
       
        observable
        ⁢
           
        variables
       
      
     
     
      
       
        1
        ,
       
      
      
       
        unobservable
        ⁢
           
        variables
       
      
     
    
    ,
   
  
 

wherein λi is an ith observation element of the observation matrix Hk-jii (k) is an ith diagonal element of a matrix is a Jk=Φk|k-1Pk-1Φk|k-1Tεi threshold of a Chi-square test, ui (k) is an ith diagonal element of an innovation matrix Vk, and bii (k) is an ith diagonal element of a matrix Bk=HkQk-1HkT+Rk 
wherein the adaptive threshold is γ=ƒ·γa, wherein, η
   =
   
    
     
      
       V
       k
       T
      
      ⁢
      
       V
       k
      
     
     +
     
      Trace
      ⁢
      
       (
       
        
         
          H
          k
         
         ⁢
         
          P
          
           k
           |
           
            k
            -
            1
           
          
         
         ⁢
         
          H
          k
          T
         
        
        +
        
         R
         
          k
          -
          1
         
        
       
       )
      
     
    
    
     
      V
      k
      T
     
     ⁢
     
      V
      k
     
    
   
  
  ,, γa=ρ(LkTLk(Pk−1+HkTHk)−1), Trace( ) represents a matrix trace operation, and ρ( ) represents a spectral radius of a matrix.