Sine Rotation Vector Method for Attitude Estimation of an Underwater Robot

This paper describes a method for estimating the attitude of an underwater robot. The method employs a new concept of sine rotation vector and uses both an attitude heading and reference system (AHRS) and a Doppler velocity log (DVL) for the purpose of measurement. First, the acceleration and magnetic-field measurements are transformed into sine rotation vectors and combined. The combined sine rotation vector is then transformed into the differences between the Euler angles of the measured attitude and the predicted attitude; the differences are used to correct the predicted attitude. The method was evaluated according to field-test data and simulation data and compared to existing methods that calculate angular differences directly without a preceding sine rotation vector transformation. The comparison verifies that the proposed method improves the attitude estimation performance.


Introduction
Determining the attitude of underwater robots is one of the major research areas in the robotics field, because an underwater robot requires attitude information to estimate its location and control its motion [1]. The rotation and attitude with respect to a reference can be described in terms of Euler angles, quaternions, rotation matrices, rotation vectors and angle-axis representations. This paper proposes an attitude estimation approach that uses a new method called the sine rotation vector to represent rotations. If exteroceptive measurements of range to beacons and communication between the robot and beacons are available [2,3], it is possible to estimate the location without the information on attitude. However, for long-range navigation, it is practically not feasible to implement the system for range measurements or a communication network. Therefore, most underwater navigation methods require attitude data.
A vast number of studies have focused on the attitude estimation and navigation of underwater robots. The major approaches employ the following methodologies: methods using range measurement from acoustic beacons, methods integrating velocity over time, methods using landmarks or features of the environment, methods using range and bearing measurements and cooperative navigation. With the development of sensor technologies and data-fusion theory, methods that fuse a variety of heterogeneous sensors and theories have recently been developed. The navigation methods for angles of the measured attitude and the predicted attitude does not represent the physical relationship between the measured attitude and the predicted attitude. To remove the ambiguity in the calculation of the measurement innovation, the proposed method computes the innovation by calculating the sine rotation vector from the predicted attitude to the measured attitude and then converting the sine rotation vector to Euler angles. This method produces a unique and physically-consistent measurement innovation that corresponds to the pair composed of the measured attitude and the predicted attitude.
Quaternions and SO(3) matrices have been used for attitude estimation [16][17][18]. The quaternion and SO(3) matrix uniquely determine the rotation required to rotate the predicted attitude to the measured attitude as the sine rotation vector does. The problem to be addressed in using the quaternion is keeping the magnitude of the quaternion to one [19,20]. In the stage of prediction and correction, the magnitude of the quaternion deviates from one. Therefore, it is required to use some subterfuge to normalize the quaternion. Likewise, the SO(3) matrix should have the property of orthogonality. Therefore, the methods using the SO(3) matrix inevitably use some method to approximate the predicted and corrected matrix to some orthogonal matrix [19,20]. The use of the sine rotation vector does not require approximation for the normalization of vectors or the orthogonalization of matrices.
The attitude estimation problem studied in this paper is formulated in Section 2. Sections 3-5 describe the attitude estimation procedure. Section 6 shows the results of experiments and simulations using the proposed method and the previously-reported methods and presents a comparison of these results. Section 7 concludes the paper and suggests further research regarding the proposed method.

Nomenclature
The notations that are used for the derivation of the method are listed below.

x(t)
attitude of a robot at time t; , θ(t) and ψ(t) indicate roll, pitch and yaŵ x(t) attitude estimated at time t through prediction and a correction procedure;x(t) = (φ(t),θ(t),ψ(t)) T P(t) error covariance of the estimated attitudex(t) x − (t) attitude predicted at time t, before it is corrected by the measurements; acceleration measured in the instrument coordinate frame; a(t) = (a x (t), a y (t), a z (t)) T a unit (t) normalized acceleration measurement; a(t) m(t) magnetic field measured in the instrument coordinate frame; measurements of roll, pitch and yaw calculated from the a(t) and m(t) at time t; linear velocity of the robot in the robot coordinate frame; v(t) = (u(t), v(t), w(t)) T

