Evaluation of AHRS algorithms for Foot-Mounted Inertial-based Indoor Navigation Systems

2.1 Introduction of AHRS algorithm to dynamic model

Following the method in previous research 24, the nine state of the CKF algorithm is defined as X=δpnδvnεa,where δpⁿ is the position error, δvⁿ is the velocity error, ε_a is the direction error. The state vector is under the navigation frame (n-frame). The direction, velocity and position under n-frame can be obtained by integrating the gyroscope and acceleration data. The INS navigation equation is defined as:

where gⁿ is the gravity vector under n-frame, fb=[fxb, fyb, fzb]is the acceleration vector under b-frame, ω^b = [ωxb, ωyb, ωzb]is the angular velocity under the body frame (b-frame), [ω^b ×] is the skew-symmetric matrix of angular velocity, defined as:

For the acceleration and the observation of the gyroscope at moment k, the drifts are required to be removed first, as shown in the following formula:

f˜kband ω˜kbare original observations of acceleration and angular velocity, respectively;f^kband ω^kbare the corresponding values after deviation compensation. b_g is the drift of the gyroscope; b_a is the drift of the acceleration. For the CKF under State-9, these two values are obtained by the IMU calibration and used as constants to correctf˜kband ω˜kb.For the CKF under State-15, these two values are the dynamic state variables of the system.

The transformation of the acceleration from Moment k to Moment (k+1) is:

⊗ denotes vector cross-product, representing the rotation correction of acceleration with change in angular velocity.

The transformation of the velocity from moment k to moment (k+1) is:

The transformation of the position from moment k to moment (k+1) is:

The transformation of the attitude from moment k to moment (k+1) is:

Cb, k+1nis the rotation matrix at moment (k+1), with its value representing the compensated rotation of the pose change matrix from moment k to moment (k+1). In order to improve the accuracy of the attitude, the value of Cb, k+1nneeds periodic normalization.

Equation 10 presents the AHRS Fundamental Approach. Our idea is to adopt Mahony or Madgwick algorithm instead of Equation 10 to calculate the attitude. Here, two points are worthy to be noted. First, the realization of CKF in 24 is based on the rotation matrix, while Mahony and Madgwick are based on the quaternion to calculate the attitude, so the calculated attitude results of each Mahony or Madgwick algorithm are required to be converted into rotation matrix. Second, the bias correction is already included in the Mahony and Madgwick algorithms, so it is unnecessary to correct the original accelerometer and gyroscope data in according to Equation 5 and 6.

2.2 Zero-Velocity Detectors

If a zero-velocity state is detected, the error can be calculated by the Kalman gain K _k and the observed zero-velocity deviation V_k:

The position error, velocity error and direction error are contained in the vector Xk.Regarding ε_a, a skew symmetric matrixΩε,kfor the angular error is constructed:

The attitude, velocity and position are corrected as follows:

It should be noted that since the one used in the Mahony or Madgwick algorithm is quaternion, the corrected Cb, knis required to be converted to a quaternion again for the next attitude calculation.

3.1 AHRS Fundamental Approach

The principles of attitude calculation are as follows: when expressing a certain vector n by using different coordinate systems, the size and direction must be the same. However, as there are some errors in the transformation matrix of these two coordinate systems, when a vector is transformed by a rotation matrix with some errors, it must be some deviations compared with the theoretical value in another coordinate system. And the attitude is corrected just by the correction of this rotation matrix through these deviations.

Since the rotation matrix is actually expressed by quaternions in this paper, the goal of attitude calculation is achieved by correcting the quaternions, and accelerometers and magnetometers are a main measurement object for attitude calculation.

Initially, assume that the sensor is either at a stand still or moves at a constant velocity so that only the gravity vector can be measured by the accelerometer. At the same time, assume a non-disturbed magnetic field and therefore the only the Earth magnetic field is measured by magnetometers. The pitch(θ) and roll(ϕ) can be calculated from the gravity vector, as shown in the following equation:

