MAGYC-BFG

Proposed method for the full calibration of a three-axis magnetometer using magnetic field and angular rate measurements. This particular approach is based on a factor graph processing all the data in a batch manner.

In particular MAGYC-BFG embeds the volume constraint for the soft-iron into a reparametrization for the Cholesky decomposition of the soft-iron matrix, allowing for the use of half the factors.

Parameters:

Name	Type	Description	Default
magnetic_field	ndarray or list	Magnetic field measurements in a 3xN or Nx3 numpy array or list.	required
angular_rate	ndarray or list	Angular rate measurements in a 3xN or Nx3 numpy array or list.	required
time	ndarray or list	Time stamps of the measurements.	required
measurements_window	int	Window size for the measurements.	25
optimizer	str	Optimization algorithm to use. Options are "dogleg" or "lm" for the Dogleg and Levenberg-Marquardt optimizers respectively.	'dogleg'
relative_error_tol	float	Relative error tolerance for the optimizer. Default is 1.00e-07	1e-07
absolute_error_tol	float	Absolute error tolerance for the optimizer. Default is 1.00e-07	1e-07
max_iter	int	Maximum number of iterations for the optimizer. Default is 1000	1000

Returns:

Name	Type	Description
hard_iron	ndarray	Hard iron bias.
soft_iron	ndarray	Soft iron matrix.
gyro_bias	ndarray	Gyroscope bias.
calibrated_magnetic_field	ndarray	Calibrated magnetic field measurements.
calibrated_angular_rate	ndarray	Calibrated angular rate measurements.
optimization_status	Dict[str, Union[List[float], int]]	Dictionary with the optimization status. The keys are "error" and "iterations".

Raises:

Type	Description
TypeError	If the magnetic field, angular rate, and time are not numpy arrays or lists.
ValueError	If the magnetic field and angular rate are not 3xN or Nx3 numpy arrays, or if the time is not a 1D numpy array.
ValueError	If the optimizer is not a string or not "dogleg" or "lm"
TypeError	If the relative error tolerance is not a float
TypeError	If the absolute error tolerance is not a float
ValueError	If the maximum number of iterations is not a positive integer
ValueError	If the measurements window is not a positive integer

Source code in magyc/methods/magyc.py

