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, comprising:
retrieving accelerometer and gyroscope (AG) heading data for an inertial measurement unit (IMU) of a device;
retrieving accelerometer, gyroscope, and magnetometer (AGM) heading data for the IMU; and
processing the AG heading data and the AGM heading data to determine an overall heading of the device, including:
in accordance with a determination that the AG heading data indicates that the device is rotating below a threshold angular velocity, using the AGM heading data to correct drift in the AG heading data; and
in accordance with a determination that AG heading data indicates the device is not rotating below the threshold angular velocity, forgoing using the AGM data to correct the drift in the AG heading data.
2 . The method of claim 1 further comprising determining whether a tracking state exists using the AG heading data and the AGM heading data and calculating a magnetic field error if it is determined that the tracking state exists.
3 . The method of claim 2 further comprising completing processing of the AG heading data and the AGM heading data if the tracking state does not exist.
4 . The method of claim 2 further comprising calculating a drift output as a function of the magnetic field error.
5 . The method of claim 4 further comprising completing processing of the AG heading data and the AGM heading data if the drift output does not exist.
6 . The method of claim 2 further comprising calculating a magnetic field disruption if a drift output exists.
7 . The method of claim 6 further comprising correcting the drift output if the magnetic field disruption exists.
8 . The method of claim 6 further comprising correcting the AG heading data and the AGM heading data if the magnetic field disruption exists.
9 . The method of claim 6 further comprising determining if a recovery state exists if the magnetic field disruption does not exist.
10 . The method of claim 9 further comprising correcting the drift if the recovery state does not exist.
11 . A system for processing data, comprising:
a processor operating under algorithmic control, the processor configured to perform the steps of:
retrieving accelerometer and gyroscope (AG) heading data for an inertial measurement unit (IMU) of a device;
retrieving accelerometer, gyroscope, and magnetometer (AGM) heading data for the IMU; and
processing the AG heading data and the AGM heading data to determine an overall heading of the device, including:
in accordance with a determination that the AG heading data indicates that the device is rotating below a threshold angular velocity, using the AGM heading data to correct drift in the AG heading data; and
in accordance with a determination that AG heading data indicates the device is not rotating below the threshold angular velocity, forgoing using the AGM data to correct the drift in the AG heading data.
12 . The system of claim 11 wherein the processor is further configured to calculate the drift as a function of a magnetic field error.
13 . The system of claim 12 wherein the processor is further configured to complete processing of the AG heading data and the AGM heading data if a drift output does not exist.
14 . The system of claim 12 wherein the processor is further configured to calculate a magnetic field disruption if a drift output exists.
15 . The system of claim 14 wherein the processor is further configured to correct the drift if the magnetic field disruption exists.
16 . The system of claim 14 wherein the processor is further configured to correct the AG heading data and the AGM heading data if the magnetic field disruption exists.
17 . The system of claim 14 wherein the processor is further configured to determine if a recovery state exists if the magnetic field disruption does not exist.
18 . The system of claim 17 wherein the processor is further configured to correct the drift if the recovery state does not exist.
19 . A non-transitory readable storage medium storing instructions, which, when executed by a system that includes one or more processors, causes the system to perform a set of operations comprising:
retrieving accelerometer and gyroscope (AG) heading data for an inertial measurement unit (IMU) of a device;
retrieving accelerometer, gyroscope, and magnetometer (AGM) heading data for the IMU; and
processing the AG heading data and the AGM heading data to determine an overall heading of the device, including:
in accordance with a determination that the AG heading data indicates that the device is rotating below a threshold angular velocity, using the AGM heading data to correct drift in the AG heading data; and
in accordance with a determination that AG heading data indicates the device is not rotating below the threshold angular velocity, forgoing using the AGM data to correct the drift in the AG heading data.
20 . The system of claim 19 wherein the set of instructions further comprising calculating the drift as a function of a magnetic field error.