Positioning and navigation method and system thereof

• US 6,697,736 B2
• Filed: 02/06/2002
• Issued: 02/24/2004
• Est. Priority Date: 02/06/2002
• Status: Expired due to Term

• Current Assignee: American GNC Corporation
• Original Assignee: American GNC Corporation
• Inventors: Lin, Ching-Fang
• Primary Examiner(s): Zanelli, Michael J.
• Application Number:
  US10/072,834
• Publication Number:
  US 20030149528A1
• Time in Patent Office: 748 Days
• Field of Search: 701/213, 701/214, 701/216, 701/220, 701/221, 342/357.06, 342/357.11, 342/357.14
• US Class Current: 701/214
• CPC Class Codes: G01C 21/1654 with electromagnetic compass

Abstract

A positioning and navigation method and system thereof can substantially solve the problems encountered in global positioning system-only and inertial navigation system-only, such as loss of global positioning satellite signal, sensibility to jamming and spoofing, and inertial solution'"'"'s drift over time, in which the velocity and acceleration from an inertial navigation processor and an attitude and heading solution from an AHRS processor are used to aid the code and carrier phase tracking of the global positioning system satellite signals, so as to enhance the performance of the global positioning and inertial integration system, even in heavy jamming and high dynamic environments and when the GPS satellite signals are not available.

Claims

1. A positioning and navigation system, comprising:

an IMU (Inertial Measurement Unit) providing inertial measurements including body angular rates and specific forces;

a GPS (Global Positioning System) processor receiving GPS satellite signals to derive position and velocity information and GPS raw measurements including pseudorange, carrier phase, and Doppler shift;

a magnetometer generating an Earth magnetic field measurement; and

a central navigation processor which comprises;

an INS (Inertial Navigation System) processor receiving said inertial measurements, including said body angular rates and said specific forces, for computing an inertial navigation solution which are position, velocity, acceleration, and attitude of a carrier carrying said positioning and navigation system;

a Kalman filter receiving said GPS raw measurements and said inertial navigation solution derived from said INS processor which is blended with said GPS raw measurements to derive a plurality of INS corrections and GPS corrections, wherein said INS corrections are fed back from said Kalman filter to said INS processor to correct said inertial navigation solution; and

an AHRS (Attitude and Heading Reference System) processor receiving said Earth magnetic field measurement from said magnetometer and said inertial measurements from said IMU which is blended with said Earth magnetic field measurement for computing an AHRS solution which are attitude and heading data of said carrier and being outputted as navigation solution when said GPS satellite signals are not available.

2. The system, as recited in claim 1, further comprising an I/O interface which receives said navigation solution from said INS processor to provide navigation data for an on-board system.

3. The system, as recited in claim 1, wherein said AHRS processor comprises an angular increment and velocity increment processing module which receives said body angular rates and specific forces from said IMU to produce three-axis angular increment values and three-axis velocity increment values.

4. The system, as recited in claim 3, wherein said three-axis angular increment values from said angular increment and velocity increment processing module and coarse angular rate bias obtained from an IMU calibration procedure at a high data rate in short interval are connected to a coning correction module for computing coning effect errors in said coning correction module using said three-axis angular increment values and coarse angular rate bias and outputting three-axis coning effect terms and three-axis angular increment values at reduced data rate in long interval, which are called three-axis long-interval angular increment values, into an angular rate compensation module.

5. The system, as recited in claim 4, wherein said coning effect errors and three-axis long-interval angular increment values from said coning correction module and angular rate device misalignment parameters and fine angular rate bias from said IMU calibration procedure are connected to said angular rate compensation module for compensating definite errors in said three-axis long-interval angular increment values using said coning effect errors, angular rate device misalignment parameters, fine angular rate bias, and coning correction scale factor, and outputting said real three-axis angular increments to an alignment rotation vector computation module.

6. The system, as recited in claim 5, wherein said three-axis velocity increment values from said angular increment and velocity increment processing module and acceleration device misalignment, and acceleration device bias from said IMU calibration procedure are connected into an accelerometer compensation module for compensating said definite errors in three-axis velocity increments using said acceleration device misalignment, and accelerometer bias;

outputting said compensated three-axis velocity increments to a level acceleration computation module.