The value of yaw(ψ)can be obtained by the magnetometer instead of the gravity vector. The measurement of magnetometer, denoted as mb=[mxb, myb, mzb] ,represents the value of the geomagnetic field under b-frame. Since pitch(θ) and roll(ϕ) are known, m^b can be converted to the XOY plane under the n-frame, and its value is denoted as mn=[mxn,  myn,  mzn] .The conversion relationship between the two groups of magnetic data is as follows:

Under the geographic coordinate system, the data of the geomagnetic field is b = [b_x, 0, b_z], where b_y in the east direction is 0 since the b _x direction points to the north under the geographic system. In fact, the n-frame and the geographic coordinate system are in a same XOY plane, and the difference angle happens to be the heading angle ψ,as shown in Figure 2. Among them,

Figure 2

Navigation coordinate system and geographic coordinate system.

The consistent XOY plane is provided by the correction of accelerometers, that is, navigation coordinate system and geographic coordinate system are in one plane.

Only by this way can Equation 19 be satisfied according to the relationship of trigonometric function.

The relationship between the data b of the geomagnetic field and the magnetic field under the b-frame is as follows:

The heading angle can be calculated by Equation 21:

After the initial yaw, pitch and roll are obtained, the attitude transition matrix Cb, 1ncan be constructed as follows:

The subsequent attitude transition matrix Cb, k+1ncan be obtained by Equation 10.

3.2 Madgwick AHRS

For the Madgwick algorithm, the current pose is calculated in the form of quaternion using a gyroscope:

By calculating the difference of accelerometer as well as the difference of magnetometer between n-frame and b-frame, the error of pose is corrected. Denote the vector coordinate under the n-frame and the b-frame as e = [0, e_x, e_y, e_z] and s = [0, s_x, s_y, s_z], respectively. For the accelerometer, e = [0, 0, 0, 1] and s=[0, fxb, fyb, fzb];for the magnetometer, e = [0, b_x, 0, b_z] and s = [0, mxb, myb, mzb].  qis the quaternion of the sensor pose, and its conversion relationship is as follows:

The error equation is defined as:

The above equation is solved by Gauss Newton method:

where μ_t is the step size, and ∇is the differential operator. Combine Equation 24 and Equation 27,

0.5q_tω_td_t in Equation 28 is calculated and updated by the gyroscope. β∇E| |∇E| |dtis obtained by the accelerometer and magnetometer, and it is used to correct the angle error, where β represents the error weight.

3.3 Mahony AHRS

For the Mahony algorithm, the current pose is also calculated in the form of quaternion using a gyroscope:

By calculating the difference of accelerometer as well as the difference of magnetometer between n-frame and b-frame, the error of pose is corrected.

⊗is vector cross-product. e represents the relative rotation (error) between the measured inertial vector and the predicted vector. v and w are defined as follows:

After calculating e by the proportion and integration regulator, the correction deviation δ regarding the gyroscope is obtained:

The final corrected angular velocity is:

This equation is substituted into Equation 29 to update the pose calculation.

For the Mahony or Madgwick algorithm, the quaternion of its initial state can be calculated by the AHRS fundamental approach, and then its rotation matrix is converted to a quaternion.

4.1 Analysis and comparison of Route1-2 positioning based on CKF under State-9

The open source CKF algorithm under State-9 in 24 is adopted, and the related parameters are shown in Table 1. From the positioning results of Route1 and Route2 in Figure 4, the positioning trajectory obtained by AHRS fundamental algorithm (abbreviated as “Common”) is poor since the drifts of accelerometer and gyroscope are not tracked in attitude calculation. The trajectory calculation using

Figure 4

Comparison of Route trajectories.

Mahony/Madgwick to conduct pose estimation is relatively accurate, and the result of Madgwick is the closest to the true trajectory.

