HUANG Yu,WU Lihua,and YU Qiang

1.Acoustic Science and Technology Laboratory,Harbin Engineering University,Harbin 150001,China;2.Key Lab of In- fiber Integrated Optics,Ministry Education of China,College of Physics and Optoelectronic Engineering,Harbin Engineering University,Harbin 150001,China;3.College of Automation,Harbin Engineering University,Harbin 150001,China

Abstract:This paper presents a kind of attitude estimation algorithm based on quaternion-vector switching and square-root cubature Kalman filter for autonomous underwater vehicle(AUV).The filter formulation is based on geomagnetic field tensor measurement dependent on the attitude and a gyro-based model for attitude propagation.In this algorithm,switching between the quaternion and the three-component vector is done by a couple of the mathematical transformations.Quaternion is chosen as the state variable of attitude in the kinematics equation to time update,while the mean value and covariance of the quaternion are computed by the three-component vector to avoid the normalization constraint of quaternion.The square-root forms enjoy a continuous and improved numerical stability because all the resulting covariance matrices are guaranteed to stay positively semidefinite.The entire square-root cubature attitude estimation algorithm with quaternion-vector switching for the nonlinear equality constraint of quaternion is given.The numerical simulation of simultaneous swing motions in the three directions is performed to compare with the three kinds of filters and the results indicate that the proposed filter provides lower attitude estimation errors than the other two kinds of filters and a good convergence rate.

Key words:attitude estimator,geomagnetic field tensor,quaternion-vector switching,square-root cubature Kalman filter,autonomous underwater vehicle(AUV).

1.Introduction

The attitude estimator is to determine the orientation of a body- fixed coordinate framework with respect to a reference one.Be it ground,marine or aerial,controlling an autonomous vehicle usually needs some knowledge on its attitude angles[1,2].

The attitude estimation system is one of the satellite sub-systems which plays an important role in stabilizing the satellite. Helicopter operations on moving helidecks require monitoring the roll,pitch,inclination and heave motions in the helideck’s center,and these measurements are sent to the helicopter operator prior to take-off from the shore station.Seabed mapping applications using multibeam echo sounders with wide swathe require accurate attitude measurements to ensure the minimum depth error in the outer beams.Motion damping systems or ride control systems need the foil control system receiving the roll and pitch angles and angular rate measurements.On the other side,light and moderate cost inertial measurement units are appropriate for lightweight unmanned aerial vehicles[3].

Attitude estimation is able to be solved by the deterministic approach or by the recursive algorithm which combines the dynamics and/or kinematic model with the sensor[4].Deterministic approaches such as the three-axis attitude determination(TRIAD)algorithm,the quaternion estimator(QUEST),and the fast optimal attitude matrix(FOAM),require measurements of at least two vectors to determine the attitude matrix[5–8].An advantage of both QUEST and FOAM is that the attitude can be estimated by the measurement of more than two vectors,and this is accomplished by minimizing a quadratic loss function.However,all deterministic methods fail when only one measurement vector is available,for example,only magnetometer data.

The recursive algorithm utilizes a dynamic and/or kinematic model and subsequently estimates the attitude using the measurement of a single reference vector.Sun sensors,magnetometers,star trackers,horizon scanners providing vector observation are used as attitude reference sensors,and usually consist of noisy vector measurement at a low frequency.Inertial sensors such as the gyroscope can improve the measurement which provides the angular rate relative to the inertial framework.Three-axis gyroscopes with high-precision are the most applicable to attitude determination especially in the case of underwater passive navigation and control.However,the accuracy of the sensor is limited by noise and bias error and cannot work alone during unbounded error in attitude estimation over time.

A variety of representations are used to describe the attitude including direction cosine matrix(DCM),Euler angle,rotation vector and quaternion.The quaternion is widely used to represent the attitude because of its minimal nonsingular global attitude and bilinear kinematics equation.However,the quaternion has to obey a normalization constraint addressed in attitude filtering algorithms.By far,different unconstrained and constrained approaches have been used to overcome this difficulty[9].The unconstrained approach is a natural way of maintaining the normalization constraint,which uses an unconstrained three-component vector to represent the local attitude error,and adopts the quaternion for propagation and update.The multiplicative quaternion extended Kalman filter(MQEKF)based on vector observation reconstructs state variables to reduce the system dimension and to avoid the normalized constraint of quaternion[10].The unscented quaternion estimator(USQUE)only uses a three-component representation for the attitude errors and quaternion multiplication to perform the updates,and the singularity is never encountered in practice[11].The local attitude error vector in the MQEKF is the attitude error angles in the body framework,whereas the generalized Rodrigues parameters(GRPs)are chosen to represent the local attitude error in the USQUE.