7. The system, as recited in claim 6, wherein by using said compensated three-axis angular increments from said angular rate compensation module, an east damping rate increment from an east damping rate computation module, a north damping rate increment from a north damping rate computation module, and vertical damping rate increment from a vertical damping rate computation module, a quaternion, which is a vector representing rotation angle of said carrier, is updated, and said updated quaternion is connected to a direction cosine matrix computation module for computing said direction cosine matrix, by using said updated quaternion.

8. The system, as recited in claim 7, wherein said computed direction cosine matrix is connected to said level acceleration computation module and an attitude and heading angle extract module for extracting attitude and heading angle as said AHRS solution, using said direction cosine matrix from said direction cosine matrix computation module.

9. The system, as recited in claim 8, wherein said compensated three-axis velocity increments are connected to said level acceleration computation module for computing level velocity increments using said compensated three-axis velocity increments from said acceleration compensation module and said direction cosine matrix from said direction cosine matrix computation module.

10. The system, as recited in claim 9, wherein said level velocity increments are connected to said east damping rate computation module for computing east damping rate increments using said north velocity increment of said input level velocity increments from said level acceleration computation module.

11. The system, as recited in claim 10, wherein said level velocity increments are connected to said north damping rate computation module for computing north damping rate increments using said east velocity increment of said level velocity increments from said level acceleration computation module.

12. The system, as recited in claim 11, wherein said earth magnetic field measurements in three axes from said magnetometer and attitude data from said attitude and heading angle extract module are connected to a magnetic heading computation module to produce a measured magnetic heading angle.

13. The system, as recited in claim 12, wherein said heading angle from said attitude and heading angle extract module and said measured magnetic heading angle from said magnetic heading computation module are connected to said vertical damping rate computation module for computing vertical damping rate increments.

14. The system, as recited in claim 13, wherein said east damping rate increments, north damping rate increments, and vertical damping rate are fed back to said alignment rotation vector computation module to damp said drift of errors of said attitude and heading angles.

15. The system, as recited in claim 2, wherein said AHRS processor comprises an angular increment and velocity increment processing module which receives said body angular rates and specific forces from said IMU to produce three-axis angular increment values and three-axis velocity increment values;

wherein said three-axis angular increment values from said angular increment and velocity increment processing module and coarse angular rate bias obtained from an IMU calibration procedure at a high data rate in short interval are connected to a coning correction module for computing coning effect errors in said coning correction module using said three-axis angular increment values and coarse angular rate bias and outputting three-axis coning effect terms and three-axis angular increment values at reduced data rate in long interval, which are called three-axis long-interval angular increment values, into an angular rate compensation module;

wherein said coning effect errors and three-axis long-interval angular increment values from said coning correction module and angular rate device misalignment parameters and fine angular rate bias from said IMU calibration procedure are connected to said angular rate compensation module for compensating definite errors in said three-axis long-interval angular increment values using said coning effect errors, angular rate device misalignment parameters, fine angular rate bias, and coning correction scale factor, and outputting said real three-axis angular increments to an alignment rotation vector computation module;

wherein said three-axis velocity increment values from said angular increment and velocity increment processing module and acceleration device misalignment, and acceleration device bias from said IMU calibration procedure are connected into an accelerometer compensation module for compensating said definite errors in three-axis velocity increments using said acceleration device misalignment, and accelerometer bias;

outputting said compensated three-axis velocity increments to a level acceleration computation module;

wherein by using said compensated three-axis angular increments from said angular rate compensation module, an east damping rate increment from an east damping rate computation module, a north damping rate increment from a north damping rate computation module, and vertical damping rate increment from a vertical damping rate computation module, a quaternion, which is a vector representing rotation angle of said carrier, is updated, and said updated quaternion is connected to a direction cosine matrix computation module for computing said direction cosine matrix, by using said updated quaternion;

wherein said computed direction cosine matrix is connected to said level acceleration computation module and an attitude and heading angle extract module for extracting attitude and heading angle as said AHRS solution, using said direction cosine matrix from said direction cosine matrix computation module;

wherein said compensated three-axis velocity increments are connected to said level acceleration computation module for computing level velocity increments using said compensated three-axis velocity increments from said acceleration compensation module and said direction cosine matrix from said direction cosine matrix computation module;

