A Direct and Non-Singular UKF Approach Using Euler Angle Kinematics for Integrated Navigation Systems

This paper presents a direct and non-singular approach based on an unscented Kalman filter (UKF) for the integration of strapdown inertial navigation systems (SINSs) with the aid of velocity. The state vector includes velocity and Euler angles, and the system model contains Euler angle kinematics equations. The measured velocity in the body frame is used as the filter measurement. The quaternion nonlinear equality constraint is eliminated, and the cross-noise problem is overcome. The filter model is simple and easy to apply without linearization. Data fusion is performed by an UKF, which directly estimates and outputs the navigation information. There is no need to process navigation computation and error correction separately because the navigation computation is completed synchronously during the filter time updating. In addition, the singularities are avoided with the help of the dual-Euler method. The performance of the proposed approach is verified by road test data from a land vehicle equipped with an odometer aided SINS, and a singularity turntable test is conducted using three-axis turntable test data. The results show that the proposed approach can achieve higher navigation accuracy than the commonly-used indirect approach, and the singularities can be efficiently removed as the result of dual-Euler method.


Introduction
Integrated navigation systems, in which two or more navigation systems are combined to yield greater precision than any single component system operating in isolation, are usually employed in practice. It is common to use strapdown inertial navigation systems (SINSs) to provide data with good short term accuracy, while other sensors are employed to provide good long term stability.
The variety of modern navigation aids now available is extensive, but external navigation aids are limited for military and underwater applications where accurate navigation is required without the help of external information, such as GPS, which may be disturbed or blocked. In such circumstances, the aid of velocity is popular because it is more reliable and accessible. As a result, the integration of SINSs with velocity aid has become a standard approach and received much attention. Velocity observations may be provided by velocimeters [1,2], e.g., odometers are used for land vehicles [3,4], and Doppler velocity logs (DVLs) are generally mounted on the shell for autonomous underwater vehicles (AUVs) [5,6]. Given the popularity of the integration of SINS/odometer and SINS/DVL,

System Model and Measurement Model
The local geographic navigation frame mechanization is used. The main coordinate frames used in this paper different from other references are defined as follows: the body frame (b-frame) coordinate system of the vehicle is the orthogonal reference frame aligned with the inertial measurement unit (IMU) axes, which is defined with x axis along the transversal direction right, y axis along the longitudinal direction forward, and z axis along the vertical direction upward, completing a right-handed system (right-forward-upward, RFU). The local geographic frame is used as the navigation frame (n-frame), which is the orthogonal reference frame aligned with east-north-up (ENU) geodetic axes.
Velocity measurements in the b-frame are used to aid the on-board navigation system. Suitable measurements may be provided by odometers or DVLs. In order to provide estimates of the attitude, the velocity and the position in the on-board inertial navigation system, a ten state UKF is designed.
A block diagram representation of the direct data fusion approach for a b-frame velocity aided SINS is shown in Figure 1. A block diagram representation of the direct data fusion approach for a b-frame velocity aided SINS is shown in Figure 1. are the velocity aid measurements along x, y, z-axis of body. In Figure 1, the vehicle motion changes the outputs of the inertial sensors and the aid, which are inputs of the system equations and measurement equations of the UKF, respectively. The UKF compares the b-frame velocity derived from aid outputs with the corresponding quantity estimated from the SINS, and estimates the inertial sensor biases, the attitude and the velocity in the n-frame. The n-frame velocity is then integrated into the position. The associated nonlinear system equations and measurement equations are described in the sections that follow.

System Model
The system process comprises the SINS mechanization and the inertial sensor error modeling. For a terrestrial navigation system operating in the local geographic reference frame, the navigation equation may be expressed as:   2 n n n n n n en ie en en where n en v represents the velocity with respect to the earth expressed in the n-frame defined by the directions of east, north , and the local vertical in component form: (2) where vup is the vertical component of vehicle velocity with respect to the Earth, [•] T is the transpose of [•]. The specific force vector expressed in the n-frame f n is given by: (3) ( ) n bT b n  C C (4) where n b C is the direction cosine matrix (DCM) representing the rotational transform of vectors from the b-frame to the n-frame. Rotating by the sequence of heading, pitch and roll leads to n b C , and the three successive rotations may be expressed mathematically as three separate direction cosine matrices defined as Cψ, Cθ, and Cγ, respectively. ψ ∈ [−π,+π], and it is defined with a positive Where f b ibx,y,z and ω b ibx,y,z are the accelerometer and gyro measurements along x, y, z-axis of body, θ, γ, ψ are the pitch, roll and heading, respectively, v east and v north are the east and north vehicle velocities with respect to the earth, respectively, L is the geographic latitude, λ is the longitude, v bx,y,z are the velocity aid measurements along x, y, z-axis of body. In Figure 1, the vehicle motion changes the outputs of the inertial sensors and the aid, which are inputs of the system equations and measurement equations of the UKF, respectively. The UKF compares the b-frame velocity derived from aid outputs with the corresponding quantity estimated from the SINS, and estimates the inertial sensor biases, the attitude and the velocity in the n-frame. The n-frame velocity is then integrated into the position. The associated nonlinear system equations and measurement equations are described in the sections that follow.

