Design and Development of a Real-Time GPS-Aided SINU System

A Strapdown Inertial Navigation Unit (SINU) is a low cost motion measurement device commonly used for navigation solutions. Global Positioning System (GPS) is usually adopted as an external reference source to minimize the SINU's accumulation errors by applying a Kalman filter to obtain best estimations in positions, velocities and orientations. However, due to the low sampling rate of GPS, such a configuration does not provide intensive orientation estimation. In this paper, a new and efficient real-time GPS-aided SINU system with incorporated magnetometers is developed to enhance the orientation estimation. An intensive orientation estimation algorithm is developed by combining the extra sensory inputs from magnetometers with the inputs from accelerometers. The estimated orientation was applied in the Kalman filtering, replacing the GPS-aided orientation estimation. The offline implementation shows promising results in reducing both the accelerometers' and gyroscopes' errors. Finally, the design is successfully implemented in real-time.


Introduction
An inertial navigation system, which relies on inertial sensors [1] to navigate, has been an important tool in position, velocity and orientation measurements for the past few decades.A high performance inertial navigation system which uses accurate inertial sensors is generally very expensive and is controlled by government regulations [2], resulting in such a system being unusable for civilian applications. These constraints have led to the development of a low cost Strapdown Inertial Navigation Unit (SINU) [3]. A typical SINU consists of three orthogonally aligned accelerometers and three orthogonally aligned gyroscopes fixed in a prescribed position.However, its measurements are highly inaccurate and are jeopardized by various noises [4,5], which include both deterministic and random noises. The effect of these noises that cause the inaccuracy can be reduced, but not eliminated, through proper calibration [3,4]. To increase the SINUʹs accuracy, the dead reckoning technique through the use of Global Positioning System (GPS) and the implementation of a Kalman filtering algorithm has been proposed [6]. Such a technique is known as a GPS-aided SINU system with Kalman filtering, and it had been successfully applied in various applications such as unmanned aerial vehicle (UAV) navigation [7], land vehicle navigation [8,9] and autonomous underwater vehicle (AUV) navigation [10].
A fully calibrated SINU can achieve short-term accuracy with a relatively fast sampling rate (40~200Hz), but it always suffers from long-term measurement inaccuracy. On the other hand, a GPS possesses the characteristic of long-term accuracy, but it has a relatively slow sampling rate (<5Hz) as compared to SINU. The conventional GPSaided SINU system combines the merits of both systems to achieve position, velocity and orientation measurements [6][7][8][9][10][11]. However, due to GPSʹs inability to provide reliable orientation measurements, especially during stationary [2], this conventional system cannot achieve efficient orientation measurements.
In this paper, an improved navigation system is proposed by adding-in additional sensory inputs from three orthogonally aligned magnetometers. The magnetometers measure the magnetic field from the South Pole to the North Pole, which can be processed to provide an angular measurement. Such a configuration improves the orientation measurement, which in turn improves the overall performance of the navigation system. Both the proposed system and the conventional system have been developed. Navigation experiments were conducted to compare and analyse the performance of both systems offline. The developed navigation system will eventually be incorporated into an Unmanned Aerial Vehicle Synthetic Aperture Radar (UAVSAR) [12] system to measure the UAVʹs motion in order to achieve motion compensation in SAR signal processing [13].
The remainder of this paper is organized as follows. Section 2 describes the fundamental principles of inertial sensing and motion equations. Section 3 outlines the modelling of both conventional and improved GPS-aided SINU systems using a Kalman filter. Section 4 presents the offline implementation of both conventional and improved GPS-aided SINU systems. Section 5introduces the real-time implementation of the improved GPS-aided SINU system using an embedded computer, and lastly,section 6 concludes the findings.

Coordinate Frames
A conventional inertial sensor provides measurements in terms of acceleration and angular velocity. It is customary to represent these measurements in various frame representations [2,3,6,7], which include:


The inertial frame, or i-frame, with its origin positioned at the centre of the Earth and axes which are non-rotating with respect to the fixed stars.  The Earth frame, or e-frame, with its origin positioned at the centre of the Earth and rotating axes. The zaxis points toward the North Pole, the x-axis points to the reference meridian and y-axis completes the right-handed orthogonal frame. This frame is also known as an Earth-Centred Earth-Fixed (ECEF) frame.


The navigation frame, or n-frame, is a local geographic reference frame with its origin located at the centre of the navigation system, and its axes follow the right-handed orthogonal rule with x-axis pointing toward the North Pole, y-axis pointing toward the East, and z-axis pointing toward the Earthʹs centre. It is also known as a North-East-Down (NED) frame.
 The body frame, or b-frame, is a set of axes that follows the right-handed orthogonal rule which is aligned with the yaw, pitch and raw axes of the system.
