A Simple Observer for Gyro and Accelerometer Biases in Land Navigation Systems

Abstract

In various applications of land vehicle navigation and automatic guidance systems, Global Navigation Satellite System/Inertial Measurement Unit (GNSS/IMU) positioning performance crucially depends on the attitude determination accuracy affected by gyro and accelerometer bias instabilities. Traditional bias estimation approaches based on the Kalman filter suffer from implementation complexity and require non-intuitive tuning procedures. In this paper we propose, as an alternative, a simple observer that estimates inertial sensor biases exclusively in terms of quantities with obvious geometrical meaning. By this, any multidimensional vector-matrix operations are avoided and observer tuning is substantially simplified. The observer has been successfully tested in a farming vehicle navigation system.

1. INTRODUCTION

The advent of compact and inexpensive Micro-Electromechanical Sensors (MEMS) has led to the development of consumer-grade Inertial Measurement Units (IMU). These devices have proved to be applicable in many areas unreachable for classical inertial instruments. One of the successfully addressed problems is the navigation and automatic control of various land vehicles. Whereas the primary positioning device for such vehicles is usually a receiver of a Global Navigation Satellite System (GNSS), the desired control performance is achieved only if this receiver is integrated with an IMU or other sensors.

This problem is highlighted in some specific applications such as precision agriculture where the aim is to guide an automated farming machine along a swath with minimum cross-track displacement. The control problem is usually formulated for the “reference point” located at the centre of gravity or in the middle of the rear or the front axle, depending on the vehicle kinematics and the implement used. On the other hand, the GNSS receiver is installed atop the machine cabin roof. Therefore, the position and velocity provided by the receiver are substantially different from those observed at the “reference point”. The differences depend on the continuously varying attitude angles that cannot be measured by a single GNSS receiver (Thuilot et al., 2002; Jongmin, 2014). There appears a need for an IMU-based Attitude and Heading Reference System (AHRS) integrated with the GNSS receiver (Topcon Precision Agriculture, 2014; John Deere, 2014).

Among the error sources of such an integrated system, gyro and accelerometer biases play a key role. MEMS devices are known to have large and unpredictable run-to-run and in-run bias instabilities. For slope compensation in precision agriculture problems, only the longitudinal and lateral gyros and accelerometers are of interest, as they directly determine the accuracy of attitude computations (Salychev, 2004).

Consider a simple numerical example. Let the vertical displacement of the GNSS receiver from the vehicle “reference point” be h = 3 m, and the vehicle roll angle caused by the terrain slope be ϕ = 5^°. If this slope is neglected, it results in a cross-track positioning error of h sin ϕ = 26 cm. It is a dozen times larger than the GNSS inherent errors in the most precise Real-Time Kinematic (RTK) navigation mode. Now suppose we combine this GNSS receiver with a MEMS AHRS to compensate for the slope, but the lateral accelerometer has a run-to-run bias of b _a = 0·2 m/ s². This bias causes a roll computation error of $\tilde \varphi = b_a /g = 1.2\;^\circ $ (where we assume g = 9·8 m/ s²), so the corresponding positioning error will be $h\sin \tilde \varphi = 6\;{\rm cm}$. It still exceeds typical RTK errors, so the use of RTK is meaningless until the accelerometer bias is estimated.

A traditional way to estimate gyro and accelerometer biases online is to apply the Kalman filter (Farrell and Barth, 1999; Salychev, 2004; Ding et al., 2007). The AHRS is treated as a multidimensional dynamical system with external measurements provided by its GNSS receiver and IMU. Some of its state variables are identified with the attitude angles, some others with the unknown gyro and accelerometer biases. Using the Kalman framework, all the state variables are estimated as long as their observability conditions are satisfied.

The observability issue is very non-trivial. While gyro bias estimation does not require any specific vehicle manoeuvres, accelerometer biases are completely unobservable in a straight motion as they cannot be separated from the attitude errors. To make the estimation possible, the vehicle should perform a series of turns (Salychev, 2004). This is an inherent property of any AHRS and does not depend on the chosen estimation technique. Fortunately, the typical path of a land vehicle is composed of straight segments and turns, so the estimation of accelerometer biases can be successfully performed.