ω(t)
rotational velocity of the robot in the robot coordinate frame; ω(t) = (p(t), q(t), r(t)) T g(·) motion model of a robot that relates the robot attitude x(t) and the linear velocity v(t) to the rotational velocityẋ(t) of the robot;ẋ(t) = g (x(t), v(t)) h(·) the measurement model that relates state x(t) to the measurement z(t); z(t) = h(x(t)) t k the k-th discretized sampling time instant ∆t k time period between t = t k−1 and t = t k ; ∆t k = t k − t k−1 In many studies, an underwater robot is often referred to as an underwater vehicle. Thus, in the following sections, the terms vehicle and robot will be used interchangeably.

Problem Formulation
The following is a description of the problem to be solved. Attitude estimation problem: Find the attitudex(t k ) of an underwater robot at time t = t k , given the provided measurements a(t k ), m(t k ). The attitudex(t k−1 ) estimated at time t = t k−1 , the linear velocity v(t k ) and the angular velocity ω(t k ) are given.
The estimation procedure repeats at every time t k when new measurements are available, and it runs recursively based on the results provided at the previous estimation sequence. The estimated attitudex(t k ) will be used for the estimation ofx(t k+1 ) at the next iteration of the procedure. The estimation procedure consists of two sub-procedures: prediction and correction. The prediction and correction procedures will be derived in the following sections.
The rotational velocity of the vehicle ω(t k ) detected with a gyroscope in the AHRS is used for the prediction. The velocity ω(t k ) is measured in the instrument coordinate frame. It is assumed that the instrument coordinate frame is adjusted to coincide with the vehicle coordinate frame. The acceleration a(t k ) and magnetic field m(t k ) measurements are used for the correction; furthermore, these are measured by an AHRS in the instrument coordinate frame that is also assumed to be coincident with the vehicle coordinate frame.
The linear velocity of the vehicle v(t k ) is detected with the DVL. The velocity v(t k ) is measured in the instrument coordinate frame that also coincides with the vehicle coordinate frame. Using the estimated attitude, the linear velocity v(t k ) is converted to the velocity in the Earth-fixed coordinate frame; this converted velocity is then integrated to find the location of the robot in the Earth-fixed coordinate frame. The linear velocity v(t k ) can also be used to extract the pure gravitation field by removing the vehicle acceleration from the acceleration measurement a(t k ).
The proposed method utilizes the short-term superiority of the angular rate measurement and the long-term stability of the attitude algebraically calculated from the acceleration and magnetic field measurement. Table 1 depicts the complementary aspects of the these measurements. The short-term superiority of the gyroscope measurement is utilized in the prediction stage where the attitude is predicted using the angular rate measurement. Though the angular rate provides the short-term change of the attitude with high accuracy, the accuracy of the attitude calculated from the numerical integration of the attitude change deteriorates with time. To compensate for the deterioration, the long-term stability of the attitude that is algebraically calculated from the acceleration and the magnetic field is used in the measurement update or correction stage. The DVL is used to calculate the location using the estimated attitude. Furthermore, it can be used to reduce the system dynamics effect that perturbs the gravitation field in calculating the roll and pitch of the robot.

Predictions of Attitude and Covariance
The prediction step that is otherwise called the "time update step" is used to predict the state and error covariances for time t k ; to complete the step, the measured angular velocity ω(t k−1 ) at time t = t k−1 , the statex(t k−1 ) and the error covarianceP(t k−1 ) that was estimated previously at time t k−1 are used. The prediction is based on the function g(·) that is used to determine the time derivative of the attitude, as follows:ẋ Using Equations (1) and (2), the attitude is predicted as x − (t k ) according to Equations (3) and (4), as follows: In these equations, S(·), C(·) and T(·) represent sin(·), cos(·) and tan(·), respectively. The Jacobian matrix J of the function g(·) is used to find the linearized state transition matrix A according to Equation (5), as follows: The Jacobian matrix J of the function g(·) is given according to Equation (6), as follows: where the time variable (t k−1 ) is deleted from p, q, r,φ andθ for notational simplicity. The error covariance P − (t k ) of the predicted attitude x − (t k ) is derived as Equation (7) through the use of the linearized state transition matrix A, as follows: where R is the error covariance of the vehicle motion. The two Equations (4) and (7) complete the prediction step.

