Improvement of Strapdown Inertial Navigation Using PDAF - IEEE Xplore

I. INTRODUCTION

Improvement of Strapdown Inertial Navigation Using PDAF

JEAN DEZERT, Member, IEEE ONERA France

A new application of PDAF (probabilistic data association filter) for improving the accuracy of autonomous strapdown inertial navigation systems (SINS) is presented. The proposed method is a terrain-aided navigation (TAN) algorithm based on landmark detection combined with a classical SINS. It is shown via a set of simulations that the method can improve significantly the precision of autonomous navigation if the landmark spatial density and quality of landmark detectors are good enough. This new concept of navigation called PDANF (probabilistic data association navigation filter) can be integrated with a relatively low cost in existing operational TAN systems.

Manuscript received August 10, 1996; revised December 23, 1998. IEEE Log No. T-AES/35/3/06400. Author’s address: ONERA, 29 Avenue de la Division Leclerc, 92320 Chaˆ tillon, France, E-mail: ([email protected]). c 1999 IEEE 0018-9251/99/$10.00 °

Today, navigation systems for UAV (unmanned aircraft vehicle) can be broadly separated into two distinct approaches: dead reckoning and reference. Dead reckoning refers to navigation based on odometry, inertial guidance or any other self-contained sensing. We are concerned here with inertial navigation systems (INS). Such navigation systems which are self-contained, nonradiating, and nonjammable present the major drawback that position error grows without bound as a function of time [32] unless an independant reference is used periodically to reduce navigation error. Almost all INS for UAV are usually aided by some other techniques based either on radio navigation or on terrain-aided navigation (TAN) [1, 16—19, 39] to achieve precise navigation. We propose a new approach for improvement of INS based on landmark detection combined with probabilistic data association filter (PDAF). Reference guidance approaches as radio navigation or radio positions (Satellite/Star) [9, 15, 22, 27, 29] aids present the main advantage that position errors are bounded. Clearly, dead reckoning and reference navigation (DRR) are full complementary and combination of the two approaches can provide very accurate positioning systems. Recently, GPS and INS have been combined in systems that estimate INS sensor errors as misalignment and drift rates, to lead to the slowest possible growth of INS navigation error in between GPS updates [35, 36]. But most of existing integrated hybrid radio-INS depend on “the outer world” and therefore are no longer self-contained, nonradiating, and nonjammable. Actually only TAN systems (TANS) allow fully autonomous navigation. But contrary to previous assertion [4], not only TANS based on topographical mapping (TM) allow autonomous navigation and we present in this paper an alternative issue for TANS. Classical TM-TANS, like TERCOM (Terrain Contour Matching) [1] or SITAN (Sandia inertial TAN) [16] for instance, work well whenever the topographical information of the terrain is consistent enough. But for some specific military applications this condition obviously is not fulfilled and TM-TANS do not work at all because of unresolvable ambiguities in the correlation function. A major drawback of classical TM-TANS is their complexity for implementation and mission planning. All TM-TANS require a high memory storage capacity with a very high speed access (to store/retrieve digitized map strips/altitude profiles corresponding to the region where the UAV is supposed to fly over). Moreover UAV becomes lost forever as soon as its real flight path goes out of the map strip defined at the time of the mission planning. In previous works [10, 11] only a theoretical TANS issue, called PDANF (probabilistic data association navigation filter), has been proposed to

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 35, NO. 3

JULY 1999

835

overcome the major drawbacks of TM-TANS. In this work, we show for a realistic navigation scenario and realistic simulations how such a kind of technique can improve inertial navigation. The basic idea of this approach is to store into an on-board memory the list of landmarks (characterized by their coordinates and attribute) which might be flown over and use the range and bearing measurements concerning detected landmarks in the field of view (FOV) of auxiliary on-board sensors (like infrared sensor, radar, laser: : : ). The data association problem between detected landmarks and the referenced ones is then solved by PDANF technique. The measurement processing is not based on common forms of triangulation/trilateration-based location systems [2, 7, 20, 21, 33, 38] but only on the probabilistic data association method commonly used in radar multitarget tracking (MTT) applications [5, 6]. The major contribution of this work is to show precisely how PDANF can be used to estimate strapdown inertial navigation system (SINS) errors and to prove that this new approach can efficiently improve the performance of the strapdown inertial navigation for an UAV flying at very low altitude above areas where classical navigation techniques usually fail. The PDANF is a promising and attractive issue for improving existing TANS and DRR-based navigation systems. II.

STRAPDOWN INS ERROR MODELIZATION

In this work, evolution of inertial errors taking into account Schuler phenomenon of a SINS have been modeled as the following continuous dynamic system [30] _ ±p(t) = ±v(t) ¡ ½(t) £ ±p(t)

is the rotational angular velocity of triad T with respect to the Earth; Lat(t) being the geodetic latitude (positive North) of UAV at time t; ¢ −T (t) =[−T cos Lat(t) 0 ¡ −T sin Lat(t)]0 is the rotational velocity of the Earth at the latitude of the UAV; −T is the terrestrial rotation velocity (−T = ¢ 2¼=86400 rad/s) and B(t) = B[atrue (t)] is the classical (true) attitude matrix given in Appendix B. This (noise-free) model must be completed by adding a process noise representative of the error in our modelization of the physical SINS. The previous set of differential equations can be expressed as (6)

¢

(1)

¡ [½(t) + 2−T (t)] £ ±v(t) ¡ ±(½(t) + 2−T (t)) £ v(t)

(2)

¡ ±(½(t) + −T (t))

(3)

_ ±a(t) = B(t) ±!(t) + ±a(t)[½(t) + −T (t)]

836

g is the gravitational constant (g = 9:80665 m/s2 ) and re is the ellipsoidal equatorial radius of the Earth (re = 6378137 m for WGS-84 [23]); · ¸0 v (t) v (t) ¢ v (t) ¡ n ¡ e tan Lat(t) ½(t) = e re re re

_ ˜ ±x(t) = A(t)±x(t) + w(t)

_ ±v(t) = B(t) ±°(t) + ±a(t) £ B(t)°(t) + ±g(t)

_ ±°(t) =0

(4)

_ ±!(t) =0

(5)

where the £ symbol denotes here the vector cross ¢ product; ±p(t) =[±xn (t) ±xe (t) ±xd (t)]0 = p(t) ¡ ptrue (t) is the inertial position error between current (approximated) position p(t) returned by the inertial measurement unit (IMU) and the true unknown position ptrue (t) of the UAV at time t. Components of ±p(t) are expressed in a geographical local triad T defined by north, east, and down (ned) axis and having IMU position M for origin. In T triad, one

always has by definition p ´ 0. The inertial velocity ¢ error expressed in T is ±v(t) =[±vn (t) ±ve (t) ±vd (t)]0 = ¢ v(t) ¡ vtrue (t). ±a(t) =[±'(t) ±µ(t) ±Ã(t)]0 = a(t) ¡ atrue (t) is the inertial attitude (roll, pitch, yaw) error expressed ¢ in T ; ±°(t) =[±°x (t) ±°y (t) ±°z (t)]0 is the accelerometric ¢ error expressed in the body frame M =(M, x, y, z) of ¢ UAV; ±!(t) =[±!x (t) ±!y (t) ±!z (t)]0 is the gyrometric error expressed in the body frame M. The error on gravitational acceleration expressed in T induced by position error ±p(t) is given by · ¸0 g g g ¢ ±g(t) = ¡ ±xn (t) ¡ ±xe (t) 2 ±xd (t) re re re

where ±x(t) =[±p(t) ±v(t) ±a(t) ±°(t) ±!(t)]0 is the 15-dimensional state vector of SINS errors to be estimated; A(t) is the continuous transition matrix ˜ of the system given in Appendix A; w(t) is the zero-mean continuous Gaussian process noise vector ˜ with covariance Q(t)±(¿ ) (±(¿ ) being the Dirac delta function). Since variation of velocity and latitude of the UAV is generally very slow during small sampling period, we can assume matrix A(t) constant ¢ during sampling period T = tk+1 ¡ tk . Hence, previous continuous plant equation can be easily discretized by classical method (cf. [5, pp. 52—55]) and can be expressed as ±x(k + 1) = F(k + 1, k)±x(k) + w(k):

(7)

III. OPTIMAL NAVIGATION FILTER To improve inertial navigation, we propose here to use two kinds of additional informations. 1) The first (prior) information is a navigation map stored aboard the UAV Computation Unit (UCU)

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999

