1. INTRODUCTION
High accuracy integrated navigation systems based on carrier-phase satellite navigation systems such as the Global Positioning System (GPS) and Inertial Navigation System (INS) are under development for a variety of applications in unmanned air vehicles, airborne survey and gravimetry and remote sensing by direct geo-referencing of aerial imagery [Farrell and Barth, Reference Farrell and Barth1999, Farrell, Givargis, and Barth, Reference Farrell, Givargis and Barth2000, Yang, Farrell and Tan, Reference Yang, Farrell and Tan2001a, Reference Yang, Farrell and Tan2001b]. With the availability of several fully operational satellite navigation systems, it has been recognized that an optimal combination of one or more satellite navigation systems with inertial navigation has a number of advantages over stand-alone inertial or satellite navigation. Satellite contributes its high accuracy and stability over time, enabling continuous monitoring of inertial sensor errors. Implementation of closed-loop INS error calibration allows continuous and adaptive error update that bounds INS errors, leading to increased estimation accuracy. Thus the satellite navigation aiding information is used to reduce the estimate errors in the INS state and to continuously calibrate the inertial sensors. This results in improved INS accuracy. On the other hand, INS contributes immunity to satellite outages. During periods when signals from some or all of the satellites become unavailable, the INS continues to provide vehicle state information. The INS also provides for continuous attitude monitoring, and the reduction of the carrier phase ambiguity search volume/time. Using a carrier phase based and calibrated satellite navigation system and a high to medium accuracy inertial system, attitude accuracy in the range of 10–30 arc-secs can be achieved in principle [Grejner-Brzezinska and Wang, Reference Grejner-Brzezinska and Wang1998]. Therefore, the integrated approach has been shown to result in reliability, latency, bandwidth, and update rate improvements relative to the satellite navigation-only approach.
Although there have been several studies of the integration of satellite navigation systems with inertial navigation systems (see for example Wang, Lachapelle and Cannon, Reference Wang, Lachapelle and Cannon2004), most of these have been restricted to low cost solutions. With the low cost solutions it is practically impossible to obtain accurate estimates of the attitude. Most of the low cost solutions use a complement of solid state accelerometers and do not use the more expensive rate or even attitude gyros required for precise attitude estimation. Accurate estimation of the attitude will require an independent measurement of the attitude or even the attitude and angular velocity vector. Compared to a low cost solution the development of a high precision integrated system would involve fibre-optic gyro based angular velocity measurements (Bye, Hartmann and Killen, Reference Bye, Hartmann and Killen1998) and multiple-antenna based attitude measurements.
In addition to the pseudo-range measurement, a carrier phase measurement is usually provided in many modern satellite navigation receivers. Two types of measurements are available from a typical satellite navigation system (Hatch, Reference Hatch1982). The relative phase between the received reconstructed carrier phase and the receiver clock phase at a particular epoch may be measured. This measurement is a fine measurement of pseudo-range in terms of the non-integer number of cycles with the integer number or whole cycles deleted. Another form of carrier related measurement that is more common is obtained by integrating the rate of change of relative phase over a specific time interval as determined by the receiver clock. To complement the angular velocity and/or attitude measurements either of the carrier phase measurements are used in several high precision satellite navigation applications to recursively smooth and improve code based range measurements via the use of an embedded filter and an embedded fast ambiguity resolution method.
The carrier-smoothed code processing is based on the concept that estimating the bias in the integrated carrier phase measurement is essential in order to convert it into an absolute measurement of range. Although the carrier phase can be very accurately measured, the integrated carrier phase information cannot be directly mixed with the pseudo-range since there is a phase ambiguity between the receiver and satellite which is equal to an integral multiple of 2π. However, the change in the pseudo-range between observations at different points of time (epochs) approximately equals the change in the integrated carrier phase. The change in the integrated carrier phase can however be determined with far more accuracy than the change in pseudo-range. Carrier-smoothed code processing uses the carrier phase information to correct the code phase tracking loop to reduce multipath and receiver noise on the pseudo-ranges. Navigation equipment with a high precision requirement (e.g., aircraft autopilots for aircraft landing) and satellite navigation reference stations for differential correction (e.g., LAAS) are two examples. The smoothing of pseudo-range observations using carrier phase observations has been elaborated by Hatch (Reference Hatch1982) who introduced a recursive algorithm known as the Hatch filter. The Hatch filter is a simple one-dimensional filter that uses the carrier-phase measurement to recursively update the pseudo-range. Two issues associated with the Hatch filter are multi-path and ionospheric error induced divergence of the Hatch filter. Several modifications of the Hatch filter have been proposed to mitigate the effects of multi-path and ionospheric error. Ray, Cannon and Fenton (Reference Ray, Cannon and Fenton2001) have proposed a multi-antenna method for mitigation of multi-path effects.
Tropospheric and ionospheric delays are significant, requiring accurate evaluation to implement a stable Hatch filter. The ionosphere advances the phases and delays the codes on a carrier signal in equal magnitude. Although tropospheric delays are independent of frequency, the magnitude of the ionospheric delay is inversely proportional to the square of carrier frequency. This relationship is used to form an ionospheric independent observable using dual and triple frequency phase and code observations. The ionospheric error at a single frequency could be estimated adaptively by a method outlined by Kim, Walter and Powell (Reference Kim, Walter and Powell2007). As the ionospheric error is dependent on the carrier frequency several multi-frequency methods have also been presented for eliminating the ionospheric errors.
Recent developments in satellite navigation include GPS modernisation and the development of the European Galileo system which have led to the development of new algorithms. Following the GPS modernisation scheme, a third GPS frequency, L5, centred at 1176·45 MHz is being transmitted from Block IIF satellites, the first launch of which was in 2005 (Fontana et al., Reference Fontana, Cheung and Stansell2001). Using the three frequency observations, a number of linear combinations are possible with characteristics such as longer wavelength, low ionospheric delay, less measurement noise and retention of the integer property of phase ambiguities.
The ambiguity estimators in combination with long wavelength, low ionosphere delays and low noise converge more rapidly. A stepwise algorithm may be developed which commences with ambiguity convergence of the combination with longest wavelength, low ionosphere content and low noise. Once resolved, the ambiguities in combinations with shorter wavelengths may be estimated with greater reliability. Such an algorithm has been proposed for three-frequency relative positioning (Hatch, Reference Hatch1996). The concept of stepwise ambiguity resolution has facilitated simultaneous code and carrier update (CCU). Hwang (Reference Hwang1991) introduced the concept of carrier phase riding (CPR) as one is able to update the integer ambiguity, provided it is initially known, given incremental measurements or rate of change of the relative carrier phase. Teunissen (Reference Teunissen1994) has also presented a method for ambiguity resolution based on transforming the variables to re-parameterise the integer ambiguity. Forssell, Martin-Neira and Harris (Reference Forssell, Martin-Neira and Harris1997) have proposed a method that uses the measurements at all three carrier frequencies. Henderson, Raquet and Maybeck (Reference Henderson, Raquet and Maybeck2002) have presented a multi-filter approach to ambiguity resolution. Yang, Sharpe and Hatch (Reference Yang, Sharpe and Hatch2002) presented a technique based upon the concept of a Residual Sensitivity Matrix proposed by Hatch and Sharpe (Reference Hatch and Sharpe2001), which relates the search integer ambiguity-set to each carrier phase residual directly. The technique uses the singular value decomposition of the Residual Sensitivity Matrix to find the minimum search space. The technique not only improves the calculation efficiency and ambiguity resolution time, but also improves the reliability. The search space is minimized by selecting only those combinations of possible ambiguity values which are consistent with the satellite geometry and the measurement residuals.
When an additional ground station or pseudo-satellite can provide differential type corrections all the three methods mentioned above i.e. carrier smoothed code, carrier phase riding and code and carrier update can be applied concurrently with the differential correction thus facilitating the application of satellite and INS mixing to aircraft landing.
In this paper initially the feasibility of constructing a satellite aided inertial navigation system was explored using a set of real time kinematic measurements as well as measurements from accelerometers and a three-axis fibre-optic gyroscope. Our primary interest is in designing a system that is suitable for aircraft landing applications. Initial studies indicated that whenever any drift was present in the accelerometer measurements the aircraft estimated velocity vector components drifted away from the true components, albeit at a very slow rate. Such drifts are unacceptable for landing applications. It was then decided to include Doppler based aircraft velocity vector component measurements in the navigation frame. The velocity measurement aided system clearly indicated the feasibility of developing a high-precision kinematic satellite and Doppler aided inertial navigation system. In this paper we compare the results obtained for the state estimates using an adaptive UKF without and with Doppler based aircraft velocity vector component measurements in the navigation frame and demonstrate the feasibility of developing a high precision aided inertial navigation system.
2. PROCESS MODELLING WITH CARRIER PHASE MEASUREMENTS
The basic navigation equations have been derived by Farrell and Barth (Reference Farrell and Barth1999). These are summarised here for completeness:
![\hskip -2pt\eqalign{ \left[ {\matrix{ {\dot{V}_{N} } \cr {\dot{V}_{E} } \cr {\dot{V}_{D} } \cr} } \right] \equals \left[ {\matrix{ {A_{N} } \cr {A_{E} } \cr {A_{D} } \cr} } \right] \plus \left[ {\matrix{ 0 \cr 0 \cr g \cr} } \right] \tab\plus \left[ {\matrix{ { \minus \left( \!{2\omega _{s} \sin \lambda \plus \displaystyle{{V_{E} } \over {R_{P} \plus h}}\tan \lambda } \right)V_{E} \plus \displaystyle{{V_{N} V_{D} } \over {R_{M} \plus h}}} \cr\!\! {\left( \!{2\omega _{s} \sin \lambda \plus \displaystyle{{V_{E} } \over {R_{P} \plus h}}\tan \lambda } \right)V_{N} \plus \left(\! {2\omega _{s} \cos \lambda \plus \displaystyle{{V_{E} } \over {R_{P} \plus h}}} \right)V_{D}\!\! } \cr { \minus \displaystyle{{V_{N}^{\setnum{2}} } \over {R_{M} \plus h}} \minus \left( \!{2\omega _{s} \cos \lambda \plus \displaystyle{{V_{E} } \over {R_{P} \plus h}}} \right)V_{E} } \cr} } \right]\comma \cr \tab \dot{\lambda } \equals {{V_{N} } \over {R_{M} \plus h}}\comma \ \dot{\varphi } \equals {{V_{E} \sec \lambda } \over {R_{P} \plus h}}\comma \ \dot{h} \equals \minus V_{D} \comma }\hskip -16pt\vskip 40pt](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn1.gif?pub-status=live)
where V N, V E and V D are the north, east and down velocities in the local tangent plane, with reference to a local geodetic frame often referred to as the navigation frame (n-frame) or north-east-down frame. The last three equations relate these velocities to the rate of change of the geodetic latitude (λ), the rate of change of longitude (ϕ) and the altitude (h) rate. A N, A E and A D are the north, east, down components of the measured acceleration in the n-frame which must be compensated by adding the acceleration due to gravity g, in down direction, ωs is angular velocity of the Earth, R M and R P are the radii of curvature in the meridian and prime vertical at a given latitude. Unit vectors in the n-frame are related to the unit vectors in the Earth centred inertial frame according to the relations:
![\eqalign{ \tab \left[ {\matrix{ {i_{G} } \cr {j_{G} } \cr {k_{G} } \cr} } \right] \equals {\bf D}_{n\comma I} \left[ {\matrix{ {i_{I} } \cr {j_{I} } \cr {k_{I} } \cr} } \right]\comma \quad {\bf D}_{n\comma I} \equiv \left[ {\matrix{ { \minus \sin \lambda \cos \left( {\varphi \plus \rmXi } \right)} \tab { \minus \sin \lambda \sin \left( {\varphi \plus \rmXi } \right)} \tab {\cos \lambda } \cr { \minus \sin \left( {\varphi \plus \rmXi } \right)} \tab {\cos \left( {\varphi \plus \rmXi } \right)} \tab 0 \cr { \minus \cos \lambda \cos \left( {\varphi \plus \rmXi } \right)} \tab { \minus \cos \lambda \sin \left( {\varphi \plus \rmXi } \right)} \tab { \minus \sin \lambda } \cr} } \right] }](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn2.gif?pub-status=live)
where Ξ is the hour angle of the vernal equinox. The vector of the north, east, down components of the measured acceleration in the n-frame are related to the body components of the measured acceleration, by the transformation,
where the transformation of the measured body acceleration components to the north, east, down components in the n-frame Dn,b, satisfies the differential equation:
In equation 4 the matrix ΩG is obtained from the components of the angular velocity vector of the local geodetic frame or n frame. The angular velocity vector of the local geodetic frame or n frame may be expressed in terms of the Earth angular velocity in the local geodetic frame ωs as:
![\bmomega _{G} \equals \bmomega _{s} \plus \left[ {\matrix{ {\dot{\varphi }\cos \lambda } \cr { \minus \dot{\lambda }} \cr { \minus \dot{\varphi }\sin \lambda } \cr} } \right]{\rm \ with\ }\bmomega _{s} \equals \omega _{s} \left[ {\matrix{ {\cos \lambda } \cr 0 \cr { \minus \sin \lambda } \cr} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn5.gif?pub-status=live)
Given a vector, ![\bmomega \equals \left[ {\matrix{ {\omega _{\setnum{1}} } \tab {\omega _{\setnum{2}} } \tab {\omega _{\setnum{3}} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqnU1.gif?pub-status=live){kind=small}
, ω× is defined by the relation:
![\bmomega _{ \times } \equals \left[ {\matrix{ \hskip 4pt 0 \tab {\minus \omega _{\setnum{3}} } \tab {\hskip 9pt\omega _{\setnum{2}} } \cr {\hskip 10pt \omega _{\setnum{3}} } \tab \hskip 4pt 0 \tab { \minus \omega _{\setnum{1}} } \cr { \minus \omega _{\setnum{2}} } \hfill\tab { \hskip 9pt\omega _{\setnum{1}} } \tab \hskip 4pt0 \cr} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn6.gif?pub-status=live)
Then ΩG is defined as ΩG=ωG×. Similarly Ωb is defined as Ωb=ωb× where ωb is the body angular velocity in the body fixed frame.
In principle, the scalar acceleration measurements may be expressed as:
![a_{\bf i} \equals {\bf z}^{i} \cdot \left[ {{ \ddot{\bf R}}_{\bf I} \minus {\bf r}^{i} \times \dot{\bmomega } \plus \bmomega \times \bmomega \times {\bf r}^{i} } \right]\comma \ i \equals 1\comma \ 2\comma \ 3 \,....\, 6,](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn7.gif?pub-status=live){kind=small}
where zi, is the direction of sensitivity of the i th accelerometer, ri is the position vector of the accelerometer location in the body fixed frame, ω=ωb and 
is the inertial acceleration of the origin of the body frame. Assuming that all accelerometers are co-located and with three independent accelerometer measurements it is, in principle, possible to express:
![{\bf z}^{i} {\ddot{\bf R}}_{\bf I} \equals a_{\bf i} \plus \left[ {{\bf z}^{i} \cdot {\bf r}_{ \times } } \right]\dot{\bmomega } \minus {\bf z}^{i} \cdot \left[ {\bmomega \times \bmomega \times {\bf r}} \right] \equals a_{i} \plus \left[ {{\bf z}^{i} \cdot {\bf r}_{ \times } } \right]\dot{\bmomega } \plus f_{i} \left( {{\bf z}^{i} \comma {\bf r}\comma \bmomega } \right)\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn8.gif?pub-status=live){kind=small}
where, r=ri
![f_{i} \left( {{\bf z}^{\bf i} \comma {\bf r}\comma \bmomega } \right) \equiv \minus {\bf z}^{i} \cdot \left[ {\bmomega \times \bmomega \times {\bf r}} \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn9.gif?pub-status=live){kind=small}
It follows that:
Defining the vectors di as:
![\openup 2pt\eqalign{\left[ {{\bf d}_{i} } \right] \equiv \tab \left[ {\matrix{ { \minus {\bf z}^{i} \cdot {\bf r}_{ \times }^{i} } \tab {{\bf z}^{i} } \cr} } \right] \equals \left[ {\matrix{ {z_{\setnum{3}}^{i} r_{\setnum{2}}^{i} \minus z_{\setnum{2}}^{i} r_{\setnum{3}}^{i} } \tab {z_{\setnum{1}}^{i} r_{\setnum{3}}^{i} \minus z_{\setnum{3}}^{i} r_{\setnum{1}}^{i} } \tab {z_{\setnum{2}}^{i} r_{\setnum{1}}^{i} \minus z_{\setnum{1}}^{i} r_{\setnum{2}}^{i} } \tab {z_{\setnum{1}}^{i} } \tab {z_{\setnum{2}}^{i} } \tab {z_{\setnum{3}}^{i} } \cr} } \right] \cr \tab \left[ {{\bf d}_{i} } \right] \equiv \left[ {{\bf z}^{i} } \right] \equals \left[ {\matrix{ {z_{\setnum{1}}^{i} } \tab {z_{\setnum{2}}^{i} } \tab {z_{\setnum{3}}^{i} } \cr} } \right] \cr}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn11.gif?pub-status=live){kind=small}
equation 8 may be expressed as:
![{\bf D}\ddot{\bf R}}_{\bf I} \equals {\bf A} \plus {\bf D}\left[ {{\bf r}_{ \times } } \right]\dot{\bmomega } \plus {\bf DF}\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn12.gif?pub-status=live){kind=small}
where, ![{\bf D} \equals \left[ {\matrix{ {{\bf d}_{\setnum{1}}^{T} } \tab {{\bf d}_{\setnum{2}}^{T} } \tab {{\bf d}_{\setnum{3}}^{T} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqnU3.gif?pub-status=live){kind=small}
, ![{\bf A} \equals \left[ {\matrix{ {a_{\setnum{1}} } \tab {a_{\setnum{2}} } \tab {a_{\setnum{3}} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqnU4.gif?pub-status=live){kind=small}
, and
![\openup 1pt{\bf F} \equals \left[ {\matrix{ {\left( {\omega _{\setnum{3}}^{\setnum{2}} \plus \omega _{\setnum{2}}^{\setnum{2}} } \right)r_{\setnum{1}} \minus \omega _{\setnum{1}} \omega _{\setnum{2}} r_{\setnum{2}} \minus \omega _{\setnum{1}} \omega _{\setnum{3}} r_{\setnum{3}} } \cr { \minus \omega _{\setnum{2}} \omega _{\setnum{1}} r_{\setnum{1}} \plus \left( {\omega _{\setnum{1}}^{\setnum{2}} \plus \omega _{\setnum{3}}^{\setnum{2}} } \right)r_{\setnum{2}} \minus \omega _{\setnum{2}} \omega _{\setnum{3}} r_{\setnum{3}} } \cr { \minus \omega _{\setnum{3}} \omega _{\setnum{1}} r_{\setnum{1}} \minus \omega _{\setnum{3}} \omega _{\setnum{2}} r_{\setnum{2}} \plus \left( {\omega _{\setnum{2}}^{\setnum{2}} \plus \omega _{\setnum{1}}^{\setnum{2}} } \right)r_{\setnum{3}} } \cr} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqnU5.gif?pub-status=live)
Equation 12 may be expressed as:
![{\ddot{\bf R}}_{\bf I} \equals {\bf D}^{ \minus {\bf \setnum{1}}} {\bf A} \plus \left[ {{\bf r}_{ \times } } \right]\dot{\bmomega } \plus {\bf F}.](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn13.gif?pub-status=live){kind=small}
At this stage it is important to recognise that the definition of the function vector F, must be modified after considering that measurements of acceleration must be compensated by adding the local acceleration due to gravity. Furthermore the definition of the acceleration of gravity generally includes the centripetal acceleration due to the Earth's rotation rate vector, ωs. For this reason, one defines,
![\rmDelta {\bf F} \equiv \left( {\left[ {\bmomega _{s} \times \bmomega _{s} \times {\bf r}} \right] \minus \left[ {\bmomega \times \bmomega \times {\bf r}} \right]} \right)\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn14.gif?pub-status=live){kind=small}
and equation Reference Julier, Uhlmann and Durrant-Whyte13 gives the body components of the acceleration as
where, G is the gravitational component of the acceleration in the body frame, Am is the actual measured acceleration vector obtained from a triad of pendulous accelerometers, b1 is a measurement bias and drift vector and n1 is a measurement white noise vector. The north east and down accelerations are:
![{\bf A}_{NED}^{T} \equals {\bf D}_{n\comma b} \left[ {{\bf D}^{ \minus {\bf \setnum{1}}} \left( {{\bf A}_{m} \plus {\bf G} \plus {\bf b}_{\setnum{1}} \plus {\bf n}_{\setnum{1}} } \right) \plus \rmDelta {\bf F}} \right]\comma \ {\bf D}_{n\comma b} \equals {\bf D}_{n\comma I} {\bf D}_{b\comma I}^{ \minus \setnum{1}}.](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn16.gif?pub-status=live){kind=small}
The north east and down accelerations may be expressed in terms of the measured north, east and down components of the acceleration and north, east and down components of the gravity vector as:
where,
![{\bf A}_{NED} \equals {\bf D}_{n\comma b} \left[ {{\bf D}^{ \minus {\bf \setnum{1}}} \left( {{\bf A}_{m} \plus {\bf b}_{\setnum{1}} \plus {\bf n}_{\setnum{1}} } \right) \plus \rmDelta {\bf F}} \right]\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn18a.gif?pub-status=live){kind=small}
and
![{\bf G}_{NED} \equals {\bf D}_{n\comma b} \left[ {{\bf D}^{ \minus {\bf \setnum{1}}} {\bf G}} \right] \equals \left[ {\matrix{ 0 \tab 0 \tab g \cr} } \right]^{T}.](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn18b.gif?pub-status=live){kind=small}
The drift and bias vectors are assumed to be a first order Gauss-Markov process given by:
where n2, n3, are a white noise vector driving the processes.
The body angular velocity vector, ω=ωb is assumed to be measured by a triad of fibre optic laser gyros. Thus the measure angular velocity vector is assumed to be related to the body angular vector,
where L is a matrix of the three directions of sensitivity of the fibre-optic laser gyros, ωm the actual measured angular velocities, b2 is a measurement bias and drift vector and n3 is a measurement white noise vector. Following Savage (Reference Savage1998a) and Savage (Reference Savage1998b) the bias and drift vector is assumed to be a first order Gauss-Markov process given by:
where n5, n6, are a white noise vector driving the processes.
It is assumed that there is no need to scale either the acceleration or angular velocity measurements as the sensors are assumed to be calibrated. Thus no provision is made for scaling the measurements. Furthermore when the three accelerometer measurement axes and fibre-optic gyro measurement axes coincide with the body axes, it can be assumed that nominally, D=L=I3×3.
The attitude quaternion is then computed from the equations:
![{\dot{\bf q}} \equals {{\bf 1} \over {\bf 2}} \ \bmOmega \left( \bmomega \right){\bf q}\comma \ \bmOmega \left( \bmomega \right) \equals \left[ {\matrix{ \hskip 4pt 0 \tab {\hskip 9pt \omega _{\setnum{3}} } \tab { \minus \omega _{\setnum{2}} } \tab {\omega _{\setnum{1}} } \cr { \minus \omega _{\setnum{3}} } \tab \hskip 4pt 0 \tab {\hskip 9pt \omega _{\setnum{1}} } \tab {\omega _{\setnum{2}} } \cr {\hskip 10pt \omega _{\setnum{2}} } \tab { \minus \omega _{\setnum{1}} } \tab \hskip 4pt 0 \tab {\omega _{\setnum{3}} } \cr { \minus \omega _{\setnum{1}} } \tab { \minus \omega _{\setnum{2}} } \tab { \minus \omega _{\setnum{3}} } \tab 0 \cr} } \right]\comma \ \bmomega \equals \left[ {\matrix{ {\omega _{\setnum{1}} } \cr {\omega _{\setnum{2}} } \cr {\omega _{\setnum{3}} } \cr} } \right]\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn22.gif?pub-status=live)
where the quaternion components are subject to the constraint q 12+q 22+q 32+q 42=1. Once the solution for the quaternion is known, the transformation from the inertial to the body fixed frame Db,I is computed from:
![\openup 6pt{\bf D}_{b\comma I} \left( {\bf q} \right) \equals \left[ {\matrix{ {q_{\setnum{4}}^{\setnum{2}} \plus q_{\setnum{1}}^{\setnum{2}} \minus q_{\setnum{2}}^{\setnum{2}} \minus q_{\setnum{3}}^{\setnum{2}} } \tab {2\left( {q_{\setnum{1}} q_{\setnum{2}} \plus q_{\setnum{3}} q_{\setnum{4}} } \right)} \tab {2\left( {q_{\setnum{1}} q_{\setnum{3}} \minus q_{\setnum{2}} q_{\setnum{4}} } \right)} \cr {2\left( {q_{\setnum{1}} q_{\setnum{2}} \minus q_{\setnum{3}} q_{\setnum{4}} } \right)} \tab {q_{\setnum{4}}^{\setnum{2}} \minus q_{\setnum{1}}^{\setnum{2}} \plus q_{\setnum{2}}^{\setnum{2}} \minus q_{\setnum{3}}^{\setnum{2}} } \tab {2\left( {q_{\setnum{2}} q_{\setnum{3}} \plus q_{\setnum{1}} q_{\setnum{4}} } \right)} \cr {2\left( {q_{\setnum{1}} q_{\setnum{3}} \plus q_{\setnum{2}} q_{\setnum{4}} } \right)} \tab {2\left( {q_{\setnum{2}} q_{\setnum{3}} \minus q_{\setnum{1}} q_{\setnum{4}} } \right)} \tab {q_{\setnum{4}}^{\setnum{2}} \minus q_{\setnum{1}}^{\setnum{2}} \minus q_{\setnum{2}}^{\setnum{2}} \plus q_{\setnum{3}}^{\setnum{2}} } \cr} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn23.gif?pub-status=live)
and its inverse is obtained by the same equation by changing the sign of q 4. The required transformation Dn,b may then be computed without matrix inversion from Dn,IDb,I−1, the transformations from the inertial to the n-frame and the inverse transformation from the inertial to the body fixed frame. Alternately Dn,b may be computed directly from the associated quaternion, representing the relative attitude of the navigation from relative to the body frame.
3. GPS MEASUREMENTS MODELLING
Pseudo-range observations are obtained by measuring the time taken for the signal to propagate from the satellite to the receiver and multiplying the measurement by the speed of light after it has been corrected for bias errors. Due to the lack of complete synchronization between the receiver and satellite clocks, errors in the orbital ephemeris, the delays due to the ionosphere and troposphere and multipath effects the measured pseudo-range is always biased. The pseudo-range measurement for a single satellite may be expressed in terms of the speed of light c, the satellite clock delay error dt, the receiver clock error delay dT, as:
where, ρm is the measured pseudo-range, ρ is the true magnitude of the pseudo-range vector, δρe is the pseudo-range error due to errors in the orbital ephemeris, δρion is the pseudo-range error due to delays in the ionosphere, δρtrop is the pseudo-range error due to delays in the troposphere, δρp is the pseudo-range error due to multi-path effects and v ρ is the measurement noise. The errors due to errors in the orbital ephemeris, satellite and receiver clock biases, delays in the ionosphere, delays in the troposphere, and those due to multi-path effects may be estimated. Thus the estimate of the pseudo-range may be expressed in terms of the actual magnitude of the pseudo-range vector ρ, as:
When the measurement is entirely based on code phase measurements, the noise v ρ is composed of a multi-path error, v ρm, a code phase measurement error, v ρc and a third component v ρ0, which is a zero-mean white noise process, representing the receiver noise. The multi-path error may be modelled as a first order Gauss Markov process and hence can be considered to be the output of the process defined by:
where v mpi is a white noise process, representing the multi-path component of the noise in the i th code pseudo-range measurement.
However in most modern receivers it can be modelled as a mix of code-phase and carrier phase measurements as the Hatch filter can be interpreted as mixing filter of the two measurements. Thus, if a total L φ measurements are made between two successive code measurements the Hatch filter output at epoch k can be expressed in terms of the filter's output at epoch k−1 in the form:
where φmk is the measured carrier phase at epoch k and ρmci is the measured i th code pseudo-range. As the carrier phase has an integer cycle ambiguity, which causes a very poor initial position, it is assumed that the code phase measurement is used to initialize N. Consequently it is assumed that the ambiguity error in the measured carrier phase is initially estimated, corrected and eliminated within the receiver. Thus the effect of the filter in only to reduce the noise and the filter may be equivalently modelled as:
![\openup 1.5pt\eqalign{ \left[ {\matrix{ {\hat{v}_{k \plus \setnum{1}}^{\phi } } \cr {\hat{m}_{k \plus \setnum{1}} } \cr {\hat{v}_{k \plus \setnum{1}} } \cr} } \right] \equals \tab {1 \over {L_{\phi } }}\left[ {\matrix{ 0 \tab 0 \tab 0 \cr 0 \tab {L_{\phi } a_{m} } \tab 0 \cr { \minus \left( {L_{\phi } \minus 1} \right)} \tab {a_{m} } \tab {\left( {L_{\phi } \minus 1} \right)} \cr} } \right]\left[ {\matrix{ {\hat{v}_{k}^{\phi } } \cr {\hat{m}_{k} } \cr {\hat{v}_{k} } \cr} } \right] \cr \tab \plus {1 \over {L_{\phi } }}\left[ {\matrix{ {L_{\phi } } \tab 0 \tab 0 \cr 0 \tab {L_{\phi } } \tab 0 \cr {\left( {L_{\phi } \minus 1} \right)} \tab 1 \tab 1 \cr} } \right]\left[ {\matrix{ {v^{\phi } } \cr {v_{mp} } \cr {v_{\rho c} } \cr} } \right].}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn28a.gif?pub-status=live)
In the above filter the additional assumption is made that the zero-mean white noise processes v φ, v ρm and v ρc are stationary.
The actual pseudo-range vector is related to the geodetic latitude λ, geocentric latitude λs, longitude φ and altitude h, by the relations:
![\openup 2.3pt\bmrho \equals \left[ {\matrix{ {r_{s} \cos \lambda _{s} \cos \phi \plus h\cos \lambda \cos \phi } \cr {r_{s} \cos \lambda _{s} \sin \phi \plus h\cos \lambda \sin \phi } \cr {r_{s} \sin \lambda _{s} \plus h\sin \lambda } \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn29.gif?pub-status=live)
where ρ is the Earth centred, Earth fixed position vector of the aircraft, r s the radius at a surface point of the flattened Earth ellipsoid and λs the geocentric latitude are respectively defined in terms of the flattening f and the equatorial radius R e as:
The change in attitude of an aircraft over a period of time could be observed by comparing the current measured phase differential with the initial phase differential measured at some initial reference time. Thus this difference in the measured phase differential could be expressed as:
where rB is the navigation satellite's sight line vector at the current time and r0 is the navigation satellite's sight line vector at the initial reference time. The navigation satellite's sight line vector rB could be expressed in terms of the satellite's body coordinates. However since the body attitude may be defined in terms of the quaternion, the transformation relating the estimate of current sight line vector 
in the inertial coordinates to the current sight line vector rB in body coordinates may be expressed in terms of the quaternion components. Hence:
An estimate of current sight line vector in the orbiting coordinates can generally be obtained by an independent Kalman filter or by employing an algorithm such as NORAD's SDP4, SDP8 or SGP4 methods (Hoots et al. Reference Hoots, Schumacher and Glover2004). It therefore follows that the difference in the measured phase differential could be expressed as:
and using the constraint on the components of the quaternion, q 12+q 22+q 32+q 42=1, we may write, Db,I(q)−I as:
![\eqalign{{\bf D}_{b\comma I} \left( {\bf q} \right) \minus {\bf I} \equiv \tab \rmDelta {\bf D}_{b\comma I} \left( {\bf q} \right) \equals \minus 2\left[ {\matrix{ {\left( {q_{\setnum{2}}^{\setnum{2}} \plus q_{\setnum{3}}^{\setnum{2}} } \right)} \tab { \minus \left( {q_{\setnum{1}} q_{\setnum{2}} \plus q_{\setnum{3}} q_{\setnum{4}} } \right)} \tab { \minus \left( {q_{\setnum{1}} q_{\setnum{3}} \minus q_{\setnum{2}} q_{\setnum{4}} } \right)} \cr { \minus \left( {q_{\setnum{1}} q_{\setnum{2}} \minus q_{\setnum{3}} q_{\setnum{4}} } \right)} \tab {\left( {q_{\setnum{1}}^{\setnum{2}} \plus q_{\setnum{3}}^{\setnum{2}} } \right)} \tab { \minus \left( {q_{\setnum{2}} q_{\setnum{3}} \plus q_{\setnum{1}} q_{\setnum{4}} } \right)} \cr { \minus \left( {q_{\setnum{1}} q_{\setnum{3}} \plus q_{\setnum{2}} q_{\setnum{4}} } \right)} \tab { \minus \left( {q_{\setnum{2}} q_{\setnum{3}} \minus q_{\setnum{1}} q_{\setnum{4}} } \right)} \tab {\left( {q_{\setnum{1}}^{\setnum{2}} \plus q_{\setnum{2}}^{\setnum{2}} } \right)} \cr} } \right]\comma}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn34.gif?pub-status=live)
which is a homogeneous quadratic function of the components of the quaternion. Thus a discrete measurement of the error in the difference of the phase differentials due to changes in the attitude can be expressed as:
where v φ is an additive Gaussian random variable representing a white noise or delta-correlated stochastic process. For three axis measurement of the attitude one would require three independent measurements which may be expressed as:
To complement these pseudo-range measurements and carrier phase we assume that we also have independent measurements of the altitude and east geodetic longitude. This is necessary as the altitude and longitude kinematics have been included in the process model. Measurements of the altitude may be obtained from a radar altimeter while there are a variety of ways to obtain the east geodetic longitude. Alternately the longitude kinematics may be deleted from the process model.
4. ADAPTIVE MIXING FILTER
Most dynamic models employed for purposes of estimation or filtering of pseudo range errors or orbit ephemeris errors are generally not linear. To extend and overcome the limitations of linear models, a number of approaches such as the EKF have been proposed in the literature for nonlinear estimation using a variety of approaches. Unlike the Kalman filter, the EKF may diverge, if the consecutive linearizations are not a good approximation of the linear model over the entire uncertainty domain. Yet the EKF provides a simple and practical approach to dealing with essential non-linear dynamics. The main difficulty in applying the algorithm to problems related to the estimation of a spacecraft's orbit and attitude is in determining the proper Jacobian matrices. The UKF is a feasible alternative that has been proposed to overcome this difficulty, by Julier, Uhlmann and Durrant-Whyte (Reference Julier, Uhlmann and Durrant-Whyte2000) and Julier and Uhlmann (Reference Julier and Uhlmann2000) as an effective way of applying the Kalman filter to nonlinear systems. It is based on the intuitive concept that it is easier to approximate a probability distribution than it is to approximate an arbitrary nonlinear function or transformation, of a random variable.
Consider a random variable w with dimension L which is going through the nonlinear transformation, y=f(w). The initial conditions are that w has a mean 
and a covariance Pww. To calculate the statistics of y, a matrix χ of 2L+1 sigma vectors is formed. We have chosen to use the scaled unscented transformation proposed by Julier (Reference Julier2002), as this transformation gives one the added flexibility of scaling the sigma points to ensure that the covariance matrices are always positive definite.
Given a general discrete nonlinear dynamic system in the form:
where xk∊R n is the state vector, uk∊R r is the known input vector, yk∊R m is the output vector at time k. wk and vk are, respectively, the disturbance or process noise and sensor noise vectors, which are assumed to be Gaussian white noise with zero mean. Furthermore Qk and Rk are assumed to be the covariance matrices of the process noise sequence, wk and the measurement noise sequence, vk respectively. The unscented transformations of the states are denoted as:
while the transformed covariance matrices and cross-covariance are respectively denoted as:
and
The UKF estimator can then be expressed in a compact form. The state time-update equation, the propagated covariance, the Kalman gain, the state estimate and the updated covariance are respectively given by:
![{\bf \hat{x}}_{k} \equals {\bf \hat{x}}_{k}^{ \minus } \plus {\bf K}_{k} \left[ {{\bf z}_{k} \minus {\bf h}_{k}^{UT} \left( {{\bf \hat{x}}_{k}^{ \minus } } \right)} \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041145822-0500:S0373463310000329_eqn40d.gif?pub-status=live){kind=small}
Equations 40 are in the same form as the traditional Kalman filter and the extended Kalman filter. Thus higher order non-linear models capturing significant aspects of the dynamics may be employed to ensure that the Kalman filter algorithm can be implemented to effectively estimate the states in practice.
In order to employ the UKF when precise statistics of the process and measurement noise vectors are not available, the adaptive filter method proposed by Song, Qi and Han, Reference Song, Qi and Han2006 is used to estimate the orbit parameters. The covariance matrices of measurement residuals are recursively updated in the UKF. The measurement noise covariance matrices, in the case of the UKF, may be expressed as:
where, Crk,N is defined in terms of the sample size N and the residual rk as:
Equation 41 involves the further computation of 
khh, by applying the unscented nonlinear transformation, hkUT(○k) to the state estimate, ○k. The measurement noise covariance may be updated in principle by employing equation 41. The nonlinear relationships between the covariance matrices also suggests that the update of Rk could be done by employing the covariance of the residual.
In the application considered in this paper, the adaptation of Qk is implemented as it is the process statistics that are often unknown or may be considered to vary. It was observed that the magnitudes of the filter gains were relatively small and for these reasons the exact expression for an estimate of Qk:
was approximated as:
where CΔxk, N is defined as:
and
5. GPS-INS INTEGRATION MIXING FILTER MECHANISATION
The process model for applying the adaptive UKF is given by equations 1, 20 and 22 while the ANED vector in equations 1 is given by equations 16–18, 23 and 2. In using equation 22 with the UKF it is important to ensure that the constraint that the quaternion must satisfy is met by the estimates. This is ensured by repeated application of the method proposed by Vepa (2009) where the quaternion normalisation is considered as a non-linear transformation and performed by applying the unscented transformation sequentially. The GPS measurement model is given by equations 28 while the carrier phase measurement model is given by equations 34 to 36. It must be recognised at the outset that the process error covariance is relatively quite low as both the accelerometers and rate gyros being used are high precision type sensors. The implication of the use of these accelerometers which are characterised by a relatively low standard deviation in the measured acceleration is that the pseudo-range measurement error correction, due to the availability of the additional accelerometer measurements, is expected to be relatively of the same order in comparison with the total user equivalent range error. The real issue is that the navigation mixing filter is not only able to deal with the uncertainties associated with the sensors but also able to estimate the user position within a desired level of accuracy. Bearing this in mind, the UKF is first implemented as a mixing filter to facilitate GPS-INS integration and these results are discussed in the first instance. Furthermore the estimates of the attitude quaternion are expected to be unique or consistent, due to the presence of the rate gyro measurements and the associated direction cosine matrix is expected to be uniquely estimated.
To test the filter performance, rather than subject it to realistic accelerations over an extended period of time, the system is subjected to intense accelerations and sustained rotations over a short time frame. The initial altitude of the vehicle was assumed to be 10 000 metres while the initial location was assumed to be above London Heathrow. In the first instance the north, east and down velocity equations and the angular velocity equations were each subjected to three independent slowly varying biases and the corresponding 31 states of the filter were estimated by applying the UKF algorithms. Only the non-adaptive (standard) UKF based user position error estimates relative to simulations are shown Figure 1. The time step for implementing the estimator was chosen as, Δt=0·0002 seconds. The comparison is made over a typical epoch of the first 4 seconds (=2×104 time steps) as the UKF filters converge to a steady state well before the end of this time frame. The number of visible satellites is assumed to be three. While the predictions of the error estimates are very good, one major difficulty of the non-adaptive approach is the need for the process disturbance covariance matrix, which may not always be available in practice. For this reason the adaptive UKF was also implemented.
Errors in the UKF estimated three-axis user position components over 20 000 time steps (equivalent to a time frame of 4 seconds).
In Figure 2a, just the adaptive UKF estimates of the latitude, longitude and attitude are compared with the corresponding simulations. The corresponding velocities in the north, east and down directions were also estimated. It should be noted that in the standard UKF and adaptive UKF comparisons, the simulated responses are slightly different due to differences in the disturbances. However they are of the same orders of magnitude thus facilitating the comparison of errors. It was observed that the errors in the horizontal velocity components (north and east) are relatively high. This is due to the fact that there is no information in the measurements that will help separate the components of velocity in the horizontal plane. For this reason the case with additional three axis Doppler measurements are considered later in the paper. Figure 2b compares the estimated and simulated components of the velocity in the horizontal and vertical planes which shows almost an insignificant error in these two components. Thus the difficulty is in resolving the velocity in the horizontal plane into its north and east components. The corresponding body attitude quaternion components were also computed but not shown here. The user position estimate error is similar to that shown in Figure 1.
Comparison of simulated and adaptive UKF estimated navigation positions (LLA, latitude (rad), longitude (rad) and attitude in metres) over 20 000 time steps (equivalent to a time frame of 4 seconds).
Comparison of simulated and adaptive UKF estimated navigation horizontal and vertical velocities in m/s over 20 000 time steps.
Although all the estimated errors are relatively very small as expected, only the estimated north and east velocity components differed slightly from the simulated components. To remedy the situation additional Doppler aided measurements of the velocities were assumed to be available. These additional measurements improved the performance of the filter.
The results of applying the adaptation of Qk exactly (equation 43a) on the velocity and quaternion components are shown in Figures 3a and 3b over an epoch of the first 4 seconds (=2×104 time steps). All the other estimate errors behave quite similarly to those shown in Figures 2a and 2b and are not shown. Moreover the user position estimate errors remain within the same bounds shown in Figure 1 and are not shown. It is observed that the accuracy of the estimate of components of the quaternion is maintained in spite of considerable variations in their magnitude.
Comparison of simulated and adaptive Doppler-aided UKF estimated navigation velocities in m/s (north east and down) over 20 000 time steps (equivalent to a time frame of 4 seconds).
Comparison of simulated and adaptive Doppler-aided UKF estimated body attitude quaternion components over 20 000 time steps (equivalent to a time frame of 4 seconds).
Generally it was observed that the most inaccurate component of the quaternion was in fact the one with the lowest magnitude. This error is due to the fact that both the simulated quaternion and the estimated quaternion are being forced to satisfy the normalisation constraint q 12+q 22+q 32+q 42=1 exactly. Consequently the errors in the major components of the quaternion cause a significant error in the component with the lowest magnitude. In practice it may be essential to trade-off the error in the component with the lowest magnitude by allowing a small normalisation error.
It is observed that the performance of the adaptive UKF based estimations improved when the additional Doppler measurements were made available to the filter, particularly in resolving the velocity in the horizontal plane to its north and east components. It must be noted that the tests that were carried out have been done with exceptionally high translational and angular velocities. In reality a satellite aided inertial navigation system would be subjected to such extremes only on certain rare but critical occasions.
Finally to demonstrate the efficacy of the adaptive UKF estimator over a relatively long period of the time, the filter is run over a 30 seconds time frame and the velocity estimates obtained over the last four seconds compared with the simulations in Figure 4. The navigation position, (latitude, longitude and altitude) and the estimated user position error are not shown as these are similar in magnitude to those shown in Figures (2a) and (1) respectively. The behaviour of the body quaternion component errors is similar to that shown in Figure 3b.
Comparison of simulated and adaptive UKF estimated navigation horizontal and vertical velocities in m/s over the last 20 000 time steps (equivalent to the last 4 seconds in a 30 seconds time frame).
It is particularly interesting to observe that the adaptive UKF estimator is able to predict the north, east and down velocity components without any assistance from the Doppler measurements. Moreover it is able to predict the body attitude accurately over the entire time frame, even though the aircraft seems to have been reduced to a state of sustained rotations at the end of the 30 second time frame and the body attitude components are therefore continuously changing.
6. CONCLUSIONS
In this paper the authors have demonstrated the feasibility of implementing an adaptive unscented Kalman filter based mixing filter which may be used to develop a high accuracy satellite aided inertial navigation system. While the estimates of the pseudo-range by using a standard UKF were of acceptable accuracy, it was also found the estimates based on the adaptive UKF algorithm provided extremely accurate estimates of the positioning variables and of the body quaternion components. Moreover, when additional Doppler aided measurements of the velocity components were available the estimated velocities were seen to converge to the simulated values quite rapidly. The methodology may be developed as a stand-alone system or employed in conjunction with a traditional strapped down inertial navigation system for purposes of initial alignment. Moreover the feasibility of employing adaptive mixing facilitates the possibility of using the system in an interoperable fashion with satellite navigation measurements.