Besides the issues associated with the physical essence of the AHRS operation, there is a practical problem with the Kalman filter itself. The universality of this estimation method is achieved at the cost of implementation complexity. The filter requires an efficient framework for manipulating multidimensional, often sparse, matrices and vectors. This is especially challenging for embedded computer systems with very limited hardware resources. Moreover, Kalman filter performance crucially depends on the a priori information. The designer should specify three covariance matrices: one for the initial state uncertainty, and two others for the process noise and the measurement noise. At least one of these, the process noise covariance matrix, has no clear physical meaning and cannot be deduced from the sensor characteristics available to the designer (Salychev, 2004). Therefore, in most engineering applications some laborious and non-intuitive iterative procedures are needed to tune the Kalman filter. For the same reason, the Kalman filter optimality, though guaranteed by theory, is rarely achieved in practice.

2. GEOMETRIC OBSERVERS

The search for a less demanding estimation method than Kalman filtering has led to the invention of a group of techniques which we call geometric observers. What they all have in common is that the estimates of all desired variables are expressed only in terms of quantities with clear geometrical meaning. These observers make use of three-dimensional vectors in conventional Euclidean space, and no multidimensional vector-matrix operations are needed.

2.1. Attitude Estimation

Geometric observers for GNSS/IMU integrated navigation have been developed from two completely different standpoints. The first approach (Shaw et al., 1981; Salychev, 2012) is based on the deep similarities of error behaviour in gimballed and strapdown inertial devices. To emphasise this fact, the notion of a “mathematical”, or “virtual”, gimballed platform is introduced to study the operation of a strapdown AHRS. The direction cosine matrix, updated by the AHRS computer, therefore describes the orientation of the vehicle body with respect to this “virtual platform”. Furthermore, any attitude errors are treated as angular deviations of the “virtual platform” plane from the true local level plane. To correct the errors, a control angular rate vector is applied to the “virtual platform” unless these two planes coincide. This control angular rate replaces the correction term of the Kalman filter.

The second standpoint is the group theory, from which a family of so-called “invariant” observers can be derived (Bonnabel and Rouchon, 2005; Mahony et al., 2008). This research area is very attractive and fruitful, yet the practical results are the same as obtained by the “virtual platform” technique.

2.2. Bias Estimation

It is natural to equip a geometric attitude observer with a gyro bias estimator (Mahony et al., 2008). In steady-state operation, the averaged control angular rate applied to compensate the effects of the gyro bias becomes equal and opposite to that bias. This immediately determines the necessary observer structure. Nevertheless, to the best of our knowledge, no one has succeeded in incorporating an accelerometer bias estimator into the same observer. The only known attempt (Grip et al., 2011) seems to be impractical since it relies on the squared norm of the measured specific force vector to determine its bias. This leads to doubtful performance on noisy data.

In the following sections we will derive the unified estimator for gyro and accelerometer biases in the context of a geometric attitude observer. The outline of this proposal (Tereshkov, 2013) suffered from some unnecessary restrictions imposed on the vehicle trajectory. In the more general theory presented here, these restrictions are removed and a more feasible estimation technique is developed.

3. BIAS OBSERVER DESIGN

We start the observer construction with several assumptions that are reasonable in the context of precision agriculture applications:

1. A) The IMU is subjected to factory calibration, so the residual sensor biases are small, as well as the attitude errors caused by these biases.

2. B) Farming machines typically move on nearly flat terrain, so the roll and pitch angles and the corresponding angular rates are small; vertical accelerations are negligible compared with gravity.

3. C) The vehicle does not suffer from sideslip and the GNSS signals are not blocked by any obstacles, so the heading angle is perfectly known as the direction of the vehicle velocity measured by the GNSS receiver.

4. D) The effects of Earth's rotation and curvature are not taken into consideration.

Now, if we neglect all the products of at least two small values, we can arrive at a linearized error dynamics model, from which the bias observer structure can be deduced and its stability proven. Any possible violations of our assumptions, e.g., during the motion on a hill slope, will not necessarily impair the practical performance of the observer but will of course invalidate its theoretical justifications.