which consists of a simple register of referenced landmarks Li that might be flown over. Each landmark Li is referenced both by its position vector Li (latitude, longitude and altitude) in WGS-84 and its identity (crossroad, bridges, : : : ) Ii . Landmarks must be chosen to have a high probability of detection and identification. In practice they will be obtained for example from satellite observations and terrain analysis during the mission planning. 2) We assume moreover there exists aboard auxiliary sensors (typically RADAR and/or infrared sensors) which generates for each landmark detected in their FOV a range and bearing (elevation ¢ and azimuth) measurement vector denoted z(k) = 0 [r(k) e(k) a(k)] . These measurements are always provided with respect to the UAV body frame M. The orientation of the body frame M of UAV is itself characterized by its attitude vector (roll, pitch, yaw) ¢ a(k) =['(k) µ(k) Ã(k)]0 . A. Pseudomeasurement Equation for Navigation System Some attention must be paid to obtain measurement equation for the navigation system (7). Here we use a pseudomeasurement consisting in position y(k) of the detected landmark expressed in T frame (centered at the position given by IMU). Under perfect data association hypothesis, it can be shown in Appendix B that y(k) can be expressed as w

y(k) ´ L(k) = g(L , M)

(8)

where Lw is the absolute position of the landmark in WGS-84 (once data association has been solved). The w subscript will always refer to WGS-84 in ¢ the sequel. M =[Lat Long Alt]0 is the erroneous absolute position of the UAV given by its IMU. In Appendix B, we show that landmark position y(k) is also related with SINS errors ±x(k) through the following pseudomeasurement equation y(k) = g(Lw , M) = h[±x(k), a(k), M(k), ztrue (k)] (9) where h[¢] is a nonlinear observation function detailed in Appendices B and C; ztrue (k) is the true range-bearing measurement from landmark with respect auxiliary sensor aboard; a(k) is the attitude of UAV given by its IMU; M is the absolute (but erroneous) position of UAV given by its IMU, and ±x(k) is the SINS error vector to estimate. In practice, true range-bearing measurement ztrue (k) is not known but we can assume that ztrue (k) is actually corrupted by an additional zero-mean white Gaussian noise nz (k) with known covariance Rz (k), i.e., z(k) = ztrue (k) + nz (k):

(10)

Under such condition, one gets the following noisy (pseudo) measurement equation y(k) ¼ h[±x(k), a(k), M(k), z(k)] + vy (k) with

¢

vy (k) = ¡Jz [h(k)]nz (k)

(11) (12)

and vy (k) is a zero-mean white Gaussian pseudomeasurement noise vector with covariance Ry (k) = Jz [h(k)]Rz (k)J0z [h(k)]

(13)

¢

where Jz (k) = Jz [h(k)] is the Jacobian matrix of h with respect to ztrue (k). Jz (k) is given in Appendix C. Under small range and bearing noises assumption, we use z(k) instead of ztrue (k) to estimate Jz [h(k)] since ztrue (k) is unknown. In the previous measurement equation, one has implicitly assumed that location Lw (k) of the detected landmark was known perfectly. In practice however, this strong assumption does not hold since the quality of navigation map highly depends on the quality of satellite observations and image analysis/pattern recognition algorithms used for the mission planning. Actually, only an approximated location of each referenced landmark (useful for navigation) is stored on the navigation map. Precision on referenced landmark locations is characterized by a given covariance matrix RL . From this remark and for taking into account navigation map quality itself in (11), vy (k) must be replaced now by ¢

vy (k) = ¡Jz [h(k)]nz (k) + JL [g(k)]nL (k)

(14)

where nL (k) is a zero-mean white Gaussian noise having a known covariance matrix RL (k) and JL [g(k)] is the Jacobian matrix of the WGS to T mapping function g given in Appendix C. Covariance of vy (k) is given by Ry (k) = Jz (k)Rz (k)J0z (k) + JL [g(k)]RL (k)J0L [g(k)]: (15) B. Optimal Navigation Filter Structure From plant equation (7) and measurement equation (11) we can now formulate the optimal navigation filter based on landmark detection and perfect data association. Its structure is very close to the classical extended Kalman filter (EKF) structure [5] and consists of the following five steps. 1) Available Information at Time k. From navigation filter at previous prediction step, ˆ one has predicted inertial errors ±x(k j k ¡ 1) = ˆ ˆ ˆ ˆ [±p(k j k ¡ 1) ±v(k j k ¡ 1) ±a(k j k ¡ 1) ±°(k j k ¡ 1) ˆ ¢ ±!(k j k ¡ 1)]0 and the predicted covariance matrix P(k j k ¡ 1). From IMU at given time k, one has the inertial state of UAV, i.e., approximated absolute position

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

837

Fig. 1. TAN filter scheme.

M(k), velocity v(k), attitude a(k), and accelerometric and gyrometric biases °(k) and !(k) (°(k) and !(k) being expressed in body frame M). From auxiliary sensors and if we assume there is a landmark detection at time k, one gets range-bearing measurement z(k) and Rz . From the stored map (assuming perfect landmark association), we get Lw (k) and RL (k). 2) Pseudomeasurement Prediction and Construction. Pseudomeasurement prediction is obtained by ˆ yˆ (k j k ¡ 1) = h[±x(k j k ¡ 1), a(k), M(k), z(k)]: (16) Under perfect data association hypothesis, the pseudomeasurement y(k) is given by (8). 3) Intermediate Computations. y˜ (k) = y(k) ¡ yˆ (k j k ¡ 1)

(17)

(18) (19)

H(k) = [r±x h0 [±x(k), a(k), M(k), z(k)]]0±x=±x(k ˆ j k¡1)

(20)

Ry (k) = Jz (k)Rz (k)J0z (k) + JL [g(k)]RL (k)J0L [g(k)] (21) S(k) = H(k)P(k j k ¡ 1)H0 (k) + Ry (k)

(22)

K(k) = P(k j k ¡ 1)H0 (k)S(k)¡1

(23)

Jz [h(k)], JL [g(k)] and H(k) are presented in Appendix C. 4) Update Equations. ˆ ˆ ±x(k j k) = ±x(k j k ¡ 1) + K(k)y˜ (k) P(k j k) = [I ¡ K(k)H(k)]P(k j k ¡ 1): 838

ˆ ˆ ±x(k + 1 j k) = F(k + 1, k)±x(k j k)

(26) 0

P(k + 1 j k) = F(k + 1, k)P(k j k)F (k + 1, k) + Q(k) (27) where P(k j k) is the covariance matrix of ±x(k) ¡ ˆ ±x(k j k). S(k) is the covariance of pseudoinnovation y˜ (k). K(k) is the Kalman gain of the navigation filter. Initialization is done by choosing P(0 j ¡1) and ˆ ±x(0 j ¡1) ´ 0 (no alignment errors). In case of no landmark detection at time k, we will take ˆ ˆ ±x(k j k) = ±x(k j k ¡ 1) (28) P(k j k) = P(k j k ¡ 1):

ˆ Jz [h(k)] = [rz h0 [±x(k j k ¡ 1), a(k), M(k), ztrue (k)]]0z=z(k) JL [g(k)] = [rL g0 (k)]0L=Lw (k)

5) Prediction Equations.

(29)