One method of using the norm-constrained Kalman filter to seek solution of attitude quaternion is to use pseudomeasurements[12].The algorithm is to introduce a perfect measurement consisting of the constraint equation into the estimation solution,however,a perfect measurement results in a singular estimation for processing noise-free measurements in a Kalman filter.Another approach is to project the Kalman solution into the desired and constrain subspace.A performance index can be defined to find the optimal projection for the linear state equality constraint problem[13],and projection of the Kalman solution can be done at any time,not only during the update.A twostep constrain application algorithm for handling nonlinear equality constraints about quaternion normalization is constructed in the Kalman filtering framework[14].The first step applies the projection method to the unconstrained estimate.As a result,the probability distribution of the estimate is constrained to lie along the constraint surface.In the second step,the distribution is translated so that its mean value lies on the constraint surface.A new estimator structure is derived by minimizing a constraint cost function,where the constrained estimate is equivalent to the brute force normalization of the unconstrained estimate[15].The norm constraint of the quaternion particle filter associated with vector observation is naturally maintained,and this filter is augmented with a maximum likelihood estimator of the gyro biases,which is implemented via the use of a genetic algorithm[16].The re-parameterization approach simply re-parameterizes the system so that the equality constraint is not required[17].

The cubature Kalman filter(CKF)is rooted in the third-degree spherical-radial cubature rule for numerically computing Gaussian-weighted integrals and the weights of the cubature point are always positive,which makes its numerical stability and accuracy better than the unscented Kalman filter(UKF)with the probability of negative weights in high dimension system.The negative weight of the sigma-points for UKF introduces great truncation error into the moment integrals and deteriorates the filtering precision.A few quaternion-based attitude estimation algorithms in the cubature Kalman filtering framework were studied.A kind of quaternion square-root CKF estimates the quaternion directly in vector space and uses the two-step projection theory to maintain the quaternion normalization constraint along the estimation process[18].A robust strong tracking nonlinear filtering problem is the case of model uncertainties including the model mismatch,unknown disturbance and status mutation in the spacecraft attitude estimation system with the quaternion constraint.Two multiple fading factor matrices are employed to regulate the predication error covariance matrix to guarantee its symmetry,and the quaternion constraint is maintained by utilizing the gain correction method[19].

The autonomous underwater vehicle(AUV)needs to float on the sea surface to receive signals from the global positioning system(GPS)or the star sensor,and will lose its good concealment performance.The Earth’s gravity field measured by the accelerometer is very sensitive to the motion of AUV.Magnetic compass is a kind of attitude sensor with long history,however,small error in calculating the tilt angle due to magnetic disturbances leads to large inaccuracy of the head angle.

Measurement of the field gradient or tensor delivers more geological details than a scalar measurement of a single component or of the scalar total magnetic intensity.In addition,the measurement is relatively insensitive to orientation noise and diurnal variations.The magnetic field tensor including five independent elements reserves the advantage of removing the variation with day of the Earth’s magnetic field and is not sensitive to the direction of the field[20].The Earth’s normal magnetic field is expressed by the Gauss spherical harmonic model,and generally its tensor is only several nT/km.The magnetic storm originating from a sunspot activity would be several hundreds of nT.Nevertheless,because the source of the magnetic storm is very far apart from the Earth surface,the field tensor produced by the magnetic storm is very weak in a short range compared with that of the Earth’s magnetic anomaly field from underground rock,mineral,buried ferromagnetic targets or geology structure[21].Consequently,the Earth’s magnetic field tensor with a short baseline of about 1 m can be regarded as that of the Earth’s magnetic anomaly field.

The motivation of this paper is to present a kind of underwater square-root cubature quaternion attitude estimation algorithm through switching between the quaternion and the three-component vector,where the squareroot CKF as the attitude filtering framework is employed in the observation of the geomagnetic field tensor with nonlinearity of quaternion.Switching between the quaternion and the three-component vector through a pair of transform and anti-transform is adopted to meet the quaternion constraint condition.

The rest of this paper is organized as follows.The geomagnetic field tensor and its transformation relationship between the navigation coordinate system and the body coordinate system are given in Section 2.The attitude kinematics and the measurement model using five independent components of the field tensor are described in Section 3.Section 4 is devoted to the quaternion attitude estimator in the filtering framework of CKF using both quaternionvector switching and iterative weighted-mean,and gives its form of square-root.Section 5 provides the results of the numerical simulation and presents the comparisons of the two proposed filters and CKF.Section 6 summarizes the results and the conclusions.