A serious challenge in the observer construction is to decide what available quantities are suitable to drive the observer. The most obvious choice is to use the raw measurements provided by the IMU and the GNSS receiver. Unfortunately, they are coupled with the sensor biases through several complicated differential equations (Titterton and Weston, 2004), and this choice returns us to the implementation problems we wished to avoid. Then, following the guidelines of the geometric observers theory, we select the control angular rate to drive the bias observer. So we now need to find how this angular rate is coupled with the quantities of interest, i.e., with gyro and accelerometer biases.

3.1. Attitude Dynamics

Consider a strapdown AHRS that computes the direction cosine matrix C between the body frame b = {x _b, y_b , z_b} and the “virtual platform” frame p = {x _p, y_p, z_p}. The body frame is chosen such that x _b points forwards, y _b rightwards, z _b downwards. The “virtual platform” frame is slightly misaligned from the North-East-Down frame n = {N, E, D}.

The matrix dynamics are described by the equation (Salychev, 2004)

(1)$$\dot{\bf C} = {\bf C} \breve{{\bf \omega}}_b^b - \breve{{\bf \omega}}_p{\hskip-3pt^p} {\bf C}$$

Here a subscript denotes the frame for which the angular rate is specified, and the superscript denotes the frame to which the rate is projected. Thus, ω_b^b is the body frame angular rate as measured by the gyros, ω_p^p is the “virtual platform” control angular rate as seen from this “virtual platform”. An arc denotes a skew-symmetric matrix corresponding to a vector, so that $ {\breve{\bf a} \bf b} = {\bf a \times b}$.

Let us project the accelerometer measurements f_b onto the “virtual platform”:

(2)$${\bf f}_p = {\bf Cf}_b $$

For a perfect AHRS installed on a vehicle that moves on a flat non-rotating Earth (Assumption D), the “virtual platform” lies in the local level plane, so that f_p = f_n. Therefore, the first two components of f_p = [f _N, f_E, f_D]^Tcontain only the actual accelerations, but not the projections of the gravity vector g. If this ideal condition is violated and the “virtual platform” is not horizontal, there appears a difference between the measured specific force f_p and the true specific force ${\bf f}_n ^{GNSS} = \dot {\bf v}_n ^{GNSS} - {\bf g}$. The latter is provided by differentiating the vehicle velocity v_n^GNSS measured by the GNSS receiver. The difference is fed back to the “virtual platform” in the form of the control angular rate:

(3)$${\bf \omega}_p{\hskip-3pt^p} = - \breve{{\bf k}} _p ({\bf f}_n^{GNSS} - {\bf f}_p )$$

Here the vertical vector k = −k g/g represents the attitude correction gain, and the quantity τ = 1/(kg) is the correction time constant. Since in the perfect case ω_p^p = 0, any variation of this angular rate is equal to the rate itself: δ ω_p^p = ω_p^p.

3.2. Attitude Error Dynamics

In the presence of gyro and accelerometer biases, b_g and b_a, the “virtual platform” tilt error θ appears. This error is small due to Assumption A, so we are able to represent it by a vector and express the direction cosine matrix variation as $\delta {\bf C} = \breve{{\bf \theta}}_p^p {\bf C}$. By varying Equation (1) and substituting δ ω_b^b = b_g and δ ω_p^p = ω_p^p, we can get

(4)$$\dot {\bf \theta}_p^p = {\bf Cb}_g - {\bf \omega}_p^p $$

Equation (4) has a clear physical meaning. The rate of change of the attitude error vector is determined by two opposite factors: the gyro biases (projected onto the “virtual platform”) and the control angular rate. However, this equation is not convenient for the analysis of strapdown AHRS performance, since its terms are not constant during the vehicle turns. Instead, they are modulated by the C matrix, even if the sensor biases, b_g and b_a, are constant by themselves. To avoid this problem, Equation (4) can be transformed to the body frame:

(5)$$\dot {\bf \theta} _b^b + \breve{{\bf \omega}}_b^b {\bf \theta} _b = {\bf b}_g - {\bf \omega} _p^b $$