System Model
The system process comprises the SINS mechanization and the inertial sensor error modeling. For a terrestrial navigation system operating in the local geographic reference frame, the navigation equation may be expressed as: where v n en represents the velocity with respect to the earth expressed in the n-frame defined by the directions of east, north , and the local vertical in component form: where v up is the vertical component of vehicle velocity with respect to the Earth, The specific force vector expressed in the n-frame f n is given by: where C n b is the direction cosine matrix (DCM) representing the rotational transform of vectors from the b-frame to the n-frame. Rotating by the sequence of heading, pitch and roll leads to C n b , and the three successive rotations may be expressed mathematically as three separate direction cosine matrices defined as C ψ , C θ , and C γ , respectively. ψ ∈ [−π,+π], and it is defined with a positive direction anticlockwise from the north. Therefore, C b n is expressed as the product of these three separate transformations as follows: The true specific force vector f b ib is given by: where δ f b ibx,y,z are the accelerometer measurement errors along x, y, z-axis of body. The other terms in Equation (1) are given by: r N = r q (ρsin 2 L + 1) (10) where ω n ie is the Earth rotation rate with respect to the inertial frame expressed in the n-frame, and ω n en represents the turn rate of the n-frame with respect to the Earth-fixed frame expressed in the n-frame, i.e., the transport rate, ω ie is the Earth rotation rate, r M and r N are the meridian and transverse radius of curvature of the Earth's reference ellipsoid, respectively, r q is the length of the semi-major axis, ρ is the flattening of the ellipsoid, and g is the local Earth's gravitational acceleration. A large number of parameterizations exist in the literature for attitude matrix. Reference [27] contains a complete survey of attitude representations. DCM, Euler angles, the quaternion, Rodrigues parameters (RP), and modified Rodrigues parameters (MRP) are the most popular ones. A generalization of the Rodrigues parameters (GRP) is presented in [28]. The GRP can be used to construct a set of three symmetric stereographic parameters, or to construct a set of three asymmetric stereographic parameters. The RP and MRP can be considered a special case of the general symmetric stereographic parameters. The RP, MRP and GRP are all derived from the quaternion by means of a stereographic projection of the four-dimensional unit sphere onto a three-dimensional hyperplane. A redundant parameter of the quaternion is removed, and the amount of calculation is reduced. Every parameterization has its own advantages and disadvantages. The main properties of some parameterizations of attitude rotational matrix are listed in [29].
The DCM is of little use except for analytical studies and transformation of vectors largely because of its nine dimensions with six redundant parameters. The quaternion representation is the subject of many literatures and frequently uses in practices, but the quaternion constraint makes undesirable implementation of the quaternion UKF as mentioned before. The Euler angles, RP, MRP and GRP have  They all have the minimum  dimensions for attitude representation, and the Euler angle representation is the most famous and the  most commonly used. The RP, MRP and GRP are often used in attitude determination and control for  spinning bodies like satellites, where the vehicles rotate mainly about a certain axis, i.e., the vehicles have regular angular motion parameters. The Euler angles are easiest to visualize geometric recipe to describe the motion of any vehicle with respect to a reference frame by three successive rotation angles. There are twelve Euler angle sequences associated with twelve representations of the DCM.
As we all know, any set of three parameters has singularities. Different three-parameter sets distinguish themselves by having their singularities at different orientations. The RP, MRP, GRP have their singular point determined by different stereographic projections. The singularity for the classical RP and MRP is ±π and ±2π respectively with respect to rotation angles. The singularity for the symmetric GRP can be anywhere between a principal rotation of 0 and 2π, and the singularity for the asymmetric GRP is determined by both a principal angle and an axis of rotation. Different rotation sequences have different singularities for Euler angle sets. The 3-2-1 Euler angle set has a singularity at θ = ±π/2. The problem of how to avoid singularity associated with a three-element attitude parameterization has been well studied in the literatures. The approaches are mainly based on the method of sequential rotations (MSR), which removes the singularity by switching from singular sets to non-singular sets at the singularity for RP, MRP and Euler angles. Two applications are presented in detail to show how to avoid the singularity when using RP and MRP in [30]. And this problem is discussed for GRP in [28] and for Euler angles in [29,31,32].
The Euler angle representation is perhaps one of the simplest techniques in terms of physical appreciation. Most of all, the three Euler angles are uncorrelated. Therefore, when the Euler angles are used as the filter states, there is no extra change in the filtering.
Although the Euler angle representation suffers from the so-called singularity, the tests covered in this paper, conducted in a land vehicle with little horizontal attitude change, will not suffer from it. The Euler angle representation is suitable for this case because the calculated attitude angles have high accuracy when θ is close to 0 or ±π. The Euler angle sequence 3-2-1 is adopted, as indicated by Equation (5). If operation θ→±π/2 is desired singularity could occur, and in this circumstance an alternative set of Euler angles, e.g., the 3-1-2 Euler angle set in which the rotation sequence is heading-roll-pitch, can be employed to remove the singularity [27,29,31,32]. The best conversion angles are ±π/4 and ±3π/4, which are discussed and examined in [32,33]. Moreover, the Euler angles are very intuitive to work with, making the final filtering simpler. The Euler angle representation, like the above mentioned three-parameter representation, is employed in this paper.
The propagation of the Euler angles with time is governed by the following differential equations: where ω b nb is angular velocity of the b-frame relative to the n-frame expressed in the b-frame. The solutions of the . γ and . ψ equations become indeterminate at the singularities where θ = ±π/2. ω b nb is given by: The true angular rate vector ω b ib is given by: where δω b ibx,y,z are the gyro output errors along x, y, z-axis of body. Equation (12) is known as the Euler angle kinematics equation. The way to obtain the Euler angle rates from the angular velocity is  [27], but Equation (12) is not identical to it because of the definition of the b-frame, n-frame and the Euler angles.
The gyro measurement errors δω b ibx,y,z in Equation (14) and the accelerometer measurement errors δ f b ibx,y,z in Equation (6) are modeled as the sum of a Gaussian white noise and a bias expressed in the b-frame. Therefore, the IMU measurement errors are given by: where ε gx,y,z and w gx,y,z are the gyro bias and noise components along x, y, z-axis of body respectively, ∇ ax,y,z and w ax,y,z are the accelerometer bias and noise components along x, y, z-axis of body respectively. With the assumption that the gyro and accelerometer biases can be represented as random constants, inertial sensor bias dynamics may be given by the trivial differential equations as follows: .
East and north velocities, all three Euler angles, and the inertial sensor biases are chosen as the states. The state vector can be expressed in component form as: where x is the ten-dimensional state vector.
The system noise vector that represents the inertial sensor noise can be written as: where w is the ten-dimensional system noise vector, which is modeled as Gaussian white noise. w has zero mean and is normally distributed (Gaussian) with a power spectral density of Q. Q is a 10 × 10 diagonal matrix, the elements of which are selected in accordance with the anticipated level of inertial sensor measurement noise. According to the selected state vector in Equation (18) and the system noise vector in Equation (19), Equations (1), (12) and (17) can be combined and expressed in state space form as: where F c (x) is the nonlinear state transfer function, and G(x) is the system noise input function. F c (x) is a 10 × 1 function matrix, and G(x) is a 10 × 10 function matrix. They can be expressed in concrete formulas as: · · · · · · · · · · · · · · · · · · · · · · · · 0 5×10 Equation (20) is the continuous nonlinear system equation. A fourth-order Runge-Kutta scheme with two steps between successive inertial sensor sampling instances is employed for numerical integration . (20) is assumed to be converted to a difference equation shown as follows: where x k and x k − 1 represent the state x at time t k and t k − 1 respectively, F (x k − 1 ) represents the discrete nonlinear state transfer function, which can be supposed to be the integration form of F c (x) at the state x k − 1 within the integration period denoted as the time interval T 1 , w k − 1 represents the discrete system noise at time where w k and w j represent the discrete system noise at time t k and t j , respectively, and w k will be characterized by the covariance matrice Q k , δ kj is the Dirichlet function which means that δ kj is equal to 1 when k is equal to j, and otherwise δ kj is equal to 0, and G(x k ) is the value of G(x) at time t k .

