Skip to content

AHRS Calibration Module Documentation

This module contains functions to calibrate AHRS.

Functions:

Name Description
cal_ahrs_so3

Calibration of the attitude extrinsics between two AHRS systems using the Special Orthogonal Group (SO(3)) optimization. The calibration is performed by minimizing the sum of squared residuals of the error of projecting the attitude of AHRS 2 to the attitude of AHRS 1.

Calibration of the attitude extrinsics between two AHRS systems using the Special Orthogonal Group (SO(3)) optimization. The calibration is performed by minimizing the sum of squared residuals of the error of projecting the attitude of AHRS 2 to the attitude of AHRS 1.

Parameters:

Name Type Description Default
time_ahrs_1 Union[ndarray, List[float]]

Time data of AHRS 1.

 required
time_ahrs_2 Union[ndarray, List[float]]

Time data of AHRS 2.

 required
rph_ahrs_1 Union[ndarray, List[float]]

Roll-Pitch-Heading data of AHRS 1.

 required
rph_ahrs_2 Union[ndarray, List[float]]

Roll-Pitch-Heading data of AHRS 2.

 required
low_pass_filter bool

Apply low-pass filter to the input signals. Defaults to True.

 True
ignore_heading bool

If True, fit the calibration using only each AHRS's gravity direction (derived from roll/pitch), instead of the full attitude. This is immune to heading miscalibration in either AHRS -- a rotation about the vertical axis cannot change the measured gravity direction, so heading has no influence on the fit at all. Note this is NOT equivalent to zeroing the heading angle before building the full rotation matrices: roll-pitch-heading is not an orthogonal decomposition of SO(3), so that approach leaves a residual heading-dependent term entangled in the result and does not remove heading's influence. Recovering the full 3-DOF misalignment this way requires the data to have enough roll/pitch excitation (a vehicle that stays nearly level throughout the dataset under-constrains the misalignment's own heading-axis component). Defaults to False.

 False

Returns:

Name Type Description
rot_1_2 ndarray

Rotation matrix of AHRS 2 with respect to AHRS 1.

Raises:

Type Description
TypeError

If the inputs are not of the correct type.
ValueError

If the inputs are not of the correct dimensions.
ValueError

If the data from AHRS 1 and AHRS 2 do not have the same number of measurements.
ValueError

If the time_ahrs_1 is not in the time range of AHRS 2.
Source code in navlib/cal/cal_ahrs.py 
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
def cal_ahrs_so3(
    time_ahrs_1: Union[np.ndarray, List[float]],
    time_ahrs_2: Union[np.ndarray, List[float]],
    rph_ahrs_1: Union[np.ndarray, List[float]],
    rph_ahrs_2: Union[np.ndarray, List[float]],
    low_pass_filter: bool = True,
    ignore_heading: bool = False,
) -> np.ndarray:
    """
    Calibration of the attitude extrinsics between two AHRS systems using the
    Special Orthogonal Group (SO(3)) optimization. The calibration is performed
    by minimizing the sum of squared residuals of the error of projecting the
    attitude of AHRS 2 to the attitude of AHRS 1.

    Args:
        time_ahrs_1 (Union[np.ndarray, List[float]]): Time data of AHRS 1.
        time_ahrs_2 (Union[np.ndarray, List[float]]): Time data of AHRS 2.
        rph_ahrs_1 (Union[np.ndarray, List[float]]): Roll-Pitch-Heading data of AHRS 1.
        rph_ahrs_2 (Union[np.ndarray, List[float]]): Roll-Pitch-Heading data of AHRS 2.
        low_pass_filter (bool, optional): Apply low-pass filter to the input signals. Defaults to True.
        ignore_heading (bool, optional): If True, fit the calibration using only each AHRS's
            gravity direction (derived from roll/pitch), instead of the full attitude. This is
            immune to heading miscalibration in either AHRS -- a rotation about the vertical axis
            cannot change the measured gravity direction, so heading has no influence on the fit
            at all. Note this is NOT equivalent to zeroing the heading angle before building the
            full rotation matrices: roll-pitch-heading is not an orthogonal decomposition of SO(3),
            so that approach leaves a residual heading-dependent term entangled in the result and
            does not remove heading's influence. Recovering the full 3-DOF misalignment this way
            requires the data to have enough roll/pitch excitation (a vehicle that stays nearly
            level throughout the dataset under-constrains the misalignment's own heading-axis
            component). Defaults to False.

    Returns:
        rot_1_2 (np.ndarray): Rotation matrix of AHRS 2 with respect to AHRS 1.

    Raises:
        TypeError: If the inputs are not of the correct type.
        ValueError: If the inputs are not of the correct dimensions.
        ValueError: If the data from AHRS 1 and AHRS 2 do not have the same number of measurements.
        ValueError: If the time_ahrs_1 is not in the time range of AHRS 2.
    """
    # Convert to numpy arrays
    if isinstance(time_ahrs_1, list):
        time_ahrs_1 = np.array(time_ahrs_1)
    if isinstance(time_ahrs_2, list):
        time_ahrs_2 = np.array(time_ahrs_2)
    if isinstance(rph_ahrs_1, list):
        rph_ahrs_1 = np.array(rph_ahrs_1)
    if isinstance(rph_ahrs_2, list):
        rph_ahrs_2 = np.array(rph_ahrs_2)

    # Check inputs
    if not isinstance(time_ahrs_1, np.ndarray):
        raise TypeError("time_ahrs_1 must be a numpy array or list")
    if not isinstance(time_ahrs_2, np.ndarray):
        raise TypeError("time_ahrs_2 must be a numpy array or list")
    if not isinstance(rph_ahrs_1, np.ndarray):
        raise TypeError("rph_ahrs_1 must be a numpy array or list")
    if not isinstance(rph_ahrs_2, np.ndarray):
        raise TypeError("rph_ahrs_2 must be a numpy array or list")
    if not isinstance(low_pass_filter, bool):
        raise TypeError("low_pass_filter must be a boolean")
    if not isinstance(ignore_heading, bool):
        raise TypeError("ignore_heading must be a boolean")

    # Check dimensions
    time_ahrs_1 = time_ahrs_1.squeeze()
    if time_ahrs_1.ndim >= 2:
        raise ValueError("The time_ahrs_1 must be a (n, ), (n, 1) or (1, n) numpy array.")
    time_ahrs_2 = time_ahrs_2.squeeze()
    if time_ahrs_2.ndim >= 2:
        raise ValueError("The time_ahrs_2 must be a (n, ), (n, 1) or (1, n) numpy array.")
    if rph_ahrs_1.ndim != 2 or (rph_ahrs_1.shape[0] != 3 and rph_ahrs_1.shape[1] != 3):
        raise ValueError("rph_ahrs_1 must be a 3xN or Nx3 numpy array.")
    if rph_ahrs_2.ndim != 2 or (rph_ahrs_2.shape[0] != 3 and rph_ahrs_2.shape[1] != 3):
        raise ValueError("rph_ahrs_2 must be a 3xN or Nx3 numpy array.")

    # Force the roll, pitch, and heading to be Nx3
    if rph_ahrs_1.shape[0] == 3 and rph_ahrs_1.shape[1] != 3:
        rph_ahrs_1 = rph_ahrs_1.T
    if rph_ahrs_2.shape[0] == 3 and rph_ahrs_2.shape[1] != 3:
        rph_ahrs_2 = rph_ahrs_2.T

    # Check if the time arrays are the same length
    if time_ahrs_1.shape[0] != rph_ahrs_1.shape[0]:
        raise ValueError("The time_ahrs_1 and rph_ahrs_1 must have the same number of measurements.")
    if time_ahrs_2.shape[0] != rph_ahrs_2.shape[0]:
        raise ValueError("The time_ahrs_2 and rph_ahrs_2 must have the same number of measurements.")

    # Clip the measurements from AHRS 1 to the time range of AHRS 2
    time_ahrs_2_max = np.max(time_ahrs_2)
    time_ahrs_2_min = np.min(time_ahrs_2)
    filter_idx = (time_ahrs_1 >= time_ahrs_2_min) & (time_ahrs_1 <= time_ahrs_2_max)
    time_ahrs_1 = time_ahrs_1[filter_idx]
    rph_ahrs_1 = rph_ahrs_1[filter_idx]
    if time_ahrs_1.shape[0] == 0:
        raise ValueError("The time_ahrs_1 must have at least one measurement in the time range of AHRS 2.")

    # Resample the data from AHRS 2 to the time of AHRS 1
    rph_ahrs_2 = resample(time_ahrs_1, time_ahrs_2, rph_ahrs_2)
    time_ahrs_2 = time_ahrs_1

    # Filter input signals with a low-pass filter set to a cutoff frequency of 1/10
    # of the sampling frequency
    if low_pass_filter:
        # Sampling frequency
        frequency = 1 / median(difference(time_ahrs_1))

        # Apply low-pass filter
        rph_ahrs_1 = filter_lowpass(rph_ahrs_1, frequency, frequency / 10)
        rph_ahrs_2 = filter_lowpass(rph_ahrs_2, frequency, frequency / 10)

    # Initialize variables for the optimization problem: LHS - RHS @ x = 0
    N = rph_ahrs_1.shape[0]
    RHS = np.zeros([N, 3, 3])
    LHS = np.zeros([N, 3, 3])

    # Populate the LHS and RHS matrices
    for ix in range(N):
        # Right hand side
        RHS[ix] = rph2rot(rph_ahrs_1[ix, :])

        # Left hand side
        LHS[ix] = rph2rot(rph_ahrs_2[ix, :])

    if ignore_heading:
        # Gravity direction in each AHRS's own body frame: g_body(t) = R(t).T @ [0,0,-1],
        # which for a rotation matrix built as Rz(heading)@Ry(pitch)@Rx(roll) equals
        # -R[2,:] and is provably independent of heading (a rotation about the vertical
        # axis cannot change which way vertical points). If R2(t) = R1(t) @ M, then
        # g2_body(t) = M.T @ g1_body(t) for every t, so fitting M this way is immune to
        # whatever either AHRS's heading channel is doing.
        g1_body = -RHS[:, 2, :]
        g2_body = -LHS[:, 2, :]

        rot_1_2 = so3_optimization(
            _objective_function_cal_ahrs_so3_tilt,
            g2_body,
            g1_body,
        )
    else:
        # SO(3) optimization. The objective minimizes ||LHS - RHS @ x|| = ||R2 - R1 @ x||,
        # so the optimizer already converges to x = R1.T @ R2 -- the rotation that maps
        # AHRS2-frame vectors into AHRS1's frame, i.e. rot_1_2 itself. No further inversion
        # is needed (a prior version of this function incorrectly transposed this result,
        # returning the inverse of the intended calibration).
        rot_1_2 = so3_optimization(
            _objective_function_cal_ahrs_so3,
            LHS,
            RHS,
        )

    return rot_1_2