2.Geomagnetic field tensor

Bx,ByandBzrepresent respectively the three components of the geomagnetic field vector in thex,yandzdirections,and the geomagnetic field tensor is the spatial derivatives of three orthogonal components of the geomagnetic field vector.Its expression is

whereTBis the matrix of the geomagnetic field tensor with nine componentsBij(i,j=x,y,z).

The magnetic full-tensor gradiometer can be generally achieved in two ways.One is the superconducting magnetic full-tensor gradiometer,such as the airborne magnetic tensor gradiometer in Germany[22],the superconducting quantum interference device(SQUID)magnetic tensor gradiometer-GETMAG[23]and the magnetic tensor gradiometer with a pyramidal structure developed lately in Australia[24].The other is the fluxgate magnetic tensor gradiometer[25].The construction of a magnetic full-tensor gradiometer is composed of ten single-axis with a planar cross structure[26].The vehicle-carried northeast-down(NED)system is associated with the AUV and represented byonxnynzn.Its origin denoted byonis located at the center of gravity of the AUV.Thex-axis denoted byxnpoints toward the geodetic north.They-axis denoted byynpoints toward the geodetic east.Thez-axis denoted byznpoints downward along the ellipsoid normal.The body coordinate system is vehicle-carried and is directly defined on the body of the AUV.Its origin denoted byobis also located at its center of gravity.Thex-axis denoted byxbpoints forward,lying in the symmetric plane of the AUV.They-axis denoted byybis starboard and thez-axis denoted byzbpoints downward to comply with the right-hand rule.

The transformation relationship of the geomagnetic field tensor between the NED frame coordinate and the body frame coordinate can be expressed[27]by

whereandare respectively the geomagnetic field tensors in NED and body frames, the symbol‘?’is the operation of matrix transpose,andis the DCM from the NED frame to the body frame.

The optimal quaternion can be estimated by use of Newton Down-hill to optimize the object function about quaternion according to(2)[27].However,it is a deterministic method and not able to compensate gyro,and it wastes much calculation time due to the use of the optimization algorithm.

The reference tensoris interpolated by geomagnetic field tensor surveying or calculated from the geomagnetic anomaly field data,and it is pre-stored into the navigation computer as a reference map of the tensor.The location provided by other navigation systems is used to withdraw the local tensor from the reference map.The tensoris the real-time measurement using magnetic full-tensor gradiometer strapped to the vehicle.

3.System model of quaternion attitude estimator

3.1Attitude kinematics with gyro model

Various parameters describe the attitude of a rigid body such as the Euler angles,the quaternion parameters,the Gibbs vector and the DCM.The quaternion parameters are the most desired and widely utilized means of attitude representation due to linear propagation equations and their non-singular characteristics for any arbitrary rotation angle.Compared to the quaternion,the attitude representation of the three Euler angles leads to the appearance of singularity in the motion modelling due to the trigonometric function.In the representation of the Gibbs vector,the rotation itself is represented as a three-dimensional vector,which is parallel to the axis of rotation.The transform of its three components is covariant on change of coordinates, and however,180?rotations cannot be represented in the Gibbs vector.

The DCM contains no singularities and is frequently used by the aeronautics community to avoid the possibility of gimbal lock.However,there are nine components in the DCM,which cannot be independent.As the smallest attitude representation with global non-singularity at the cost of normalization constraint,the 4-dimensional quaternionqwith a scalar and a 3-dimensional vector part is a hypercomplex number defined as

It is well known that the quaternion parameters propagate in the time domain according to

where

The unknown true angular velocity vectorωBIis usually measured or estimated in the sensor frame.A common sensor measuring the angular velocity is rate-integrating gyro.Both the gyro frame and the body frame are combined each other to simplify the model of rate gyro,which is given by

3.2System state equation

For the attitude estimation problem using both quaternion representation and measurements of gyro,the state vector is defined asx=[q?,β?]?.From(4)and(6),the system state equation can be rewritten as

3.3Measurement equation of geomagnetic field tensor

The DCM from the NED frame to the body frame with the use of quaternion is given by

The divergence and rotation of the geomagnetic field are all zero, so the tensoris a symmetric square matrix with zero trace. The trace of matrix is similarity-invariant, and it means that the tensoris also a symmetric square matrix with zero trace according to(2).There are only five independent components for the tensor,which are chosen as the observation vector related to the attitude.