wherein said level velocity increments are connected to said east damping rate computation module for computing east damping rate increments using said north velocity increment of said input level velocity increments from said level acceleration computation module;

wherein said level velocity increments are connected to said north damping rate computation module for computing north damping rate increments using said east velocity increment of said level velocity increments from said level acceleration computation module;

wherein said earth magnetic field measurements in three axes from said magnetometer and attitude data from said attitude and heading angle extract module are connected to a magnetic heading computation module to produce a measured magnetic heading angle;

wherein said heading angle from said attitude and heading angle extract module and said measured magnetic heading angle from said magnetic heading computation module are connected to said vertical damping rate computation module for computing vertical damping rate increments;

wherein said east damping rate increments, north damping rate increments, and vertical damping rate are fed back to said alignment rotation vector computation module to damp said drift of errors of said attitude and heading angles.

16. The system, as recited in claim 2 or 15, wherein said central navigation processor further comprises a carrier phase integer ambiguity resolution module, wherein said INS processor provides velocity and acceleration data injecting into a micro-processor of said GPS processor to aid code and carrier phase tracking of GPS satellite signals, wherein an output of said micro-processor of said GPS processor, an output of said INS processor and an output of said Kalman filter are injected into said carrier phase integer ambiguity resolution module to fix global positioning system satellite signal carrier phase integer ambiguity number, wherein said carrier phase integer ambiguity resolution module outputs carrier phase integer number into said Kalman filter to further improve positioning accuracy.

17. The system, as recited in claim 16, wherein said microprocessor of said GPS processor outputs said pseudorange, said Doppler shifts, global positioning system satellite ephemeris, and atmosphere parameters to said Kalman filter.

18. The system, as recited in claim 17, wherein said INS processor processes said inertial measurements, which are said body angular rates and said specific forces, and said position error, velocity error, and attitude error coming from said Kalman filter to derive said corrected navigation solution.

19. The system, as recited in claim 18, wherein said INS processor comprises an IMU I/O interface, an IMU error compensation module, a coordinate transformation computation module, an attitude position velocity computation module, a transformation matrix computation module, and an earth and vehicle rate computation module, wherein said IMU I/O interface collects said signal of said body angular rates and specific forces from said IMU for processing and converting to digital data which are corrupted by said inertial sensor measurement errors to form contaminated data that are passed to said IMU error compensation module, wherein said IMU error compensation module receives sensor error estimates derived from said Kalman filter to perform IMU error mitigation on said IMU data, said corrected inertial data being sent to said coordinate transformation computation module and said transformation matrix computation module, where said body angular rates are sent to said transformation matrix computation module and said specific forces are sent said coordinate transformation computation module, wherein said transformation matrix computation module receives said body angular rates from said IMU error computation module and an earth and vehicle rate from said earth and vehicle rate computation module to perform transformation matrix computation, said transformation matrix computation module sending said transformation matrix to said coordinate transformation computation module and attitude position velocity computation module, an attitude update algorithm in said transformation matrix computation module using said quaternion method because of its advantageous numerical and stability characteristics, wherein said coordinate transformation module collects said specific forces from said IMU error computation module and said transformation matrix from said transformation matrix computation module to perform said coordinate transformation, said coordinate transformation computation sending said specific forces transferred into said coordinate system presented by said transformation matrix to said attitude position velocity computation module, wherein said attitude position velocity computation module receives said transformed specific forces from said coordinate transformation computation module and said transformation matrix from said transformation matrix computation module to perform said attitude, position, velocity update.

20. The system, as recited in claim 19, wherein after computation of said position and velocity, said position and velocity errors calculated by said Kalman filter are used in said attitude position velocity computation module to correct said inertial solution.

21. The system, as recited in claim 20, wherein said attitude errors computed by said Kalman filter is sent to said attitude position velocity computation module to perform attitude correction in said attitude position velocity computation module.

22. The system, as recited in claim 20, wherein said attitude errors computed by said Kalman filter is sent to said transformation matrix computation module to perform said attitude correction before said attitude position velocity computation module.

