Highly Accurate Accelerometer- and Magnetometer-based Elevation and Azimuth Estimation Procedure Under Stationary Conditions

2.1 Elevation Estimation Procedure

In this paper, we aim to derive the rotation angle of single-axis servo motors for elevation/azimuth rotation from accelerometer and magnetometer signals. This section explains the elevation estimation procedure.

In the elevation estimation, the inertial frame {I}, motor-fixed frame {M}, motor-rotation frame {M’}, and sensor frame {S} are used (Fig. 1). The inertial frame is a fixed reference frame in which the Z-axis is in the gravitational direction and the X-axis is in the magnetic north direction, the motor-fixed frame is a fixed reference frame in which the Y-axis is in the direction of the motor rotation axis and the Z-axis is in the direction of the gravity vector projected on the motor rotation axial plane, and the motor rotation frame corresponds to the motor-fixed frame after rotating by θ about the Y-axis.

Fig. 1.

Coordinate systems for estimating elevation

Under undisturbed conditions, where the dynamic/magnetic disturbances are negligible, the accelerometer (A) and magnetometer (M) provide the gravitational acceleration and Earth’s magnetic field signals observed in the sensor frame, respectively. These include the vertical and horizontal axis vectors of the inertial frame observed in the sensor frame, respectively. The accelerometer and magnetometer signals are modeled as follows, respectively:

where g is the gravitational acceleration vector, m is Earth’s magnetic field vector, g and m are the magnitudes of vectors g and m, respectively, and ^SZ_I and ^SX’_I are the Z-axis-and X-axis vectors tilted by the magnetic dip angle of the inertial frame observed in the sensor frame, respectively. Based on the accelerometer and magnetometer signals, the directional cosine matrix representing the 3D orientation of the sensor frame with respect to the inertial frame can be determined as follows:

Here, the three axis vectors are determined as follows:

However, when calculating the elevation from the matrix in Eq. (2), a difference may occur between the calculated elevation angle and the motor rotation angle because the reference and motor rotation axes are different. Accordingly, rotation axis alignment is performed to align the reference frame from the inertial frame to the motor-fixed frame and the object frame from the sensor frame to the motor rotation frame, as follows:

where $$ {}_I{}^M\mathbf{R}$$ is an alignment matrix from the inertial frame to the motor-fixed frame, $$ {}_{M^{\text{'}}}{}^S\mathbf{R}$$ is an alignment matrix from the sensor frame to the motor rotation frame, and these matrices are determined as follows:

Here, $$ {}_S{}^I{\mathbf{R}}_{t=0}$$ is the initial orientation of the sensor frame with respect to the inertial frame, which is determined from the initial sensor signals through Eq. (2); $$ {}_S{}^M{\mathbf{R}}_{t=0}$$ is the initial orientation matrix of the sensor frame with respect to the motor-fixed frame, which is determined as follows:

Here, the three axis vectors are determined as follows:

where y_G is the gyroscope signal measured during the alignment experiment, and ^SY_M corresponds to the rotation axis vector observed in the sensor frame. Finally, the elevation angle θ corresponding to the pitch of the ZYX Euler angles is calculated as follows from the direction cosine matrix of Eq. (4):

where r_ij is the element of the i-th row and j-th column of the direction cosine matrix.

The elevation calculation process described above does not consider inaccuracies owing to sensor errors. In addition, because the servo motor system permits only single-axis rotation, the existing 3D rotation-based sensor calibration is not applicable.

Therefore, this paper proposes a high-precision procedure for estimating the elevation of a system with only one rotational degree of freedom. The proposed procedure is as follows (Fig. 2): (1) accelerometer and magnetometer bias calibration, (2) rotation axis alignment, (3) rotation error calibration of the sensor signals, and (4) pivoting of the rotational axial component of the sensor signals. In the proposed procedure, the alignment data for determining the rotation axis alignment parameters, calibration data for determining the bias and rotation error calibration parameters, and validation data for evaluating the elevation accuracy are used. The experimental procedure for collecting each dataset is described in Section 3.1. The sensor signal from the calibration data is denoted as y_cal and the sensor signal from the validation data is denoted as y_val.

Fig. 2.

Flowchart of the proposed procedure

The first step is to estimate and calibrate the biases of the accelerometer and magnetometer using the calibration data. When the motor rotates, the accelerometer and magnetometer signals form an unbiased circle; however, in practice, they appear as biased circles owing to sensor bias errors (Fig. 3). To calibrate the bias errors, the biases of the accelerometer (b_A,xz) and magnetometer (b_M,xz) on the XZ plane of the sensor frame are determined through the following optimization problem:

Fig. 3.

Accelerometer and magnetometer biases (XZ plane)

where g_xz and m_xz correspond to the magnitudes of the gravitational acceleration and the Earth’s magnetic field vectors projected onto the XZ plane of the sensor frame, respectively.

The second step is the rotation axis alignment, which converts $$ {}_S{}^I{\mathbf{R}}_{}$$ to $$ {}_{M^{\text{'}}}{}^M\mathbf{R}$$ using Eq. (4). The alignment matrices $$ {}_I{}^M{R}$$ and $$ {}_{M^{\text{'}}}{}^S\mathbf{R}$$ are determined based on alignment data. The ^SY_M in Eq. (7.a) is calculated using the average of the gyroscope signals during 10 rotations at 200 rpm in the alignment data, $$ {}_S{}^I{\mathbf{R}}_{t=0}$$ and in Eq. (5.a) is calculated using the averages of the accelerometer and magnetometer signals during an initial static period of 10 s.

The third step is to calibrate the rotation error of the sensor signal with respect to the elevation rotation axis. This approach generates a lookup table by calculating the rotation error angle of the sensor signals for each elevation from the calibration data and then estimates and calibrates the rotation error in the validation data. The lookup table consists of the elevation angles calculated from the sensor signals as input variables and the rotation error angles of the accelerometer and magnetometer signals as output variables. The error angle is defined as the angle between the ideal and actual signals with respect to the rotational axis. The ideal signals of the accelerometer and magnetometer are determined as follows:

where ^Ig and ^Im are the gravitational acceleration and Earth’s magnetic field vectors observed in the inertial frame, respectively, which are determined as follows under the assumption that the initial sensor signals are the truth values:

$$ {}_S{}^I{\mathbf{R}}_{ideal}$$ is the ideal orientation matrix of the sensor frame with respect to the inertial frame and is determined as follows:

Here, $$ {}_{M^{\text{'}}}{}^M{\mathbf{R}}_{ideal}$$ is the ideal orientation matrix of the motorrotation frame with respect to the motor-fixed frame and is determined as follows:

Based on the ideal signals y_ideal produced using Eq. (10) and the sensor signals y_cal from the calibration data, the error angle with respect to the rotation axis can be calculated as follows:

Here, $$ {\overline{\mathbf{y}}}_{xz}$$ is a signal projected onto the XZ plane of the motor rotation frame, where the Y-axis component is zero. After a lookup table is generated, the rotation error angles of the accelerometer signal (δθ_A) and magnetometer signal (δθ_M) are estimated using the calculated elevation angle as the input through linear interpolation, and then the sensor signals are calibrated as follows:

Here, the magnitude of the signals on the rotation axis plane

(XZ) should be constant even if the motor rotates; however, the magnitude may vary depending on the rotation of the motor.

Therefore, after the rotation errors are calibrated, the magnitude of the XZ-axis signal is normalized to that of the initial XZ-axis signal. The rotation error calibration process, including normalization, is expressed as follows.

Here, matrix K is a matrix of the magnitude factors for each axis, expressed as follows:

where k_x and k_y are expressed as

The fourth step is to fix the Y-axis component of $$ {}^{M^{\text{'}}}\mathbf{y}$$ to the initial Y-axis component, based on the assumption that the rotation axial component of the sensor signal should be constant even when the motor rotates. Hence, the Y-axis magnitude factor in the matrix K of Eq. (17) is applied as follows:

After the sensor signals are calibrated through the above process, the orientation matrix $$ {}_{M^{\text{'}}}{}^M\mathbf{R}$$ is determined using Eqs. (2)–(4) based on the corrected signals $$ {\widehat{\mathbf{y}}}_{A,val}$$ and $$ {\widehat{\mathbf{y}}}_{M,val}$$, and the elevation angle is calculated using Eq. (8).

2.2 Azimuth Estimation Procedure

This section describes the procedure to estimate the azimuth. Four frames are used for this, similar to the elevation estimation (Fig. 4). Here, the motor-fixed frame is a fixed reference frame in which the Z-axis is the rotation axial direction and the X-axis is the direction of the Earth’s magnetic field vector projected onto the rotation axis plane, and the motor rotation frame corresponds to the motor-fixed frame after rotating by ψ about the Z-axis.

Fig. 4.

Coordinate systems for azimuth estimation

The processes for determining the orientation matrix of the sensor frame with respect to the inertial frame based on the accelerometer and magnetometer signals and converting $$ {}_S{}^I{\mathbf{R}}_{}$$ to $$ {}_{M^{\text{'}}}{}^M\mathbf{R}$$ are the same as those in Eqs. (2)–(5) in estimating the elevation. In azimuth estimation, because the rotational axial direction corresponds to the Z-axis of the motor-fixed frame, the initial orientation matrix of the sensor frame with respect to the motor-fixed frame, which is required to calculate $$ {}_I{}^M\mathbf{R}$$ and $$ {}_{M^{\text{'}}}{}^S\mathbf{R}$$ of Eq. (5), is determined as follows:

Here, the three axis vectors are determined as follows:

The azimuth angle ψ corresponding to the yaw of the ZYX Euler angles is calculated as follows from the direction cosine matrix of Eq. (4).

However, as in the case of estimating the elevation angle, the azimuth estimation is inaccurate because of sensor errors. Therefore, the following procedure is performed for highprecision azimuth calculation (Fig. 2): (1) magnetometer bias calibration, (2) rotation axis alignment, (3) rotation error calibration of the sensor signal, and (4) pivoting the rotation axial component of the sensor signal.

The first step is to calibrate the magnetometer bias. The magnetometer bias b_M,xy was determined from the calibration data to minimize the following cost function (Fig. 5):

Fig. 5.

Magnetometer bias (XY plane)

where m_xy corresponds to the magnitude of the Earth’s magnetic field vector projected onto the XY plane of the sensor frame.

The second step is the rotation axis alignment, which converts $$ {}_S{}^I{\mathbf{R}}_{}$$ to $$ {}_{M^{\text{'}}}{}^M\mathbf{R}$$ (Eq. (4)). The alignment matrices $$ {}_I{}^M\mathbf{R}$$ and $$ {}_{M^{\text{'}}}{}^S\mathbf{R}$$ are determined based on the alignment data. ^SZ_M in Eq. (21.a) is calculated using the average of the gyroscope signals during 10 rotations at 200 rpm in the alignment data and $$ {}_S{}^I{\mathbf{R}}_{t=0}$$ in Eq. (5.a) is calculated using the average of the accelerometer and magnetometer signals during an initial static period of 10 s.

The third step is to calibrate the rotation error angle of the sensor signal with respect to the azimuthal rotation axis. As with estimating the elevation, after a lookup table containing the rotation error angles of the accelerometer and magnetometer signals is generated from the calibration data, the error angles are estimated and calibrated using the validation data. The error angle of the sensor signal is the angle between the ideal and actual signals on the plane of the rotation axis. The ideal signals of the accelerometer and magnetometer are determined using Eq. (10). The ideal orientation matrix $$ {}_{M^{\text{'}}}{}^M{\mathbf{R}}_{ideal,t}$$ is determined as follows:

Using the ideal signal y_ideal and actual signal y_cal from the calibration data, we calculate the rotation error angle of the sensor signal as follows:

Here, $$ {\overline{\mathbf{y}}}_{xy}$$ is a signal projected onto the XY plane, where the Z-axis component is zero. After a lookup table is generated, the error angles of the accelerometer signal (δθ_A) and magnetometer signal

(δθ_M) are estimated using the calculated azimuth angle as the input through linear interpolation, and then the sensor signals are calibrated as follows:

Here, the magnitude of the signal on the rotation axis plane (XY) is normalized to the initial magnitude. Rotation error calibration, including normalization, is expressed as follows:

The magnitude factors for the X and Y axes in matrix K are as follows:

The fourth step is to fix the Z-axis component, which is the direction of the rotation axis, to the Z-axis component of the initial signal. The magnitude factor for the Z-axis in matrix K is applied as follows:

After the sensor signal is calibrated through the above process, the orientation matrix is determined using Eqs. (2)–(4) based on the corrected signal, and the azimuth is then calculated using Eq. (22).

3.1 Simulation and Experiment

In this study, the accuracies of the azimuth and elevation estimation procedures were verified through simulations and experiments. For the experiment, a nine-axis IMU (MTw, Xsens Technologies B.V., Netherlands) was attached to a servo motor system (HF-SP202, Mitsubishi Electric Corp., Japan), and the IMU data were collected at a sampling rate of 100 Hz. The servo motor system was installed separately for elevation and azimuth rotations (Fig. 6) and MR-J3-200A driver (Mitsubishi Electric Corp., Japan) was used to operate the servo motor. The IMU was fixed to the servo motor using a jig. Table 1 lists the specifications of the MTw accelerometer and magnetometer.

Fig. 6.

Experimental setup for elevation and azimuth rotation

Table 1.

Sensor specification of MTw.

In the simulation test, the calibration and validation data for the elevation/azimuth estimation were generated using the following operating sequence: (1) rotating clockwise in 15° increments up to 90° (maintaining a static state for 15 s at each step), (2) returning to the original position, (3) rotating counterclockwise in 15° increments up to 90° (maintaining a static state for 15 s at each step), and (4) returning to the original position. The signals of the three-axis accelerometer and threeaxis magnetometer were generated based on the MTw specifications, and the rotation axis alignment parameters were determined from the experimental data. White Gaussian noise, misalignment error, non-orthogonality, and constant bias error were applied as the sensor error factors (Table 1). The constant bias was set to 1 mg for the accelerometer and 1 mG for the magnetometer.

For experimental validation, experiments were conducted to collect data for the rotation axis alignment and calibration/validation. The data for the rotation axis alignment were collected by maintaining the servo motor in a static state for 10 s and then rotating it to a total of 3600° at 200 rpm. The data for calibration/validation were collected with ten repetitions of the same motor operation sequence as in the simulation test. The elevation/ azimuth accuracies were evaluated sequentially using the i-th datum for calibration and the i+1-th datum for validation of ten data, yielding results for a total of nine validation data. In the simulation and experimental results, the elevation/azimuth accuracies were compared in four cases according to the steps of the proposed procedure: (Case 1) up to bias calibration, (Case 2) up to rotation axis alignment, (Case 3) up to rotation error calibration (including magnitude normalization of the sensor signal on the rotation axis plane), and (Case 4) up to pivoting of the rotation axial component. From the experimental results, the average and standard deviation of the elevation/azimuth errors of the nine validation datasets for each case were calculated and analyzed.

3.2 Results

The simulation results for elevation estimation are described first. Fig. 7 shows the elevation error results for each motor rotation during the simulation. Case 1 had absolute errors greater than 1.2° except when the elevation was ±180°, which was a very large level of error compared to Cases 2−4. In Case 2, the error increased significantly when the elevation was ±90°, but decreased as it approached 0° and ±180°. For example, errors were 0.387° and 0.508° at −90° and 90° elevations, respectively, whereas at ±180°, the error was within 0.02°. Cases 3 and 4 exhibited absolute errors within 0.04° for most rotation angles. However, In Case 3, the error increased significantly when the elevation was ±90°, similar to the results in Case 2. For example, the errors at −90° and 90° elevations were 0.370° and −0.602°, respectively. In contrast, Case 4 maintained errors within 0.04° for all the rotation angles.

Fig. 7.

Average elevation errors from Cases 1−4 according to the elevation in the simulation tests

Next, the simulation results for the azimuth estimation are described. Fig. 8 shows the azimuthal error results from the simulation. In Case 1, the error varied significantly with the azimuth angle. For instance, the error was 0.109° at an elevation of −165°, while it was −0.083° at −60°, showing a variation of approximately 0.19°. Case 2 exhibits results similar to those of Case 1, and the difference between the two cases is within 0.002°. Cases 3 and 4 had absolute errors within 0.04°, and the difference between the two cases was within 0.001°, indicating that the improvement in accuracy owing to the transition from Cases 3 to 4 was insignificant.

Fig. 8.

Average azimuth errors from Cases 1−4 according to the azimuth in the simulation tests

The following describes the experimental results of elevation estimation. Fig. 9 shows the average and standard deviation results for the elevation errors of the nine validation datasets. Case 1 exhibited a significant average error exceeding 1.5°, which is similar to the simulation results, with a standard deviation of 0.02° for the nine data points. In Case 2, the error varied depending on the motor rotation angle. Case 3 exhibited a similar error pattern to Case 2 for all elevation angles and had larger errors than Case 2 in counterclockwise rotation. Case 4 had absolute errors within 0.02°, which were very low compared to those of Cases 2 and 3, and the variation in error according to the elevation angle was insignificant.

Fig. 9.

Average (with standard deviation) of elevation errors from Cases 1−4 according to the elevation in the experimental tests

The following describes the experimental results of azimuth estimation. Fig. 10 shows the average and standard deviation results for the azimuthal errors of the nine validation datasets. In the results of Cases 1 and 2, the errors were larger than those in Cases 3 and 4, and they varied significantly depending on the azimuthal angle. In addition, the difference between the errors in Cases 1 and 2 was insignificant (within 0.001°). In contrast, Cases 3 and 4 had low errors and low variability depending on the azimuth angle.

Fig. 10.

Average (with standard deviation) of azimuth errors from Cases 1−4 according to the azimuth in the experimental tests

3.3 Discussion

The comparison results of Cases 1–4 from the simulation and experiment described above show the step-by-step effects of the elevation and azimuth estimation procedures, i.e., rotation axis alignment, rotation error correction, and pivoting of the rotation axis direction component. The effect of the rotation axis alignment was more significant in elevation estimation than in azimuth estimation. For example, in the elevation estimation results, Case 2 exhibited accuracies higher than those of Case 1 by more than 1°, whereas in the azimuth estimation results, the difference between Cases 1 and 2 was within 0.002°. To investigate the cause of these results, we applied the misalignment angles of roll 1°, pitch 1°, and yaw 0°, and the aligned/misaligned rotation frames were compared when the elevation/azimuth angles were 0° and 60° (Fig. 11). Here, the 3D frame results and projected frame results on the rotation axis plane (XZ axis for elevation, XY axis for azimuth) were analyzed. The results showed that the differences between the aligned and misaligned frames on the rotation axis plane were large for the elevation but small for the azimuth. This appears to be because the rotation about the Z-axis (yaw angle) was not considered in the misalignment. This is because the misalignment in azimuth estimation is due to the difference between the motor rotation axis and the gravity axis, which only causes tilt rotation (roll, pitch) without yaw rotation. This result indicates that the rotation axis alignment does not affect the azimuth accuracy but affects the elevation accuracy. Additionally, in the elevation estimation, Case 1 had minimal errors at the elevation angles of ±180°. For elevation estimation, the initial accelerometer signal (at an elevation angle of 0°) was considered as the true value, and at an elevation angle of 180°, the accelerometer signal was measured as the opposite vector of the initial signal. Accordingly, we assumed that the accelerometer signals on the rotation axis plane were not distorted by misalignment at elevation angles of 0° and ±180°.

Fig. 11.

Comparison of 3D and projection (on the plane of the rotation axis) results of aligned and misaligned coordinate systems for elevation ((a) aligned, (b) misaligned) and azimuth ((c) aligned, (d) misaligned) rotations

The accuracies of Cases 2 and 3 differed in the elevation and azimuth results, whereas Case 4, which applied all steps of the procedure, exhibited the highest level of accuracy in most results. In particular, the accuracy improvement in Case 4 was greater in the experimental results than in the simulation results. In the simulation, the sensor signals were generated considering only four error factors, whereas the actual sensor signals exhibited more uncertainty. In addition, even if the experimental conditions were set to exclude dynamic/magnetic disturbances, unintended disturbances owing to motor operation or the environment could occur, which would cause errors in the elevation and azimuth. Case 4 was also affected by the reproducibility of the sensor signals; however, the experimental results were meaningful in that they showed that a high elevation/azimuth accuracy can be achieved despite such inaccuracy factors.

Compared with Case 3, Case 4 had a higher accuracy in most results, which indicates that fixing the rotation axis direction component is effective for precise elevation/azimuth estimation. To analyze this result, we compared the magnetometer signals corrected in Cases 2–4 for 24 azimuth rotations with the ideal magnetometer signal (Fig. 12). Fig. 12 shows the sensor signals projected onto the XY and XZ planes of the motor rotation frame {M’}. In the XY-plane results, Case 2 exhibited differences from the ideal signal, whereas Cases 3 and 4 showed that the signals were corrected in the same direction as the ideal signal. However, in the XZ plane, only the corrected signal in Case 4 matched the ideal signal. This is because the rotation axial component of the sensor signal should be constant even when the motor rotates in an ideal situation. Case 4 applied the rotation axis direction component as a fixed constant. Accordingly, we can conclude that Case 4 enabled a more precise elevation/azimuth estimation than Case 3.