Substituting(8)to(2),the measurement equation of the quaternion attitude estimator based on the geomagnetic field tensor is given as follows:

where the observation matrix of the tensor is given by

4.Filter design of quaternion attitude estimator

4.1Discrete equations of filtering model

The gyro bias vector is a constant,i.e,βk,during the very small sampling intervalTsof gyro.It is worthy of noting the third term in the right-hand side of the first equation for(7),and the quaternionqin that term can be approximated byqksince it mainly affects the strength of the random noise[28].The system state equation is discretized as

where

whereI4×4is a unitary matrix,andζkis given by

where

Discrete-time gyro measurements can be generated according to the following equations[29]:

whereNvandNuare zero-mean Gaussian white-noise processes with covariance given by the identity matrix.

From(10),the discrete-time quaternion attitude measurement model for the geomagnetic field tensor is given by

whereandare the observation-vector pair acquired at timetkin the body and NED coordinates,respectively,Km(qk)andvkare the measurement matrixKmand noisevat timetk,respectively.

4.2The filtering algorithm

The attitude kinematics equation about quaternion has a linear form and discrete analytical solutions.The CKF is suitable to estimate the attitude using quaternion representation with minimal computational effort for dimensionality due to the nonlinearity of the tensor measurement model[30].As a four-dimensional vector to describe three dimensions,the independence among four components is denoted by the normalization constraint of the quaternion,i.e,q?q=1.In the time-update of the CKF-based quaternion attitude estimator,the unsatisfied normalization constraint in computing the weighted-mean value of quaternion produces the extra attitude error.Square-root CKF as the filtering is applied to the quaternion attitude estimator based on the geomagnetic field tensor,where switching between the quaternion and the three-component vector is employed by a pair of transforms and an iterative algorithm is used to calculate the weighted-mean value of quaternion.

A pair of transforms between the quaternion and the three-component vector are defined as(20)and(21)in[31]as

whereqis a quaternion,andeuis a unit vector representing the direction of rotation axis.

The iterative steps to calculate the weighted-mean valueof quaternion cubature points{qi,i=1,2,...,N,N=2m}form-dimensional state vector are given as follows:

Step 1j=0,initialize the reference quaternionqr0,the maximum iterative numberjmax,and a small thresholdεu.

Step 2j=j+1,calculateaccording to(22),(23)and(24).

whereis the inverse quaternion ofqrj,?is the quaternion multiplication,=1/(N?2)(i=1,2,...,N?2)is the weight of cubature points.

Step 3Ifandj

Step 4The iteration stops and outputs

After the iteration stops,the covariance of the quaternion prediction is calculated by

The algorithm of the quaternion attitude estimator embedded with both the iterative calculation of the weightedmean value in the framework of square-root CKF and the observation of the geomagnetic field tensor is summarized as follows:

(i)Initialization:

where E[·]denotes expectation operator,chol(·)denotes a Cholesky decomposition of a matrix returning a lower triangular Cholesky factor,andare the estimated value and error covariance of the decrement state vector,respectively.

(ii)Loop,k=1,2,...,∞:

i)Calculate the cubature points:

whereis the cubature point of the decrement state vector.

ii)Time update equations:

iii)Measurement update equations:

where=1/(2m),Zl,k+1|k(l=1,2,...,2m)is the propagated cubature point through measurement update equations.

5.Simulation results

In this section,a simulation with one field tensor measurement and three angular rate measurements is used to determine the attitude of a rotating AUV,whose angular motion model expressed by three Euler angles is

whereψ,θandγare head,pitch and roll angles of the AUV with the rotation order ofz-x-y.

The performance comparison between the two proposed filters and CKF is demonstrated through the simulation.The reference map of the geomagnetic field tensor is produced by six spheres and four rectangles.The longitude and latitude of the original point for the reference map are respectively 120?and 28?,and the size of the reference map is 2?×2?with 256×256 grids.The simulation parameters of six spheres and four rectangles are listed in Table 1,whereris the radius of the sphere,Sis the size of the rectangle,Mis magnetization intensity,IandDare respectively magnetic inclination and magnetic declination,andPis the position in the reference map.

Table 1Parameters of six spheres and four rectangles