Measurement Model
We directly choose the b-frame velocity from sensor outputs as the filter measurement, which can be expressed as: where z is the three-dimensional filter measurement vector. The b-frame velocity measurements are compared with the corresponding quantities from the SINS. Estimates of the b-frame velocity may be obtained from the SINS estimates of the n-frame velocity (v east , v north , v up ) and the C b n , therefore, the filter measurement equation can be expressed as: where η is the three-dimensional filter measurement noise vector that accounts for the error in the aid sensor measurement, η vbx , η vby and η vbz denote the measurement noise components along x, y, z-axis of body. η is assumed to be a zero mean and additive white Gaussian noise with a power spectral density of R. R is a 3 × 3 diagonal matrix, the elements of which are selected in accordance with the anticipated level of velocity measurement noise. According to the filter measurement in Equation (26), the filter measurement noise vector in Equation (28) and the state vector in Equation (18), Equation (27) is written in state space form as: where H(x) is the nonlinear measurement function, which is derived from Equation (27). H(x) is a 3 × 1 function matrix as follows: The measurement equation at time t k expressed in terms of the state can be shown as follows: where z k is the filter measurement at time t k , H(x k ) is the value of H(x) at time t k , η k is the discrete filter measurement noise vector at time t k , and η k is a zero mean white noise sequence that can be characterized by the covariance matrice R k as follows: where T 2 is the aid sensor data updating period, and η j is the discrete filter measurement noise vector at time t j . The terms w k and η k are assumed to be independent, i.e.,:

Filter State Model Analysis
The system model in Equation (25) and the measurement model in Equation (31) construct the filter state model. For the purpose of illustration, the state vector of conventional integrated navigation model in many references usually consists of three attitude errors, three velocity errors, three position errors, and the inertial sensor errors. Well-known SINS error models are derived from the perturbation theory. After the errors are estimated, the navigation information calculated by the SINS navigation algorithm is corrected by these errors.
In this paper, navigation parameters instead of their associated errors are chosen as the states. The system model in Equation (25), which contains the differential equations of velocity, Euler angle, and inertial sensor bias, is nonlinear and simple without any restrictions and approximations. The system model is also the SINS mechanization. Note that the system equations are considered a direct formation, as opposed to the alternative indirect (error) formulation. The navigation information is obtained from the states directly after the filtering, so the error correction is no longer needed. The navigation computation is performed simultaneously in the filter time updating.
For the b-frame velocity aided SINS like SINS/DVL and SINS/Odometer in the indirect approaches aforementioned, the velocity measured in the b-frame is usually first transformed into the velocity expressed in the n-frame using the DCM calculated by the SINS. Then the difference of the transformed velocity and the corresponding quantity from the SINS is used as the filter measurement. The measurement models are indirect and linear. The cross-noise will arise from the formation of the filter measurement [6].
In this research, the aid outputs are chosen as shown in Equation (26), instead of the associated difference, as the filter measurements. The measurement equations contain the aid measurement noise, while the system equations contain the inertial sensor noise; therefore, the inertial sensor noise is separated from the aid measurement noise. The cross-noise problem is solved. The measurement model is direct and nonlinear. The velocity transformation is realized in the measurement equations. Hence the measurement model in Equation (31) is more accurate and easier to be comprehended, compared with those in the indirect approaches.
In Section 2, the system equations and the measurement equations are developed. An UKF will be employed to estimate the state vector because of their high nonlinearity. In the succeeding section (Section 3), the UKF is presented in detail.