def magyc_bfg(magnetic_field: Union[np.ndarray, list], angular_rate: Union[np.ndarray, list],
              time: Union[np.ndarray, list], measurements_window: int = 25, optimizer: str = "dogleg",
              relative_error_tol: float = 1.00e-07, absolute_error_tol: float = 1.00e-07,
              max_iter: int = 1000) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray,
                                             Dict[str, Union[List[float], int]]]:
    """
    Proposed method for the full calibration of a three-axis magnetometer
    using magnetic field and angular rate measurements. This particular approach
    is based on a factor graph processing all the data in a batch manner.

    In particular MAGYC-BFG embeds the volume constraint for the soft-iron into
    a reparametrization for the Cholesky decomposition of the soft-iron matrix,
    allowing for the use of half the factors.


    Args:
        magnetic_field (numpy.ndarray or list): Magnetic field measurements in a
            3xN or Nx3 numpy array or list.
        angular_rate (numpy.ndarray or list): Angular rate measurements in a 3xN or
            Nx3 numpy array or list.
        time (numpy.ndarray or list): Time stamps of the measurements.
        measurements_window (int): Window size for the measurements.
        optimizer (str): Optimization algorithm to use. Options are "dogleg" or "lm"
            for the Dogleg and Levenberg-Marquardt optimizers respectively.
        relative_error_tol (float): Relative error tolerance for the optimizer. Default is 1.00e-07
        absolute_error_tol (float): Absolute error tolerance for the optimizer. Default is 1.00e-07
        max_iter (int): Maximum number of iterations for the optimizer. Default is 1000

    Returns:
        hard_iron (numpy.ndarray): Hard iron bias.
        soft_iron (numpy.ndarray): Soft iron matrix.
        gyro_bias (numpy.ndarray): Gyroscope bias.
        calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field measurements.
        calibrated_angular_rate (numpy.ndarray): Calibrated angular rate measurements.
        optimization_status (Dict[str, Union[List[float], int]]): Dictionary with
            the optimization status. The keys are "error" and "iterations".

    Raises:
        TypeError: If the magnetic field, angular rate, and time are not numpy arrays or lists.
        ValueError: If the magnetic field and angular rate are not 3xN or Nx3 numpy
            arrays, or if the time is not a 1D numpy array.
        ValueError: If the optimizer is not a string or not "dogleg" or "lm"
        TypeError: If the relative error tolerance is not a float
        TypeError: If the absolute error tolerance is not a float
        ValueError: If the maximum number of iterations is not a positive integer
        ValueError: If the measurements window is not a positive integer
    """
    # Check if the magnetic_field, angular_rate, and time are lists and convert them to numpy arrays
    if isinstance(magnetic_field, list):
        magnetic_field = np.array(magnetic_field)
    if isinstance(angular_rate, list):
        angular_rate = np.array(angular_rate)
    if isinstance(time, list):
        time = np.array(time)

    # Check if the magnetic_field, angular_rate, and time are numpy arrays
    if not isinstance(magnetic_field, np.ndarray):
        raise TypeError("The magnetic field must be a numpy array or a list.")
    if not isinstance(angular_rate, np.ndarray):
        raise TypeError("The angular rate must be a numpy array or a list.")
    if not isinstance(time, np.ndarray):
        raise TypeError("The time must be a numpy array or a list.")

    # Check if the magnetic_field and angular_rate are 3xN or Nx3 numpy arrays
    if magnetic_field.ndim != 2 or (magnetic_field.shape[0] != 3 and magnetic_field.shape[1] != 3):
        raise ValueError("The magnetic field must be a 3xN or Nx3 numpy array.")
    if angular_rate.ndim != 2 or (angular_rate.shape[0] != 3 and angular_rate.shape[1] != 3):
        raise ValueError("The angular rate must be a 3xN or Nx3 numpy array.")

    # Check if the time is a 1D numpy array
    time = time.flatten()
    if time.ndim != 1:
        raise ValueError("The time must be a (n, ), (n, 1) or (1, n) numpy array.")

    # Force the magnetic_field and angular_rate to be Nx3 numpy arrays
    if magnetic_field.shape[0] == 3:
        magnetic_field = magnetic_field.T
    if angular_rate.shape[0] == 3:
        angular_rate = angular_rate.T

    # Check if the magnetic_field, angular_rate, and time have the same number of samples
    if magnetic_field.shape[0] != angular_rate.shape[0] or magnetic_field.shape[0] != time.shape[0]:
        raise ValueError("The magnetic field, angular rate, and time must have the same number of samples.")

    # Check that the optimizer is a string and is either "dogleg" or "lm"
    if not isinstance(optimizer, str) or optimizer not in ["dogleg", "lm"]:
        raise ValueError("The optimizer must be a string and either 'dogleg' or 'lm'.")

    # Check that the relative error tolerance is a float
    if not isinstance(relative_error_tol, float) or relative_error_tol <= 0:
        raise TypeError("The relative error tolerance must be a float.")

    # Check that the absolute error tolerance is a float
    if not isinstance(absolute_error_tol, float) or absolute_error_tol <= 0:
        raise TypeError("The absolute error tolerance must be a float.")

    # Check that the maximum number of iterations is a positive integer
    if not isinstance(max_iter, int) or max_iter <= 0:
        raise ValueError("The maximum number of iterations must be a positive integer.")

    # Check that the measurements window is a positive integer
    if not isinstance(measurements_window, int) or measurements_window <= 0:
        raise ValueError("The measurements window must be a positive integer.")

    # Compute the magnetic field derivative
    magnetic_field_derivative = np.diff(magnetic_field, axis=0) / np.diff(time).reshape(-1, 1)
    magnetic_field_derivative = np.concatenate([np.zeros((1, 3)), magnetic_field_derivative], axis=0)

    # Compute the magnetic calibration
    # Smoothing and Mapping Factor Graph
    # 1. Create the non-linear graph
    graph = gtsam.NonlinearFactorGraph()

    # 2. noise model for each factor.
    residual_noise = gtsam.noiseModel.Isotropic.Sigma(3, 1e-6)

    # 3. Creates values structure with initial values
    initial = gtsam.Values()
    initial.insert(S(0), np.array([0.0, 0.0, 0.0, 0.0, 0.0]))
    initial.insert(B(0), gtsam.Point3(0, 0, 0))
    initial.insert(W(0), gtsam.Point3(0, 0, 0))
    keys = [S(0), B(0), W(0)]

    # 4. Add factor for each measurement accumulates in the measurements window into a single node
    measurements_window = int(measurements_window)
    m_dot_window = np.empty((measurements_window, 3))
    m_window = np.empty((measurements_window, 3))
    w_window = np.empty((measurements_window, 3))

    # 5. Add factors to the graph
    for i in range(magnetic_field.shape[0]):
        # Get sensor measurements and estimated magnetic field derivative
        m_dot_window[i % measurements_window, :] = magnetic_field_derivative[i, :]
        m_window[i % measurements_window, :] = magnetic_field[i, :]
        w_window[i % measurements_window, :] = angular_rate[i, :]

        if (i % measurements_window == 0 and i != 0):
            # Average measurements by the measurements window size.
            m_dot_meadian = np.median(m_dot_window, axis=0).reshape(3, 1)
            m_median = np.median(m_window, axis=0).reshape(3, 1)
            w_median = np.median(w_window, axis=0).reshape(3, 1)

            # 5.1 Residual factor
            rf = gtsam.CustomFactor(residual_noise, keys, partial(_residual_factor, m_dot_meadian, m_median, w_median))
            graph.push_back(rf)

            # 5.2 Reset the measurements window
            m_dot_window = np.empty((measurements_window, 3))
            m_window = np.empty((measurements_window, 3))
            w_window = np.empty((measurements_window, 3))

    # 6. Optimize the graph
    # 6.1 Create optimizer parameters
    params = gtsam.DoglegParams() if optimizer == "dogleg" else gtsam.LevenbergMarquardtParams()
    params.setRelativeErrorTol(relative_error_tol)
    params.setAbsoluteErrorTol(absolute_error_tol)
    params.setMaxIterations(max_iter)
    params.setLinearSolverType("MULTIFRONTAL_CHOLESKY")

    # For dogleg method set the trust region. For good estimations, it ranges between 0.1 and 1.0
    if optimizer == "dogleg":
        params.setDeltaInitial(0.5)

    # 6.2 Create optimizer
    if optimizer == "dogleg":
        optimizer = gtsam.DoglegOptimizer(graph, initial, params)
    else:
        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)

    # 6.3 Optimize
    result, optimization_status = _gtsam_optimize(optimizer, params)

    # 7. Process Results
    l_params = result.atVector(S(0))
    b = result.atVector(B(0))
    d = result.atVector(W(0))

    lower_triangular_matrix = np.array([[exp(l_params[0]), 0, 0],
                                        [l_params[1], exp(l_params[2]), 0],
                                        [l_params[3], l_params[4], 1 / exp(l_params[0] + l_params[2])]])
    soft_iron = np.linalg.inv(lower_triangular_matrix @ lower_triangular_matrix.T)
    hard_iron = soft_iron @ np.vstack(b)
    gyro_bias = np.vstack(d)

    # Calibrate magnetic field
    calibrated_magnetic_field = (np.linalg.inv(soft_iron) @ (magnetic_field.reshape(3, -1) - hard_iron.reshape(3, 1))).T

    # Calibrated gyroscope measurements
    calibrated_angular_rate = angular_rate - gyro_bias.flatten()

    return (hard_iron.flatten(), soft_iron, gyro_bias.flatten(), calibrated_magnetic_field, calibrated_angular_rate,
            optimization_status)