23. The system, as recited in claim 20, wherein the corrected inertial solution obtained from said attitude position velocity computation module is passed to said Kalman filter to construct said measurements of said Kalman filter, moreover the corrected inertial navigation solution is also sent to said carrier phase integer ambiguity resolution module to aid said global positioning system satellite carrier phase integer ambiguity fixing, and that the corrected velocity and accelerate is passed to microprocessor of said GPS processor to aid said global positioning system satellite signal carrier phase and code tracking, wherein attitude, position, and velocity computed by said attitude position velocity computation module are sent to said earth and vehicle rate computation module to calculate an Earth rotation rate and a vehicle rotation rate which are sent to said transformation matrix computation module, wherein said attitude, position, and velocity information is send to said I/O interface which provides a navigation data source for avionics systems onboard a vehicle.

24. The system, as recited in claim 21, wherein the corrected inertial solution obtained from said attitude position velocity computation module is passed to said Kalman filter to construct said measurements of said Kalman filter, moreover the corrected inertial navigation solution is also send to said carrier phase integer ambiguity resolution module to aid said global positioning system satellite carrier phase integer ambiguity fixing, and that the corrected velocity and accelerate is passed to microprocessor of said GPS processor to aid said global positioning system satellite signal carrier phase and code tracking, wherein attitude, position, and velocity computed by said attitude position velocity computation module are sent to said earth and vehicle rate computation module to calculate an Earth rotation rate and a vehicle rotation rate which are sent to said transformation matrix computation module, wherein said attitude, position, and velocity information is sent to said I/O interface which provides a navigation data source for avionics systems onboard a vehicle.

25. The system, as recited in claim 22, wherein the corrected inertial solution obtained from said attitude position velocity computation module is passed to said Kalman filter to construct said measurements of said Kalman filter, moreover the corrected inertial navigation solution is also sent to said carrier phase integer ambiguity resolution module to aid said global positioning system satellite carrier phase integer ambiguity fixing, and that the corrected velocity and accelerate is passed to microprocessor of said GPS processor to aid said global positioning system satellite signal carrier phase and code tracking, wherein attitude, position, and velocity computed by said attitude position velocity computation module are sent to said earth and vehicle rate computation module to calculate an Earth rotation rate and a vehicle rotation rate which are sent to said transformation matrix computation module, wherein said attitude, position, and velocity information is sent to said I/O interface which provides a navigation data source for avionics systems onboard a vehicle.

26. The system, as recited in claim 25, wherein said Kalman filter is a robust Kalman filter for providing near-optimal performance over a large class of process and measurement models and for blending said GPS measurements and said inertial sensor measurements.

27. The system, as recited in claim 26, wherein said robust Kalman filter comprises a GPS error compensation module for gathering said pseudorange, carrier phase, and Doppler frequency of said GPS measurements from said GPS processor, and said position and velocity corrections from an updating state vector module to perform GPS error compensation to form corrected GPS raw data, including pseudorange, carrier phase, and Doppler frequency, which are sent to a preprocessing module, wherein said preprocessing module receives GPS satellite ephemeris from said GPS processor said corrected GPS raw data from said GPS error compensation module, and INS solutions from said INS processor, said preprocessing module performing calculation of state transit matrix and sending with said state vector to a state vector prediction module, wherein said calculated state transit matrix is also sent to a covariance propagation module which calculates a measurement matrix and a current measurement vector according to a computed measurement matrix and a measurement model, and that said measurement matrix and said computed current measurement vector are passed to a computing measurement residue module, said state vector prediction module receiving said state transit matrix and said state vector from said preprocessing module to perform state prediction of current epoch, said predicted current state vector being passed to said computing measurement residue module which receives predicted current state vector from said state vector prediction module and said measurement matrix and said current measurement vector from said preprocessing module, wherein said computing measurement residue module calculates measurement residues by subtracting said multiplication of said measurement matrix and said predicted current state vector from said current measurement vector, and said measurement residues are sent to a residue monitor module and said updating state vector module, wherein said residue monitor module performs a discrimination on said measurement residues received from said computing measurement residue module, wherein said covariance propagation module gathers covariance of system process from said residue monitor module, said state transit matrix from said preprocessing module, and covariance of estimated error to calculate current covariance of said estimated error which is sent to a computing optimal gain module, wherein said computing optimal gain module receives said current covariance of said estimated error from said covariance computing module to compute optimal gain which is passed to a covariance updating module and said updating state vector module, said covariance updating module updating said covariance of said estimated error and sending to said covariance propagation module, wherein said updating state vector module receives said optimal gain from said computing optimal gain module and said measurement residues from said computing measurement residue module, said updating state vector calculating said current estimate of state vector including position, velocity and attitude errors and sending to said GPS error compensation module and said INS processor.