The accuracy of the trajectory is inseparable from the accuracy of the direction, and the directional calculation results of the three algorithms are compared in Figure 5-6. It can be seen that for both Route 1 and Route 2, the differences of pitch and roll among the three calculation methods are not significant. This is mainly because the zerovelocity detection is periodically correcting the deviations of pitch and roll. For the yaw, the direction calculation of Madgwick algorithm is between that of Mahony and direct algorithm. Since there is no real direction for reference, from the results of the positioning trajectory, the calculation of the direction conducted by the Madgwick algorithm is the most accurate, which is also reflected in the comparison of the yaw covariance of the three algorithms in Figure 7. The covariance as well as the uncertainty of the Madg wick algorithm is the smallest.

Figure 5

Comparison of heading angles of the three algorithms of Route1.

Figure 6

Comparison of heading angles of the three algorithms of Route2.

Figure 7

Comparison of the covariance of heading angles.

4.2 Analysis and comparison of Route1-2 positioning based on CKF under State-15

The performance of the AHRS algorithms depends strongly on the strategies used to reject perturbations, such as sudden accelerations or deformations of the Earth magnetic field, and the ability to estimate the biases of the gyroscopes. In the EKF under State-9, only the bias of position, velocity and angle are corrected, however, due to continuous integration of sensor noise and biases, the drift of gyroscope and accelerometer needs to be corrected too. Therefore, the CKF algorithm under State-15 23 is referred, adding bias estimation of accelerometer and gyroscope to the state. The bias estimation is denoted as X = [δpnδvnє bgba],where b_g is the gyroscope bias, and b_a is the acceleration bias. In addition, HDR and ZARU mentioned in 23 are introduced.

The positioning results of Route1-2 are shown in Figure 8-9. 15M represents CKF under State-15, and the Madgwick algorithm is used to calculate attitude; 15ZH represents CKF under State-15, and the Common algorithm is used to calculate attitude, with HDR and ZARU as auxiliary; 15MZH represents that the Madgwick algorithm is used to calculate attitude, with HDR and ZARU as auxiliary. From the positioning results in Figure 8, the profiles of the positioning trajectory obtained by the three algorithms are basically the same, while considering the overall direction, the trajectory of 15MZH is closer to the real trajectory, as can be seen from the dashed circle in the figure. From the three dashed circles in Figure 9, the overall distribution of trajectory obtained from 15M and 15MZH based on the Madgwick algorithm is compacter than that of 15ZH, and the trajectory obtained from 15MZH is closer to the true trajectory in the overall direction.

Figure 8

Comparison of the three AHRS algorithms of Route 1.

Figure 9

Comparison of the three AHRS algorithms of Route 2.

On the other hand, the positioning accuracy of the three algorithms can also be reflected by the positioning results on the Z-axis, as shown in Figure 10. Since the walking is on a plane, in theory, data on the Z-axis should be close to zero. From the positioning results of Route1 and Route2, it can be seen that the CKF algorithm with the introduction of Madgwick algorithm can reduce more positioning error to some extent.

Figure 10

Comparison of the errors of positioning height.

4.3 Adaptive zero-velocity detection algorithm

The zero-velocity detection algorithm based on angular velocity proposed in 24 is adopted, and ωb=[ωxb,  ωyb,  ωzb]is the angular velocity under b-frame. The detection conditions are as follows:

The norm value ||ω^b|| of angular velocity of Route1 and Route2 are respectively shown in Figure 11. Route1 and Route2 are in walking state and running state, respectively. In the walking state with a constant velocity, the delay of the gait static state is longer, and the static state can be detected by a relatively small threshold δ₁. However, in the running state with an increased velocity, the delay of the static state is relatively short, and part of the static state in a gait may be missed by δ₁. When the sampling rate is low, it is possible that the overall static state in a gait is missed by δ₁. These missed static states will reduce the constraints on velocity, resulting in larger positioning errors. However, it does not mean that the threshold can be enlarged. If the threshold is too large, such as δ₂, the velocity is forced to zero before it reaches zero, causing the effective velocity value to fail to participate in the position integration which will also lead to positioning error.

Figure 11

Norm values of Gyroscope of Route 1 and Route 2.