It is the last term ω_p^b = C^Tω_p^p that will be used for the estimation of inertial sensor biases. The relation between ω_p^b and b_g is already given by Equation (5). To establish a similar relation between ω_p^b and b_a, we vary Equation (2) with δ f_b = b_a:

(6)$$\delta {\bf f}_p = \;\breve{{\bf \theta}}_p^p {\bf f}_p + {\bf Cb}_a $$

Then we substitute Equation (6) into Equation (3):

(7)$${\bf \omega} _p{\hskip-3pt^p} = \breve{{\bf k}} _p \breve{{\bf \theta}}_p^p {\bf f}_p + \breve{{\bf k}} _p {\bf Cb}_a \; = \left( {{\bf k}_p^T {\bf f}_p}\right)\;{\bf \theta} _p - \left( {{\bf k}_p^T {\bf \theta} _p}\right)\;{\bf f}_p + \breve{{\bf k}} _p {\bf Cb}_a $$

The first term in the right-hand side of Equation (7) is proportional to the attitude error θ we wish to mitigate. It provides the desired error feedback that justifies our choice of the control law (Equation (3)). According to Assumption B, vertical accelerations are small, so f _D = −g and k_p^Tf_p = kg. The second term is an additional error that appears when the vectors k and θ are not orthogonal. Due to Assumption C, there is no heading error and only attitude errors are present. Then, θ lies in the level plane, while k is always vertical by its definition, so the whole term vanishes. The third term reflects the influence of accelerometer biases on the attitude correction accuracy. Though generally harmful, this term allows for the estimation of accelerometer biases by means of the ω_p^b signal. As stated in Assumption B, roll and pitch angles are small, so we can freely replace k_b by k_p and treat this vector as constant in any vehicle manoeuvres.

After accepting these simplifications and transforming Equation (7) to the body frame, we finally get

(8)$${\bf \omega} _p^b = kg\;{\bf \theta} _b + \breve{{\bf k}} _p {\bf b}_a $$

Equations (5) and (8) completely determine the attitude error dynamics and relate it to the sensor biases and to the “virtual platform” control angular rate.

3.3. Control Rate Dynamics

The attitude error by itself is neither measurable nor important for estimation. From our control law (Equation (3)) we know that, once sensor biases are estimated and compensated, the attitude error tends to zero. So it is expedient to eliminate this quantity from the final equations. First, we differentiate Equation (8), provided that b_a is constant, and get $\dot {\bf \omega} _p^b = kg\;\dot {\bf \theta} _b^b $. Second, we substitute Equation (8) and its derivative into Equation (5):

(9)$$\dot {\bf \omega}_p^b = - \left( {\breve{{\bf \omega}}_b^b + kg{\bf I}} \right){\bf \omega}_p^b + kg{\bf b}_g + \breve{{\bf \omega}}_b^b \breve{{\bf k}}_p {\bf b}_a $$

The vector triple product in the last term of Equation (9) can be significantly simplified if we utilise Assumption B and set ω_b^b = [0, 0, ω _zb]^T. Then, $\breve{{\bf \omega}} _b^b \breve{{\bf k}} _p {\bf b}_a = k\omega _{zb} {\bf b}_a $, so the product is collinear with b_a. Further, we recall the definition of the attitude correction time constant τ = 1/(kg) and rewrite Equation (9) in the form

(10)$$\tau \dot {\bf \omega}_p^b = - \left( {\tau \breve{{\bf \omega}}_b^b + {\bf I}} \right){\bf \omega}_p^b + {\bf b}_g + \omega _{zb} {\bf b}_a /g$$

The eigenvalues of the dynamics matrix $ - \left( {\tau \breve{{\bf \omega}} _b^b + {\bf I}} \right)$ are −1 and−1 ± iτω _zb. Therefore, the solution of Equation (10) is always stable.

This is exactly what we desired to obtain. Equation (10) describes a linearized coupling between sensor biases and control angular rate applied to the “virtual platform”. We will use it to justify the bias observer structure.

3.4. Bias Estimate Dynamics