28. The system, as recited in claim 25, wherein said carrier phase integer ambiguity resolution module collects position and velocity data from said INS processor, said carrier phase and Doppler shift measurement from said microprocessor of said GPS processor, and covariance matrix from said Kalman filter to fix said global positioning system satellite signal integer ambiguity number, wherein after fixing of carrier phase ambiguities, said carrier phase ambiguity number is passed to said Kalman filter to further improve said measurement accuracy of said global positioning system raw data.

29. The system, as recited in claim 28, wherein said carrier phase integer ambiguity resolution module comprises a geometry distance computation module, a least square adjustment module, a satellite clock model, an ionospheric model, a tropospheric model, a satellite prediction module, and a search space determination module, wherein said satellite prediction module collects ephemeris of visible global positioning system satellites from said GPS processor to perform satellite position calculation, a predicted satellite position is passed to said geometry distance computation module which receives a vehicle'"'"'s precision position information from said INS processor and computes a geometrical distance between a satellite and a vehicle that is sent to said least square adjustment module, wherein said tropospheric model collects a time tag from said GPS processor and calculates a tropospheric delay of said global positioning system satellite signal using said embedded tropospheric delay model, in which said calculated troposheric delay is sent to said least square adjustment module, besides said ionospheric model collects said time tag and ionospheric parameters broadcast by said global positioning system satellite from said GPS processor, so that said ionospheric model calculates a minus time delay introduced by said ionosphere that is sent to said least square adjustment module, moreover said satellite clock model collects global positioning system satellite clock parameters to perform satellite clock correction calculation, in which said satellite clock correction is also sent to said least square adjustment module, and that said search space determination module receives covariance matrix of said measurement vector from said Kalman filter, so that based on said covariance matrix, said search space determination module derives said measurement error and determines said global positioning system satellite carrier phase integer ambiguity search space which is sent to said least square adjustment module, wherein said least square adjustment module gathers said geometrical distance from said vehicle to said global positioning system satellite from said geometry distance computation module, said tropospheric delay from said tropospheric model, said ionospheric delay from said ionospheric model, and said satellite clock correction from said satellite clock model to calculate an initial search origin, said least square adjustment module also receiving a search space from said search space determination module wherein a standard least square adjustment algorithm is applied to said initial search origin and said search space to fix said carrier phase ambiguity.

30. A positioning and navigation method, comprising the steps of:

(a) receiving a plurality of global positioning system satellite signals to derive position and velocity information and a plurality of global positioning system (GPS) raw measurements, including pseudorange, carrier phase, and Doppler shift, of a carrier;

(b) sending said GPS raw measurements to a central navigation processor from a GPS (Global Positioning System) processor;

(c) receiving a plurality of inertial measurements including body angular rates and specific forces from a IMU (Inertial Measurement Unit);

(d) seeding said inertial measurements from said IMU to an INS (Inertial Navigation System) processor of said central navigation processor for computing an inertial navigation solution, which are position, velocity, acceleration, and attitude of said carrier;

(e) receiving an Earth magnetic field measurement from a magnetometer;

(f) sending said Earth magnetic field measurement from said magnetometer to an AHRS (Attitude and Heading Reference System) processor of said central navigation processor;

(g) sending said inertial measurements, including said body angular rates and said specific forces, from said IMU to said AHRS processor to blend with said Earth magnetic field measurement for computing an AHRS solution which are attitude and heading data of said carrier;

(h) blending an inertial navigation solution derived from said INS processor and said GPS raw measurements from said GPS processor in a Kalman filter to derive a plurality of INS corrections and GPS corrections; and

(i) feeding back said INS corrections from said Kalman filter to said INS processor to correct said inertial navigation solution.

31. The method, as recited in claim 30 after the step (i), further comprising a step of:

(j) outputting said AHRS solution as navigation data through an I/O interface when said GPS satellite signals are not available.

32. The method, as recited in claim 30, after the step (i), further comprising the steps of:

injecting said GPS raw measurements from said GPS processor, said inertial navigation solution from said INS processor, and said inertial corrections and said GPS corrections from said Kalman filter into a carrier phase integer ambiguity resolution module to fix a plurality of GPS satellite signal carrier phase integer ambiguity numbers; and

sending said GPS satellite signal carrier phase integer numbers from said carrier phase integer ambiguity resolution module to said Kalman filter to derive a further improved navigation solution.

33. The method, as recited in claim 31, after the step (j) further comprising the steps of:

injecting said GPS raw measurements from said GPS processor, said inertial navigation solution from said INS processor, and said inertial corrections and said GPS corrections from said Kalman filter into a carrier phase integer ambiguity resolution module to fix a plurality of GPS satellite signal carrier phase integer ambiguity numbers; and

sending said GPS satellite signal carrier phase integer numbers from said carrier phase integer ambiguity resolution module to said Kalman filter to derive a further improved navigation solution.

34. The method, as recited in claim 30, 31, 32, or 33, wherein said Kalman filter is a robust Kalman filter.

35. The method, as recited in claim 31, wherein said AHRS processor comprises an angular increment and velocity increment processing module which receives said body angular rates and specific forces from said IMU to produce three-axis angular increment values and three-axis velocity increment values;

wherein said three-axis angular increment values from said angular increment and velocity increment processing module and coarse angular rate bias obtained from an IMU calibration procedure at a high data rate in short interval are connected to a coning correction module for computing coning effect errors in said coning correction module using said three-axis angular increment values and coarse angular rate bias and outputting three-axis coning effect terms and three-axis angular increment values at reduced data rate in long interval, which are called three-axis long-interval angular increment values, into an angular rate compensation module;

wherein said coning effect errors and three-axis long-interval angular increment values from said coning correction module and angular rate device misalignment parameters and fine angular rate bias from said IMU calibration procedure are connected to said angular rate compensation module for compensating definite errors in said three-axis long-interval angular increment values using said coning effect errors, angular rate device misalignment parameters, fine angular rate bias, and coning correction scale factor, and outputting said real three-axis angular increments to an alignment rotation vector computation module;

wherein paid three-axis velocity increment values from said angular increment and velocity increment processing module and acceleration device misalignment, and acceleration device bias from said IMU calibration procedure are connected into an accelerometer compensation module for compensating said definite errors in three-axis velocity increments using said acceleration device misalignment, and accelerometer bias;

outputting said compensated three-axis velocity increments to a level acceleration computation module;

wherein by using said compensated three-axis angular increments from said angular rate compensation module, an east damping rate increment from an east damping rate computation module, a north damping rate increment from a north damping rate computation module, and vertical damping rate increment from a vertical damping rate computation module, a quaternion, which is a vector representing rotation angle of said carrier, is updated, and said updated quaternion is connected to a direction cosine matrix computation module for computing said direction cosine matrix, by using said updated quaternion;

wherein said computed direction cosine matrix is connected to said level acceleration computation module and an attitude and heading angle extract module for extracting attitude and heading angle as said AHRS solution, using said direction cosine matrix from said direction cosine matrix computation module;

wherein said compensated three-axis velocity increments are connected to said level acceleration computation module for computing level velocity increments using said compensated three-axis velocity increments from said acceleration compensation module and said direction cosine matrix from said direction cosine matrix computation module;

wherein said level velocity increments are connected to said east damping rate computation module for computing east damping rate increments using said north velocity increment of said input level velocity increments from said level acceleration computation module;

wherein said level velocity increments are connected to said north damping rate computation module for computing north damping rate increments using said east velocity increment of said level velocity increments from said level acceleration computation module;

wherein said earth magnetic field measurements in three axes from said magnetometer and attitude data from said attitude and heading angle extract module are connected to a magnetic heading computation module to produce a measured magnetic heading angle;

wherein said heading angle from said attitude and heading angle extract module and said measured magnetic heading angle from said magnetic heading computation module are connected to said vertical damping rate computation module for computing vertical damping rate increments;