UKF
The UKF uses the unscented transformation (UT) which is developed as a method to propagate mean and covariance information through nonlinear transformations. The UT is founded on the intuition that it is easier to approximate a probability distribution than to approximate an arbitrary nonlinear function or transformation. The UT chooses a set of sigma points to approximate a Gaussian distribution. The sigma points match the mean and covariance. The nonlinear function is applied to each point, and in turn yields a cloud of transformed points. The statistics of the transformed points can then be calculated to form an estimate of the nonlinearly transformed mean and covariance. A description of the UKF can be found in [9,13,14].
We summarize the steps involved in the time update and the measurement update of an UKF as shown in Algorithm 1.
where x 0 and P 0 are the initial state and the initial covariance matrix, respectively.

Time update
Evaluate the propagated sigma points ( j = 1, 2 . . . N s ): Estimate the predicted state:x Estimate the predicted state error covariance: where ξ j and a j are the j-th sigma point and the associated weight, respectively, N s denotes the number of the sigma points, and Q k-1 is the discrete system noise covariance matrix at time t k − 1 .

Measurement update
Factorize: Evaluate the sigma points ( j = 1, 2 . . . N s ): Evaluate the propagated sigma points (j = 1, 2 . . . N s ): Estimate the predicted measurement:ẑ Estimate the innovation covariance matrix: Estimate the cross-covariance matrix: Estimate the Kalman gain: Estimate the updated state:x Estimate the corresponding error covariance: There are a series of techniques to determine the sigma points [14]. The symmetric sigma point sets are selected, which is the most commonly used UT [13]. The sigma points are given by: where d is the dimension of the states and d is equal to 10 here as shown in Equation (18), κ = −7 is chosen in accordance with the heuristic d + κ = 3, [1] j is the j-th column vector, the right-hand side of (50) has 2d column vectors, each of which has d components.
Note that the period of the time updating differs from that of the measurement updating. Since the system equations include the inertial sensor outputs, the time updating is processed once new data of the inertial sensors arrive, and thus the time updating period is T 1 . The time updating is implemented at a fast inertial sensor sample rate, e.g., 200 Hz here. At the end of the time updating, if the aid measurements do not arrive, the predicted statex k|k−1 and the predicted error covariance P k|k−1 are transferred to the updated statex k|k and the updated error covariance P k|k , respectively, the first step of time updating is then repeated, and the new time updating loop begins. Upon the arrival of a new navigation aid measurement, the measurement updating is processed, and thus the measurement period is T 2 . The measurement updating is implemented at a slow navigation aid sample rate, e.g., 10 Hz here.

Experimental Results
The purpose of the experiment is to show the advantages of the new direct algorithm over the typical indirect Kalman filter.
The performance of the proposed direct UKF approach for the aided SINS is examined with road test data from a land vehicle. The test vehicle is shown in Figure 2. The SINS/odometer integrated navigation system is used. The navigation aid is a precision odometer from which the forward speed is obtained. The odometer is installed at the right rear wheel. The inertial sensors used in the experiments are from a navigation grade IMU. A GPS is installed on the top of the vehicle. Note that the GPS is not integrated in the navigation system, but it is only used as the position reference. The GPS receiver provides position with precision of about 10 m, and update rates up to 1 Hz. There are a series of techniques to determine the sigma points [14]. The symmetric sigma point sets are selected, which is the most commonly used UT [13]. The sigma points are given by: where d is the dimension of the states and d is equal to 10 here as shown in Equation (18), κ = −7 is chosen in accordance with the heuristic d + κ = 3, [1] j is the j-th column vector, the right-hand side of (50) has 2d column vectors, each of which has d components.
Note that the period of the time updating differs from that of the measurement updating. Since the system equations include the inertial sensor outputs, the time updating is processed once new data of the inertial sensors arrive, and thus the time updating period is T1. The time updating is implemented at a fast inertial sensor sample rate, e.g., 200 Hz here. At the end of the time updating, if the aid measurements do not arrive, the predicted state | 1kk x and the predicted error covariance Upon the arrival of a new navigation aid measurement, the measurement updating is processed, and thus the measurement period is T2. The measurement updating is implemented at a slow navigation aid sample rate, e.g., 10 Hz here.