The individual observers for gyro and accelerometer biases will be both driven by the control angular rate ω_p^b multiplied by some scalar gains. The choice of these gains should respect the observability conditions imposed by the nature of the system under study. In the Introduction we noticed that accelerometer biases are completely unobservable in a straight motion. This conclusion is confirmed by Equation (10). As far as ω _zb = 0, the control angular rate ω_p^b is by no means affected by accelerometer biases. Therefore, we will set the accelerometer bias observer gain proportional to the vehicle angular rate ω _zb.

Our observer equations finally take the form:

(11)$${\dot {\hat {\bf b}}}_g = {\bf \omega}_p^b /\tau_g $$

(12)$${\dot {\hat{\bf b}}}_a = g \omega_{zb} {\bf \omega}_p^b /\omega_a $$

Here, τ _g can be called the gyro bias observer time constant, and ω _a, by analogy, the accelerometer bias observer “rate constant”. These quantities play the role of adjustable parameters of the observers. They determine the settling time and the noise level of the obtained estimates.

At this stage, having derived the general observer equations, we can construct the estimation block diagram (Figure 1).

Figure 1. Bias Observer Block Diagram.

As seen from Equation (3), the bias observer requires the derivative of the measured velocity v_n^GNSS. It is generally admitted that numerical differentiation, when applied to real-world signals, results in large noise levels. Nevertheless, GNSS receivers are capable of providing quite smooth velocity measurements based on carrier phase raw data, even if the receiver position is updated using much noisier pseudorange information (Farrell and Barth, 1999). Since the observer does not rely on position measurements, its accuracy is enough for our particular purposes (Section 4).

3.5. Stability

When the current bias estimates are fed back to the input of the attitude observer, as shown in Figure 1, the residual sensor biases affecting the attitude accuracy are expressed as ${\bf b}_g = {\bf b}_g (0) - \hat{\bf b}_g $ and ${\bf b}_a = {\bf b}_a (0) - \hat{\bf b}_a $. The complete linearized closed-loop dynamics of these biases is determined by Equations (10) – (12) and admits the trivial zero solution. To prove its stability, we can construct a positive-definite candidate Lyapunov function:

(13)$$V = \displaystyle{\tau \over 2}\left\vert {{\bf \omega} _p^b}\right\vert^2 + \displaystyle{{\tau _g}\over 2}\left\vert {{\bf b}_g}\right\vert^2 + \displaystyle{{\omega _a}\over {2g^2}} \left\vert {{\bf b}_a}\right\vert^2 $$

Its time derivative according to dynamics equations is

(14)$$\dot V = - {\bf \omega} _p^{b\;T} \left( {\tau \breve{{\bf \omega}}_b^b + {\bf I}} \right){\bf \omega} _p^b $$

This quadratic form is negative-definite due to the specific properties of the matrix $ - \left( {\tau \breve{{\bf \omega}} _b^b + {\bf I}} \right)$ pointed out in Section 3.3. Therefore, V approaches its minimum value at the origin as long as f_p ≠ f_n^GNSS, and the trivial solution of Equations (10) – (12) is stable.

3.6. Steady-State Operation

Suppose that the vehicle is moving straight. The open-loop solution ω_p^b of Equation (10) will eventually converge to its steady value such that $\dot {\bi\omega} _{\bi p} ^{\bi b} = {\bf 0}$. Equation (10) will then take the simplest possible form

(15)$${\bf b}_g = {\bf \omega} _p^b $$

This result is in agreement with what was said in Section 2.2. The control angular rate merely compensates the gyro bias and can be directly used to estimate it.

Now suppose that the gyro bias has already been estimated and compensated, and the vehicle is turning at some constant angular rate ω _zb. Then from the steady-state solution of Equation (10) we get:

(16)$${\bf b}_a = \displaystyle{g \over {\omega _{zb}}} \left( {\tau \breve{{\bf\omega}}_b^b + {\bf I}} \right){\bf \omega} _p^b = g\left[ {\matrix{ {1/\omega _{zb}}& { - \tau}& 0 \cr \tau & {1/\omega _{zb}}& 0 \cr 0 & 0 & 0 \cr}} \right]{\bf \omega} _p^b $$

