A GPS-aided Inertial Navigation System in Direct Configuration
R. Munguía
Departamento de Ciencias Computacionales Universidad de Guadalajara Guadalajara, Jalisco, México *[email protected]
ABSTRACT This work presents a practical method for estimating the full kinematic state of a vehicle, along with sensor error parameters, through the integration of inertial and GPS measurements. This kind of system for determining attitude and position of vehicles and craft (either manned or unmanned) is essential for real time, guidance and navigation tasks, as well as for mobile robot applications.
The architecture of the system is based in an Extended Kalman filtering approach in direct configuration. In this case, the filter is explicitly derived from the kinematic model, as well as from the models of sensors error. The architecture has been designed in a manner that it permits to be easily modified, in order to be applied to vehicles with diverse dynamical behaviors.
The estimated variables and parameters are: i) Attitude and bias-compensated rotational speed of the vehicle, ii) Position, velocity and bias-compensated acceleration of the vehicle and iii) bias of gyroscopes and accelerometers. Experimental results with real data show that the proposed method is enough robust for its use along with low-cost sensors.
Keywords: Inertial Navigation, Sensor Fusion, State Estimation. 1. Introduction
The process for autonomously estimating the state of 100 to 200 Hz) information but only for short of a vehicle (e.g position, velocity, orientation, term. This fact is especially notorious when low- etc.), while it is maneuvering along a trajectory, is cost sensors are used. On the other hand, a often referred as navigation. Global Positioning System (GPS) provides global- referenced position and velocity estimations at low The autonomous navigation is an important rate (typically in the range of 1 to 4 Hz). The capability for both manned and unmanned integration of both systems (INS and GPS) can vehicles. In our context the term "autonomous" generate a navigation system capable of exploiting refer to the capacity of the system for estimating the advantages of both, and also limits the the state of the vehicle without the aid of a human drawbacks of the systems viewed by separate. operator. Often, the autonomous navigation is a Thus, a GPS-aided INS can produce estimates of prerequisite for control tasks. the full state of the vehicle, both at high frequency as drift-free. The Inertial Navigation System (INS) is one of the most widely used dead reckoning systems. A The integration of inertial sensors with GPS is typical INS fuses sensory information taken from broadly classified as follows [25]: inertial sensors (accelerometers) and rotational sensors (gyroscopes) in order to continuously • Loosely coupled system. estimate position and orientation of the body (vehicle). Because different sources of sensor • Tightly coupled system. errors are integrated over time, an INS can provide correct and high frequency (typically in the range • Ultra-tightly coupled system
Journal of Applied Research and Technology 803
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
In loosely coupled systems [8-13],[16],[18] and [19], The EKF in Indirect formulation (also referred as the GPS data (e.g. position velocity, etc) are fused the error state space formulation), estimates a explicitly with INS data. This kind of systems is state vector which represents the errors defined significantly dependent on the availability of GPS data. by the estimated state and the estimated nominal trajectory. An error model for each component of In tightly coupled systems [6] and [13], the GPS raw the state is needed in order to estimate the measurements (e.g pseudo ranges) are fused measurement residual. The measurement in the directly with the INS data. The main advantage of this error state space formulation is made up entirely kind of methods is that the system can carry out GPS of system errors and is almost independent of measurement updates, even if there are less than the kinematic model. Most of the approaches four satellites available. Their downside has to be found in literature are based in this kind of with the increase of complexity. configuration [3-13].
In Ultra-tightly coupled systems [26], the INS output The EKF in Direct configuration (also referred to (position, velocity and attitude) is used as an external as total state space formulation) updates the input to the GPS receiver. The INS output aid in vector state implicitly from the predicted state and prepositioning calculation for faster signal acquisition the measurement residual (the difference and in interference rejection during signal tracking. between the predicted and current measurement). The implementation of this kind of systems is often complicated because access to the GPS's firmware Integration Estimated Estimation Method Attitude is required. type errors technique I-EKF [16] Loosely G ,G ,A ,A Quat. Different techniques of state estimate have been b s b s (Unscented) used for integrating INS and GPS. Schemes Neural [19] Loosely No Euler presented in [1] and [2] are based in techniques of Networks linear Kalman filtering. The Kalman filter (KF), i-EKF [6] Tightly Gb,Ab Euler commonly used in estimating the system state (Quadratic) variables and suppressing the measurement noise, [9] Loosely Gb Quat. i-EKF has been recognized as one of the most powerful [8] Loosely Gb,Ab DCM i-EKF state estimation techniques. The KF allows to merge [10] Loosely Gb,Ab Euler i-EKF Particle information obtained from different sensor sources in [18] Loosely No Euler Filter a structured manner. For example in [29], a KF- [11] Loosely Ab Euler i-EKF based method for estimating position is presented, [12] Loosely * * i-EKF this approach combines visual data with wireless [13] Tightly Gb,Ab,Tb Quat. i-EKF sensors network information. Commonly methods This Loosely G ,A Quat. d-EKF based on linear filtering utilize simplified (linearized) work b b models. Thus, some computational time is saved, but at the cost of some decrease in performance. Table 1. Resume of Methods. *Not specified. For However, the wide variety of processing devices "Estimated errors" column: G = gyroscope, A = currently available makes feasible the implementation accelerometer, T = GPS time, b = bias, s = scale, (e.g. of complex algorithms in order to improve Gb means gyro bias). For "Attitude" column: DCM performance. means Direction Cosine Matrix, and Quat. is the abbreviation of quaternion. For "Estimation Technique" Due to the nonlinear nature of the problem, the column, i-EKF = Extended Kalman Filter in indirect nonlinear version of the Kalman Filter (The Extended configuration, d-EKF = Extended Kalman Filter in direct configuration. In parentheses are indicated. Kalman Filter or EKF) has been the technique
typically used to compute the GPS-INS solution. In this kind of EKF configuration, the system is There are two basic ways for implementing the EKF: essentially derived from the kinematics. One of the
characteristics of the direct configuration is its • Indirect formulation. conceptual clarity and simplicity. A review on the
EKF and its configurations can be found in [14]. • Direct formulation.
804 Vol. 12, August 2014
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
In addition, it is possible to find others methods which ' xxˆ qranb b nvn n x (1) rely in some variations of Kalman filtering as: ga Adaptative Kalman filtering [15], Unscented Kalman filtering [16]. Particle filtering (PF) is a nonlinear nb where q = [q1,q2,q3,q4] is a unit quaternion estimation technique which has been also utilized for representing the orientation (roll, pitch and yaw) of integrating INS and GPS. The advantage of PF is the b the body (vehicle). = [ x y z] is the bias- capability to deal with nonlinear non-Gaussian compensated velocity rotation of the body expressed models. Some methods based on PF are [17] and b in the body frame ( ). xg = [xg_x xg_y xg_z] is the bias of [18]. Another kind of methods relies in estimation n gyros. r = [xv yv zv] represents the origin of the body techniques coming from the artificial intelligent (AI) coordinate frame (b) expressed in the navigation research community, as [19], which is based on n n frame ( ). v = [vx vy vz] is the velocity of the body Neural Networks. Alternatively, there is also possible n expressed in the navigation frame. A = [ax ay az] to find hardware-oriented approaches as [20]. represents the bias-compensated acceleration of the
body expressed in the navigation frame. xa = [xa_x xa_y Table I presents a resume of some methods xa_z] is the bias of the accelerometers. Figure 1 published in journals in recent years. The (upper plot) illustrates the relation between the characteristics of the methods presented in Table I vehicle (body) frame and the coordinate systems are: i) Integration type, ii) estimated parameters of used as navigation frame. sensors error (e.g. bias, scale, etc.) iii) attitude representation, and iv) main estimation technique. While it is possible to find different families of approaches in literature, the EKF is still the standard estimation technique used for GPS-aided Inertial Navigations Systems. Nevertheless, the use of the EKF in direct configuration has been much less explored than its counterpart; the EKF in indirect configuration (See Table I). This is especially the case when sensor errors (e.g. bias of gyros and accelerometers) want to be included in the estimated vector state.
This paper describes a novel method for implementing a GPS-INS integrated system. The proposed system architecture is based in an Extended Kalman filtering approach in direct configuration. Thus, the filter is explicitly derived from the kinematic model as well as from the models of sensors error.
The paper is organized as follows: Section II describes in detail the proposed method. In section III experimental results, carried out with real data, are presented in order to validate the performance of the proposed method. Finally section IV presents conclusions and final remarks.
2. Method description Figure 1. Relation between the vehicle frame and the coordinate system used as navigation frame (upper). 2.1 Vector state and system specification The local tangent frame is used as the navigation frame (lower). For navigation in a local tangent frame, the The goal of the proposed method is the estimation origin of the ned coordinate axes would be at some of the following system state x : convenient point near the area of operation.
Journal of Applied Research and Technology 805
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
n The proposed method is mainly intended for local where r = [xv yv zv] represents the position of the autonomous vehicle navigation. Thus, the local GPS antenna expressed in the navigation frame n tangent frame is used as the navigation frame ( ) (in Cartesian coordinates), and vr is a Gaussian 2 n (Figure 1, lower plot). In this work, the axes of the white noise with PSD r . is the course angle of coordinate systems follow the NED (North, East, the vehicle, measured by the GPS unit (respect to Down) convention. the geographic north), and v is a Gaussian white 2 noise with PSD . See [28] for an extended In order to estimate the system state x , two kinds of discussion about GPS measurement models. measurement are considered: Commonly, position measurements are obtained from GPS devices in geodetic coordinates (latitude a) High rate measurements. longitude height). If this is the case, the GPS An inertial measurement unit (IMU) of 9DOF position measurements must be transformed to formed by i) 3-axis gyroscope, ii) 3-axis their corresponding local tangent frame accelerometer, and iii) 3-axis magnetometer, is coordinates. considered. See [27] for an extended discussion about estimating orientation using a 9DOF IMU. 2.2 Architecture of the system Whereas magnetometers are used only for the system initialization, it is assumed that at every k The architecture of the system is based in the steps there is availability of gyros and typical loop of prediction-update steps, defined by accelerometers measurements. an EKF in direct configuration (Figure 2).
b Gyroscopes measurements: the angular rate of System Prediction: Prediction equations propagate the vehicle, measured by the gyros (in the body in time the estimation of the system state, by frame) as yg, can be modeled by: means of the measurements obtained from gyros and accelerometers. Prediction equations offer b correct estimates at high frequency, but only for yg xg vg (2) short term.
where x is an additive error (bias) and v is a g g System Updates: In order to correct and limit the Gaussian white noise with power spectral density drift in estimates of the system state, globally (PSD) 2. g referenced information along with a priori
assumptions about the systems dynamics, are Accelerometer measurements: the acceleration of fused into the system, by means of the update the vehicle ab, measured by the accelerometers (in equations. Three kinds of system updates are the body frame) as y can be modeled by: a considered:
b b yagaa xa v (3) • Updates by means of dynamical constraints: a- priori knowledge about the vehicle dynamics is where, gb is the gravity vector expressed in the incorporated into the system at medium rate. body frame, xa is an additive error (bias), and va is 2 a Gaussian white noise with PSD a . • Updates by means of the observation of the gravity vector: information about the attitude of the b) Low rate measurements: vehicle (roll and pitch) is incorporated into the GPS measurements of position zr and orientation system at variable rate. z , available at n*k steps, can be modeled by: • Updates by means of GPS measurements: zrvnn information about position and heading of the rr (4) zvnn vehicle are incorporated into the system at low rate.
806 Vol. 12, August 2014
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
2.3 System prediction Parameters xg and xa are correlation time factors which respectively model how fast the At every k step that measurements of gyroscopes bias of gyro and accelerometers are varying. g and accelerometers are available, the system state is the gravity vector and t, the sample time of is taken a step forward by the following (discrete) the system. nonlinear model: The state covariance matrix P is taken a step nb nb bn b qqqRt(1)()kk ([ (1) k ]) forward by: b Attitude (1)kgkgk y () x () PFPFFUF (6) (1)kk x () xuu x(1-)xg(k 1) xg t g(k) (5) rrnnn (v ta ) ( n t2 ) The measurement noise of gyroscopes and (1)kkk () (1) (1) k2 nnn accelerometers (respectively vg and va) are vv()(1)kkk ()at (1) Position incorporated into the system by means of the nbn aRy(1)kakak [x] () () g process noise covariance matrix U, through 2 2 parameters g and a : xa(k 1) (1- xa t )x a(k) 2222 bn b being q(R [ t]´) a quaternion computed from the UdiagI gxgaxa33 I 33 I 33 I 33 (7) bn b rotation vector R [ t]´ (using Eq. 25). The rotation vector [ b t] is determined by the bias-compensated The full model used for propagating the sensor b b gyro readings . Note that [ t] is converted to bias error is: biask+1=(1- t)biask + vb. Where vb navigation coordinates via the body-to-navigation models uncertainty in bias drift. The uncertainty rotation matrix Rbn. The matrix Rbn is computed from in bias for gyro and accelerometers nb the current quaternion q , (using Eq. 26). A similar (respectively vxg and vxa) is incorporated into model for integrating attitude by means of gyro the system through the noise covariance matrix 2 2 measurements was used in previous author work [21]. U via PSD parameters xg and xa .
Figure 2. Block diagram showing the system architecture.
Journal of Applied Research and Technology 807
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
fqnb fq nb One of the implications of coupling attitude and nb b 00000 q position equations is: If one of the axis of the IMU f b (usually the x axis) is assumed to be aligned 00 0000 (fixedly) toward the displacement of the vehicle (as x g it occurs in a typical land-vehicle configuration), then fx g 00 0000 GPS position measurements can be used for x g updating attitude. However, there are some cases nnn (8) F fr fr fr x 000nnn 0 where the assumption described above does not rav apply. For example, this is the case of an airplane ffvvnn 0000 0 where the pitch angle does not coincide with the vnna flight path angle due to the angle of attack. In this fafann 00000 work it is assumed that attitude and position qnb x g equations are coupled ( fan/ qnb 0). fx 000000 g x g 2.4 System update
0000 Every time that is needed, the filter can be updated as follows: f b 000 y g xxˆˆ Wz ( h ) (10) kk 1 ii fx g 000 vxg (9) P PWSW (11) F kk 1 i u 0000 0000 where zi is the current measurement and hi = h(x ) fan is the predicted measurement. W is the Kalman 00 0 gain computed from; y a fx a 000 WP HS 1 (12) vxa k 1 ii
Si is the innovation covariance matrix The Jacobian Fx (Eq. 8) is formed by the partial derivatives of the nonlinear prediction model SHPHR (13) (Eq. 5) with respect to the system state x . In iik 1 ii Jacobians the notation " fx/ y" is used for partial derivatives. And it reads as the partial derivative Hi is the Jacobian formed by the partial of the function f (which estimates the state derivatives of the measurement prediction model variable x), with respect to the variable y. h(x ) with respect to the system state x . Ri is the measurement noise covariance matrix. Equations Jacobian Fu (Eq. 9) is formed by the partial derivatives of the nonlinear prediction model 10 to 13 are either used for each one of the system (Eq. 5) with respect to the system inputs. updates that are carried out, along with their corresponding: i) zi ii) hi iii) Hi and iv) Ri.
In Jacobian Fx, it can be observed that the only term that correlates attitude and position a) Dynamical constraints update n nb Knowledge about the vehicle dynamics can be equations (see Eq. 5) is: fa / q . Therefore: incorporated into the system through the filter
n nb updates [22]. For this work, the data used for • fa / q 0 couples attitude and position experiments was captured by measurement estimation. devices mounted over a land vehicle. In this case,
n nb nonholonomic constraints are included in the • fa / q = 0 decouples attitude and position system in the form of zero velocity updates. estimation
808 Vol. 12, August 2014
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
nb If axis x of the IMU is assumed to be aligned with where gc is the constant gravity and R is the the axis x of the land vehicle, then y and z navigation to body rotation matrix computed from b nb velocities (in the body frame), respectively vy and the current quaternion q . For updating the filter b vz , can be modeled as zero plus an additive (Equations 10 to 13): 2 Gaussian white noise vv with PSD v . zy hh 2 Hh xˆ (18) i a i g R I33 a i g b v y 0 v v (14) b c) Position and heading updates v 0 vv z In this work, a loosely coupled approach is used for incorporating the data provided by the GPS unit The measurement prediction hv0 is computed from: into the system state. In a loosely coupled approach, the high level outputs provided by the bb (15) hv0 [vyz v ] GPS unit are directly incorporated into the system state via their corresponding measurement where prediction model.
[vbbb v v ] R nbn v xyz On the other hand, the architecture proposed in this work could be extended in order to be suitable nb R is the navigation to body rotation matrix for a tightly coupled approach. Among other things nb n computed from the current quaternion q , and v is (as the use of an adequate measurement model), the velocity of the vehicle (expressed in the in order to implement such adaptation, the system navigation frame) obtained from the current vector state should be augmented for including GPS state x . errors (e.g. time bias).
For updating the filter (Equations 10 to 13): The model hr used for predicting the GPS measurements about the vehicle position is: 2 (16) zi [0 0]' hhi V0 R I22 v Hhi v0 xˆ (19) hr [0310 I 33 0 39 ]xˆ b) Roll and pitch updates b If the vehicle is not accelerating, (i.e. a 0) then For updating the filter (Equations 10 to 13): b Eq. 3 can be approximated as ya g + va (neglecting or estimating x ). In this situation, a n 2 zzi r hhi r R I r Hi 0I0 accelerometers measurements ya provide noisy 33 31033 39 observations (in the body frame) about the gravity (20) vector. The gravity vector g can be used as an external reference for correcting roll and pitch In order to update the vehicle yaw using GPS estimations [21]. measurements, it is assumed that the heading
(yaw) of the vehicle is fixed with respect to the In order to detect k instants, where the body is in a angle, of course, measured by the GPS. If the non-accelerating mode, the Stance Hypothesis above assumption is not valid, as it could be the Optimal Detector (SHOE) is used [23]. case for some aerial vehicles (e.g. helicopter), then
an external reference should be used for updating The gravity vector g is predicted to be measured the yaw (e.g. earth´s magnetic field). by the accelerometers as h : g
The model h used for predicting the angle of nb (17) hgg R[00c ] course measured by the GPS is:
Journal of Applied Research and Technology 809
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
hqqqqqq atan2 2( ),1 2(22 ) (21) shows information about the parameters used in 23 14 3 4 experiments.
nb where q =[q1,q2,q3,q4] is the current quaternion. Figure 3 shows some of the experimental results For updating the filter (Equations 10 to 13): obtained from the input signals that have been described above. The left graphic, shows an aerial n zz hh R 2 Hhxˆ (22) view of the path followed by the vehicle as well as i i i its environment. Plot (a) shows the trajectory estimated by the proposed method. Position and heading updates can be also implemented together in a single filter update step. In experiments some GPS outages were simulated in order to test the performance of the method for 2.5 System initialization periods where navigation is solely based in inertial measurements (in the original dataset, the GPS The system is initialized as follows: signal is available for the whole trajectory). In Figure 3 (plots a and b) observe that the estimated ' trajectory is composed for green and red x0ˆ qnb (23) segments. Green segments indicate periods of ini 118 time where GPS is available, whereas red
segments indicate periods of time where GPS nb 2 outages occur. P()ini diag q ini 118 (24) Parameter Description Value Unit where is an arbitrary very small positive value. 2 4.8 × (rad/s) PSD for gyroscopes -6 2 nb g 10 The initial attitude q ini and its corresponding initial 2 nb 2 2 4.8 × (m/s ) PSD for accelerometers -2 2 uncertainty (q ini) are estimated in the same a 10 manner as [27]. It is important to remark that in this 2 PSD for GPS position 1.0 × 2 -2 (m) work, magnetometers are used only for having an r readings. 10 2 PSD for GPS heading 7.0 × 2 initial estimation of heading (yaw). -3 (rad) readings. 10 2 2 PSD for drift rate of 4.0 × (rad/s -14 2 In Eq. 23, note that the initial position of the vehicle xg gyro bias 10 ) 3 is set to zero due to the fact that local tangent 2 PSD for drift rate of 4.0 × (m/s ) -14 plane is used as navigation frame. On the other xa accelerometers bias 10 2 2 PSD for zero velocity 1.0 × 2 hand, for transforming GPS measurements, from v -2 (m/s) geodetic coordinates to the tangent plane constraint 10 Correlation time for 1.0 × -3 1/s coordinates, it is necessary to know the xg gyro bias 10 geographic location of the tangent plane origin. Correlation time for 1.0 × -4 1/s xa gyro bias 10 1.0 × 3. Experimental results t Sample time -2 s 10
In order to test the performance of the proposed Table 2. Parameters. method, a MATLAB implementation of the method was executed off-line using the Malaga In order to validate the capability of the method for dataset [24] as input signals. The Malaga dataset estimating the parameters of sensors error, the is freely available online, and contains several algorithm was executed again. In this case, sources of sensors signals along with an accurate running over the same input signals and conditions ground truth. The inertial measurement data were (GPS outages), but ignoring both, the estimated collected at 100 Hz, whereas GPS data were gyro bias xg and the estimated accelerometer bias collected at 4 Hz. The measurements devices, xa. Plot (b) shows the adverse effects in the used for collecting the data, were mounted over an estimated trajectory, when sensor errors are electric buggy-type vehicle, (see [24]). Table II ignored. In this case, and as it could be expected,
810 Vol. 12, August 2014
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814 there is a huge drift in the estimated trajectory for 4. Conclusions the periods of inertial navigation, where GPS is not available. Therefore, by comparing plots (a) and This work presents a practical method for plot (b), it can be deduced that the method is estimating the full kinematic state of a vehicle, performing the parameter estimation tasks along with sensor error parameters, through the reasonably well. integration of inertial and GPS measurements. The estimated vector state is formed by 22 state Figure 4. shows the progression over time for the variables: of which, 16 describes the full motion of rest of the estimated vector state of the vehicle the body in 6DOF, and 6 describes the parameters nb b n n [q , , xg, v , a , xa], for the experiment executed of sensors error (gyros and accelerometers bias). with the full method (Fig.3 (a)). The architecture of the system is based in an Extended Kalman filtering approach in direct The lower plots illustrate periods of time where configuration. The EKF is still the standard attitude and GPS updates take place. estimation technique for this kind of systems. However, practically all recent approaches found in For this experiment, observe that attitude literature are based in the indirect configuration of updates (roll and pitch) are suitably suspended the filter (called also error configuration). when the vehicle is experimenting some sudden turns. The GPS outages can also be appreciated One of the motivations of the proposed approach is in the lower-right plot. due to the clarity and simplicity associated with the
Figure 3. Left plot: Aerial view illustrating the path followed by the vehicle, while the sensor data were collected. Plot (a), shows its corresponding estimated trajectory. Green segments indicate periods of time with GPS availability. Red segments indicate periods of time where GPS outages occur. Plot (b) shows the trajectory obtained when the parameters of the sensors error (xg and xa) are ignored.
Journal of Applied Research and Technology 811
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
EKF in direct configuration. Moreover, the EKF in Different experiments with real data were carried direct configuration is the approach typically taught out in order to validate the performance of the in academic courses. In that sense, it is considered proposed method. The experimental results show that the proposed method could be straightforwardly that the method is capable of estimating the implemented, for example, by junior engineers and parameters of sensors error reasonably well. practitioners. Thereby, improving the system estimates in The proposed architecture has been also periods where GPS outages occur, and the designed in a manner that permits the scalability navigation is based solely in inertial of the system. For example, by means of the use measurements. of the adequate dynamical constraints (or possibly without them), the method could be easily modified for its use with another kind of Finally, it is considered that the method is enough vehicle or craft (e.g. aerial robots). robust for its use along with low-cost sensors.
Figure 4. Estimation results for a) Attitude of the vehicle qnb (expressed in Euler angles), b) Velocity rotation b b n of the vehicle (expressed in the body frame ), c) Gyroscopes bias xg, d) Velocity of the vehicle v (expressed in the navigation frame n), e) Acceleration of the vehicle an (expressed in n),
and f) Accelerometers bias xa. The lower plots illustrate periods where the attitude updates (left) and the GPS updates (right) occur.
812 Vol. 12, August 2014
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
Acknowledgment [11] J.H. Wang and Y. Gao, “Land Vehicle Dynamics- Aided Inertial Navigation,” IEEE Transactions on Author wants to thanks to Jessica Davalos for its Aerospace and Electronic Systems, vol.46, no.4, valuable support for correcting the language. pp.1638-1653, Oct. 2010.
References [12] L. Hang et al., “Two-Filter Smoothing for Accurate INS/GPS Land-Vehicle Navigation in Urban Centers,” [1] S. Sukkarieh et al., “Achieving integrity in an IEEE Transactions on Vehicular Technology, vol.59, INS/GPS navigation loop for autonomous land vehicle no.9, pp.4256-4267, Nov. 2010. applications,” IEEE International Conference on Robotics and Automation, 1998. [13] T.S. Bruggemann et al., “GPS Fault Detection with IMU and Aircraft Dynamics,” IEEE Transactions on [2] Q. Honghui and J.B. Moore, “Direct Kalman filtering Aerospace and Electronic Systems, vol.47, no.1, approach for GPS/INS integration,” IEEE Transactions pp.305-316, January 2011. on Aerospace and Electronic Systems, vol.38, no.2, pp.687-693, Apr 2002. [14] J. Dah-Jing and C. Ta-Shun, “Critical remarks on the linearised and Extended Kalman filters with geodetic [3] Y. Yunchun and J.A. Farrell, “Magnetometer and navigation examples,” Measurement, Vol. 43, No. 9, pp. differential carrier phase GPS-aided INS for advanced 1077-1089, 2010. vehicle control,” IEEE Transactions on Robotics and Automation , vol.19, no.2, pp. 269- 282, Apr 2003 [15] A.H. Mohamed and K.P. Schwarz, ”Adaptive Kalman Filtering for INS/GPS,” Journal of Geodesy 73: [4] S. Godha and M.E. Cannon, “Integration of DGPS 193-203, 1999. with a Low Cost MEMS - Based Inertial Measurement Unit (IMU) for Land Vehicle Navigation Application,” [16] J.L. Crassidis, “Sigma-point Kalman filtering for Proceedings of the ION GNSS 2005. integrated GPS and inertial navigation,” IEEE Transactions on Aerospace and Electronic Systems, [5] I. Skog and P. Händel, “A low-cost GPS aided inertial vol.42, no.2, pp. 750- 756, April 2006. navigation system for vehicle applications,” in Proc. EUSIPCO 2005. [17] H. Carvalho H., et al., “Optimal nonlinear filtering in GPS/INS integration,” IEEE Transactions on Aerospace [6] W. Wang, et al., “Quadratic extended Kalman filter and Electronic Systems, vol.33, no.3, pp.835-850, July approach for GPS/INS integration, Aerospace Science 1997. and Technology,” Volume 10, Issue 8, December 2006.
[7] P. Davidson et al., “Using low-cost MEMS 3D [18] J. Georgy et al., “Low-Cost Three-Dimensional accelerometer and one gyro to assist GPS based car Navigation Solution for RISS/GPS Integration Using navigation system,” Proceedings of International Mixture Particle Filter,” IEEE Transactions on Vehicular Conference on Integrated Navigation Systems, 2008. Technology, vol.59, no.2, pp.599-615, Feb. 2010.
[8] Z.F. Syed et al., “Civilian Vehicle Navigation: [19] N. El-Sheimy et al., “The Utilization of Artificial Required Alignment of the Inertial Sensors for Neural Networks for Multisensor System Integration in Acceptable Navigation Accuracies,” IEEE Transactions Navigation and Positioning Instruments,” IEEE on Vehicular Technology, vol.57, no.6, pp.3402-3412, Transactions on Instrumentation and Measurement, Nov. 2008. vol.55, no.5, pp.1606-1615, Oct. 2006.
[9] Johan B., Willem S., Kalman filter configurations for a [20] V. Agarwal et al., “Design and Development of a low-cost loosely integrated inertial navigation system on Real-Time DSP and FPGA-Based Integrated GPS-INS an airship, Control Engineering Practice, Volume 16, System for Compact and Low Power Applications,” IEEE Issue 12, December 2008. Transactions on Aerospace and Electronic Systems, vol.45, no.2, pp.443-454, April 2009. [10] A. Noureldin et al., “Performance Enhancement of MEMS-Based INS/GPS Integration for Low-Cost [21] R. Munguía and A. Grau, “Closing Loops with a Navigation Applications,” IEEE Transactions on Virtual Sensor Base don Monocuar SLAM,” IEEE Vehicular Technology, vol.58, no.3, pp.1077-1096, Transactions on Instrumentation and Measurement 2009. 2009.
Journal of Applied Research and Technology 813
A GPS aided Inertial Navigation System in Direct Configuration, R. Munguía / 803 814
[22] D. Simon, “Kalman filtering with state constraints: a Euler angles , , , (roll, pitch and yaw) can be survey of linear and nonlinear algorithms,” IET Control computed from a quaternion q by: Theory & Applications, vol.4, no.8, pp.1303-1318, August 2010. atan2 2qq qq , 1 2 q22 q 34 12 2 3 [23] I. Skog et al., “Zero-Velocity Detection An Algorithm
Evaluation,” IEEE Transactions on Biomedical (27) asin -qqqq 2 Engineering, vol.57, no.11, pp.2657-2666, November 13 24 2010. 22 atan2 2qq23 qq 14 , 1 2 q 3 q 4 [24] J.L. Blanco et al., “A collection of outdoor robotic datasets with centimeter-accuracy ground truth,” Autonomous Robots 27, 4, 2009.
[25] J. Zhou et al., “INS/GPS Tightly-coupled Integration using Adaptive Unscented Particle Filter,” The Journal of Navigation, vol. 63, pp. 491-511, 2010.
[26] E,J. Ohlmeyer, "Analysis of an Ultra-Tightly Coupled GPS/INS System in Jamming," Position, Location, And Navigation Symposium IEEE/ION, pp. 44-53, 2006.
[27] R. Munguia and A. Grau, "A Practical Method for Implementing an Attitude and Heading Reference System," International Journal of Advanced Robotic Systems, vol. 11, 2014.
[28] J. Rankin, "GPS and differential GPS: An error model for sensor simulation," In Position, Location, and navigation Symposium, pp. 260-266, 1994.
[29] F. Tsai et al., "A positioning scheme combining location tracking with vision assisting for wireless sensor networks," Journal of Applied Research and Technology, vol.11, no.2, pp. 292–300, 2013.
Appendix
In this appendix include some transformations: The rotation vector to quaternion q transfor- mation is defined by:
q cos sin (25) 22
The rotation matrix Rnb can be computed from the quaternion q by:
()2()2()qqqq2222 qqqq qqqq 1234 2314 1324 nb 2222 Rqqqqqqqqqqqq 2(23 14 ) ( 1 2 3 4 ) 2( 34 12 ) 2(qq qq ) 2( qq qq ) ( q2222 q q q ) 24 13 12 34 1 2 3 4 (26)
814 Vol. 12, August 2014