At every sampling time, global state estimate of UAV will be obtained from x(k) returned by IMU and ˆ current estimate of SINS errors ±x(k j k), i.e., ¢ ˆ j k)vˆ (k j k)aˆ (k j k)°(k ˆ j k)!(k ˆ j k)]0 xˆ (k j k) =[p(k

ˆ = x(k) ¡ ±x(k j k)

(30)

ˆ w (k j k) of absolute EarthThe optimal estimate M referenced position of UAV in WGS-84 will be given by ˆ w (k j k) = g¡1 (p(k ˆ j k), M) M (31) ˆ j k) being the best estimated position of the UAV p(k expressed in the local geographical (ned) triad T centered on the erroneous location M returned by the IMU. The principle of optimal navigation filter based on landmark detection comes down to the general scheme presented in Fig. 1. IV. NAVIGATION FILTER BASED ON PDAF

(24) (25)

Previous optimal navigation filter cannot be used for real applications since usually the strong

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999

assumption of perfect data association does not hold. Even if there is a good recognition rate on detected landmarks there still can exist association uncertainties. Specially if some landmarks of the same identity are close together from the position where the sensors are looking at. So we must face to the problem of taking into account such origin uncertainties in our previous optimal navigation filter. Moreover in the data processing itself we also have to take into account detections in the FOV corresponding to true non referenced landmarks and/or false alarms. An attractive solution of this problem has already be given in [10] for a simplier model of UAV evolution (2D-space) and measurement equation for which no INS was used. We present here a new solution for TANS based on detection of landmarks for more interesting applications. The main idea is to use a probabilistic data association (PDA) of all measurements (stored referenced landmarks) which might correspond to the detections in the FOV of sensors. In the sequel, we assume there is at most one detection in FOV at a time. If more than one detection is present, we can always select one of them for processing with some criteria (SNR, recognition rate,: : : ). Extension to multidetections case can be done using joint PDA formalism as already described in [11]. The main steps of the PDANF are presented below.

1) Available Information at Time k. Same information as for optimal navigation filter. 2) Pseudomeasurement Prediction. yˆ (k j k ¡ 1) is obtained by (16). 3) Intermediate Computations. H(k), Ry (k), S(k), and K(k) are obtained by (20)—(23). 4) Pseudomeasurement Generation and Gating. Select mk referenced landmarks fLwi g from the navigation map stored in the UCU memory such that pseudomeasurements fyi (k) = g(Lwi (k), M(k)g satisfy validation criterion

b(k) Pmk j=1 ®j (k)qj (k)

where qi (k) are identification likelihood terms which take into account recognition decision on detected landmark and also the quality of recognition processing itself. This is discussed in the next section. Pg is the prior probability that the true measurement will fall in the validation gate [5]. Pd is the (averaged) detection probability of referenced landmarks (see discussion in Section IVC). 6) Compute Weighted Pseudoinnovation. y˜ (k) =

mk X

¯i (k)y˜ i (k):

(37)

i=1

7) Update Equations. ˆ ˆ ±x(k j k) = ±x(k j k ¡ 1) + K(k)y˜ (k)

(38)

˜ P(k j k) = ¯0 (k)P(k j k ¡ 1) + (1 ¡ ¯0 (k))Pc (k j k) + P(k) (39) with Pc (k j k) = [I ¡ K(k)H(k)]P(k j k ¡ 1) (40) "m # k X ˜ P(k) = K(k) ¯ (k)y˜ (k)y˜ 0 (k) ¡ y˜ (k)y˜ 0 (k) K0 (k): i

i

(41) ˆ 8) Prediction Equations. ±x(k + 1 j k) and P(k + 1 j k) are given by (26) and (27). Initialization is done as previously by choosing ˆ P(0 j ¡1) and ±x(0 j ¡1) ´ 0. In case of no landmark detection at time k we will use (28) and (29). At every sampling time, state estimate of UAV will be obtained by (30). The optimal estimate of absolute ˆ w (k j k) will be Earth-referenced position of UAV M still obtained by (31). B. Computation of Coefficients qj (k)

(33)

(34)

where N + 1 is the number of mutually exclusive and exhaustive identification hypotheses (class I0 being the complementary class [10]), S is the number of sensors aboard, p[ml (k) j TR = Ii ] is the measurement probability density function from lth

®i (k)qi (k) P k ¯i (k) = b(k) + m j=1 ®j (k)qj (k) i = 1, : : : , mk

(36)

We recall [10] that every identification likelihood coefficient qj (k) lies always in [0, 1] interval and is theoretically given by PN QS i=0 PfT(Lj ) = Ii gPR fIi g l=1 p[ml (k) j TR = Ii ] qj (k) = PN QS i=0 PR fIi g l=1 p[ml (k) j TR = Ii ] (42)

(32)

¢ where y˜ i (k) = yi (k) ¡ yˆ (k j k ¡ 1) and °v is a given validation threshold [5]. 5) Compute Association Probabilities [10].

b(k) +

(35)

i=1

From plant equation (7), measurement equation (11) and using the PDAF formalism [5], the navigation filter consists of the following eight steps.

¯0 (k) =

®i (k) = exp[¡y˜ 0i (k)S¡1 (k)y˜ i (k)=2] µ ¶3=2 1 ¡ Pd Pg 3 2¼ b(k) = mk 4¼ °v Pd

i

A. Main Steps Involved in PDANF

y˜ 0i (k)S¡1 (k)y˜ i (k) · °v

with

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

839

sensor concerning identity Ii ; PR fIi g the a priori probability of observing a landmark having identity Ii . This probability must be evaluated from the known navigation map information (e.g., using the percentage of landmarks present on the navigation map with identity Ii ). PfT(Lj ) = Ii g is the a priori probability (assumed to be known) that the identity of a referenced landmark Lj should be Ii . Coefficient qj (k) is actually an agreement factor between the identity of the detected landmark and the identity of a referenced landmark Lj . If we know the identity of the detected landmark with certainty, and if all landmarks referenced on the navigation map have this same identity, then the recognition information will become useless to improve the estimation of SINS drifts. The noninformativness of the recognition information can be written mathematically as qj (k) ´ 1 for j = 1, : : : , mk . Under these singular conditions computations of posterior association probabilities ¯j (k) become exactly the same as for the standard PDAF. In this section we assume navigation map and recognition information to be Bayesian (i.e., we have access to p[ml (k) j TR = Ii ] and PfT(Lj ) = Ii g). In practice however, these probabilities are very difficult to obtain with good precision. A more realistic description of available recognition information can also be done using the theory of evidence [37]. Computation of qj (k) and ¯j (k) are then still possible but becomes a little bit more complex. Details of this approach can be found in [10]. From a practical point of view, because the use of direct recognition informations requires a large data transfer, a decision-maker (i.e., classifier) is most of the time a prerequesite for data processing. The available recognition information then consists of: 1) a global decision d(k) quantifying the best presumed identity of the detected landmark from all recognition informations given by sensors, 2) a given (N + 1) £ (N + 1) global quality ¢ matrix C =[Cmi ] = [P(d(k) = Im j TR = Ii )] of the decision-maker (classifier) which can be obtained from theoretical computations, simulations, or experimental trials. Diagonal terms of C represent the probabilities of correct decision (good recognition rate). Arithmetic mean of diagonal terms of C is called the mean recognition rate (MMR) of the classifier. With such information, coefficients qj (k) are now given by [10] PN i=0 PfT(Lj ) = Ii gPR fIi gPfd(k) j Ii g qj (k) = PN i=0 PR fIi gPfd(k) j Ii g (43) where Pfd(k) j Ii g is given by the classifier quality matrix C. For example if one has d(k) = Im (m = 0, : : : , N) then Pfd(k) j Ii g = Cmi . 840

Probabilities PfT(Lj ) = Ii g and Pfd(k) j Ii g are usually well known. Only prior probabilities PR fIi g needs more attention to be obtained. Essentially two solutions can be used to estimate these probabilities. 1) Optimal (but unimplementable) Solution. Suppose all spatial densities of detectable landmarks to be known. Let Ireal be the set of all identities of all physical detectable landmark of the real map overflown by UAV. Let Iˆ be the set of all identities of all referenced landmark of the stored navigation map on board. The cardinality of identification classes relative to Ireal and Iˆ are ˆ We assume denoted respectively by jIreal j and jIj. ˆ for simplicity jIj · jIreal j. Let ¸i be the real spatial density of landmark of type Ii and ¸fa be the false alarm spatial density. A false alarm is a detection corresponding to no physical landmark (ghost). Under such hypotheses and notations PR fIi g can be directly approximated from conditional spatial densities ¸i by PR fIi g =

¸fa +

¸ Pi

Ii 2Ireal ¸i

and PR fI0 g = 1 ¡

X Ii 2Iˆ

8 Ii 2 Iˆ

PR fIi g:

(44)

(45)

2) Basic Heuristic (implementable) Solution. A basic (crude) heuristic is to assume equiprobability among all N + 1 classes of referenced landmarks. Such assumption leads us to take PR fI0 g ´ PR fIi g = 1=(N + 1)

ˆ 8 Ii 2 I:

(46)

This approximation of course does not give a very good estimate of PR fIi g. Simulations have however shown that algorithm still performs well even with such crude heuristic if informations are sufficient. More sophisticated heuristics however, based on local conditional spatial density of referenced landmarks for example, can be chosen to improve estimation of PR fIi g. C. Evaluation of Landmark Detection Probability Pd Pd represents the prior probability to detect a referenced landmark. This probability is generally unknown but can be evaluated from 1) the conditional probabilities of detection Pdi (z(k)) (i = 1, : : : , N) of every type Ii of referenced landmark, 2) the prior probabilities PR fIi g, 3) the recognition decision d(k) and classifier quality matrix C as follows PN i i=0 Cd(k)i PR fIi gPd (z(k)) Pd ¼ : (47) PN i=0 Cd(k)i PR fIi g

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999