Route 3 corresponds to the mixed motion state. As shown in Figure 12, the part in the red circle and the parts outside represent the norm values of gyroscope in running state and non-running state, respectively. Obviously, it is difficult to use a fixed zero-velocity detection threshold at this time. The three-axis velocity of Route3 is tracked by the CKF algorithm, as shown in Figure 13. The results in Figure 13 are obtained by a fixed zero-velocity detection threshold, and the change rule of the velocity can be roughly seen. The Route 3 experiences several stages, including walking, decelerating, running, decelerating and accelerating. Therefore, it is naturally conceivable to use velocity adaptation to set the zero-velocity detection threshold.

Figure 12

Norm value of Gyroscope of Route 3.

Figure 13

Three-axis velocity of Route3.

The extraction process of velocity feature is as follows. First, calculate the norm value of velocity, as shown in Figure 14. In order to determine the velocity state at a certain moment, the average velocity within 1 second can be calculated. The data sampling rate is 100HZ and the corresponding data length is 100. Therefore, the window value W is equal to 100, and the window value is used to calculate the average velocity of Route 3, as shown in Figure 15.

Figure 14

Norm value of velocity of Route 3.

Figure 15

Average velocity of Route 3 (W=100).

In order to make the state within the 1 second much clearer, the maximum velocity in this data window is calculated, as shown in Figure 16 and Equation 36. In Figure 17, the velocity above the red dashed line correspond to the running state, the parts between the red and green dashed lines correspond to the walking state, and the parts under the green dashed line correspond to the slow walking state. The corresponding zero-velocity detection threshold δ for angular velocity can be set accordingly.

Figure 16

Maximum velocity in each data window of Route 3 (W=100).

Figure 17

Comparison of different zero-velocity detection thresholds δ for angular velocity of Route 3.

Figure 17 shows the positioning trajectories of Route 3 obtained by fixed and adaptive zero-velocity detection threshold δ for angular velocity (algorithm denoted as 15MZHA). Obviously, the adaptive δ presents a more significant improvement on the positioning accuracy. This is because a more accurate correction of the zero-velocity moment can reduce the covariance of the direction and eliminate more errors of velocity and position, thereby improving the accuracy of the direction and positioning trajectory.

In the specific implementation of the algorithm, considering that the velocity is a real-time tracked variable, δ is initially given an empirical value, and then dynamically set with the velocity change.

4.4 Test based on actual experimental data

In order to better verify the proposed algorithm, the experimenter walks three times anticlockwise according to the route in Figure 18. The start and end points are shown in the top right corner, denoted by a triangle and a circle, respectively. The experimenter walks at normal speed in the first and third laps, and jogs in the second lap. The foot-mounted X-IMU is produced by X-IO Company in Britain, as shown in Figure 19.

Figure 18

The design of the route in the experiment.

Figure 19

Foot-mounted IMU.

The positioning results of 15MZH and 15MZHA are shown in Figure 20 and 21, respectively. In the overall trajectory, the positioning results of the two algorithms are basically consistent with the reference trajectory under the Madgwick algorithm and the assistive technologies of ZARU and HDR. However, compared with the first lap, the directional error of the last two laps gets bigger and bigger due to the inevitable accumulative error of MEMS-level IMU itself.

Figure 20

The positioning results of 15MZH.

Figure 21

The positioning results of 15MZHA.

As for the 15MZH algorithm, when jogging in the second lap, since it cannot identify the change of pace, it still uses a fixed zero-speed threshold value to detect the zerospeed state, which will lead to the missing detection of many zero-speed states. These quiescent states that fail to be detected will reduce the constraint of speed, thus resulting in the much longer overall positioning trajectory. It can be known from the area within a broken circle in the left in Figure 20 that the trajectories of the last two laps almost overlap. Meanwhile, the missing detection of zero-speed states will reduce the correction of direction, and it can be seen from the area within a broken circle in the right in Figure 20 that the direction is abnormal in the second lap.