Corrections of Predicted Attitude and Covariance
The correction step that is otherwise called the "measurement update step" adjusts the predicted attitude x − (t k ) and error covariance P − (t k ) by using the measurements at time t k , z(t k ). For the corrections, the measurement model matrix H that is the linearized measurement model of h(·) is required. Because the measurement z(t k ) consists of the roll, pitch and yaw of the Euler angles that match those in state x(t k ), the measurement model matrix H is identity, that is H = I. Using the measurement model H, the Kalman gain for the adjustment of x − (t k ) and P − (t k ) is given according to Equation (8), as follows: where Q is the error covariance of the measurement. The predicted attitude x − (t k ) is corrected tox(t k ) by Equation (9), as follows: The predicted covariance P − (t k ) is adjusted toP(t k ) by Equation (10), as follows: The innovation (9) will be derived in a manner that is different from the usual methods. The proposed method that uses the sine rotation vector will be described in Section 5.

Calculation of Innovation by Sine Rotation Vector
The innovation ν(t k ) is given by Equation (11), as follows: The innovation ν(t k ) is the difference between the measured attitude z(t k ) and the presumed measurement h(x − (t k )) that is calculated under the assumption that the robot is in its predicted attitude x − (t k ). Many existing methods calculate the measurement z(t k ) from the acceleration a(t k ) and magnetic field m(t k ), whereby the predicted attitude x − (t k ) is used as the presumed measurement h(x − (t k )).
This paper proposes a new approach for finding the innovation, which is the difference between the measured attitude z(t k ) and the presumed attitude h(x − (t k )). The proposed method finds the sine rotation vector between the presumed attitude and the measured attitude. Because vehicle acceleration is negligible compared to gravity, the measured acceleration a(t k ) is directed toward the −z direction of the Earth-fixed coordinate frame; that is, the acceleration measurement a(t k ) is the Earth-fixed −z direction vector that is represented in the instrument coordinate frame. Let the presumed −z direction represented in the vehicle coordinate frame be denoted by −z pres (t k ). The presumed −z direction vector −z pres (t k ) is calculated according to Equation (12), as follows: where v R w (t k ) is the rotation matrix from the Earth-fixed coordinate frame to the vehicle coordinate frame at time t = t k . v R w (t k ) is calculated using the predicted attitude x − (t k ); then, the cross product of −z pres (t k ) and a unit (t k ) meets the sine rotation vector r −z (t k ). The sine rotation vector r −z (t k ) represents the rotation from the presumed −z direction to the measured −z direction, as follows: The direction of the sine rotation vector r −z (t k ) represents the axis of rotation from the presumed −z direction to the measured −z direction. The length of the sine rotation vector r −z (t k ) is the sine of the rotation angle from the presumed −z direction to the measured −z direction. Figure 1a represents graphically how the r −z (t k ) is calculated and represents the rotation.
Likewise, the measured magnetic field m(t k ) directs the Earth-fixed x direction that is represented in the instrument coordinate frame. The bias of the measured magnetic field m(t k ) from the Earth-fixed x direction should be adjusted by using the methods for bias estimation [15,21]. Let the presumed x direction that is represented in the vehicle coordinate frame be denoted by x pres (t k ). Then, Equation (14) provides the presumed x direction vector x pres (t k ) that is represented in the vehicle coordinate frame, as follows: The cross product of x pres (t k ) and m unit (t k ) then becomes the sine rotation vector r x (t k ). The sine rotation vector r x (t k ) represents the rotation from the presumed x direction to the measured x direction, as follows: Figure 1b represents graphically how the r x (t k ) is calculated and represents the rotation. Both of the sine rotation vectors r −z (t k ) and r x (t k ) bear the rotation information from the presumed attitude to the measured attitude. These two vectors are the rotations determined by two different sensors (the accelerometer and the magnetometer) and are combined using Equation (16), as follows: γ −z and γ x in Equation (16) are mixing parameters with a sum value of one, as follows: The values of γ −z and γ x need to be set based on the general rule: the larger value is assigned to the γ that corresponds to the more reliable measurement between the acceleration and magnetic field. If the acceleration measurement is more reliable than the magnetic field measurement, then γ −z will have a larger value, and vice versa. In the experiment, the values are determined through trial and error, as well as based on the general rule.
From the sine rotation vector, the rotation axis b and rotation angle β are found by using Equations (18) and (19), as follows: The rotation matrix R(β, b) that is represented in terms of the rotation angle and rotation axis is described in Equation (20), as follows: where V(β) = 1 − C(β) is applied. The rotation matrix can also be represented in terms of the Euler angles (φ, θ, ψ) as the matrix R(φ, θ, ψ), which is shown in Equation (21).
From the equivalence of the two rotation matrices R(β, b) and R(φ, θ, ψ), the Euler angles are found by using Equation (22), as follows.
The Euler angles in Equation (22) represent the rotation from the presumed attitude to the measured attitude; consequently, it is concluded that the innovation ν(t k ) = z(t k ) − h(x − (t k )) that was used for Equation (9) is found by using Equation (23), as follows: A brief explanation regarding the calculation of the measurement z = (φ, θ, ψ) T from the acceleration a = (a x , a y , a z ) T and magnetic field m = (m x , m y , m z ) T follows [22]. First, the roll φ and pitch θ are calculated using the acceleration a = (a x , a y , a z ) T in Equations (24) and (25), as follows: φ and θ are used to obtain the transformation matrix w R v through the use of Equation (26), as follows: Then, the yaw ψ is calculated using Equation (27), as follows:

