Patent ID: 11912433
Assignee: SOUTHEAST UNIVERSITY
Field: Transport (Mechanical engineering)
Classification: CPC B  G | IPC B  G

Claim 4:
5. The dual-filter-based transfer alignment method under dynamic deformation according to claim 4, wherein in step (4), the model of filter 2 is established with a “speed+angular speed” matching method, the equation of measurement quantities is established by using the expression of speed error and the expression of angular speed error derived in step (3), and a Kalman filter model is established as follows:
the state quantities of the Kalman filter 2 are selected as follows:

x2=[δ{right arrow over (v)}{right arrow over (∇)}s {right arrow over (θ)} {right arrow over ({dot over (θ)})} δ{right arrow over (r)}]T,

wherein, δ{right arrow over (v)} represents the speed error, and {right arrow over (∇)}s represents the zero drift of accelerometer measurement of the subsystem;
the state equation of the filter is:

{dot over (x)}2=F2x2+G2w2,

wherein, G2 represents the system noise distribution matrix of filter 2, w2 represents the system noise of filter 2, and the state transition matrix F2 is expressed as:, F
    2
   
   =
   
    [
    
     
      
       
        F
        11
       
      
      
       
        C
        s
        n
       
      
      
       
        F
        13
       
      
      
       
        F
        14
       
      
      
       
        F
        15
       
      
     
     
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
     
     
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        I
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
     
     
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        B
        1
       
      
      
       
        B
        2
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
     
     
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        F
        53
       
      
      
       
        F
        54
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
     
    
    ]
   
  
  ,
 

wherein,

F11=−[(2{right arrow over (ω)}ien+{right arrow over (ω)}enn)×], F13=R0B2+R0M(B1B2+B2), F14=(2({right arrow over (ω)}imn)R0+R0B1)+R0MB12, F15=[({right arrow over (ω)}imn×)({right arrow over (ω)}imn×)+({right arrow over ({dot over (ω)})}imn×)], F53=R0MB2, F54=R0+R0MB1,

and the system measurement equation is as follows:

y2=H2x2+μ2,

wherein, y2 represents the difference between the true value of speed or angular speed and the estimated value from the filter, μ2 represents the measurement noise in the filter 2, and, H
   2
  
  =
  
   
    [
    
     
      
       
        I
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
     
     
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
      
       
        (
        
         
          
           ω
           →
          
          im
          n
         
         ×
        
        )
       
      
      
       
        I
        
         3
         ×
         3
        
       
      
      
       
        0
        
         3
         ×
         3
        
       
      
     
    
    ]
   
   .