The inertial sensors provide direct measurements in terms of b-frame. To utilize these measurements for navigation applications, the b-frame measurements must be converted into n-frame. This process is known as plane rotation and it is an essential process for all inertial navigation system.

Plane Rotations
Plane rotations are commonly applied to mathematically expressthe coordinate transformation from one coordinate frame to another. Coordinate transformation involves a sequence of plane rotations equivalent to multiplying the coordinate frame with the directional cosine matrix (DCM): where F a and F b representthe coordinate framesa and b, with φ, θ and ψ representing the roll, pitch and yaw, respectively, and equation (2) representing the DCM of frame transformation from frame a to frame b: where c(.) and s(.) denote the cosine and sine operators.

Navigation Equations
The GPS-aided SINU system provides outputs in terms of position, velocity and orientation measurements. The navigation equations describe the relationship between the GPS-aided SINU systemʹs outputs and inputs in terms of accelerations and angular velocities. The navigation equations, in their general form, can thus be derived based on the inertial measurements as follows: where r n and v n represent the 3-dimensional position and velocity in n-frame,� � � represents the DCM that transform b-frame to n-frame, s b and g n represent the 3-dimensional acceleration measurement in b-frame and the gravitational force in n-framerespectively, Ω �� � represents the skewed rotation rate matrix in e-frame with respect to i-frame projected to n-frame, Ω �� � represents the skew matrix of angular velocity measurement in b-frame with respect to iframe projected to b-frame, and Ω �� � represents the skewed transport rate matrix in n-frame with respect to i-frame projected to b-frame.
The navigation equations can be rewritten in discrete form since the GPS-aided SINU system is usually implemented in discrete time, as follows: where Δt denotes the discrete sampling time.

Dynamic Error Model
Perturbation is a process of linearizing the nonlinear differential equations in order to perform error analysis [14]. The derivation of SINUʹs dynamic error equations can be achieved by performing perturbation analysis on equation (3), and the perturbed equations are: where Arr, Arv, Avr, Avv, Aer and Aev represent the partial derivative matrices, or the Jacobians, of position, velocity and orientation error equations [2,6] respectively, with the subscripts r, v and e representing the position, velocity and orientation, respectively. � � refers to the orientation errors,����represents a matrixʹs cross product,s n and�� �� � represent the 3-dimensional acceleration measurement in n-frame and 3-dimensional angular velocity measurement in n-frame with respect to i-frame projected in n-frame,δs b and�� �� � represent the 3dimensional acceleration measurement error in b-frame and 3-dimensional angular velocity measurement error in b-frame with respect to i-frame projected in b-frame.
Equation (6)  Equation (6) can be rewritten in terms of state space model for error prediction using the Kalman filter [15], as shown below: with: , where 03x3 is a three-times-three zero matrix.
It is noted that the error vectors u shown in equation (9) are equivalent to white noises, in which its spectral density can be estimated through Allan variance analysis [4,5].
Equation (7) can be transformed into its discrete form, as shown below: where ψk-1 denotes the transition matrix with: and wk-1is the discrete white noise response with its covariance matrix expressed as: where Qk represents the process noise covariance.