Sine Rotation Vector
The sine rotation vector is defined by the following Equation (28).
where b is the unit vector in the direction of the rotation axis and β is the angle of rotation. The sine rotation vector is different from the rotation vector, which is defined by rb = βb [20,23]. The difference between the sine rotation vector and rotation vector is the magnitude of the vectors. The magnitude of the sine rotation vector is the sine of the rotation angle, while the magnitude of the rotation vector is the rotation angle itself. The use of the sine rotation vector improves the attitude estimation in calculating the innovation vector in the EKF measurement update stage. First, the sine rotation vector uniquely determines the physical rotation regardless of the attitude to be rotated. In the case of the Euler angle difference between two attitudes represented in Euler angles, the rotation by the Euler angle difference does not necessarily correspond to the rotation between the attitudes. For example, the rotation of the attitude (φ 1 , θ 1 , ψ 1 ) = (0, π 2 , π 3 ) by the rotation (φ 2 , θ 2 , ψ 2 ) = ( π 6 , π 6 , π 6 ) does not result in the attitude (φ 1 + φ 2 , θ 1 + θ 2 , ψ 1 + ψ 2 ) = (0 + π 6 , π 2 + π 6 , π 3 + π 6 ). On the other hand, the rotation of the attitude (φ 1 , θ 1 , ψ 1 ) = (0, 0, 0) by the rotation (φ 2 , θ 2 , ψ 2 ) = ( π 6 , π 6 , π 6 ) does result in the attitude (φ 1 + φ 2 , θ 1 + θ 2 , ψ 1 + ψ 2 ) = (0 + π 6 , 0 + π 6 , 0 + π 6 ). This example illustrates that the same rotation results in different physical rotation depending on the attitude to which the rotation is applied. On the contrary, the sine rotation vectors r x and r −z uniquely rotate the predicted directions x pres and −z pres to the measured directions m unit and a unit , regardless of the attitudes. In summary, the sine rotation vector uniquely determines the physical rotation regardless of the attitude to be rotated.
Second, the sine rotation vector relieves the effect of the bias and interference in measurement of the gravitational field and the magnetic field. The sine rotation vectors r x and r −z deal only with x and −z directional measurements, respectively. r x is free from the bias in the direction of y and z. Likewise, r −z is free from the bias in the direction of x and y. Therefore, use of the sine rotation vector mitigates the effect of bias in the field measurement, even though the method does not explicitly estimate and compensate the bias.