Fig. 2. Real and stand-alone inertial groundtrack.

Fig. 3. Altitude profiles.

If recognition information is not available, we will use instead PN PR fIi gPdi (z(k)) Pd ¼ i=0 (48) PN i=0 PR fIi g where conditional probabilities Pdi (z(k)) (i = 1, : : : , N) are obtained from some conditional detection models and Pd0 (z(k)) is estimated by the crude heuristic 2 3,2 3 X X ¸i Pdi (z(k))5 4 ¸i 5 : Pd0 ¼ 4 i2Iˆ

i2Iˆ

(49)

D. Practical Implementation of Landmark Gating Landmark gating via (32) requires the knowledge of S(k) and therefore Ry (k). Computation of Ry (k) based on (21) is correct if and only if one knows with certainty the identity I of detected landmark (i.e., Rz (k) ´ Rz (k j I)). In practical applications we do not know with certainty identity of detected landmark. In such case, (21) is replaced by PN i P (z(k))P fI gJ (k)Rz (k j Ii )J0z (k) Ry (k) = i=0 d PN R i i z i=0 Pd (z(k))PR fIi g + JL [g(k)]RL (k)J0L [g(k)]:

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

(50) 841

TABLE I Real-World Parameters for Simulation

Landmark Type Ii

Range-Bearing Noise (¾r m, ¾e deg, ¾a deg)

Detection Bounds [Pdi (FOVmin ), Pdi (FOVmax )]

Spatial Density ¸i landmark/ 100 km2

I1 I2 I3 I4 I5 I6 I7 I8 I9 I10 I11

(20, 2, 2) (20, 2, 2) (30, 2, 2:5) (20, 2, 2) (30, 2, 2:5) (40, 3, 2:5) (10, 1, 1) (20, 2, 2) (30, 2, 2:5) (30, 2, 2:5) (10, 1, 1)

[0:7, 0:9] [0:7, 0:8] [0:6, 0:9] [0:5, 0:7] [0:6, 0:9] [0:6, 0:9] [0:5, 0:8] [0:5, 0:7] [0:7, 0:9] [0:7, 0:8] [0:6, 0:8]

15 10 10 8 7 11 10 9 8 7 5

V.

SIMULATION RESULTS

The simulation program requires that four pieces of information be supplied: an UAV flight path, a real-world model characterizing both navigation environment and on-board sensors, filter tuning parameters, and scenarios. Each of these items is

discussed briefly in the following paragraphs before showing simulations results. Flight Path: A typical nap-of-the-Earth flight path was used to evaluate filter performance. The real and open-loop inertial (i.e., without filter updates) trajectory groundtrack are plotted in Fig. 2. The length of the flight path is about 110 km and flight time is 500 s. For this trajectory, a nominal altitude of 200 m (above ground level) has been used. The real and open-loop inertial altitude profile of UAV during its flight is given on Fig. 3. The 0 m level corresponds to sea level and the mean terrain height is around 110 m. From real altitude profile we directly see the flatness of the overflown terrain. Open-loop inertial trajectories have been obtained using simulation of a common strapdown inertial measurement unit having the following features (one sigma): 0.001 g for accelerometric drift and 5 deg/h for gyrometric drift. The alignment errors at initial time with respect to the true absolute position were chosen to 150 m for North and East position, 20 m for altitude, 3.5 m/s for North and East velocity, 2.5 m/s for vertical velocity, 3 mrad for roll and pitch, and 6 mrad for yaw. These two figures clearly show the poor quality of navigation based on such stand-alone SINS. Only improvment of altitude estimation could be achieved by adding

Fig. 4. Estimation errors on position drifts with cfa = 0:1 for LMQ case (one run).

Fig. 5. Estimation errors on position drifts with cfa = 0:1 for HMQ case (one run). 842

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999

Fig. 6. Estimation errors on velocity drifts with cfa = 0:1 for LMQ case (one run).

Fig. 7. Estimation errors on velocity drifts with cfa = 0:1 for HMQ case (1 run).

Fig. 8. Estimation of position drifts with cfa = 0:1 for LMQ case (1 run).

a complementary radioaltimeter in the system. In our simulations however, we do not use altimeter measurements at all to volontary show that nominal altitude can be recovered with the new navigation PDA filter. Obviously, navigation performance would be improved by taking into account more external sensors like (radio or baro) altimeter and/or GPS receiver in navigation system. Real-World Environment: The real-world environment is used in the filter evaluation program

to simulate the behavior of the navigation system during flight. Features of the FOV of UAV are 600 m for the minimal vision distance dmin , 3 km for the maximal vision distance dmax , and 60 deg for vertical and horizontal apertures, and 30 deg for vertical steering angle. The minimal and maximal MRR for global quality matrix C have been set to 0.6 and 0.9. In simulation, an arbitrary physical map was generated randomly using 11 kinds of different landmarks. Fictive physical landmarks were uniformly

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

843

Fig. 9. Estimation of position drifts with cfa = 0:1 for HMQ case (1 run).

Fig. 10. Estimation of velocity drifts with cfa = 0:1 for LMQ case (1 run).

Fig. 11. Real and estimated altitude profiles with cfa = 0:1 for LMQ case (1 run).

and spatially distributed on the ground with the spatial densities given in Table I. Altitude of each landmark is uniformly distributed on [100 m, 130 m]. The total spatial density ¸T of physical landmarks present on the ground is therefore equal to 100 landmarks per 100 km2 . To simulate false (ghost) landmark detections in FOV, we generate, at each observation time, a map of random points having the following spatial density ¸fa = cfa ¸T where cfa is a simulation parameter called false alarm rate. In simulations, a 844

false alarm in FOV is always prefered to a detected physical landmark if it is at the closest distance of UAV’s head. Conditional (i.e., for a given landmark identity Ii ) range and bearing measurement noises and probability of detection of each physical landmark are also given in Table I. Maximal (minimal) probability of detection will be reached whenever a physical landmark is at the minimal (maximal) acceptable distance (in the FOV) of UAV. In our simulations, we used the following

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999

Fig. 12. Real and estimated altitude profiles with cfa = 0:1 for LMQ case (1 run).

Fig. 13. Real and estimated altitude profiles with cfa = 0:1 for HMQ case (1 run). TABLE II Percentage of Detections Kind of Detections GL detections RL detections UL detections

2

are coefficients given by

HMQ Case

LMQ Case

9% 46% 45%

9% 23% 68%

simple model Pdi (z) = eai r +bi for computation of Pdi (z). r is the distance (first component of z) between UAV and the detected landmark having identity Ii . ai and bi

2 2 ai = [ln Pdi (FOVmin ) ¡ ln Pdi (FOVmax )]=[dmin ¡ dmax ] (51) 2 bi = ln Pdi (FOVmin ) ¡ ai dmin :

(52)

Navigation Filter Parameters: The sampling time used in navigation filter is T = 0:2 s. Minimal period during two consecutive landmark detections (minimal detection period) is set to 1 s. Gating probability Pg is set to 0.999 (°v = 16:266). Crude heuristic for PR fIi g

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

845

Fig. 14. Estimation errors on position drifts with cfa = 0:6 for LMQ case (1 run).

Fig. 15. Estimation errors on position drifts with cfa = 0:6 for HMQ case (1 run).

Fig. 16. Estimation errors on velocity drifts with cfa = 0:6 for LMQ case (1 run).

estimation is chosen in our filter implementation. ˆ ±x(0 j ¡1) is set to null vector and initial standard deviation for inertial drifts (square-root of diagonal terms of P(0 j ¡1)) are set to 200 m for North and East position, 20 m for altitude, 5 m/s for North, East, and vertical velocity, 10 mrad for roll and pitch and 20 mrad for yaw, 0.0010 g for accelerometric drift and 5 deg/h for gyrometric drift. All terms of process noise covariance 15 £ 15 matrix Q(k) are set to zero except diagonal terms corresponding 846

to the velocity and attitude biases which are set to 2 q44 = q55 q = q88 = q99 = q2att T with p = q66 = qv T and p 77 qv = 6= 3600 (m/s)= s and qatt =

¼ p 180 3600

p rad= s:

Uncertainty on referenced landmark position (square root of diagonal terms of RL ) are set to 9 10¡5 deg on latitude, 1.4 10¡4 deg on longitude, and 3 m on altitude. These values correspond to a location

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999

Fig. 17. Estimation errors on velocity drifts with cfa = 0:6 for HMQ case (1 run).