The motion model of the AUV is the constant velocity of 10 m/s and 8 m/s in the two different directions with the perturbations from the acceleration variance of 0.05 m/s2and inverse correlation time constant of 0.04 h.Compared with a constant acceleration trajectory for nonmaneuver norm,the trajectory of the motion model is the important special case of the constant acceleration trajectory with zeros acceleration,and is more applicable to aircraft,ship and submarine targets because of more vehicle information[32].The total simulation time is 180 s with the sampling frequency of 100 Hz.The uncertainty of the magnetic full-tensor gradiometer is 0.02 nT/m,and the standard deviationsσvandσufor the gyro model are 0.06?·s?1and 0.008?·s?2,respectively.The initial attitude errors of head,pitch and roll are 2?,3?and–1?,respectively.

The five components of the tensor map are shown in Fig.1 as the reference map of the attitude estimator,where Fig.1(a),Fig.1(b),Fig.1(c),Fig.1(d)and Fig.1(e)are respectively the maps ofBxx,Byy,Bxy,ByzandBxz,and the black line is the moving trajectory of the AUV with the initial position of 300 m and 300 m inxandydirections relative to the map original point.

The absolute errorsδψ,δθandδγof the three Euler angles are respectively defined by

Fig.1Moving trajectory of AUV and reference maps of five independent components for geomagnetic field tensor with the unit of nT/m

To refrain from the normalized constraint of quaternion,the transformation from quaternion to the three-component vector using(20)and an iterative algorithm to calculate the weighted-mean of quaternion are applied in the socalled iterative switching quaternion cubature Kalman filter(ISQCKF).The square-root forms of ISQCKF have the added benefit of numerical stability and guaranteed positive semi-definiteness of the state variances, which is called by the iterative switching quaternion square-root cubature Kalman filter(ISQSRCKF).The three kinds of quaternion cubature attitude estimators using respectively CKF,ISQCKF and ISQSRCKF are simulated to compare the errors of the attitude determination.The absolute errors of the attitude using the three kinds of filters with the same initial error attitude and gyro measurements are shown in Fig.2,where Fig.2(a),Fig.2(b)and Fig.2(c)are the absolute error curves of head,pitch and roll angles,respectively.

Fig.2Curves of attitude determination errors using respectively CKF,ISQCKF and ISQSRCKF

The black,red and blue lines denote the attitude determination errors using CKF,ISQCKF and ISQSRCKF,respectively.

From Fig.2,we know that all of the three filters can estimate the three Euler angles of the attitude with a fast coverage rate and a good stability.The collapse times of estimating the three Euler angles using the three filters are shown in Table 2,and the three filters only run about 2.5 s to estimate precisely these Euler angles,which indicate clearly the fast convergence of filters.

Table 2Collapse time of estimating the three Euler angles

To demonstrate clearly the Euler angle error using the three filters,comparisons on the mean values and standard deviations(STD)of the absolute angle errorsδψ,δθandδγusing the three filters are shown in Table 3,where the data to calculate the mean values and STD are all from50 s to the end.Both the mean value and STD using ISQCKF are lower than the related ones using CKF,and this is because ISQCKF frees from the normalization constraint of quaternion through switching between the quaternion and the three-component vector compared to CKF.And moreover,both the mean value and STD using ISQSRCKF are also lower than the related ones using ISQCKF,and it is because ISQSRCKF enjoys a continuous and improved numerical stability compared to ISQCKF.

Table 3Mean value and STD of the three Euler angle errors

6.Conclusions

Few passive attitude estimator methods can be effectively applied to underwater navigation and control besides gyrobased attitude determination,however,integrating the angular rate measurements with noise and other inaccuracy issues causes a slow degradation in attitude knowledge over time.If the error is not compensated for or corrected,all attitude knowledge will eventually be lost.Building an attitude determination system that can compensate for attitude drift is a non-trivial problem.

In order to improve the precision of the attitude estimator in the underwater application,a passive way of UAV quaternion attitude determination based on the geomagnetic field tensor and quaternion-vector switching is studied in the paper due to the normalization constraint of quaternion and the insensitivity to orientation noise and diurnal variations of geomagnetic field tensor measurement.This algorithm only uses geomagnetic field and squareroot CKF to estimate the quaternion and gyro drift,and then to calculate the three Euler angles by the estimated quaternion,where the estimated gyro drift is used to compensate the output of angular rate measurement.The results of quaternion attitude determination are demonstrated by the numerical simulations and are compared with the three filters including the standard CKF,ISQCKF and ISQSRCKF.The comparison shows that attitude determination errors using the ISQSRCKF are lower than the other two kinds of filters in the period of the steady run for the three filters.