Experiments and Results
The method is tested in simulations and two experimental environments. The simulation uses two measurement datasets, which are subject to both random Gaussian noise and bias. The first experiment uses an underwater robot in a test pool. The second test uses a ground vehicle equipped with GPS, AHRS and a high-precision FOG (fiber optic gyroscope). The results were analyzed and compared to the results of other methods that calculate innovation ν(t k ) by using the Euler angles of Equations (24), (25) and (27), without transforming to the sine rotation vector; these other methods include EKF, the unscented Kalman filter (UKF) and the complementary filter (CF).

Test through Simulation
The sine rotation vector method is tested using simulation data. For comparison, EKF, which calculates innovation by subtracting the measured Euler angles from the predicted Euler angles without transforming to the sine rotation vector, is also implemented. Two simulated test datasets, which differ in measurement error, are used. They assume the same robot motion, where the robot moves with the linear velocity and angular velocity given as Equations (29) and (30), over the traveled distance 600 m for the time period of 600 s.
0.03π sin(t/10)rad/s 0.03π cos(t/10)rad/s 0.03π sin(t/100)rad/s    As for the measurement of linear velocity by a DVL, the measurement is assumed to have Gaussian noise with zero mean and σ = 0.2 m of standard deviation in all of the x, y and z directions. The measurement data for acceleration and magnetic field are generated by simulation, such that the attitude includes some bias, as well as random Gaussian noise. Table 2 describes the bias and random noise in attitude applied to produce the two simulated measurement datasets. Measurement data for Simulation 1 are affected more by bias than the data for Simulation 2, while they are corrupted by the same degree of random Gaussian noise. Table 2. Uncertainty in the measurement in the simulation tests.

Simulation
Measurement Uncertainty Value (degree)

Simulation 2
Acceleration bias (∆φ, ∆θ, ∆ψ) (5, 5, 5) random noise (σ φ , σ θ , σ ψ ) (1, 1, 1) Magnetic field bias (∆φ, ∆θ, ∆ψ) (1, 1, 1) random noise (σ φ , σ θ , σ ψ ) (1, 1, 1) Table 3 shows the error statistics for the simulation tests. The average and root mean square (RMS) of the attitude estimation error in roll, pitch and yaw are shown together with the distance error ratio relative to the traveled distance. In the table, the elements of a vector are ordered roll, pitch and yaw. In both simulations, the proposed method performs better than the other methods. Because the measurement data of the magnetic field for Simulation 2 are biased less than the data for Simulation 1 (while the measurement data of acceleration for Simulation 2 are biased a little more than the data for Simulation 1), most methods work better in Simulation 2 than in Simulation 1. Figure 2 depicts the roll error and estimated trajectory for Simulation 1. The robot starts at the origin and stops at the destination (83.75, −3.49, 4.61) m.