Fig. 18. Estimation of position drifts with cfa = 0:6 for LMQ case (1 run).

Fig. 19. Estimation of position drifts with cfa = 0:6 for HMQ case (1 run).

precision of around 10 m on x and y (for the latitude of center of the navigation map). Scenarios for Navigation Filter Simulation: Two typical scenarios for navigation filter have been simulated using the following navigation map qualities. 1) Low map quality case (LMQ): In the first scenario, navigation filter uses a navigation map of low quality which is a register of only 2 classes

of physical landmarks (I1 and I2 ). The total spatial density corresponding to all referenced landmarks is 2 therefore ¸ref T (LMQ) = 25 landmarks per 100 km . 2) High map quality case (HMQ): In the second scenario, navigation filter uses a more informative navigation map which consists of the register of the physical landmarks corresponding to classes I1 to I5 . The total spatial density corresponding to all referenced landmarks is then ¸ref T (HMQ) = 50 landmarks per 100 km2 .

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

847

Fig. 20. Estimation of velocity drifts with cfa = 0:6 for LMQ case (1 run).

Fig. 21. Estimation of velocity drifts with cfa = 0:6 for HMQ case (1 run).

For each case, two arbitrary values of false alarm rate have been tested cfa = 0:1 (moderate false alarm rate) and cfa = 0:6 (high false alarm rate). Simulation Results: Figs. 4—7 show, for a typical run, results of estimation errors on position and velocity drifts of a SINS with the PDANF for LMQ and HMQ cases and for a moderate false alarm rate. Corresponding drift estimates are plotted on Figs. 8 to 11. Estimated altitude profile for LMQ and HMQ cases are plotted on Figs. 12 and 13. Estimated groundtracks are not plotted for space saving reasons because we do not see difference between real and estimated groundtracks on figures (at the map scale). For HMQ and LMQ cases we got almost one detection (either a referenced landmark (RL), an unreferenced landmark (UL) or a ghost landmark (GL)) at every look in the FOV of sensor. Table II gives the repartition of the three kinds of detections obtained for this simulation. As we could expect, one gets half of RL detections for LMQ case than for HMQ case. Therefore navigation filter in LMQ case will use only about half correct information (i.e., referenced landmark detection) than the one in HMQ case. Figs. 4 to 13 show that under moderate false alarm rate, autonomous navigation can be achieved with the PDANF for both LMQ and HMQ cases. 848

Figs. 14—23 show the results obtained for the same physical environment but with a higher false alarm rate. For HMQ and LMQ cases we have the repartition of detections as presented in Table III. From these results, we see that the PDANF still performs well in the HMQ case but fails in the LMQ case because there is too much unreferenced and ghost landmark detections and the navigation map is not very informative. In such case, most of the time, validation gate does not contain referenced landmarks at all . This proves that the navigation performances depend highly on the map quality and the environmental conditions as we could legitimatly expect. VI.

CONCLUSION

A new application of the PDA method has been given to improve the navigation of an UAV flying at low altitude using a SINS. This new TANS can become an interesting alternative to traditional methods usually based on topographical correlation techniques. From a finite number of sensors (typically radar and/or infrared sensors) and a navigation map stored aboard the UCU, precise estimate of SINS errors and improvement of quality of autonomous

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999

Fig. 22. Real and estimated altitude profiles with cfa = 0:6 for LMQ case (1 run).

Fig. 23. Real and estimated altitude profiles with cfa = 0:6 for HMQ case (1 run).

TABLE III Percentage of Detections Kind of Detections GL detections RL detections UL detections

HMQ Case

LMQ Case

37% 30% 33%

37% 14% 49%

navigation have been presented through simulation results for a realistic SINS error model. The main advantage of this algorithm lies mainly in its low

cost computational burden, its relative simplicity for operational implementation (both for mission planning and real-time running) and its potential ability to be included in most existing TANS. This new TAN algorithm takes into account both alignment errors, recognition and range-bearing errors on detected landmarks and also identification and location precision on referenced landmarks stored on navigation maps. Integration of this new kind of TANS within existing systems and its performances evaluation via Monte-Carlo

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

849

APPENDIX A. TRANSITION MATRIX FOR SINS ERRORS

be considered as positive from the Earth’s center toward the sky. The latitude, longitude, and altitude of the center of mass M of an UAV referenced in the World Geodetic System (WGS-84) is denoted as

The continuous 15 £ 15 transition matrix A(t) which describes the evolution of SINS errors in (6) can be expressed in the following partionned form of 3 £ 3 elementary matrices Aij (t) (i, j = 1, : : : , 5), i.e.,

The latitude, longitude, and altitude of any referenced landmark L (considered as a point) in WGS is denoted as

simulations for realistic scenarios is under investigations.

¢

A(t) = [Aij (t)]:

Mw (k) =[Lat(M)Long(M)Alt(M)]0 :

(53)

¢

Lw =[Lat(L)Long(L)Alt(L)]0 :

Almost all submatrices Aij (t) are null except A12 (t) which is equal to the 3 £ 3 identity matrix, A24 (t) = A35 (t) which are equal to attitude matrix B[a(t)] presented in Appendix B and the following ones

2

¡

0

We denote by T (M) the geographical local triad (GLT) centered at the UAV location Mw (k) and defined by local north, east, and down axis (ned). The

3 vn (t) re 7 7 ve (t) 7 7 7 re 7 7 5 0

ve (t) tan(Lat(t)) re

6 3 2 6 ¡g=re 0 0 6 v (t) 7 6 6 0 A21 = 4 0 A11 (t) = 6 e tan(Lat(t)) ¡g=re 0 5 6 re 6 0 0 2g=re 4 v (t) v (t) ¡ n ¡ e re re · ¸ 3 2 ve (t) vn (t) vd (t) ¡2 tan(Lat(t)) + −T sin(Lat(t)) 7 6 re re re 7 6 7 6 v (t) vn (t) vd (t) ve (t) 7 6 e tan(Lat(t)) + 2−T sin(Lat(t)) tan(Lat(t)) + + 2−T cos(Lat(t)) 7 A22 (t) = 6 7 6 re re re re 7 6 · ¸ 5 4 ve (t) vn (t) ¡2 + −T cos(Lat(t)) 0 ¡2 re re 3 3 2 2 0 °d (t) ¡°e (t) 0 ¡1=re 0 7 7 6 6 A23 (t) = 4 ¡°d (t) A32 (t) = 4 1=re 0 °n (t) 5 0 05 °e (t)

2

¡°n (t)

0

0

0

6 6 6 v (t) 6 A33 (t) = 6 e tan(Lat(t)) + −T sin(Lat(t)) 6 re 6 4 v (t) ¡ n re

¡

·

0

ve (t) tan(Lat(t)) + −T sin(Lat(t)) re 0 ·

v (t) ¡ e + −T cos(Lat(t)) re

APPENDIX B. USEFUL MAPPINGS FOR NAVIGATION SYSTEMS In this appendix we recall some important mappings entering in many navigation problems. More details can be found in [28, 31, 32, 40]. Notations: Lat(x) and Long(x) denote, respectively, the geographical latitude and longitude of any point x. Alt(x) is the altitude of x which will 850

tan(Lat(t))=re

¸

¸

vn (t) re

3

7 7 7 ve (t) 7 + −T cos(Lat(t)) 7 : 7 re 7 5 0

north, east, down coordinates of any landmark L in T (M) frame is denoted ¢

L =[xn (L) xe (L) xd (L)]0 : WGS to GLT Mapping Function g: Given the origin Mw of a GLT T (M) and the location of a referenced landmark Lw , the coordinates of landmark L can be expressed in T (M) using the following

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999

mapping function g (see [31] for details) 2

xn (L)

3

2

3

fre [1 ¡ E + 32 E sin2 (Lat(M))] + Alt(M)gfLat(L) ¡ Lat(M)g

7 7 6 6 L = g(Lw , Mw ) = 4 xe (L) 5 = 4 fre [1 + 12 E sin2 (Lat(M))] + Alt(M)gfLong(L) ¡ Long(M)g cos(Lat(M)) 5 xd (L)

¡fAlt(L) ¡ Alt(M)g

re is the ellipsoidal equatorial Earth’s radius (re = 6378137 m for WGS-84) and E is the square of the eccentricity of ellipsoid for WGS-84 (E = 0:00669437999 m). GLT to WGS Mapping Function g¡1 : Reciprocally from L and Mw we can compute Lw using the following relationship [31]

2

3

2

