Systems and methods for stabilizing magnetic field of inertial measurement unit
A method for stabilizing a magnetic field of an inertial measurement unit (IMU), is provided that includes initializing accelerometer and gyroscope (AG) heading data for the IMU and initializing accelerometer, gyroscope and magnetometer (AGM) heading data for the IMU. Determining whether a tracking state exists and completing processing of the AG heading data and the AGM heading data if the tracking state does not exist. Calculating a magnetic field error if the tracking state exists.
1. A method for stabilizing a magnetic field of an inertial measurement unit (IMU), comprising:
initializing accelerometer and gyroscope (AG) heading data for the IMU;
initializing accelerometer, gyroscope and magnetometer (AGM) heading data for the IMU;
determining whether a tracking state exists using the AG heading data and the AGM heading data;
completing processing of the AG heading data and the AGM heading data if the tracking state does not exist; and
calculating a magnetic field error if the tracking state exists.
2. The method of claim 1 further comprising calculating a drift as a function of the magnetic field error.
3. The method of claim 2 further comprising completing processing of the AG heading data and the AGM heading data if the drift output does not exist.
4. The method of claim 2 further comprising calculating a magnetic field disruption if a drift output exists.
5. The method of claim 4 further comprising correcting the drift if the magnetic field disruption exists.
6. The method of claim 4 further comprising correcting the AG heading data and the AGM heading data if the magnetic field disruption exists.
7. The method of claim 4 further comprising determining if a recovery state exists if the magnetic field disruption does not exist.
8. The method of claim 7 further comprising correcting the drift if the recovery state does not exist.
9. A system for stabilizing a magnetic field of an inertial measurement unit (IMU), comprising:
a processor operating under algorithmic control, the processor configured to perform the steps of:
initialize accelerometer and gyroscope (AG) heading data for the IMU;
initialize accelerometer, gyroscope and magnetometer (AGM) heading data for the IMU;
determine whether a tracking state exists using the AG heading data and the AGM heading data;
complete processing of the AG heading data and the AGM heading data if the tracking state does not exist; and
calculate a magnetic field error if the tracking state exists.
10. The system of claim 9 wherein the processor is further configured to calculate a drift as a function of the magnetic field error.
11. The system of claim 10 wherein the processor is further configured to complete processing of the AG heading data and the AGM heading data if the drift output does not exist.
12. The system of claim 10 wherein the processor is further configured to calculate a magnetic field disruption if the drift output exists.
13. The system of claim 12 wherein the processor is further configured to correct the drift if the magnetic field disruption exists.
14. The system of claim 12 wherein the processor is further configured to correct the AG heading data and the AGM heading data if the magnetic field disruption exists.
15. The system of claim 12 wherein the processor is further configured to determine if a recovery state exists if the magnetic field disruption does not exist.
16. The system of claim 15 wherein the processor is further configured to correct the drift if the recovery state does not exist.