Experiment in a Test Tank
The environment for the test-tank experiments is shown in Figure 3. The experiments involved the use of an AHRS (Microstrain 3DM-GX3 25) [24] and a DVL (LinkQuest NavQuest 600 Micro) [25]. The robot navigated through the circular and rectangular trajectories shown in Figure 4. For each trajectory, once the robot circulates and returns to its initial location, it turns back and traces the path to the initial location in a reverse direction. One of the problems with the test tank experiment is that the navigation range is limited, and the performance of the method in practical use is not fully verifiable. In order to mitigate the limitation, the test tank experiment adopted turn around and trace back motion. In the experiment, the robot repeats round trip circulation to extend the navigation range, so that the traveled distance is 282.7 m and 100.8 m for the circular and rectangular motion, respectively. In addition, once the robot circulates and returns to its initial location, it turns back and traces the path to the initial location in the reverse direction. This large and sharp turning motion provides a harsher and more serious circumstance to attitude estimation. The robot finally returns to its initial location; thus, the destination of the navigation is the robot's initial location.  Because the experiment does not involve the use of any instrument that can provide the true attitude, the performance of the methods was verified indirectly from the estimated trajectories that are calculated using the estimated attitude. The estimated location deviates from the true location, and the deviation increases as the travel time and distance are extended. The deviation at the destination reflects the performance of the attitude estimation method; therefore, the distance error between the estimated destination and the actual destination is used as an index for the performance evaluation. Table 4 presents a comparison of the distance errors of the four methods. The distance error ratio indicates the ration of the distance error at the destination relative to the overall traveled distance.
In Table 4, "SRV" denotes the method proposed in this paper, and the results show that it outperforms the other methods.  Figure 5 shows the estimated trajectory for the SRV method. The blue line depicts the estimated trajectory, while the red line indicates the trajectory provided by dead reckoning, whereby the velocity is integrated (without any filtering) to estimate the attitude. Figure 6 shows the results of the pure EKF method, which uses Equations (24), (25) and (27) for the Euler angle measurement. As in Figure 5, the blue line is the estimated trajectory of the EKF, while the red line is the trajectory provided by dead reckoning without filtering.    Figure 6. Circular path test tank experiment: trajectory in the xz plane estimated by the proposed method compared to the trajectories calculated without filtering and with the pure-EKF method. (a) Trajectory by the proposed method and that calculated without filtering; (b) Trajectory by the pure-EKF method and that calculated without filtering.

Experiment on the Ground
The experiment on the ground to test the proposed method uses a ground vehicle equipped with GPS, AHRS (Microstrain 3DM-GX4 25) and FOG. The model's FOG (Spatial FOG by Advanced Navigation) [26] provides the attitude data used as the reference to compare the attitudes estimated by the methods. Figure 7 shows the vehicle and sensors used for the experiment. The vehicle navigated for 170 s over a distance of approximately 551 m. Figure 8 shows the route of the navigation.   Figure 9 shows the yaw estimates of the methods compared to the yaw provided by FOG, as well as the error of the estimates. The results of the experiment are listed in Table 5, where the statistics of the yaw error are shown. EKF, UKF and CF exhibit comparable error statistics, while the proposed method outperforms these methods.

Conclusions
This paper proposes a new method for estimating the attitude of an underwater robot. The method uses the sine rotation vector to determine the rotational difference between the measured attitude and the presumed attitude. Compared to existing methods that directly calculate the Euler angle difference, the proposed method resolves the problem that arises from a non-unique mapping between the rotation and the Euler angle difference, when the attitude is described by the parameters of the Euler angles.
The experiments compared the proposed method to the existing methods that use EKFs, UKFs and CFs based on the differences of the Euler angles. The proposed method outperforms all of the other methods in terms of the location estimation of an underwater robot and the attitude estimation of a ground vehicle.
An investigation especially through underwater experiment to compare the properties of the sine rotation vector with other attitude representation methods, such as Euler angles, rotation matrices and quaternions with respect to attitude estimation, remains as the focus of further research. Furthermore, fusing more sensor measurements that provide directional information in the framework of the sine rotation vector is expected. Finally, it is expected that the proposed method will be implemented for navigation in a lake or ocean environment for the verification of the practical applicability, in a future study.