xe (L) = fre [1 + 12 E sin2 (Lat(M))] + Alt(M)g £ fLong(L) ¡ Long(M)g cos(Lat(M)) (58) xd (L) = ¡fAlt(L) ¡ Alt(M)g:

(59)

From (59) we get directly Alt(M) = xd (L) ¡ Alt(L):

(60) 3

xn (L)

Lat(M) +

6 7 fre [1 ¡ E + 32 E sin2 (Lat(M))] + Alt(M)g 6 7 7 6 7 6 w ¡1 w xe (L) L = g (L, M ) = 4 Long(L) 5 = 6 7: 6 Long(M) + 7 2 1 4 fre [1 + 2 E sin (Lat(M))] + Alt(M)g cos(Lat(M)) 5 Alt(L) Lat(L)

¡fxd (L) ¡ Alt(M)g

Computation of Mw from L and Lw : From the true (noise free) range-bearing measurement vector z(k) = [r(k) e(k) a(k)]0 , the coordinates of landmark L expressed in the body frame M = (M, x, y, z) are given by 3 2 3 2 r(k) cos(e(k)) cos(a(k)) xL ¢6 7 7 6 LM = g1 (z(k)) = 4 yL 5 = 4 r(k) cos(e(k)) sin(a(k)) 5

After elementary calculus using (57) and (60), Lat(M) is obtained by solving the nonlinear equation sin2 (Lat(M))[Lat(L) ¡ Lat(M)]

¢

® = 32 re E

(54)

cos µ cos Ã

6 B(k) ´ B[atrue (k)] = 4 cos µ sin Ã

¢

sin µ sin ' cos à ¡ cos ' sin Ã

sin µ cos ' cos à + sin ' sin Ã

cos µ sin '

cos µ cos '

sin µ sin ' sin à + cos ' cos Ã

¡ sin µ

one gets the coordinates of L in GLT by L = B(k)LM :

(56)

The computation of Mw from L and Lw is then a little bit more complicated since we have to solve the following set of equations xn (L) = fre [1 ¡ E + 32 E sin2 (Lat(M))] + Alt(M)g £ fLat(L) ¡ Lat(M)g

(62)

¯ = re (1 ¡ E) + xd (L) ¡ Alt(L):

and from exact attitude matrix B(k) given by [32]

2

(57)

(61)

with

r(k) sin(e(k))

zL

1 [x (L) ¡ ¯(Lat(L) ¡ Lat(M))] ® n

=

(63)

3

7 sin µ cos ' sin à ¡ sin ' cos à 5

(55)

Since (61) cannot be solved analytically, we have to use a numerical algorithm (Gauss—Newton for instance) to get Lat(M). Once Lat(M) is found and using (58) and (60), we get Long(M) component by Long(M) = Long(L) ¡

xe (L) fre [1 + 12 sin2 (Lat(M))] + xd (L) ¡ Alt(L)g cos(Lat(M))

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

:

(64) 851

Therefore location of an UAV M in the WGS can always be computed from its range-bearing mesurement relative to some given referenced landmark L. Derivation of Pseudomeasurement Equation: The pseudomeasurement involved in PDANF is given by (8), i.e., y(k) = g(Lw , M): (65) We precisely show here how this pseudomeasurement is related with SINS errors ±x(k). From g¡1 function, one always has ¡1

w

w

L = g (L, M ):

(66)

Replacing Lw by its expression in (65) yields y(k) = g(g¡1 (L, Mw ), M):

(67)

Using (56) and since atrue (k) = a(k) ¡ H2 ±x(k) with ¢ H2 =[O O I O O] (I being the 3 £ 3 identity matrix and O the 3 £ 3 null matrix), one has

y(k) = g(g¡1 (B[a(k) ¡ H2 ±x(k)]g1 (ztrue (k)), Mw ), M): (68) ¢

Moreover Mw can also be written with H1 = [¡I O O O O] as Mw = g¡1 (H1 ±x(k), M):

(69)

Hence, we finally get the following nonlinear pseudomeasurement equation y(k) = g(g¡1 (B[a(k) ¡ H2 ±x(k)]g1 (ztrue (k)), g¡1 (H1 ±x(k), M)), M) ¢

= h[±x(k), a(k), M, ztrue (k)] = [h1 (k) h2 (k) h3 (k)]0 :

(70)

Since ztrue (k) is not known in practice but only corrupted by some range and bearing measurement noise nz (k), we can write using a first order Taylor expression: h[±x(k), a(k), M, ztrue (k) + nz (k)] ¼ h[±x(k), a(k), M, ztrue (k)] + Jz [h(k)]nz (k)

where

¢

vy (k) = ¡Jz [h(k)]nz (k):

(71)

(72) (73)

APPENDIX C. DERIVATION OF JACOBIAN MATRICES From definition of h = [h1 (k) h2 (k) h3 (k)]0 given in (9) and using elementary algebra, one has 852

® 1 [A r cos(e) cos(a) + A11 r cos(e) sin(a) D1 0 + A12 r sin(e)]

h2 (k) = ¡±x2 +

¯ 2 [A r cos(e) cos(a) + A21 r cos(e) sin(a) D2 0 + A22 r sin(e)]

h3 (k) = ¡±x3 ¡ sin(µ ¡ ±x8 )r cos(e) cos(a) + cos(µ ¡ ±x8 ) sin(' ¡ ±x7 )r cos(e) sin(a) + cos(µ ¡ ±x8 ) cos(' ¡ ±x7 )r sin(e) where r, e, and a are the exact range, elevation, and azimuth measurements (components of ztrue ) relative to the detected landmark L. ', µ and à are the roll, pitch, and yaw angles given by IMU (i.e., components of a(k)). ±xi is the ith component of INS error vector ±x(k). a, b, D1 , D2 , A10 , A11 , A12 , A20 , A21 and A22 are given by ® = re [1 ¡ E + 32 E sin2 (Lat)] + Alt ¯ = cos(Lat)[re (1 + 12 E sin2 (Lat)) + Alt] · µ ¶¸ ±x1 2 3 D1 = re (1 ¡ E) + 2 E sin Lat ¡ ® + Alt + ±x3 µ ¶ ±x D2 = cos Lat ¡ 1 ® · µ µ ¸ ¶¶ ±x1 2 1 £ re 1 + 2 E sin Lat ¡ + Alt + ±x3 ® A10 = cos(µ ¡ ±x8 ) cos(à ¡ ±x9 ) A11 = sin(µ ¡ ±x8 ) sin(' ¡ ±x7 ) cos(à ¡ ±x9 )

where Jz is the Jacobian matrix of h[¢] with respect to z components. Theoretically Jz is taken at z = ztrue (k) but in practice Jz will be evaluated with z(k) (ztrue (k) being unknown). Analytical expression of Jz is given in Appendix C. Finally (71) allows us to rewrite the previous nonlinear pseudomeasurement equation as y(k) = h[±x(k), a(k), M, z(k)] + vy (k)

h1 (k) = ¡±x1 +