Kalman Filter Modelling
Equation (7) describes the SINUʹs dynamic error model in the state space model. It is converted into discrete form in equation (10) for practical implementation using a digital processor. Equation (10) serves as the prediction equations for the Kalman filter. To complete the Kalman filter modelling, the following observation (or measurement) equations are needed: (13) where zk is the measurement update, Hk represents the measurement Jacobian andek represents the random measurement noise response with its covariance matrix expressed as: (14) where Rk represents the measurement noise covariance. Figure 1 shows the operational block diagram of the Kalman filter and the discretized navigation dynamic error equations, with �� � � � and �� � � represent the priori and posteriori estimate respectively ,� � � and � � represent the priori and posteriori estimate error covariance respectively, and Kk is the gain factor. As can be seen in Figure 1, the Kalman filtering proceduresare grouped into two processes, known as the Prediction process and the Measurement process, which can be achieved in five steps. In the Prediction process, the priori errors are estimated through equation (10) and the priori error covariance is predicted. In the Measurement process, the Kalman gain is first calculated and it is used to correct the priori measurement and the priori error covariance. In summary, the Kalman filter acts as a recursive filter to compute the state δxk based on the prediction and measurement.
In the inertial navigation system, the measurement vector zk refers to the actual measurements of position, velocity and angular orientation from GPS and magnetometers, which is represented as follows: where the vectors n r , n v and n  denote the 3-dimensional position, velocity and orientation vector in n-frame, respectively. The vectors with subscript SINUindicate the parameters obtained through navigation equations and the vectors with subscript GPS indicate the parameters obtained from GPS measurements.The SINUʹs orientation angle can be obtained from the DCM [17]. It is worth mentioning that the vector with subscript MEAS can be further elaborated as follows: where equation (16) represents the orientation measurement of conventional GPS-aided SINU with no magnetometers, and: Atan2 , before real-time implementation. The Kalman filter processing is carried out in offline mode to verify the performance of the developed system as compared to the conventional system. The update rates of GPS and SINU were 5Hz and 40Hz, respectively. Calibration [4] was performed on the SINU before carrying out the experiments. Figure 2 and Figure 3 show the offline comparison of the accelerometersʹ and gyroscopesʹ error estimations in one of the field experiments. The straight lines represent the estimated errors through conventional, no magnetometers approach. The dashed lines represent the estimated errors of the enhanced system with additional measurements from magnetometers. Figure 2 and 3 show significant improvements on the estimated errors on both accelerometers and gyroscopes, especially on δfz and δωz, by employing the enhanced system. The average estimated errors of the enhanced system are kept to be within ±2.6x10 -3 m/s 2 for accelerometers and ±3.2x10 -4 rad/s for gyroscopes. Compared with the conventional system, the average estimated errors are ±1.26x10 -2 m/s 2 for accelerometers and ±1.7x10 -3 rad/s for gyroscopes.The results clearly indicate the improvement of the performance of the enhanced GPS-aided SINU system.   Figure 4 shows the field experiment results by comparing the navigation paths under three different scenarios. A zoom-in figure is provided in Figure 4, with average difference computed to be 2.84m between the navigation paths of the GPS-aided SINU system with and without magnetometers. The difference between the navigation paths of GPS measurements and the GPS-aided SINU system with magnetometers is computed to be ±0.173m. On the other hand, the difference between the navigation paths of GPS measurements and the GPS-aided SINU system without magnetometers is computed to be ±2.67m. The results clearly indicate the improvement in the navigation path prediction with the implementation of magnetometers in the GPS-aided SINU system.

Real-Time Implementation
A Graphical User Interface (GUI) for the real-time GPSaided SINU system with magnetometers using the Kalman filter is developed using Visual Basic and implemented in a Microsoft Window environment using an embedded computer. Figure 6 shows the simplified operational block diagram of the real-time implementation.The SINUʹs data rate is 40Hz, which is comparatively faster than the GPSʹs 5Hz data rate. When GPS data is absent, the system computes the position, velocity and orientation solely from the SINUʹs data and the previous predicted errors. On the other hand, when GPS data is present, the system computes the position, velocity and orientation by combining both GPS and SINU data.  Table 1 shows the list of data that the GUI recorded. A total of 31 variables were saved into a solid state hard disk continuously in binary file format. As referred to Table 1, the first nine variables represent the raw data from the SINU, which is commonly known as the inputs of the GPS-aided SINU system. The next 15 variables represent the computed 3-dimensional position, velocity, orientation, acceleration errors and orientation errors, which is also known as the output of the GPS-aided SINU system. The last seven variables indicate the GPS data, which are particularly important in the Kalman filtering of the GPS-aided SINU system. Figure 7 shows the GUI layout of the developed real-time GPS-aided SINU system. The top left corner indicates the serial ports setting for the SINU and the GPS. An ʺOperation Startʺ button is located beneath the ʺCommPort Configurationʺ frame. An X-Y graph is used to display both the GPS path (the green line)and the SINUʹs navigation path (the red line). A group of real-time updating parameters, including the real-time updated position, velocity, orientation and GPS information, can be found below the X-Y graph. The raw SINU data is displayed at the bottom of the GUI. Note that the X-Y graph shown in Figure 7 indicates the GPSaided SINUʹs real-time implementation result in one of the field experiments.
The results from another field experiment are shown in Figure 8. Its post-processing result is shown in Figure 9. The difference between the navigation paths of GPS measurements and the GPS-aided SINU system for both the real-time processing and the post-processing are computed to be ±0.246m. Finally, Figure 10 shows the actual GPS plot using Google Earth.    The authors would like to express their appreciation to Tan Wei Qiang, Yee Kuo Shenand Cheaw Wen Guey for their technical support in carrying out the field experiments.