Abstract:
The design of the low cost GPS/IMU positioning and data integrating method, which employs integrated global positioning system/inertial measurement unit enhanced with dual antenna GPS carrier phase measurements to initialize and stabilize the azimuth of the low cost GPS/IMU integrated system, is performed. The utilization of the raw carrier phase measurement for the integration speeds up the ambiguity search.

Description:
CROSS REFERENCE OF RELATED APPLICATION  
       [0001]     This is the regular application of a provisional application having an application No. 60/503,975 and a filing date of Sep. 20, 2003. 
     
    
       [[0002]]     The present invention was made with Government support under Contract No. NAS13-02051 awarded by the U.S. NASA/John C. Stennis Space Center, Stennis Space Center, Miss.  39529 - 6000 . The Government has certain rights in the present invention. 
     
    
     BACKGROUND OF THE PRESENT INVENTION  
     TECHNICAL FIELD  
       [0003]     1. Field of Invention  
         [0004]     The present invention relates generally to the design of dual antenna GPS/INS integrated systems, and more particularly to the low cost GPS/IMU positioning and data integrating method, which employs integrated global positioning system/inertial measurement unit enhanced with dual antenna GPS carrier phase measurements to initialize and stabilize the azimuth of the low cost GPS/IMU integrated system. The utilization of the raw carrier phase measurements for the integration speeds up the ambiguity search.  
         [0005]     2. Description of Related Arts  
         [0006]     Due to the high cost of high accuracy INS (inertial navigation system) and the errors accumulation, the applications of INS are restricted. GPS can provide position, velocity, pseudorange, carrier phase and ephemeris without errors accumulation. It has been found that GPS/INS integration is superior to each one working alone.  
         [0007]     Advancement in the technology of the low cost solid-state inertial sensors has reduced the price of a full inertial sensor assembly. So far, the single frequency GPS OEM board is very cheap.  
         [0008]     Inertial navigation systems, in principle, permit pure autonomous operation and output continuous position, velocity, and attitude data of vehicle after initializing the starting position and initiating an alignment procedure. In addition to autonomous operation, other advantages of inertial navigation systems include the full navigation solution and wide bandwidth. However, an inertial navigation system is expensive and subject to drift over an extended period of time. This means that the position error increases with time. This error propagation characteristic is primarily caused by its inertial sensor error sources, such as gyro drift, accelerometer bias, and scale factor errors.  
       SUMMARY OF THE PRESENT INVENTION  
       [0009]     It is a main objective of the present invention to provide a dual antenna GPS/IMU integrated method and system thereof, in which a low cost IMU and GPS integrated system can provide high accuracy attitude, azimuth, position and velocity, wherein a micro IMU (such as the U.S. Pat. No. 6,456,939 entitled “Micro Inertial Measurement Unit” and U.S. Pat. No. 6,522,992 entitled “Core Inertial Measurement Unit”) is incorporated. The navigation fusion method involved U.S. Pat. No. 6,278,945 entitled “Fully-Coupled Positioning Process and System Thereof”, U.S. Pat. No. 6,292,750 entitled “Vehicle Positioning Method and System Thereof”, U.S. Pat. No. 6,311,129 entitled “Positioning Process and System Thereof”, U.S. Pat. No. 6,408,245 entitled “Filtering Mechanization Method of Integrating Global Positioning System with Inertial Measurement Unit”, U.S. Pat. No. 6,415,223 entitled “Interruption-Free Hand-Held Positioning Method and System Thereof”, U.S. Pat. No. 6,424,914 entitled “Fully-Coupled Vehicle Positioning Method and System Thereof”, U.S. Pat. No. 6,427,122 entitled “Positioning and Data Integrating Method and System Thereof”, U.S. Pat. No. 6,449,559 entitled “Fully-Coupled Positioning Process and System Thereof”, U.S. Pat. No. 6,480,152 entitled “Integrated GPS/IMU Method and Microsystem Thereof”, U.S. Pat. No. 6,510,354 entitled “Universal Robust Filtering Process”, U.S. Pat. No. 6,738,714 entitled “Vehicles Self-Carried Positioning Method and System Thereof”, U.S. Pat. No. 6,658,354 entitled “Interruption Free Navigator”, U.S. Pat. No. 6,240,367 entitled “Full Fusion Positioning Method for Vehicle” and U.S. Pat. No. 6,246,960 entitled “Vehicle Fully-Coupled GPS/IMU with Radar Altimeter”. The above list patents are by Ching-Fang Lin.  
         [0010]     Another objective of the present invention is to provide a dual antenna GPS/IMU integrated method and system thereof, in which dual antenna GPS can provide high accuracy elevation, azimuth, position and velocity.  
         [0011]     The present invention can substantially solve the cost problems encountered in avionics system integration by using state-of-the-art inertial sensor, integrated dual antenna global positioning system. The present invention is to balance multiple requirements imposed on the modern avionics systems design and manufacturing: low cost, high accuracy, reliability, small size and weight, low power consumption, ease of operation and maintenance, and ease of modification.  
         [0012]     These and other objectives, features, and advantages of the present invention will become apparent from the following detailed description, the accompanying drawings, and the appended claims.  
     
    
     BRIEF DESCRIPTION OF THE DRAWINGS  
       [0013]      FIG. 1  is a block diagram illustrating a dual antenna GPS/IMU integrated method and system, an inertial measurement unit (IMU), two GPS rovers (B GPS and C GPS), two GPS rover antennas (B antenna and C antenna), a rover data link, a GPS raw data collector, Centralized Processing module, a GPS base station (GPS A), a GPS base station antenna and a GPS base station data link, according to the present invention.  
         [0014]      FIG. 2  is a block diagram of a centralized processing module in the ambiguity search stage according to the present invention.  
         [0015]      FIG. 3  is a block diagram of a centralized processing module in the ambiguity fix stage according to the present invention.  
         [0016]      FIG. 4  is a block diagram of a centralized processing module for elevation and azimuth match in the ambiguity search stage according to the present invention.  
         [0017]      FIG. 5  is a block diagram of a centralized processing module for elevation and azimuth match in the ambiguity fix stage according to the present invention.  
     
    
     DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT  
       [0018]     Generally, the accuracy of inertial navigation systems (INS) can be improved by employing highly accurate inertial sensors or by compensating with data from an external sensor. The cost of developing and manufacturing inertial sensors increases as the level of accuracy improves. The advances in new inertial sensor technology and electronic technologies have led to the availability of low cost inertial sensors, such as mechanical-electronic-micro-system (MEMS) inertial sensors. Mechanical-electronic-micro-system inertial sensors borrow processes from the semiconductor industry to fabricate tiny sensors and actuators on silicon chips. The precision of these new inertial sensors may be less than what conventional sensors achieve, but they have enormous cost, size, weight, thermal stability and wide dynamic range advantages over conventional inertial sensors.  
         [0019]     The present invention employs an integrated global positioning system (GPS)/inertial measurement unit using three GPS carrier phase measurements to provide highly accurate and continuous positioning of a vehicle, highly accurate attitude determination, and platform body acceleration and rotation data, as well as time data output. These data are managed and dispensed by a control board. The advantages of the present invention include:  
         [0020]     (1) The inertial navigation system has high accuracy short term positioning capability, but is subject to long term drift leading to a poor long term navigation solution. The global positioning system (GPS) differential carrier phase positioning has long term highly accurate navigation performance, which can reach centimeter accuracy. Through integration of these two disparate systems, it is expected to obtain a high accuracy long term and short term navigation solution. In the positioning, the inertial navigation system aids the GPS to search for integer ambiguities very fast, while differential carrier phase GPS compensates the inertial navigation system drift to reach centimeter positioning accuracy.  
         [0021]     (2) The gyros of the low cost inertial navigation system can not sense earth rotation for the initial alignment. It can not provide the azimuth by itself. The dual antenna GPS carrier phase measurements aid the inertial navigation system for the initial alignment. The inertial navigation system measurements increase the dual antenna GPS carrier phase measurement ambiguity search.  
         [0022]     (3) The GPS signal is easy to block, which causes cycle slips of the GPS carrier phase measurement. The short term high accuracy of the inertial navigation system aids the GPS carrier phase measurements to detect the cycle slips.  
         [0023]     Referring to  FIG. 1 , a dual antenna GPS/IMU integrated method and system is composed of an inertial measurement unit (IMU)  1 , two GPS rovers (B GPS  2  and C GPS  4 ), two GPS rover antennas (B antenna  3  and C antenna  5 ), a rover data link  8 , a GPS raw data collector  6 , Centralized Processing module  7 , a GPS base station (GPS A  9 ), a GPS base station antenna  10  and a GPS base station data link  11 .  
         [0024]     Referring to  FIG. 2 , a Centralized Processing module  7  is composed of Navigation Solution module  71 , Kalman Filter Integration module  73 , and Ambiguity Solution module  72 .  
         [0025]     In the Kalman Filter Integration module  73 , state equation and measurement equation are required. The state dynamic models and measurement models are in the following.  
         [0026]     Tilt Angle Error Model  
                 ϕ   .     N     =       ⁢         δ   ⁢           ⁢     V   E           R   n     +   h       -       ω   ie     ⁢   sin   ⁢           ⁢   L   ⁢           ⁢   δ   ⁢           ⁢   L     -                     ⁢         (         ω   ie     ⁢   sin   ⁢           ⁢   L     +         V   E         R   n     +   h       ⁢   tan   ⁢           ⁢   L       )     ⁢     ϕ   E       +         V   N         R   m     +   h       ⁢     ϕ   D       -     ɛ   N                       ϕ   .     E     =       ⁢         δ   ⁢           ⁢     V   N           R   m     +   h       +       (         ω   ie     ⁢   sin   ⁢           ⁢   L     +         V   E         R   n     +   h       ⁢   tan   ⁢           ⁢   L       )     ⁢     ϕ   N       +                     ⁢         (         ω   ie     ⁢   cos   ⁢           ⁢   L     +       V   E         R   n     +   h         )     ⁢     ϕ   D       -     ɛ   E                       ϕ   .     D     =       ⁢         -       δ   ⁢           ⁢     V   E           R   n     +   h         ⁢   tan   ⁢           ⁢   L     +       (         ω   ie     ⁢   cos   ⁢           ⁢   L     +         V   E         R   n     +   h       ⁢     sec   2     ⁢   L       )     ⁢   δ   ⁢           ⁢   L     -                     ⁢         (         ω   ie     ⁢   cos   ⁢           ⁢   L     +       V   E         R   n     +   h         )     ⁢     ϕ   E       -         V   N           R   m     +   h     ⁢               ⁢     ϕ   N       -     ɛ   D                 
 
 Velocity Error Model  
               δ   ⁢           ⁢       V   .     N       =       ⁢         -     f   D       ⁢     ϕ   E       +       f   E     ⁢     ϕ   D       -     2   ⁢     (         ω   ie     ⁢   sin   ⁢           ⁢   L     +         V   E         R   n     +   h       ⁢   tan   ⁢           ⁢   L       )     ⁢   δ   ⁢           ⁢     V   E       +                     ⁢           V   D         R   m     +   h       ⁢   δ   ⁢           ⁢     V   N       -       (       2   ⁢           ⁢     ω   ie     ⁢   cos   ⁢           ⁢   L     +         V   E         R   n     +   h       ⁢     sec   2     ⁢   L       )     ⁢     V   E     ⁢   δ   ⁢           ⁢   L     +                     ⁢           V   N         R   m     +   h       ⁢   δ   ⁢           ⁢     V   D       +     ∇   N                     δ   ⁢           ⁢       V   .     E       =       ⁢         f   N     ⁢     ϕ   D       +       f   D     ⁢     ϕ   N       +       (           V   N         R   m     +   h       ⁢   tan   ⁢           ⁢   L     +       V   D         R   m     +   h         )     ⁢   δ   ⁢           ⁢     V   E       +                     ⁢         (       2   ⁢           ⁢     ω   ie     ⁢   sin   ⁢           ⁢   L     +         V   E         R   n     +   h       ⁢   tan   ⁢           ⁢   L       )     ⁢   δ   ⁢           ⁢     V   N       +                     ⁢         (       2   ⁢           ⁢     ω   ie     ⁢   cos   ⁢           ⁢     LV   N       +           V   E     ⁢     V   N           R   n     +   h       ⁢     sec   2     ⁢   L     -     2   ⁢     ω   ie     ⁢   sin   ⁢           ⁢     LV   D         )     ⁢   δ   ⁢           ⁢   L     +     ∇   E                         δ   ⁢           ⁢   V     .     D     =       ⁢         -     f   E       ⁢     ϕ   N       +       f   N     ⁢     ϕ   E       -     2   ⁢     (         ω   ie     ⁢   cos   ⁢           ⁢   L     +       V   E         R   n     +   h         )     ⁢   δ   ⁢           ⁢     V   E       -                     ⁢       2   ⁢       V   N         R   m     +   h       ⁢   δ   ⁢           ⁢     V   N       +     2   ⁢           ⁢     ω   ie     ⁢   sin   ⁢           ⁢     LV   E     ⁢   δ   ⁢           ⁢   L     +     ∇   D                 
 
 Position Error Model  
         δ   ⁢           ⁢     L   .       =       δ   ⁢           ⁢     V   N           R   m     +   h           
         δ   ⁢           ⁢     λ   .       =           δ   ⁢           ⁢     V   E           R   n     +   h       ⁢   sec   ⁢           ⁢   L     +         V   E         R   n     +   h       ⁢   sec   ⁢           ⁢   L   ⁢           ⁢   tan   ⁢           ⁢   L   ⁢           ⁢   δ   ⁢           ⁢   L           
         δ   ⁢           ⁢     h   .       =       -   δ     ⁢           ⁢     V   D           
 
 where R m =R e (1−2e+3e sin 2 L); R n =R e (1+e sin 2 L); R e =637817; e=1/298.257; where φ N , φ E  and φ D  are north, east and down tilt angles respectively; f N , f E  and f D  are north, east and down specific forces respectively; V N , V E  and V D  are north, east and down velocities respectively; δV N , δV E  and δV D  are north, east and down velocity errors respectively; ω ie  is earth rate; L, λ and h are latitude, longitude and altitude respectively; δL, δλ and δh are latitude, longitude and altitude errors respectively; ε N , ε E  and ε D  are north, east and down gyro drifts in the navigation frame, respectively; and ∇ N , ∇ E  and ∇ D  are north, east and down accelerometer errors in the navigation frame, respectively. 
 
         [0030]     Gyroscope Drift Error Model  
         [0031]     Suppose the gyro drift model of the three axes is the same.  
             ɛ   .     b     =         -     1     T   r         ⁢     ɛ   b       +     w   g         ;       
        where T r  is time constant; ε b  is a first order Markov process; and w g  is white noise.        
 
         [0033]     Accelerometer Error Model  
         [0034]     Suppose the accelerometer error model of the three axes is the same.  
             ∇   .     b     ⁢     =       -     1     T   a         ⁢       ∇   b     ⁢     +     w   a               ;       
 
 where T a  is time constant; ∇ b  is a first order Markov process; and w a  is white noise. 
 
         [0036]     Measurement Model  
         [heading-0037]     Measurement equation is formulated with carrier-phase of receiver A, receiver B, and receiver C, pseudorange-rate of receiver A and receiver B. Double difference technique is involved in the measurement.  
         [0038]     State and measurement equations are derived from the above state dynamic model and measurement model. 
 
 {dot over (X)}=FX+Gw  
 
 Y=HX+Mv  
 
 Where, X T =[φ N  φ E  φ D  δV N  δV E  δV D  δL δλ δh ε bx  ε by  ε bz  ∇ x  ∇ y  ∇ z  N 21    AB  N 31    AB  . . . N m1   B  N 21    CB  N 31    CB  . . . N m1   CB ]
 
 Y   T =└Δρ 21   AB  Δρ m1   AB  Δ{dot over (ρ)} 21   AB  Δ{dot over (ρ)} 31   AB  . . . Δ{dot over (ρ)} m1   AB  Δρ 21    CB  Δρ 31   CB  . . . Δρ m1   CB ┘
 
 where m is the number of visible satellites; Δρ i1   AB , Δρ i1   CB  are the double difference carrier phase measurements (satellite i to satellite  1 , antenna A to B, antenna C to B); Δ{dot over (ρ)} i1   AB  is the double difference carrier phase-velocity measurement; N i1   AB , N i1   CB  are the ambiguities of the double difference carrier phase measurements; ε bx , ε by  and ε bz  are three axes gyro drifts in the body frame, respectively; and ∇ x , ∇ y  and ∇ z  are three axes accelerometer errors in the navigation frame, respectively.  
       F   =         [           F   11           F   12         0       0           0         F   22         0       0           0       0       0       0           0       0       0       0         ]     ⁢           ⁢     F   12       =     [           C   b   n           0     3   ×   3                 0     3   ×   3             C   b   n           ]           
 
 F 11  is coefficient matrix of inertial error state equation.  
         F   22     =     Diag   ⁡     [           -     1     T   rx               -     1     T   ry               -     1     T   rz               -     1     T   ax               -     1     T   ay               -     1     T   az               ]           
       H   =     [           0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3               H   1     ⁢     H   2             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             λ   ⁢           ⁢     I       (     m   -   1     )     ×     (     m   -   1     )                 0       (     m   -   1     )     ×     (     m   -   1     )                   0       (     m   -   1     )     ×   3               H   1     ⁢     C   n   e             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×     (     m   -   1     )               0       (     m   -   1     )     ×     (     m   -   1     )                     H   4     ⁢     C   n   e     ⁢     C   b   n     ⁢     H   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×     (     m   -   1     )               λ   ⁢           ⁢     I       (     m   -   1     )     ×     (     m   -   1     )                 ]         
         H   1     =     [             e   21   AB     -     e   11   AB               e   22   AB     -     e   12   AB               e   23   AB     -     e   13   AB                   e   31   AB     -     e   11   AB               e   32   AB     -     e   12   AB               e   33   AB     -     e   13   AB               ⋮       ⋮       ⋮               e   m1   AB     -     e   11   AB               e   m2   AB     -     e   12   AB               e   m3   AB     -     e   13   AB             ]         
 
 where e i1   AB , e i2   AB , e i3   AB  are three elements of the direction cosine of the vector from satellite I to antenna A or B in the earth center earth frame (ECEF).  
         H   2     =     [             -     (       R   n     +   h     )       ⁢   sin   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ             -     (       R   n     +   h     )       ⁢   cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ           cos   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ                 -     (       R   n     +   h     )       ⁢   sin   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ             (       R   n     +   h     )     ⁢   cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ           cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ                 (           R   n     ⁡     (     1   -   e     )       2     +   h     )     ⁢   cos   ⁢           ⁢   L         0         cos   ⁢           ⁢   L           ]         
         H   3     =     [         0         -     r   CBx   b             r   CBy   b               -     r   CBy   b             r   CBx   b         0             r   CBx   b         0         -     r   CBx   b             ]         
         H   4     =     [             e   21   CB     -     e   11   CB               e   22   CB     -     e   12   CB               e   23   CB     -     e   13   CB                   e   31   CB     -     e   11   CB               e   32   CB     -     e   12   CB               e   33   CB     -     e   13   CB               ⋮       ⋮       ⋮               e   m1   CB     -     e   11   CB               e   m2   CB     -     e   12   CB               e   m3   CB     -     e   13   CB             ]         
 
 where [r CBx   b  r CBy   b  r CBz   b ] is baseline vector (antenna C to B) in body frame.  
             M   =       ⁢     [           M   1         0       0           0         M   1         0           0       0         M   1           ]                   M   1     =       ⁢     [           -   1         1       0       ⋯       0             -   1         0       1       ⋯       0           ⋮       ⋮       ⋮       ⋰       ⋮             -   1         0       0       ⋯       1         ]                 w   =       ⁢     ⌊           w   gx           w   gy           w   gz           w   ax           w   ay           w   az           ⌋                 G   =       ⁢     [           C   b   n           0     3   ×   3                 0     6   ×   3             0     6   ×   3                 0     3   ×   3             0     3   ×   3                 0     3   ×   3             I     3   ×   3                 0     2   ⁢     (     m   -   1     )     ×   3             0     2   ⁢     (     m   -   1     )     ×   3             ]               
 
         [0044]     Referring to  FIG. 3 , a Centralized Processing module  7  is composed of Navigation Solution module  71 , Kalman Filter Integration module  73 , and Cycle Slip Detection module  75 .  
         [0045]     If the ambiguities are fixed all of the estimated ambiguities are known, and are removed from the states. The state equations and measurement equations are in the following:  
         X   .     =     FX   +   Gw         
       Y   =     HX   +   Mw         
       Where   ,     
     ⁢       X   T     =     [           ϕ   N           ϕ   E           ϕ   D           δ   ⁢           ⁢     V   N             δ   ⁢           ⁢     V   E             δ   ⁢           ⁢     V   D             δ   ⁢           ⁢   L           δ   ⁢           ⁢   λ           δ   ⁢           ⁢   h           ɛ   bx           ɛ   by           ɛ   bz           ∇   x           ∇   y           ∇   z           ]           
         Y   T     =     ⌊           Δ   ⁢           ⁢     ρ   21   AB             Δ   ⁢           ⁢     ρ   31   AB           ⋯         Δ   ⁢           ⁢     ρ   m1   AB             Δ   ⁢           ⁢       ρ   .     21   AB             Δ   ⁢           ⁢       ρ   .     31   AB           ⋯         Δ   ⁢           ⁢       ρ   .     m1   AB             Δ   ⁢           ⁢     ρ   21   CB             Δ   ⁢           ⁢     ρ   31   CB           ⋯         Δ   ⁢           ⁢     ρ   m1   CB             ⌋         
       F   =         [           F   11           F   12             0         F   22           ]     ⁢           ⁢     F   12       =     [           C   b   n           0     3   ×   3                 0     3   ×   3             C   b   n           ]           
 
 F 11  is coefficient matrix of inertial error state equation.  
               F   22     =       ⁢     Diag   ⁡     [           -     1     T   rx               -     1     T   ry               -     1     T   rz               -     1     T   ax               -     1     T   ay               -     1     T   az               ]                   H   =       ⁢     [           0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3               H   1     ⁢     H   2             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3                 0       (     m   -   1     )     ×   3               H   1     ⁢     C   n   e             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3                   H   4     ⁢     C   n   e     ⁢     C   b   n     ⁢     H   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             ]                   H   1     =       ⁢     [             e   21   AB     -     e   11   AB               e   22   AB     -     e   12   AB               e   23   AB     -     e   13   AB                   e   31   AB     -     e   11   AB               e   32   AB     -     e   12   AB               e   33   AB     -     e   13   AB               ⋮       ⋮       ⋮               e   m1   AB     -     e   11   AB               e   m2   AB     -     e   12   AB               e   m3   AB     -     e   13   AB             ]               
 
 where e i1   AB , e i2   AB , e i3   AB  are three elements of direction cosine of the vector from satellite I to antenna A or B in earth center earth frame (ECEF).  
         H   2     =     [             -     (       R   n     +   h     )       ⁢   sin   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ             -     (       R   n     +   h     )       ⁢   cos   ⁢           ⁢   L   ⁢           ⁢   s   ⁢           ⁢   in           cos   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ                 -     (       R   n     +   h     )       ⁢   sin   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ             (       R   n     +   h     )     ⁢   cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ           cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ                 (           R   n     ⁡     (     1   -   e     )       2     +   h     )     ⁢   cos   ⁢           ⁢   L         0         cos   ⁢           ⁢   L           ]         
         H   3     =     [         0         -     r   CBx   b             r   CBy   b               -     r   CBy   b             r   CBx   b         0             r   CBx   b         0         -     r   CBx   b             ]         
         H   4     =     [             e   21   CB     -     e   11   CB               e   22   CB     -     e   12   CB               e   23   CB     -     e   13   CB                   e   31   CB     -     e   11   CB               e   32   CB     -     e   12   CB               e   33   CB     -     e   13   CB               ⋮       ⋮       ⋮               e   m1   CB     -     e   11   CB               e   m2   CB     -     e   12   CB               e   m3   CB     -     e   13   CB             ]         
 
 where [r CBx   b  r CBy   b  r CBz   b ] is baseline vector (antenna C to B) in body frame.  
             M   =       ⁢     [           M   1         0       0           0         M   1         0           0       0         M   1           ]                   M   1     =       ⁢     [           -   1         1       0       ⋯       0             -   1         0       1       ⋯       0           ⋮       ⋮       ⋮       ⋰       ⋮             -   1         0       0       ⋯       1         ]                 w   =       ⁢     ⌊       w   gx     ⁢           ⁢     w   gy     ⁢           ⁢     w   gz     ⁢           ⁢     w   ax     ⁢           ⁢     w   ay     ⁢           ⁢     w   az       ⌋                 G   =       ⁢     [           C   b   n           0     3   ×   3                 0     6   ×   3             0     6   ×   3                 0     3   ×   3             0     3   ×   3                 0     3   ×   3             I     3   ×   3             ]               
 
         [0049]     Referring to  FIG. 4 , a Centralized Processing module  7  is composed of Navigation Solution module  71 , Kalman Filter Integration module  73 , Ambiguity Search module  72 , and GPS Dynamic Kalman Filter module  76 .  
         [0050]     In the Kalman Filter Integration module  73 , State and measurement equations are in the following.  
         X   .     =     FX   +   Gw         
       Y   =     HX   +   Mv         
               Where   ,       X   T     =       ⁢     [       ϕ   N     ⁢           ⁢     ϕ   E     ⁢           ⁢     ϕ   D     ⁢           ⁢   δ   ⁢           ⁢     V   N     ⁢           ⁢   δ   ⁢           ⁢     V   E     ⁢           ⁢   δ   ⁢           ⁢     V   D     ⁢           ⁢   δ   ⁢           ⁢   L   ⁢           ⁢   δλ     ⁢                               ⁢     δ   ⁢           ⁢   h   ⁢           ⁢     ɛ   bx     ⁢           ⁢     ɛ   by     ⁢           ⁢     ɛ   bz     ⁢           ⁢       ∇   x     ⁢           ⁢       ∇   y     ⁢           ⁢       ∇   z     ⁢           ⁢     N   21   AB           ⁢           ⁢     N   31   AB     ⁢           ⁢   …   ⁢           ⁢     N   m1   AB       ]           ⁢               
         Y   T     =     ⌊       Δρ   21   AB     ⁢           ⁢     Δρ   31   AB     ⁢           ⁢   …   ⁢           ⁢     Δρ   m1   AB     ⁢           ⁢   Δ   ⁢       ρ   .     21   AB     ⁢           ⁢     Δρ   31   CB     ⁢           ⁢   …   ⁢           ⁢   Δ   ⁢       ρ   .     m1   AB       ⁢           ⌋         
       F   =         [           F   11           F   12         0           0         F   22         0           0       0       0         ]     ⁢           ⁢     F   12       =     [           C   b   n           0     3   ×   3                 0     3   ×   3             C   b   n           ]           
 
 F 11  is coefficient matrix of inertial error state equation.  
               F   22     =       ⁢     Diag   ⁡     [           -     1     T   rx               -     1     T   ry               -     1     T   rz               -     1     T   ax               -     1     T   ay               -     1     T   az               ]                   H   =       ⁢     [           0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3               H   1     ⁢     H   2             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             λ   ⁢           ⁢     I       (     m   -   1     )     ×     (     m   -   1     )                     0       (     m   -   1     )     ×   3               H   1     ⁢     C   n   e             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×     (     m   -   1     )               ]                   H   1     =       ⁢     [             e   21   AB     -     e   11   AB               e   22   AB     -     e   12   AB               e   23   AB     -     e   13   AB                   e   31   AB     -     e   11   AB               e   32   AB     -     e   12   AB               e   33   AB     -     e   13   AB               ⋮       ⋮       ⋮               e   m1   AB     -     e   11   AB               e   m2   AB     -     e   12   AB               e   m3   AB     -     e   13   AB             ]               
 
 where e i1   AB , e i2   AB , e i3   AB  are three elements of direction cosine of the vector from satellite I to antenna A or B in earth center earth frame (ECEF).  
               H   2     =       ⁢     [             -     (       R   n     +   h     )       ⁢   sin   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ             -     (       R   n     +   h     )       ⁢   cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ           cos   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ                 -     (       R   n     +   h     )       ⁢   sin   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ             (       R   n     +   h     )     ⁢   cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ           cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ                 (           R   n     ⁡     (     1   -   e     )       2     +   h     )     ⁢   cos   ⁢           ⁢   L         0         cos   ⁢           ⁢   L           ]                 M   =       ⁢     [           M   1         0       0           0         M   1         0           0       0         M   1           ]                   M   1     =       ⁢     [           -   1         1       0       ⋯       0             -   1         0       1       ⋯       0           ⋮       ⋮       ⋮       ⋰       ⋮             -   1         0       0       ⋯       1         ]                 w   =       ⁢     ⌊           ⁢       w   gx     ⁢           ⁢     w   gy     ⁢           ⁢     w   gz     ⁢           ⁢     w   ax     ⁢           ⁢     w   ay     ⁢           ⁢     w   az       ⌋                 G   =       ⁢     [           C   b   n           0     3   ×   3                 0     6   ×   3             0     6   ×   3                 0     3   ×   3             0     3   ×   3                 0     3   ×   3             I     3   ×   3                 0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             ]               
 
         [0053]     In the GPS Dynamic Kalman Filter module  76 , State and measurement equations are in the following.  
         X   k     =       FX     k   -   1       +     ξ     k   -   1             
         Y   k     =       HX   k     +     η   k           
       Where   ,             ⁢             ⁢       X   k   T     =     ⌊     x   ⁢           ⁢   y   ⁢           ⁢   z   ⁢           ⁢     x   .     ⁢           ⁢     y   .     ⁢           ⁢     z   .     ⁢           ⁢     N   21   CB     ⁢           ⁢     N   31   CB     ⁢           ⁢   …   ⁢           ⁢     N   m1   CB       ⌋           
         Y   k   T     =     ⌊       ρ   21   CB     ⁢           ⁢     ρ   31   CB     ⁢           ⁢   …   ⁢           ⁢     ρ   m1   CB       ⌋         
       F   =     [           I     3   ×   3             0     3   ×   3             0     3   ×     (     m   -   1     )                   Δ   ⁢           ⁢     TI     3   ×   3               I     3   ×   3             0     3   ×   3   ⁢     (     m   -   1     )                   0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             I       (     m   -   1     )     ×     (     m   -   1     )               ]         
       H   =     [                       0     1   ×   3               -   λ     ⁢                                                                       0     1   ×   3                         -   λ                                               ⁢     H   4           ⋯                               ⋰                                     0     1   ×   3                                                 -   λ           ]         
         H   4     =     [             e   21   CB     -     e   11   CB               e   22   CB     -     e   12   CB               e   23   CB     -     e   13   CB                   e   31   CB     ⁢     e   11   CB               e   32   CB     -     e   12   CB               e   33   CB     -     e   13   CB               ⋮       ⋮       ⋮               e   m1   CB     -     e   11   CB               e   m2   CB     -     e   12   CB               e   m3   CB     -     e   13   CB             ]         
 
         [0054]     [x y z] and [{dot over (x)} {dot over (y)} {dot over (z)}] are baseline vector and baseline velocity respectively; ρ i1   CB  is the double difference carrier phase measurements (satellite i to satellite  1 , antenna C to B); and ΔT is sampling period.  
         [0055]     Referring to  FIG. 5 , a Centralized Processing module  7  is composed of Navigation Solution module  71 , Kalman Filter Integration module  73 , Cycle Slip Detection module  75 , and Elevation and Azimuth Solution module  77 .  
         [0056]     If the ambiguities are fixed all of the estimated ambiguities are known, and are removed from the states. The state equations and measurement equations of Kalman Filter Integration module  73  are in the following:  
         X   .     =     FX   +   Gw         
       Y   =     HX   +   Mv         
               Where   ,       X   T     =       ⁢     [       ϕ   N     ⁢           ⁢     ϕ   E     ⁢           ⁢     ϕ   D     ⁢           ⁢   δ   ⁢           ⁢     V   N     ⁢           ⁢   δ   ⁢           ⁢     V   E     ⁢           ⁢   δ   ⁢           ⁢     V   D     ⁢           ⁢   δ   ⁢           ⁢   L   ⁢           ⁢   δλ     ⁢                               ⁢     δ   ⁢           ⁢   h   ⁢           ⁢     ɛ   bx     ⁢           ⁢     ɛ   by     ⁢           ⁢     ɛ   bz     ⁢           ⁢       ∇   x     ⁢           ⁢       ∇   y     ⁢           ⁢     ∇   z           ]           ⁢               
         Y   T     =     ⌊       Δρ   21   AB     ⁢           ⁢     Δρ   31   AB     ⁢           ⁢   …   ⁢           ⁢     Δρ   m1   AB     ⁢           ⁢   Δ   ⁢       ρ   .     21   AB     ⁢           ⁢   Δ   ⁢       ρ   .     31   AB     ⁢           ⁢   …   ⁢           ⁢   Δ   ⁢       ρ   .     m1   AB     ⁢           ⁢   Δψ   ⁢           ⁢   Δθ     ⁢           ⌋         
       F   =         [           F   11           F   12             0         F   22           ]     ⁢           ⁢     F   12       =     [           C   b   n           0     3   ×   3                 0     3   ×   3             C   b   n           ]           
 
 F 11  is coefficient matrix of inertial error state equation.  
               F   22     =       ⁢     Diag   ⁡     [           -     1     T   rx               -     1     T   ry               -     1     T   rz               -     1     T   ax               -     1     T   ay               -     1     T   az               ]                   H   =       ⁢     [           0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3               H   1     ⁢     H   2             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3                 0       (     m   -   1     )     ×   3               H   1     ⁢     C   n   e             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3             0       (     m   -   1     )     ×   3                   H   5     ⁢     C   b   n             0     2   ×   3             0     2   ×   3             0     2   ×   3             0     2   ×   3             ]                   H   1     =       ⁢     [             e   21   AB     -     e   11   AB               e   22   AB     -     e   12   AB               e   23   AB     -     e   13   AB                   e   31   AB     -     e   11   AB               e   32   AB     -     e   12   AB               e   33   AB     -     e   13   AB               ⋮       ⋮       ⋮               e   m1   AB     -     e   11   AB               e   m2   AB     -     e   12   AB               e   m3   AB     -     e   13   AB             ]               
 
 where e i1   AB , e i2   AB , e i3   AB  are three elements of direction cosine of the vector from satellite I to antenna A or B in earth center earth frame (ECEF).  
         H   2     =     [             -     (       R   n     +   h     )       ⁢   sin   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ             -     (       R   n     +   h     )       ⁢   cos   ⁢           ⁢   L   ⁢           ⁢   s   ⁢           ⁢   in           cos   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ                 -     (       R   n     +   h     )       ⁢   sin   ⁢           ⁢   L   ⁢           ⁢   cos   ⁢           ⁢   λ             (       R   n     +   h     )     ⁢   cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ           cos   ⁢           ⁢   L   ⁢           ⁢   sin   ⁢           ⁢   λ                 (           R   n     ⁡     (     1   -   e     )       2     +   h     )     ⁢   cos   ⁢           ⁢   L         0         cos   ⁢           ⁢   L           ]         
         H   5     =     [         0         cos   ⁢           ⁢   γ             -   sin     ⁢           ⁢   γ             0           sin   ⁢           ⁢   γ       cos   ⁢           ⁢   θ               cos   ⁢           ⁢   γ       cos   ⁢           ⁢   θ             ]         
 
 where γ, θ and ψ are roll, pitch and azimuth respectively.  
             M   =       ⁢     [           M   1         0       0           0         M   1         0           0       0         M   1           ]                   M   1     =       ⁢     [           -   1         1       0       ⋯       0             -   1         0       1       ⋯       0           ⋮       ⋮       ⋮       ⋰       ⋮             -   1         0       0       ⋯       1         ]                 w   =       ⁢     ⌊       w   gx     ⁢           ⁢     w   gy     ⁢           ⁢     w   gz     ⁢           ⁢     w   ax     ⁢           ⁢     w     ay   ⁢               ⁢     w   az       ⌋                 G   =       ⁢     [           C   b   n           0     3   ×   3                 0     6   ×   3             0     6   ×   3                 0     3   ×   3             0     3   ×   3                 0     3   ×   3             I     3   ×   3             ]               
 
         [0060]     Referring to  FIGS. 1, 2  and  3 , a first preferred embodiment of the present invention is illustrated, which comprises the steps as follows: 
    1) GPS Raw Data module  6  collects the raw data from GPS B  2  and GPS C  4 , and sends to the GPS Kalman Filter Integration module  73 . The raw data includes pseudorange, Doppler shift, carrier phase and ephemeris.     2) GPS Raw Data module  6  collects the raw data from GPS A  9  by Data Link  8 , and sends to the Kalman Filter Integration module  73 . The raw data includes pseudorange, Doppler shift, carrier phase and ephemeris.     3) Central Processing module  7  receives IMU data from IMU module  1 , and sends to Navigation Solution module  71 .     4) Navigation Solution module  71  receives IMU data and performs navigation solution, and outputs the vehicle&#39;s attitude, velocity and position to the Kalman Filter Integration module.     5) Kalman Filter Integration module receives the vehicle&#39;s attitude, velocity and position from the Navigation Solution module  71  and the GPS raw data from GPS raw data module  6  to construct the coefficient matrices of the Kalman Filter state equations and measurement equations.     6) Kalman Filter Integration module  73  performs Kalman filtering using the state equations and measurement equations to estimate the tilt angle errors, velocity errors, position errors, gyro drifts and accelerometer errors of the Navigation Solution module  71 , and sends to Navigation Solution module  71 . Kalman Filter Integration module  73  also sends estimated ambiguity floating solution to Ambiguity Solution module  72 .     7) Navigation Solution module  71  receives estimated tilt angle errors, velocity errors, position errors, gyro drifts and accelerometer errors, and compensates attitude, velocity, position, gyro measurements and accelerometer measurements while performing navigation solution.     8) Ambiguity Solution module  72  receives estimated ambiguity floating solution and search integer solution. Residual test verifies the integer solution. For the ambiguity of the dual antenna GPS B  2  and GPS C  4  the length of the baseline is used to verify the integer solution.     9) If the ambiguity integer solution is searched to meet the verification criteria the ambiguity is fixed. The state equations and measurement equations of Kalman Filter Integration module are changed. Ambiguity Solution  72  is replaced by Cycle Slip Detection module  75 .     10) Cycle Slip Detection module  75  receives GPS raw data from GPS Raw Data module  6  and position data from Navigation Solution module  71 , performs cycle slip detection, and sends slip cycle to Kalman Filter Integration module  73 .    
 
         [0071]     Referring to  FIGS. 1, 4  and  5 , a first preferred embodiment of the present invention is illustrated, which comprises the steps as follows: 
    1) GPS Raw Data module  6  collects the raw data from GPS B  2  and GPS C  4 , and sends GPS B  2  raw data to the Kalman Filter Integration module  76 . GPS Raw Data module  6  also sends GPS B  2  and GPS C  4  raw data to the GPS Dynamic Kalman Filter module  76 . The raw data includes pseudorange, Doppler shift, carrier phase and ephemeris.     2) GPS Raw Data module  6  collects the raw data from GPS A  9  by Data Link  8 , and sends to the Kalman Filter Integration module  73 . The raw data includes pseudorange, Doppler shift, carrier phase and ephemeris.     3) Central Processing module  7  receives IMU data from IMU module  1 , and sends to Navigation Solution module  71 .     4) Navigation Solution module  71  receives IMU data and performs navigation solution, and outputs the vehicle&#39;s attitude, velocity and position to Kalman Filter Integration module.     5) Kalman Filter Integration module receives the vehicle&#39;s attitude, velocity and position from Navigation Solution module  71  and GPS raw data from GPS raw data module  6  to construct the coefficient matrices of the Kalman Filter state equations and measurement equations.     6) Kalman Filter Integration module  73  performs Kalman filtering using the state equations and measurement equations to estimate the tilt angle errors, velocity errors, position errors, gyro drifts and accelerometer errors of the Navigation Solution module  71 , and sends to Navigation Solution module  71 .     7) Navigation Solution module  71  receives estimated tilt angle errors, velocity errors, position errors, gyro drifts and accelerometer errors, and compensates attitude, velocity, position, gyro measurements and accelerometer measurements while performing navigation solution.     8) GPS Dynamic Kalman Filter  76  receives GPS B  2  and GPS C  4  raw data from GPS Raw Data module  6 , and performs Kalman Filter to estimates the baseline vector, baseline velocity and ambiguity.     9) Ambiguity Solution module  72  receives estimated ambiguity floating solution from Kalman Filter Integration module and GPS Dynamic Kalman Filter module  76 , and search integer solution. Residual test verifies the integer solution. For the ambiguity of the dual antenna GPS B  2  and GPS C  4  the length of the baseline is used to verify the integer solution.     10) If the ambiguity integer solution is searched to meet the verification criteria the ambiguity is fixed. The state equations and measurement equations of Kalman Filter Integration module are changed. Ambiguity Solution  72  is replaced by Cycle Slip Detection module  75 .     11) Cycle Slip Detection module  75  receives GPS raw data from GPS Raw Data module  6  and position data from Navigation Solution module  71 , performs cycle slip detection, and sends slip cycle to Kalman Filter Integration module  73  and Elevation and Azimuth Solution module  77 .     12) Elevation and Azimuth Solution module  77  calculates the azimuth and elevation, and sends to Kalman Filter Integration module  73 .    
 
         [0084]     Kalman Filter Integration module  73  receives GPS raw data from GPS Raw Data module, cycle slip from Cycle Slip Detection module  75 , navigation data from Navigation module  71 , azimuth and elevation from Elevation and Azimuth module  77 , performs Kalman Filter to estimate tilt angle errors, velocity errors, position errors, gyro drifts and accelerometer errors, and sends to Navigation Solution  71  module for compensation.  
         [0085]     One skilled in the art will understand that the embodiment of the present invention as shown in the drawings and described above is exemplary only and not intended to be limiting.  
         [0086]     It will thus be seen that the objects of the present invention have been fully and effectively accomplished. It embodiments have been shown and described for the purposes of illustrating the functional and structural principles of the present invention and is subject to change without departure from such principles. Therefore, this invention includes all modifications encompassed within the spirit and scope of the following claims.