¡ cos(' ¡ ±x7 ) sin(Ã ¡ ±x9 ) A12 = sin(µ ¡ ±x8 ) cos(' ¡ ±x7 ) cos(Ã ¡ ±x9 ) + sin(' ¡ ±x7 ) sin(Ã ¡ ±x9 ) A20 = cos(µ ¡ ±x8 ) sin(Ã ¡ ±x9 ) A21 = sin(µ ¡ ±x8 ) sin(' ¡ ±x7 ) sin(Ã ¡ ±x9 ) + cos(' ¡ ±x7 ) cos(Ã ¡ ±x9 ) A22 = sin(µ ¡ ±x8 ) cos(' ¡ ±x7 ) sin(Ã ¡ ±x9 ) ¡ sin(' ¡ ±x7 ) cos(Ã ¡ ±x9 ) where Lat, Long, and Alt are the latitude, longitude, and altitude component of the (erroneous) position M(k) of the UAV returned by the IMU.

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999

Expression for Jz [h(k)]: ¢

Jz [h(k)] =

@h[±x(k), a(k), M(k), ztrue (k)] @ztrue (k)

= [rz h0 [±x(k), a(k), M(k), ztrue (k)]]0 3 2 @h1 =@r @h1 =@e @h1 =@a 7 6 = 4 @h2 =@r @h2 =@e @h2 =@a 5 @h3 =@r

@h3 =@e @h3 =@a

with @h1 =@r =

Almost all components of the first row @h1 =@±xi are null except µ ¶ @h1 =@±x1 = ¡1 + 3Are E sin Lat ¡

µ

£ cos Lat ¡

@h1 =@±x7 =

® [¡ sin(µ ¡ ±x8 ) cos(' ¡ ±x7 ) cos(Ã ¡ ±x9 ) D1 ¡ sin(' ¡ ±x7 ) sin(Ã ¡ ±x9 )]r cos(e) sin(a)

® 1 [A cos(e) cos(a) + A11 cos(e) sin(a) D1 0 +

® [sin(µ ¡ ±x8 ) sin(' ¡ ±x7 ) cos(Ã ¡ ±x9 ) D1 ¡ cos(' ¡ ±x7 ) sin(Ã ¡ ±x9 )]r sin(e)

® [¡A10 r sin(e) cos(a) ¡ A11 r sin(e) sin(a) D1 @h1 =@±x8 =

+ A12 r cos(a)]

® [sin(µ ¡ ±x8 ) cos(Ã ¡ ±x9 )]r cos(e) cos(a) D1 ® [cos(µ ¡ ±x8 ) sin(' ¡ ±x7 ) cos(Ã ¡ ±x9 )] D1

@h1 =@a =

® [¡A10 r cos(e) sin(a) + A11 r cos(e) cos(a)] D1

¡

@h2 =@r =

¯ 2 [A cos(e) cos(a) + A21 cos(e) sin(a) D2 0

£ r cos(e) sin(a) ¡

+ A22 sin(a)] @h2 =@e =

¯ [¡A20 r sin(e) cos(a) ¡ A21 r sin(e) sin(a) D2 + A22 r cos(a)]

@h1 =@a =

@h1 =@±x9 =

+

@h3 =@a = sin(µ ¡ ±x8 )r cos(e) sin(a) + cos(µ ¡ ±x8 ) sin(' ¡ ±x7 )r cos(e) cos(a):

Expression for H(k) = J±x [h(k)]: ¢

J±x [h(k)] =

@h[±x(k), a(k), M(k), ztrue (k)] @±x(k)

= [r±x h0 [±x(k), a(k), M(k), ztrue (k)]]0

2 @h =@±x ¢ ¢ ¢ @h =@±x ¢ ¢ ¢ @h =@±x 3 1 1 1 i 1 15 6 7 = 4 @h2 =@±x1 ¢ ¢ ¢ @h2 =@±xi ¢ ¢ ¢ @h2 =@±x15 5 : @h3 =@±x1

¢¢¢

@h3 =@±xi

¢¢¢

@h3 =@±x15

® [sin(µ ¡ ±x8 ) cos(' ¡ ±x7 ) sin(Ã ¡ ±x9 ) D1 ¡ sin(' ¡ ±x7 ) cos(Ã ¡ ±x9 )]r sin(e)

with ¢

+ cos(µ ¡ ±x8 ) cos(' ¡ ±x7 )r cos(e)

® [sin(µ ¡ ±x8 ) sin(' ¡ ±x7 ) sin(Ã ¡ ±x9 ) D1 + cos(' ¡ ±x7 ) cos(Ã ¡ ±x9 )]r cos(e) sin(a)

+ cos(µ ¡ ±x8 ) sin(' ¡ ±x7 ) cos(e) sin(a)

¡ cos(µ ¡ ±x8 ) sin(' ¡ ±x7 )r sin(e) sin(a)

® [cos(µ ¡ ±x8 ) sin(Ã ¡ ±x9 )]r cos(e) cos(a) D1 +

@h3 =@r = ¡ sin(µ ¡ ±x8 ) cos(e) cos(a)

@h3 =@e = sin(µ ¡ ±x8 )r sin(e) cos(a)

® [cos(µ ¡ ±x8 ) cos(' ¡ ±x7 ) cos(Ã ¡ ±x9 )] D1

£ r sin(e)

¯ [¡A20 r cos(e) sin(a) + A21 r cos(e) cos(a)] D2

+ cos(µ ¡ ±x8 ) cos(' ¡ ±x7 ) sin(e)

¶

@h1 =@±x3 = ¡®A

+ A12 sin(a)] @h1 =@e =

±x1 ®

±x1 ®

A=

[A10 r cos(e) cos(a) + A11 r cos(e) sin(a) + A21 r sin(a)] (D1 )2

:

Almost all components of the second row @h2 =@±xi are null except µ ¶ ¯ ±x1 @h2 =@±x1 = ¡ B[re + Alt + ±x3 ] sin Lat ¡ ® ® · µ ¶ ¯ ±x ¡ Br E sin3 Lat ¡ 1 2® e ® µ ¶ ±x1 ¡ 2 sin Lat ¡ ® µ ¶¸ ±x £ cos2 Lat ¡ 1 ® @h2 =@±x2 = ¡1

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

853

µ

±x @h2 =@±x3 = ¡¯B cos Lat ¡ 1 ® @h2 =@±x7 =

Expression for JL [g(k)]:

¶

¢

JL [g(k)] =

¯ [¡ sin(µ ¡ ±x8 ) cos(' ¡ ±x7 ) sin(Ã ¡ ±x9 ) D2

2 @g =@ Lat(L) @g =@ Long(L) @g =@ Alt(L) 3 1 1 1 6 7 = 4 @g2 =@ Lat(L) @g2 =@ Long(L) @g2 =@ Alt(L) 5 :

+ sin(' ¡ ±x7 ) cos(Ã ¡ ±x9 )]r cos(e) sin(a) +

¯ [sin(µ ¡ ±x8 ) sin(' ¡ ±x7 ) sin(Ã ¡ ±x9 ) D2

@g3 =@ Lat(L)

6 6 JL [g(k)] = 6 4

@h2 =@±x8 =

re [1 ¡ E + 32 E sin2 (Lat)] + Alt

0

[1]

[2]

¯ [cos(µ ¡ ±x8 ) cos(' ¡ ±x7 ) sin(Ã ¡ ±x9 )] D2 [3]

£ r sin(e) ¯ [cos(µ ¡ ±x8 ) cos(Ã ¡ ±x9 )]r cos(e) cos(a) D2

¯ + [¡ sin(µ ¡ ±x8 ) sin(' ¡ ±x7 ) cos(Ã ¡ ±x9 ) D2 + cos(' ¡ ±x7 ) sin(Ã ¡ ±x9 )]r cos(e) sin(a)

[4]

¯ [sin(µ ¡ ±x8 ) cos(' ¡ ±x7 ) cos(Ã ¡ ±x9 ) D2

[5]

+ sin(' ¡ ±x7 ) sin(Ã ¡ ±x9 )]r sin(e)

[6]

with ¢

B=

[A20 r cos(e) cos(a) + A21 r cos(e) sin(a) + A22 r sin(a)] (D2 )2

: (74)

Almost all components of the third row @h3 =@±xi are null except

[7]

[8]

@h3 =@±x3 = ¡1 @h3 =@±x7 = ¡ cos(' ¡ ±x7 ) cos(µ ¡ ±x8 )r cos(e) sin(a)

[9]

+ sin(' ¡ ±x7 ) cos(µ ¡ ±x8 )r sin(e) @h3 =@±x8 = cos(µ ¡ ±x8 )r cos(e) cos(a) + sin(' ¡ ±x7 ) sin(µ ¡ ±x8 )r cos(e) sin(a) + cos(' ¡ ±x7 ) sin(µ ¡ ±x8 )r sin(e): 854

0

3

7 7 : 07 5

¡1

REFERENCES

£ r cos(e) sin(a)

¡

2 1 2 E sin (Lat)] + Altg cos(Lat)

0

¯ ¡ [cos(µ ¡ ±x8 ) sin(' ¡ ±x7 ) sin(Ã ¡ ±x9 )] D2

@h9 =@±x9 = ¡

0 fre [1 +

0

¯ [sin(µ ¡ ±x8 ) sin(Ã ¡ ±x9 )]r cos(e) cos(a) D2

¡

@g3 =@ Long(L) @g3 =@ Alt(L)

From the expression of g(k) (see Appendix B) we easily get

+ cos(' ¡ ±x7 ) cos(Ã ¡ ±x9 )]r sin(e)

2

@g(k) = [rL g0 (k)]0 @Lw (k)

[10]

Baker, W. R., and Clem, R. W. (1977) “Terrain contour matching [TERCOM] primer. ASP-TR-77-61, Aeronautical Systems Division, Wright-Patterson AFB, OH, Aug. 1977. Bar-Itzhack, I. Y. (1977) Optimal updating of INS using landmarks. In Proceedings of AIAA Guidance and Control Conference, Aug. 1977, 492—502. Bar-Itzhack, I. Y. (1977) Minimal order time sharing filters for INS in-flight alignment. Journal of Guidance and Control, 5, 4 (1982), 396—402. Bar-Gill, A., Ben-Ezra, P., and Bar-Itzhack, I. Y. (1994) Improvment of terrain-aided navigation via trajectory optimization. IEEE Transactions on Control Systems Technology, 2, 4 (Dec. 1994), 336—342. Bar-Shalom, Y., and Fortmann, T. E. (1988) Tracking and Data Association. New York: Academic Press, 1988. Bar-Shalom, Y., and Li, X. R. (1995) Multitarget-Multisensor Tracking: Principles and Techniques. Storrs, CT: YBS Publishing, 1995. Bennett, J. E., and Hung, J. C. (1970) Application of statistical techniques to landmark navigation. Navigation, Journal of the Institute of Navigation, 17 (Winter 1970—1971), 349—357. Cox, I. J., and Wilfong, G. T. (1990) Autonomous Robot Vehicles. New York: Springer Verlag, 1990. Dean, W. N, and Horowitz, S. (1962) The LORAN-C navigation system. In T. G. Thorne (Ed.), Navigation Systems for Aircraft and Space Vehicles. Elmsford, NY: Pergamon, 1962, 156—171. Dezert, J. (1992) Autonomous navigation with uncertain reference points using the PDAF. In Y. Bar-Shalom (Ed.), Multitarget-Multisensor Tracking: Applications and Advances, Vol. 2, 1992.

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 35, NO. 3

JULY 1999

[11]

[12]

[13]

[14]

[15]

[16]

[17]

[18]

[19]

[20]

[21]

[22]

[23]

[24]

[25]

[26]

Dezert, J., and Bar-Shalom, Y. (1993) Joint probabilistic data association for autonomous navigation. IEEE Transactions on Aerospace and Electronic Systems, 29, 4 (Oct. 1993), 1275—1285. Draper, C. S. (1981) Origins of inertial navigation. Journal of Guidance and Control (Sept.—Oct.), 1981. Faurre, P. (1971) Navigation Inertielle Optimale et Filtrage Statistique. Paris: Dunod Edition, 1971. Garag, S. C., Morrow, L. D., and Mamen, R. (1978) Strap down navigation technology: A literature survey. Journal of Guidance and Control (May—June 1978). Getting, I. (1993) The Global Positionning System. IEEE Spectrum, 30, 12 (Dec. 1993), 36—47. Hollowell, J. A. (1990) Heli/SITAN: A terrain referenced navigation algorithm for helicopters. In Proceedings of the IEEE 1990 Position, Location, and Navigation Symposium (PLANS), Las Vegas, NV, Mar. 1990. Hostetler, L. D. (1975) An analysis of a terrain-aided inertial navigation system. Technical report SAND75-0299, Sandia Lab., Albuquerque, NM, Sept. 1975. Hostetler, L. D., and Beckmann, R. C. (1977) The Sandia inertial terrain-aided navigation system. Technical report SAND77-7521, Sandia Lab., Albuquerque, NM, Sept. 1977. Hostetler, L. D., and Andreas, R. D. (1983) Nonlinear Kalman filtering techniques for terrain-aided navigation. IEEE Transactions on Automatic Control, AC-28 (Mar. 1983), 315—323. Hung, J. C., and Bennett, J. E. (1972) Two new landmark navigation methods. IEEE Transactions on Aerospace and Electronic Systems, AES-8 (Mar. 1972), 229—235. Hung, J. C., and Bennett, J. E. (1972) Landmark navigation rule, a new navigation device. IEEE Transactions on Aerospace and Electronic Systems, AES-8 (May 1972), 388—391. Jorgensen, P. S. (1984) Navstar/Global Positionning System 18-Satellite Constellations. In Collected Papers in GPS, Vol 2. Institute of Navigation, 1984. Kumar, M. (1988) WGS 1984. Marine Geodesy, 12 (1988), 117—126. Lechner, W. (1982) Techniques for the development of error models for aided strapdown navigation systems. AGARD report, Mar. 1982. Lloret, P., De Cremiers, M., and Dedieu, C. (1988) L’aide a` la navigation et a` la pre´ paration de missions. Huitie` mes Journe´ es Internationales sur les Syste` mes Experts et leurs Applications, Avignon, France, (May 1988), 65—88. Mealy, G. L., and Tang, W. (1983) Application of multiple model estimation to a recursive terrain height correlation system. IEEE Transactions on Automatic Control, AC-28 (Mar. 1983), 323—331.

[27]

[28]

[29]

[30]

[31]

[32]

[33]

[34]

[35]

[36]

[37]

[38]

[39]

[40]

Milliken, R. J., and Zoller, C. J. (1980) Principle of operation of NAVSTAR and system characteristics. In Collected Papers in GPS, Vol 1. Institute of Navigation, 1980, 3—14. Moritz, H. (1980) Advanced Physical Geodesy. Kent: Abacus Press Tunbridge Press, 1980. (1983) (Special Issue on Global Navigation Systems). Proceedings of the IEEE, 71, 10 (1983). Radix, J. C. (1972) Techniques Inertielles. Paris: Editions Masson, Collection ENSTA, 1972. Radix, J. C. (1972) Re´ pertoire ge´ ode´ sique en vue de la navigation. Toulouse, France: Cepadues Editions, 1991. Radix, J. C. (1993) Syste` mes Inertiels a` composants lie´ s. Toulouse, France: Cepadues Editions, 1993. Reichert, O., Madar, F., and Berton, D. (1989) Supervision d’un recalage de navigation par vise´ es d’amers. Specialized Conference on Artificial Intelligence and Defense, (Neuvie` mes Journe´ es Internationales sur les syste` mes experts et leurs applications), Avignon, France, May 29—June 2, 1989. Savage, P. G. (1984) Strapdown system algorithms, Advances in Strapdown Inertial Systems. AGARD, 1984. Simon, D., and El-Sherief, H. (1995) Real time navigation using the Global Positioning System. IEEE AES Systems Magazine (Jan. 1995), 31—37. Sinha, P., Barckley, K., and deDoes, D. (1988) Integrated navigation system design and performance with Phase III GPS user equipment. In Proceedings of the Satellite Division (ION) Technical Meeting, Colorado Springs, CO, Sept. 19—23, 1988, 283—300. Shafer, G. (1976) A Mathematical Theory of Evidence. Princeton, NJ: Princeton University Press, 1976. Torrieri, D. J. (1990) Statistical theory of passive location systems. In Cox and Wilfong (Eds.), Autonomous Robot Vehicles. New York: Springer-Verlag, 1990, 151—166. Verderese, A. J. (1972) Radar navigation via map matching. In Proceedings of Electro. Opt. Des. Conference, Sept. 1972, 56—62. Zhu, J. (1994) Conversion of Earth-centered Earth-fixed coordinates to geodetic coordinates. IEEE Transactions on Aerospace and Electronic Systems, 30, 3 (July 1994), 957—962.

DEZERT: IMPROVEMENT OF STRAPDOWN INERTIAL NAVIGATION USING PDAF

855

Jean C. Dezert (M’91) was born in l’Hay Les Roses, France in August 1962. He received the electrical engineering degree from Ecole Fran¸caise de Radioe´ lectricite´ Electronique et Informatique (EFREI), Paris in 1985, the D.E.A. from University Paris VII, in 1986, and the Ph.D. degree from University of Paris XI, Orsay, France in 1990, all in automatic control and signal processing. During 1986—1990 he was with the Systems Department at the French National Establishment for Aerospace Research (ONERA), Chaˆ tillon, France and did research in tracking. During 1991—1992, he visited the Department of Electrical and Systems Engineering, University of Connecticut, Storrs, as an European Space Agency (ESA) Postdoctoral Research Fellow. During 1992—1993 he was a teaching assistant in Electrical Engineering at the University of Orle´ ans, France. Since 1993, he is a senior staff engineer in the Information Processing and Modelisation Department at ONERA, Chaˆ tillon, France. His current research interests include autonomous navigation, estimation theory, information fusion, stochastic systems theory and its applications to multisensor-multitarget tracking. Dr. Dezert has one international patent in the autonomous navigation field and has published several papers in international conferences and journals. He coauthored one chapter of Multitarget-Multisensor Tracking: Applications and Advances, Vol. 2 (Y. Bar-Shalom (Ed.). He is a member of Eta Kappa Nu and collaborates for the development of the the International Society of Information Fusion (ISIF). 856

IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS

VOL. 35, NO. 3

JULY 1999