Experimental Results
The purpose of the experiment is to show the advantages of the new direct algorithm over the typical indirect Kalman filter.
The performance of the proposed direct UKF approach for the aided SINS is examined with road test data from a land vehicle. The test vehicle is shown in Figure 2. The SINS/odometer integrated navigation system is used. The navigation aid is a precision odometer from which the forward speed is obtained. The odometer is installed at the right rear wheel. The inertial sensors used in the experiments are from a navigation grade IMU. A GPS is installed on the top of the vehicle. Note that the GPS is not integrated in the navigation system, but it is only used as the position reference. The GPS receiver provides position with precision of about 10 m, and update rates up to 1 Hz.  The IMU contains three fiber optic gyros and three quartz accelerometers. The inertial sensor data are sampled at 200 Hz (T 1 = 5 ms). The fixed biases for the gyros and accelerometers used here are 0.03 • /h (1 σ) and 0.2 mg (1 σ), respectively.
The odometer only provides the forward speed with precision of about 0.1 m/s at the sample rate 10 Hz (T 2 = 0.1 s). Note that the nonholonomic constraints on the land vehicle arise from the fact that the vehicle cannot move in the transversal or the vertical directions in the b-frame. In addition, the IMU-odometer misalignment angle and odometer scale factor are calibrated prior to the field test. It can be assumed that the odometer measurement error is an additional white Gauss noise. Hence, the measurement of the UKF is given by: where v by is the forward speed derived from the odometer.
The initial state vector of the filter is set as: where v by0 is the initial forward speed from the odometer, and θ 0 , γ 0 , ψ 0 are the initial Euler angles that are transferred to the filter at the end of the alignment. Apparently, unlike the conventional approaches that use the errors as the states, x 0 is not zero here because it stands for the initial navigation information which is set according to the results of the initial alignment. Given the relationship between the vehicle velocities in the b-frame and in the n-frame, the first two components of x 0 are calculated by the DCM derived from the alignment results.
The other initial parameters of the filter are set as: where diag{·} denotes the diagonal matrix. P 0 is set according to the accuracy of the alignment and the inertial sensor biases, Q is set according to the inertial sensor noises, and R is set according to the odometer noises. They can be slightly tuned to make the filter converge rapidly. After the vehicle engine was started, the vehicle was stationary at the initial position to process the initial alignment during the first five minutes. Then the vehicle ran along the trajectory until it reached the end. Three road test trajectories were carried out. The three trajectory information (time and distance) are shown in Table 1, where the time includes the five minutes for the alignment. The experiment results of the proposed direct UKF solution for SINS/odometer integration (denoted as Direct) is compared with the standard indirect approach utilizing the KF and SINS error models introduced in Section 1 (denoted as Indirect). The position errors of the direct and indirect solutions are calculated with respect to the longitude and latitude provided by GPS. Figures 3-7 show the velocity and attitude of the SINS/odometer integrated navigation system in the integrated navigation phase of about 20 min during Trajectory 1. The velocity and attitude are directly estimated by the UKF in the direct approach, while in indirect approach, the velocity and attitude are calculated by the SINS algorithm first and then corrected, employing the errors estimated                From Figures 3-7, we can see that the proposed direct approach can achieve the velocity and attitude similar to the indirect approach, and thus it can also capture the vehicle motion.
As the states of the direct approach differ from those of the indirect approach, we use the standard deviations of the attitude and the attitude errors to illustrate the convergence trend in the direct and indirect approaches, respectively. Comparing the standard deviations of the attitude with the attitude error is meaningless, as they are different quantities. Moreover, the indirect approach has been well-studied. Here we only show the standard deviations of the attitude in the direct approach during Trajectory 1 as in Figure 8. From Figure 8, it can be seen that the attitude standard deviations all decrease when time passes. Figure 8a,b shows that the pitch and roll converge very rapidly as the system effectively aligns itself to the local gravity vector, and they converge at about 20 s, and their standard deviations settle to less than 0.1′, and their accuracy is limited by any residual bias in the accelerometer measurements. Figure 8c shows that the heading converges slowly due to its lower observability as the heading error only propagates as a velocity error and therefore becomes more observable when the vehicle maneuvers, and its standard deviation settles to about 6.5′ at the end, and the accuracy is mainly limited by any residual bias in the gyro measurements. The attitude convergence characteristics show that not only attitude estimates can be achieved using the b-frame velocity as the filter measurement, but they also conform to the observability analysis. From Figures 3-7, we can see that the proposed direct approach can achieve the velocity and attitude similar to the indirect approach, and thus it can also capture the vehicle motion.
As the states of the direct approach differ from those of the indirect approach, we use the standard deviations of the attitude and the attitude errors to illustrate the convergence trend in the direct and indirect approaches, respectively. Comparing the standard deviations of the attitude with the attitude error is meaningless, as they are different quantities. Moreover, the indirect approach has been well-studied. Here we only show the standard deviations of the attitude in the direct approach during Trajectory 1 as in Figure 8. From Figures 3-7, we can see that the proposed direct approach can achieve the velocity and attitude similar to the indirect approach, and thus it can also capture the vehicle motion.
As the states of the direct approach differ from those of the indirect approach, we use the standard deviations of the attitude and the attitude errors to illustrate the convergence trend in the direct and indirect approaches, respectively. Comparing the standard deviations of the attitude with the attitude error is meaningless, as they are different quantities. Moreover, the indirect approach has been well-studied. Here we only show the standard deviations of the attitude in the direct approach during Trajectory 1 as in Figure 8. From Figure 8, it can be seen that the attitude standard deviations all decrease when time passes. Figure 8a,b shows that the pitch and roll converge very rapidly as the system effectively aligns itself to the local gravity vector, and they converge at about 20 s, and their standard deviations settle to less than 0.1′, and their accuracy is limited by any residual bias in the accelerometer measurements. Figure 8c shows that the heading converges slowly due to its lower observability as the heading error only propagates as a velocity error and therefore becomes more observable when the vehicle maneuvers, and its standard deviation settles to about 6.5′ at the end, and the accuracy is mainly limited by any residual bias in the gyro measurements. The attitude convergence characteristics show that not only attitude estimates can be achieved using the b-frame velocity as the filter measurement, but they also conform to the observability analysis. From Figure 8, it can be seen that the attitude standard deviations all decrease when time passes. Figure 8a,b shows that the pitch and roll converge very rapidly as the system effectively aligns itself to the local gravity vector, and they converge at about 20 s, and their standard deviations settle to less than 0.1 , and their accuracy is limited by any residual bias in the accelerometer measurements. Figure 8c shows that the heading converges slowly due to its lower observability as the heading error only propagates as a velocity error and therefore becomes more observable when the vehicle maneuvers, and its standard deviation settles to about 6.5 at the end, and the accuracy is mainly limited by any residual bias in the gyro measurements. The attitude convergence characteristics show that not only attitude estimates can be achieved using the b-frame velocity as the filter measurement, but they also conform to the observability analysis. Figure 9 shows the position information of the GPS as the reference, the position information of the indirect approach and the position information of the proposed direct approach for the SINS/Odometer integrated navigation system during three different trajectories. In Figure 9, the longitude and the latitude are converted into the east location and the north location in meters (not degrees) for clarity.
Sensors 2016, 16,1415 15 of 23 Figure 9 shows the position information of the GPS as the reference, the position information of the indirect approach and the position information of the proposed direct approach for the SINS/Odometer integrated navigation system during three different trajectories. In Figure 9, the longitude and the latitude are converted into the east location and the north location in meters (not degrees) for clarity. From Figure 9, it can be seen obviously that the position errors increase with the time during different trajectories in both approaches. Although the velocity errors are bounded with the velocity aid, the remainder velocity errors that accumulate in the integration process result in the increase of position errors. Compared with the indirect approach, it is also clear that the direct approach makes it possible to obtain position curves that are closer to those of the GPS. The position errors defined by the Euclidian distance between the estimated position and the GPS position at the end of the trajectories are shown in Table 2 for both approaches. In Table 2, the improvement on navigation performance of the direct approach is shown clearly, and its position errors are much smaller than those of the indirect approach. Position errors are related to the accuracy of the velocity and attitude estimates. The models developed in Section 2 in the direct approach almost involve no approximation, and the linearization and the cross-noise problems are avoided. They are more accurate and can capture the SINS nonlinearity better than the SINS error models. Moreover, the nonlinear models enable the UKF to estimate the navigation parameters directly, and the UKF shows better performance than the KF in such nonlinear cases. From Figure 9, it can be seen obviously that the position errors increase with the time during different trajectories in both approaches. Although the velocity errors are bounded with the velocity aid, the remainder velocity errors that accumulate in the integration process result in the increase of position errors. Compared with the indirect approach, it is also clear that the direct approach makes it possible to obtain position curves that are closer to those of the GPS. The position errors defined by the Euclidian distance between the estimated position and the GPS position at the end of the trajectories are shown in Table 2 for both approaches. In Table 2, the improvement on navigation performance of the direct approach is shown clearly, and its position errors are much smaller than those of the indirect approach. Position errors are related to the accuracy of the velocity and attitude estimates. The models developed in Section 2 in the direct approach almost involve no approximation, and the linearization and the cross-noise problems are avoided. They are more accurate and can capture the SINS nonlinearity better than the SINS error models. Moreover, the nonlinear models enable the UKF to estimate the navigation parameters directly, and the UKF shows better performance than the KF in such nonlinear cases.
The heading, pitch and roll of the 3-1-2 Euler angle set are denoted as ψ r , θ r , γ r respectively with the subscript r to show the difference from the 3-2-1 Euler angle set. The three separate direction cosine matrices are defined as C ψ r , C γ r and C θ r , respectively. Thus, a transformation from reference to body axes, denoted as C b rn , may be expressed as the product of these three separate transformations as follows: cosγ r cosψ r cosγ r sinψ r −sinγ r cosψ r sinγ r sinθ r − cosθ r sinψ r cosψ r cosθ r + sinψ r sinγ r sinθ r cosγ r sinθ r sinψ r sinθ r + cosψ r cosθ r sinγ r cosθ r sinψ r sinγ r − cosψ r sinθ r cosγ r cosθ r    Similarly, Equations (12) should be changed for the 3-1-2 Euler angle set as follows: where ω b nb is given by: The solutions of the . θ r and . ψ r equations become indeterminate when γ r = ±π/2. γ r = ±π/2 are the singularities of the 3-1-2 Euler angle set.
The differences between the system model and measurement model of the direct approach using the 3-1-2 and 3-2-1 Euler angle sets are the DCM and the Euler angles. Substituting ψ r , θ r , γ r for θ, γ, ψ in Equation (18), respectively, yields the state: The navigation equation as shown in Equation (1) is changed by substituting C b rn for C b n in Equation (3). Then it is combined with Equation (54) and Equation (17), and the system model of the direct approach using the 3-1-2 Euler angle set is yielded. The corresponding filter measurement equation is yielded by substituting C b rn for C b n in Equation (27). The flow chart of the direct fusing approach with the dual-Euler method, taking the SINS initial alignment for example, is illustrated in Figure 10. For the purpose of simplicity, the direct approach using the 3-2-1 Euler angle set is denoted as the positive Euler angle direct approach, and the direct approach using the 3-1-2 Euler angle set is denoted as the passive Euler angle direct approach, and correspondingly, the Euler angles are denoted as the positive Euler angles and the passive Euler angles, respectively. When flag = 0, the positive Euler angle direct approach works; when flag = 1, the passive Euler angle direct approach works.
As shown in Figure 10, the two branches are almost the same except for two steps: for passive Euler angle direct approach, positive Euler angles are converted to passive Euler angles at the beginning of the filtering, and passive Euler angles are converted to positive Euler angles at the end of the filtering, since the outputs are expressed in terms of positive Euler angles. When the switching from the positive Euler angle direct branch to the passive Euler angle direct branch occurs, the positive Euler angles in the filter state is converted to the passive Euler angles according to Equation (57), and then they are transferred to the filter state of the passive Euler angle direct branch. Notably, it is unnecessary to change the other parameters of the filter. Equation (58) is used for the conversion in the inverse switch.
A turntable test is conducted to verify the dual-Euler direct approach. The three-axis turntable is shown in Figure 11. The turntable attitude is expressed in terms of positive Euler angles. For simplicity, the roll and heading keep constant, and they are zero. The pitch changes between −π/2 and +π/2 linearly and periodically, and sometimes it is kept constant at ±π/2 for a few seconds. The turntable attitude is simultaneously recorded, and it is considered to be the true value and is used as the attitude reference (denoted as True). It should be noted that for the choice of angular motions, the positive Euler angle direct approach cannot be used to estimate the attitude at each time.
as the attitude reference (denoted as True). It should be noted that for the choice of angular motions, the positive Euler angle direct approach cannot be used to estimate the attitude at each time.   as the attitude reference (denoted as True). It should be noted that for the choice of angular motions, the positive Euler angle direct approach cannot be used to estimate the attitude at each time. Figure 10. Flow chart of the SINS initial alignment using the dual-Euler direct approach. Figure 11. Three-axis turntable. Figure 11. Three-axis turntable.
The inertial sensor data are sampled at 100 Hz (T 1 = 10 ms). The measurement of the UKF is given by z = [0,0,0] T , and the initial state vector of the filter is set as x 0 = [0,0,θ 0 , γ 0 , ψ 0 ,0,0,0,0,0] T . Other parameter settings are as same as those specified in Section 4. The test total time is about 41 min, of which the first 5 min is used for the coarse alignment. The last 36 min is used for the fine alignment.
The following figures give the singularity test results in a period of fine alignment using the dual-Euler direct approach (denoted as DualEu). Figure 12 shows the estimated east and north velocities. Figures 13-15 show the true and estimated attitudes. Figure 16 shows the estimated Pitch angle errors, i.e., the differences between the estimated pitch and the true one. Figure 17 shows the switch flag, denoting the positive or passive Euler angle direct approach is selected. The inertial sensor data are sampled at 100 Hz (T1 = 10 ms). The measurement of the UKF is given by z = [0,0,0] T , and the initial state vector of the filter is set as x0 = [0,0,θ0, γ0, ψ0,0,0,0,0,0] T . Other parameter settings are as same as those specified in Section 4. The test total time is about 41 min, of which the first 5 min is used for the coarse alignment. The last 36 min is used for the fine alignment.
The following figures give the singularity test results in a period of fine alignment using the dual-Euler direct approach (denoted as DualEu). Figure 12 shows the estimated east and north velocities. Figures 13-15 show the true and estimated attitudes. Figure 16 shows the estimated Pitch angle errors, i.e., the differences between the estimated pitch and the true one. Figure 17 shows the switch flag, denoting the positive or passive Euler angle direct approach is selected.    The inertial sensor data are sampled at 100 Hz (T1 = 10 ms). The measurement of the UKF is given by z = [0,0,0] T , and the initial state vector of the filter is set as x0 = [0,0,θ0, γ0, ψ0,0,0,0,0,0] T . Other parameter settings are as same as those specified in Section 4. The test total time is about 41 min, of which the first 5 min is used for the coarse alignment. The last 36 min is used for the fine alignment.
The following figures give the singularity test results in a period of fine alignment using the dual-Euler direct approach (denoted as DualEu). Figure 12 shows the estimated east and north velocities. Figures 13-15 show the true and estimated attitudes. Figure 16 shows the estimated Pitch angle errors, i.e., the differences between the estimated pitch and the true one. Figure 17 shows the switch flag, denoting the positive or passive Euler angle direct approach is selected.   The inertial sensor data are sampled at 100 Hz (T1 = 10 ms). The measurement of the UKF is given by z = [0,0,0] T , and the initial state vector of the filter is set as x0 = [0,0,θ0, γ0, ψ0,0,0,0,0,0] T . Other parameter settings are as same as those specified in Section 4. The test total time is about 41 min, of which the first 5 min is used for the coarse alignment. The last 36 min is used for the fine alignment.
The following figures give the singularity test results in a period of fine alignment using the dual-Euler direct approach (denoted as DualEu). Figure 12 shows the estimated east and north velocities. Figures 13-15 show the true and estimated attitudes. Figure 16 shows the estimated Pitch angle errors, i.e., the differences between the estimated pitch and the true one. Figure 17 shows the switch flag, denoting the positive or passive Euler angle direct approach is selected.     From Figures 13 and 17, it is clear that there is nearly no difference between the estimated pitch and the true one even at θ = ±π/2 and the positive and passive Euler angle direct approaches are used alternately according to the switch flag. The positive Euler angle direct approach is implemented if |θ| ≤ π/4, otherwise, the passive Euler angle direct approach is implemented. Initially, 0 ≤ θ ≤ π/4, the flag is set to 0, and the positive Euler angle direct approach is used. With the increasing of θ from 0 to π/2, once θ > π/4, the flag is set to 1, and the switching occurs, and the passive Euler angle direct approach is used. Then with the decreasing of θ from π/2 to 0, the passive Euler angle direct approach runs until θ ≤ π/4. Once θ ≤ π/4, the flag is set to 0 again, and the inverse switching occurs, and the positive Euler angle direct approach is used. The switching    From Figures 13 and 17, it is clear that there is nearly no difference between the estimated pitch and the true one even at θ = ±π/2 and the positive and passive Euler angle direct approaches are used alternately according to the switch flag. The positive Euler angle direct approach is implemented if |θ| ≤ π/4, otherwise, the passive Euler angle direct approach is implemented. Initially, 0 ≤ θ ≤ π/4, the flag is set to 0, and the positive Euler angle direct approach is used. With the increasing of θ from 0 to π/2, once θ > π/4, the flag is set to 1, and the switching occurs, and the passive Euler angle direct approach is used. Then with the decreasing of θ from π/2 to 0, the passive Euler angle direct approach runs until θ ≤ π/4. Once θ ≤ π/4, the flag is set to 0 again, and the inverse switching occurs, and the positive Euler angle direct approach is used. The switching   From Figures 13 and 17, it is clear that there is nearly no difference between the estimated pitch and the true one even at θ = ±π/2 and the positive and passive Euler angle direct approaches are used alternately according to the switch flag. The positive Euler angle direct approach is implemented if |θ| ≤ π/4, otherwise, the passive Euler angle direct approach is implemented. Initially, 0 ≤ θ ≤ π/4, the flag is set to 0, and the positive Euler angle direct approach is used. With the increasing of θ from 0 to π/2, once θ > π/4, the flag is set to 1, and the switching occurs, and the passive Euler angle direct approach is used. Then with the decreasing of θ from π/2 to 0, the passive Euler angle direct approach runs until θ ≤ π/4. Once θ ≤ π/4, the flag is set to 0 again, and the inverse switching occurs, and the positive Euler angle direct approach is used. The switching  From Figures 13 and 17, it is clear that there is nearly no difference between the estimated pitch and the true one even at θ = ±π/2 and the positive and passive Euler angle direct approaches are used alternately according to the switch flag. The positive Euler angle direct approach is implemented if |θ| ≤ π/4, otherwise, the passive Euler angle direct approach is implemented. Initially, 0 ≤ θ ≤ π/4, the flag is set to 0, and the positive Euler angle direct approach is used. With the increasing of θ from 0 to π/2, once θ > π/4, the flag is set to 1, and the switching occurs, and the passive Euler angle direct approach is used. Then with the decreasing of θ from π/2 to 0, the passive Euler angle direct approach runs until θ ≤ π/4. Once θ ≤ π/4, the flag is set to 0 again, and the inverse switching occurs, and the positive Euler angle direct approach is used. The switching will be carried out in the same way subsequently. The passive Euler angle direct approach works well when the positive Euler angle direct approach is singular at θ = ±π/2.
The test results shown in Figure 12 illustrate the convergence of the velocity at about 20 s. From Figures 13-16, it can be seen that the attitude converges with time. The convergence of the pitch and roll is greatly faster than that of the heading. Figures 13 and 16 show the plots of the pitch and the associated error with time, respectively. The pitch converges at about 20 s, and the mean absolute value of the pitch errors is 0.86 after 30 s. Figure 14 shows the plots of the roll with time. The roll converges at about 20 s, and the mean absolute value of the roll errors is 0.13 after 100 s. Figure 15 shows the plots of the heading with time. The heading converges at about 300 s, and the mean absolute value of the heading errors is 15.92 after 300 s.
According to the above singularity test results, the singularity at θ = ±π/2 of the positive Euler angle direct approach can be efficiently removed by means of the dual-Euler method, and the convergence speed and alignment precision can satisfy the requirement of initial alignment.