wherein said east damping rate increments, north damping rate increments, and vertical damping rate are fed back to said alignment rotation vector computation module to damp said drift of errors of said attitude and heading angles.

36. A positioning and navigation method, comprising the steps of:

(a) receiving a plurality of inertial measurements including body angular rates and specific forces from an IMU (Inertial Measurement Unit);

(b) sending said inertial measurements from said IMU to an INS (Inertial Navigation System) processor of a central navigation processor for computing an inertial navigation solution, which are position, velocity, acceleration, and attitude of a carrier;

(c) receiving an Earth magnetic field measurement from a magnetometer;

(d) sending said Earth magnetic field measurement from said magnetometer to an AHRS (Attitude and Heading Reference System) processor of said central navigation processor;

(e) sending said inertial measurements from said IMU to said AHRS processor of said central navigation processor to blend with said Earth magnetic field measurement for computing an AHRS solution which are attitude and heading data of said carrier;

(f) sending said inertial navigation solution from said INS processor and said AHRS solution from said AHRS processor to an I/O interface, so as to provide navigation data and attitude and heading data for said carrier;

wherein said AHRS processor comprises an angular increment and velocity increment processing module which receives said body angular rates and specific forces from said IMU to produce three-axis angular increment values and three-axis velocity increment values;

wherein said three-axis angular increment values from said angular increment and velocity increment processing module and coarse angular rate bias obtained from an IMU calibration procedure at a high data rate in short interval are connected to a coning correction module for computing coning effect errors in said coning correction module using said three-axis angular increment values and coarse angular rate bias and outputting three-axis coning effect terms and three-axis angular increment values at reduced data rate in long interval, which are called three-axis long-interval angular increment values, into an angular rate compensation module;

wherein said coning effect errors and three-axis long-interval angular increment values from said coning correction module and angular rate device misalignment parameters and fine angular rate bias from said IMU calibration procedure are connected to said angular rate compensation module for compensating definite errors in said three-axis long-interval angular increment values using said coning effect errors, angular rate device misalignment parameters, fine angular rate bias, and coning correction scale factor, and outputting said real three-axis angular increments to an alignment rotation vector computation module;

wherein said three-axis velocity increment values from said angular increment and velocity increment processing module and acceleration device misalignment, and acceleration device bias from said IMU calibration procedure are connected into an accelerometer compensation module for compensating said definite errors in three-axis velocity increments using said acceleration device misalignment, and accelerometer bias;

outputting said compensated three-axis velocity increments to a level acceleration computation module;

wherein by using said compensated three-axis angular increments from said angular rate compensation module, an east damping rate increment from an east damping rate computation module, a north damping rate increment from a north damping rate computation module, and vertical damping rate increment from a vertical damping rate computation module, a quaternion, which is a vector representing rotation angle of said carrier, is updated, and said updated quaternion is connected to a direction cosine matrix computation module for computing said direction cosine matrix, by using said updated quaternion;

wherein said computed direction cosine matrix is connected to said level acceleration computation module and an attitude and heading angle extract module for extracting attitude and heading angle as said AHRS solution, using said direction cosine matrix from said direction cosine matrix computation module;

wherein said compensated three-axis velocity increments are connected to said level acceleration computation module for computing level velocity increments using said compensated three-axis velocity increments from said acceleration compensation module and said direction cosine matrix from said direction cosine matrix computation module;

wherein said level velocity increments are connected to said east damping rate computation module for computing east damping rate increments using said north velocity increment of said input level velocity increments from said level acceleration computation module;

wherein said level velocity increments are connected to said north damping rate computation module for computing north damping rate increments using said east velocity increment of said level velocity increments from said level acceleration computation module;

wherein said earth magnetic field measurements in three axes from said magnetometer and attitude data from said attitude and heading angle extract module are connected to a magnetic heading computation module to produce a measured magnetic heading angle;

wherein said heading angle from said attitude and heading angle extract module and said measured magnetic heading angle from said magnetic heading computation module are connected to said vertical damping rate computation module for computing vertical damping rate increments;

wherein said east damping rate increments, north damping rate increments, and vertical damping rate are fed back to said alignment rotation vector computation module to damp said drift of errors of said attitude and heading angles.