Unfortunately, Equation (16), first obtained in our previous paper (Tereshkov, 2013) without considering the general observer Equations (10) – (12), is of very limited engineering importance. Indeed, the steady-state conditions are satisfied only when the vehicle angular rate ω _zb is maintained constant for a time interval three to five times longer than the attitude correction time constant τ. Typical turns performed by land vehicles are significantly shorter, so the application of Equation (16) is not justified by practice. The general observer seems to be preferable over its elegant but unreliable steady-state solutions.

4. EXPERIMENTAL VALIDATION

The bias observer discussed in Section 3.4 was implemented in the special experimental firmware of a Topcon AGI-4 GNSS/IMU integrated receiver. The receiver was installed on the cabin roof of a MacDon 9300 self-propelled windrower (Figure 2). The windrower was driven along a curved path (Figure 3) so that the observability of both gyro and accelerometer biases was achieved.

Figure 2. MacDon 9300 Self-Propelled Windrower with Topcon AGI-4 GNSS/IMU Receiver.

Figure 3. Test Path.

To assess the observer accuracy, test run data were post-processed in two stages. At the first stage, the actual gyro and accelerometer biases for the longitudinal and lateral axes were estimated by the proposed observer and then compensated. Bias values were found to be b _gx = −0·08 deg/s, b _gy = −0·13 deg/s, b _ax = −0·11 m/s², b _ay = 0·00 m/s², which is typical for the consumer-grade MEMS sensors installed in the AGI-4 receiver. Nevertheless, these values by themselves cannot give any information regarding the bias observer performance. Since any other in-run bias estimation methods could have their own performance issues, none of them could serve as a reliable reference. For that reason, the second data processing stage was needed: the precisely known artificial biases were added to the sensor measurements, and the estimation procedure was repeated. The resulting bias estimates are shown in Figures 4 and 5.

Figure 4. True and Estimated Gyro Biases.

Figure 5. True and Estimated Accelerometer Biases.

Data post-processing was performed with the following observer parameter values: τ = 4 s, τ _g = 40 s, ω _a = 45 deg/s. The values were selected manually, but, in contrast to Kalman filter tuning, this was a straightforward task, as the bias observer performance is completely determined by these three constants with a rather clear and natural meaning (Section 3.4).

After the initial convergence is complete, the residual estimate variations are of the order of $\tilde b_g = 0\!\cdot \! 01\;{\rm deg/s}$ and $\tilde b_a = 0\!\cdot \!01\;{\rm m/s}^{\rm 2} $, which can be attributed partly to observer imperfections and partly to the actual uncompensated in-run bias instabilities correctly tracked by the observer. Consider the worst case when the estimate variations are solely due to observer errors. Let the vertical displacement of the GNSS/IMU integrated receiver from the vehicle “reference point” be h = 3 m, as in the example discussed in the Introduction. Then, the “reference point” positioning errors caused by the wrong attitude determination are $h\tilde b_g \tau = 2\;{\rm mm}$ and $h\tilde b_a /g = 3\;{\rm mm}$ respectively. It is by one order of magnitude less than the GNSS receiver positioning errors even in the RTK mode.

The bias observer proved to be quite robust against parameter variations: a 20 % change in any of the three adjustable constants leads to bias estimate deviations smaller than 0·001deg/ s and 0·01 m/ s².

5. CONCLUSIONS

Motivated by the excessive implementation complexity of the traditional Kalman filter and inspired by the geometric observer theory, we sought for a simple observer that could estimate gyro and accelerometer biases of a MEMS-based GNSS/IMU integrated land navigation system. Our primary aim was to express the desired bias estimates exclusively in terms of scalars and vectors in conventional three-dimensional Euclidean space, thus avoiding any multidimensional vector-matrix operations.

The designed bias observer (Figure 1) was successfully tested on a self-propelled windrower (Figures 4 and 5). The achieved estimation accuracy proved to be sufficient for all precision agriculture tasks.

It is still unclear whether the proposed observer can be generalised to applications other than land vehicle navigation, since the typical dynamics of aerial or underwater vehicles violates the Assumptions B and C made when deriving the observer equations. A complete geometrical treatment of the bias estimation problem is the subject of future work.