Conclusions
In this paper a direct UKF approach is proposed for aided SINSs with the following highlights: (1) the direct integration navigation solution is completely nonlinear, and it can achieve better performance than typical indirect and linear approaches. The UKF outputs the navigation information directly. The approach combines the navigation computation with the state estimation, and there is no need to process navigation computation and error correction separately. As an alternative, it can be considered as a supplement to the conventional integration navigation algorithm; (2) the system model and measurement model are conceptually simple and easy to understand, as no linear restrictions or approximations are needed, while SINS error models are derived in a complex way with many assumptions; (3) the direct approach separates the inertial sensor noise from the aid measurement noise in the measurement models, and it overcomes the cross-noise problem which exists in the SINS indirect error models; (4) the Euler angle kinematics equation is employed instead of the quaternion kinematics equation, and there is no nonlinear constraint during the filtering process. Therefore, the proposed method is easy to apply and the algorithm is simple; (5) the singularity problem, which exists in the Euler angles, can be efficiently solved by means of the dual-Euler method. The proposed approach is non-singular because the positive and passive Euler angle direct approaches operate alternately according to the switch flag to remove the singularities; (6) this direct approach can be applied to cases where it is difficult to develop linear models and use the KFs, for example, the tightly coupled DVL/SINSs [34], which do not require bottom lock or allow individual validation and characterization of the DVL beam measurements, i.e., each beam velocity is treated as a separate measurement; and the tightly coupled USBL/SINSs [35], which directly exploits the acoustic array spatial information. The direct approach can also be applied to RP, MRP and GRP, since their three parameters are uncorrelated.