- Email: [email protected]
Maximum-likelihood attitude estimation using GPS signals
Digital Signal Processing 17 (2007) 1089–1100 www.elsevier.com/locate/dsp
Maximum-likelihood attitude estimation using GPS signals P.A. Roncagliolo a,∗,1 , J.G. García a,1 , P.I. Mercader a , D.R. Fuhrmann b,2 , C.H. Muravchik a,3 a Laboratorio de Electrónica Industrial, Control e Instrumentación (LEICI), Facultad de Ingeniería, UNLP, La Plata, Argentina b Department of Electrical Engineering, Washington University, St. Louis, MO, USA
Available online 3 October 2006
Abstract The maximum likelihood estimator (MLE) for a vehicle’s attitude using signals received from GPS satellites is derived in this paper. A statistical model of the measured phase double-differences is developed to form the likelihood function. The attitude is represented by a rotation matrix, a member of the group of 3 × 3 special orthogonal matrices. A steepest descent-like maximization algorithm is derived, that guarantees that the estimates belong to this manifold. Experimental results illustrate the performance and the importance of considering the right correlation between measurements. © 2006 Elsevier Inc. All rights reserved. Keywords: Attitude measurement; Maximum likelihood estimation; Global positioning system; Nonlinear optimization
1. Introduction A new algorithm to determine the attitude of a vehicle based on the phase of GPS signals is presented. Estimation of a mobile’s attitude or orientation is an important problem in many areas, such as satellite control, air and water navigation, and robotics. It consists of finding the relationship between a local coordinate frame attached to the vehicle and another frame considered as a reference. This change of coordinates amounts to a pure rotation and is described by an orthogonal matrix that takes a vector from one frame to the other. There are several ways to represent the rotation [1]; quaternions and Euler angles are among the most used in the aerospace area. In this paper the attitude is represented by a matrix belonging to the class of 3 × 3 special orthogonal matrices denoted SO(3) [2], the group of orthogonal matrices with unit determinant. Traditional systems to determine the attitude are based on specialized sensors such as sun detectors, star trackers, inertial sensors, and gyroscopes [3]. In all cases, the idea is to use the known direction of some signals in the reference coordinate system, like the direction-of-arrival of the light from some easily seen star, and measure theses directions with the sensors in the local coordinate system. Two noncollinear directions (or vectors) is the minimum required to establish the attitude uniquely. Then, a cost function is built, for instance mean squared error, that is minimized by * Corresponding author. Fax: +54 221 4259306.
E-mail address: [email protected] (P.A. Roncagliolo). 1 Supported by National Research Council (CONICET) and ANPCyT. 2 Partially supported by a Fulbright scholarship. 3 Supported by Buenos Aires Province Research Council (CIC-PBA) and ANPCyT.
1051-2004/$ – see front matter © 2006 Elsevier Inc. All rights reserved. doi:10.1016/j.dsp.2006.09.001
1090
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
the desired rotation matrix. Wahba posed the problem in this way in 1965 [4]. Since then, several other algorithms have been developed; among them, [5] estimates the quaternion that corresponds to the attitude, and [6,7] use singular values. The latter were shown to be optimal for the statistical model in [8]. GPS provides accurate 3D navigation suitable for many types of vehicles, i.e., it allows calculating the position and velocity of a mobile. Among other tasks, the receiver must calculate the distance to the visible satellites and obtain the satellites’ time and location from the message they transmit. Since the position of the satellites and of the vehicle are known or estimated, the direction-of-arrival of each satellite signal can also be determined. In order to estimate angular dynamic variables (attitude, its rate of change, and angular acceleration) an array of antennas and one or more receivers are used, where each receiver is kept locked to the phase of the received carrier signals. Distances to the satellites can be calculated when the GPS receiver is locked to the phase and to the modulation of the incoming signal to provide for message decoding. The phase information as recovered by a phase-lock loop (PLL) is noisy and can be modeled as a signal in additive Gaussian noise. The noise variance depends mainly on the carrier-to-noise ratio (CNR) at the loop input and the loop equivalent noise bandwidth [9]. The measurements needed to compute the attitude are the phase differences among the signals received at each antenna and for each satellite. Hence, a single PLL is required for each antenna and tracked satellite. To the best of our knowledge, existing approaches to attitude estimation using GPS either (i) employ single phase differences [10], (ii) convert the phase differences to vector observations of the direction-of-arrival [11,12], or (iii) form double differences with the measured phases [13,14]. Then, they estimate the attitude minimizing a cost functional based on a least-squareserror criterion (coincident with the maximum likelihood (ML) under certain hypotheses). The noise contaminating the phase-difference measurements is assumed to be uncorrelated in the cited references what leads to suboptimum solutions, except for some restricted cases. In this paper, we derive an algorithm that uses double-difference of phase as measurements and estimates the attitude maximizing their likelihood function, under the assumption of known phase noise variances. Double-differences are preferred since, as it will be explained, they are free of several perturbations that corrupt single-differences especially when independent GPS receivers are used for each antenna [13]. The functional to be minimized looks similar to other approaches cost functions, but it is truly a ML procedure on the manifold of rotation matrices, that takes into account the proper noise correlation in the phase double-difference measurements. The connection between the GPS-based attitude determination and the orthogonal Procrustes problem was established in [15]. The orthogonal Procrustes can be solved explicitly, but the weighted orthogonal Procrustes problem (WOPP) requires a numerical solution with an iterative algorithm. Our likelihood function corresponds to a WOPP and its associated cost function is minimized by means of a steepest descent-like algorithm. Our optimization procedure is naturally carried on within the class of SO(3) matrices. Hence, it always produces solutions that preserve the rotation nature of the desired attitude matrix, without need of any kind of projection. The paper is organized as follows. In Section 2 the assumed measurements model is explained and the noise contribution is specially analyzed. Section 3 deals with the maximum likelihood approach and obtaining the cost function that is to be optimized. Section 4 briefly explains the geometry of the space of rotation matrices. Then, this geometric setup is exploited in the optimization phase to produce estimates of the attitude matrix that belong to SO(3). Section 5 presents experimental results obtained with our test platform. We conclude and discuss further work in Section 6. 2. Double difference phase model The basic measurements for attitude estimation using GPS are the carrier phases of the satellites signals received with an array of antennas. The carrier phase of the ith GPS satellite signal tracked by a receiver with antenna α can be expressed as [9] i − dtri + εαi , (1) φαi = ραi − λNαi + φ0i − c τ i + τα + dio where φ0i is the transmitted satellite signal phase, ραi is the propagation distance between the ith satellite and the phase center of the antenna α, λ is the wavelength of the GPS signal, c is the speed of light, Nαi is the unknown number of i is carrier integer cycles in ραi (usually called cycle ambiguity), τ is the associated satellite or receiver clock bias, dio i i the advance of the carrier due to the ionosphere and dtr is the delay of the carrier due to the troposphere, and εα is the measurement error or phase noise mainly due to space and electronic thermal noise, propagation inhomogeneities and
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
1091
Fig. 1. Carrier phase model.
Fig. 2. The phase difference between a pair of antennas.
multipath. Note that the quantities are all scaled to meters (for instance, by multiplying all phases in radians by the factor λ/2π ) and correspond to the same time instant. This situation is depicted in Fig. 1. The signals arrive to the array as a planar wavefront hence, the line of sight (LOS) vectors to the same satellite can be considered parallel. This is because the distance separating the antennas is much smaller than the distance from the vehicle and any of the satellites, as illustrated in Fig. 2 where the signal of the ith satellite is received by the antennas α and β. It is clear that the difference between propagation paths can be expressed as the following inner product, i
ραβ (t) = ραi (t) − ρβi (t) ≈ bTαβ si ,
(2)
where bαβ is called the baseline vector, si is the unitary line of sight vector, both expressed in some common coordinate system, and (·)T denotes matrix transpose. However, both vectors are typically known in different coordinate systems. The expression of si is in the reference coordinate system where the satellite and vehicle positions are determined. The baseline vector bαβ is known in the body system fixed to the vehicle where the antennas are attached to. The relationship between these two systems is the unknown attitude matrix A ∈ SO(3) that rotates one system into the other. The set of matrices SO(3) is a manifold that can be shown to be a Lie group [2], whose elements satisfy AAT = I = AT A and det(A) = 1. The information about the attitude of the array is not directly in the phases, but in their differences, thus, it is useful to form differences with them. Let bαβ be the baseline vector as above and Asi the LOS vector to satellite i, both referenced to the body system. Using the operator (·)iαβ = (·)iα − (·)iβ , the single difference of measured phases is i i i
φαβ = bTαβ Asi + λ Nαβ − c ταβ + εαβ .
(3)
The terms corresponding to the ionosphere and troposphere, like the other common terms, cancel out because they are practically equal given the short distance between antennas, and their residues can be neglected compared with the other error terms. Moreover, the ambiguity term is now bounded by the baseline length, helping to find a solution for it. In contrast, the receiver clock biases are different in general, unless special receiver configurations are used as in [10]. This special configurations lead to an increase in hardware and software complexity and therefore we disregarded them. Another way to eliminate receiver clock biases is to calculate double differences, from differences ij j j of single differences of two satellites. The corresponding operator is ∇ (·)αβ = (·)iα − (·)iβ − (·)α + (·)β , where
1092
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
refers to difference between receivers, or antennas, and ∇ to difference between satellites. The double differences of carrier phase of antennas α and β and satellites i and j is ij
ij
ij
ij
ij
ij
∇ φαβ ≈ ∇ ραβ + λ∇ Nαβ + ∇ εαβ ≈ bTαβ Asij + λ∇ Nαβ + ∇ εαβ ,
(4)
where sij = si − sj . This differentiating process is computationally inexpensive but increases the noise variance, which approximately doubles with each differentiation. Also, the uncertainty of the ambiguity term is doubled in the last differentiation. Another consequence of utilizing double-differences is that the known directions used to estimate the attitude are not the LOS, but differences of them. Therefore, at least three satellites are needed, instead of two in the case of single differences. This is not a real problem since GPS receivers need at least four satellites for 3D ij navigation. Thus, the attitude matrix A can be estimated once ∇ φαβ is computed for a sufficient number of satellites ij and antennas (or receivers), the corresponding double differences of cycle ambiguities ∇ Nαβ are estimated, and a ij model for the measurement noise ∇ εαβ is assumed. The noise in (1) is modeled as an additive Gaussian process with zero mean since it is the output of a tracking loop at high CNR. The phase noise variance depends on the CNR, the loop equivalent noise bandwidth and the level of multipath [10,16]. If the phases φαi are obtained from independent loops, one for each antenna and each visible satellite, independence of these measurements can be assumed. Consider that there are n + 1 visible satellites and m + 1 antennas. The antenna and the satellite taken as a reference to make the differences are labelled with index 0. Then, the nm phase double differences are given by i0 , ∇ φα0
α = 1, . . . , m and i = 1, . . . , n.
(5)
In fact, more double differences can be formed but they do not provide any more information because they are linear combinations of the first nm. According to this model, each phase double difference is a random variable with Gaussian distribution and i0 i0 , = bTαβ Asi0 + λ∇ Nα0 E ∇ φα0 i0 2 2 2 = σαi + σ0i2 + σα0 + σ00 , Var ∇ φα0 2 is the variance of φ i , for α = 0, 1, . . . , m and i = 0, 1, . . . , n. Note that these variances remain constant, where σαi α while longer baselines produce larger phase differences to measure, increasing the signal-to-noise ratio in the attitude estimation. On the other hand, the longer the baselines, the more difficult it is to resolve the cycle ambiguities, in addition to the constructive problems associated with a large array (flexion, vibrations, etc.). The use of a common antenna and a common satellite to form the nm phase differences causes them to be correlated. In the present case, the covariances are easily obtained as j0 i0 2 2 2 ∇ φβ0 = σαi δij δαβ + σ0i2 δij + σα0 δαβ + σ00 , (6) Cov ∇ φα0
where δab is the Kronecker delta (1 if a = b, or 0 if a = b). The presence of this correlation, as it will be proved in Section 5, is essential to the attitude estimation, and must be taken into account in this process. 3. Maximum likelihood estimation The likelihood function of a set of measurements consists of their joint probability distribution taken as a function of the parameters to be estimated. In this case it is a multivariate Gaussian distribution which depends on the attitude matrix A. Assuming that the ambiguities have been estimated in a previous stage, it is convenient to define the corrected double difference phase measurements as i0 i0 − λ∇ Nα0 fαi = ∇ φα0
(7)
for α = 1, . . . , m and i = 1, . . . , n. There are seldom enough grounds to consider different variances for each of the array antennas while they receive the same satellite signals. Thus, the following simplifying assumption is made: 2 σi2 = σαi ,
∀α = 0, . . . , m
for i = 1, . . . , n. Therefore, the mean and covariance of the corrected measurements are j E fαi = bTα0 Asi0 , Cov fαi , fβ = σi2 δij + σ02 (δαβ + 1).
(8)
(9)
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
1093
These nm measurements can be arranged into n vectors according to the satellite to which they correspond, T (10) fi = f1i f2i . . . fmi for i = 1, . . . , n. The means and covariance matrices of these vectors are given by E{fi } = BT Asi0 , Cov fi fTj = σi2 δij + σ02 Im + Im ITm where Im is the m × m identity; Im is the m-dimensional vector I = [1, . . . , 1]T and B = [b10 b20 Defining the matrices S = [s10 s20 . . . sn0 ], D = diag σ12 σ22 . . . σn2 , F = [f1 f2 . . . fn ], Cn = D + σ02 In ITn Cm = Im + Im ITm , it is easily found that E{F} = BT AS,
Cov vec(F) = Cn ⊗ Cm = C,
(11) ...
bm0 ].
(12)
(13)
where vec(F ) stacks the columns of F to form a single column vector, and ⊗ indicates Kronecker product. Then the likelihood function of the nm measurements is vec(F − BT AS)T C−1 vec(F − BT AS) 1 . (14) exp − l(A) = 2 (2π)nm/2 |C|1/2 The value of A maximizing l(A) is the desired estimate. Taking the logarithm of this function, changing its sign and discarding the terms that do not depend on A, the cost function J (A) to minimize is given by J (A) = vec(F − BT AS)T C−1 vec(F − BT AS).
(15)
Using the following properties of the Kronecker product [17] tr{PT QRTT } = vec(P)T (T ⊗ Q)vec(R),
(U ⊗ V)−1 = U−1 ⊗ V−1 ,
when U and V are nonsingular matrices, the function J (A) can be rearranged as −1/2 −1/2 2 T −1 J (A) = tr (F − BT AS)T C−1 = Cm (F − BT AS)Cn F , m (F − B AS)Cn
(16)
R−1/2
where represents a square root of R, its inverse, and · F is the Frobenius norm. The functional J (A) of (16) looks similar to the cost function in [10]. However, (16) is formed with double difference phase measurements. Moreover, (16) has optimal weighting matrices chosen according to the maximum likelihood criteria as opposed to [10] where an ad-hoc value is used that facilitates computations but loses optimality except for special array configurations. This minimization problem appears frequently in data matching contexts and is called Penrose regression or weighted orthogonal Procrustes problem, see [15,18], for example. Unlike the classical orthogonal Procrustes problem which has a unique closed-form solution via the singular value decomposition [19], the WOPP can have more than one local minimum, as discussed at the end of Section 4, and only numerical methods are known to solve it. A relatively new method to deal with this kind of nonlinear optimization problems over manifolds is known to use geometric differential tools. A general approach to the subject can be found in [20]. In particular, this technique was applied to attitude estimation with simple differences in [21,22]. R1/2
4. Optimization in SO(3) The special cost function (16) to be minimized has the particular feature that the solution must belong to SO(3), i.e., it must satisfy (AAT = I = AT A) with no reflections (det(A) = 1). Standard, unrestricted minimization techniques should not be applied since they can attain solutions that are not rotations. This is the main motivation to devise our own optimization algorithm in SO(3). A steepest descent method [23] starts from an initial rotation matrix A0 , calculates the gradient of J (A) at A0 denoted ∇J (A0 ), and moves on SO(3) along this direction ∇J (A0 ) until a minimum of J (A) is found at A1 , i.e., J (A1 ) J (A0 ). This step is repeated computing ∇J (A1 ), finding a minimum of J (A) moving on SO(3) along ∇J (A1 ), say at A2 with J (A2 ) J (A1 ). The procedure continues repeating these steps until a stopping rule is satisfied.
1094
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
This steepest descent method is chosen because (i) the gradient ∇J (A) can be readily computed and (ii) moving along this direction restricted to always stay on SO(3) until a minimum is found, is easily done as explained below. These features derive from the geometric structure of SO(3) which we briefly review in Section 4.1. 4.1. Geometry of the manifold The set of matrices SO(3) is not Euclidean but it has the structure of a manifold, thus it has the property of being locally Euclidean and allows defining a tangent plane in each point of the manifold [2,24]. The curved geometry of the group implies that the sum of two elements does not belong to the set. Moreover, SO(3) is a group whose group operation is the matrix product and whose identity element is the 3 × 3 identity matrix I. It is also a Lie group, and therefore it is parallelizable, i.e., the tangent plane at each point can be defined as a rotated version of the tangent plane in the identity element. The tangent space at the identity point I is denoted TI (SO(3)) and defined by the 3 × 3 skew-symmetric matrices. An orthogonal base for TI (SO(3)) is given by 0 0 0 0 0 1 0 −1 0 Y2,I = 0 0 0 , Y3,I = 1 0 0 . Y1,I = 0 0 −1 , 0 1 0 −1 0 0 0 0 0 Let Ya , Yb ∈ TI (SO(3)) be elements of the tangent space, a metric is defined as Ya , Yb = tr{Ya T · Yb }. Notice that the induced norm is the Frobenius norm. The translation of any element Yi,I on the tangent plane at I to the tangent plane at A ∈ SO(3) is obtained by simply pre-multiplying by the rotation A, Yi,A = AYi,I ∈ TA (SO(3)). A curve, in fact a 1-parameter subgroup, θ → ξ(θ ) ∈ SO(3) can be generated such that (i) its velocity vector dξ(θ )/dθ |θ=0 = Y ∈ TI (SO(3)) and (ii) for a θ > 0 such that ξ(θ ) = A, dξ(θ )/dθ = AY ∈ TA (SO(3)). This curve is built with the help of the so-called exponential map eθY and is the locus of the rotation matrices “parallel” to, or in the direction of Y. As an example, suppose the rotations in R3 about the x-axis are to be described. Then, choose Y = Y1,I and let 1 0 0 θY1,I = 0 cos(θ ) − sin(θ ) = A, ξ(θ ) = e 0 sin(θ ) cos(θ ) It is easily checked that ξ(θ ) satisfies the properties (i) and (ii) required above. The curve ξ(θ ) describes all the rotations about the x-axis and the parameter θ ∈ [0, 2π) denotes the size of the rotation. The example generalizes to any rotation about a given vector. Indeed, suppose that n = [n1 n2 n3 ] denotes a unit vector and θ ∈ [0, 2π), if Y = θ (n1 Y1,I + n2 Y2,I + n3 Y3,I )
(17)
then A = represents a rotation of angle θ about the axis defined by n. This is also seen by noting that the matrix exponential of a skew symmetric matrix is a rotation matrix. Computationally, the evaluation of these matrix exponentials is not complex since they can be expressed as a 2-degree matrix polynomial as is shown in Appendix A. eY
4.2. Steepest descent algorithm The key point of this algorithm is to calculate the gradient of the functional to be minimized and to move on the manifold in the direction it defines. This step is done calculating the curve ξ(θ ) as in Section 4.1 and finding the value of θ that minimizes the functional. A formula for the gradient of the cost function is derived first. Evaluating the functional at a point slightly off A0 ∈ SO(3) is a function of the perturbation, J (A0 + x1 Y1,A0 + x2 Y2,A0 + x3 Y3,A0 ) = J A0 (I + x1 Y1,I + x2 Y2,I + x3 Y3,I ) = J (x1 , x2 , x3 ) = J (x). (18) The gradient of J evaluated at the point A0 is found differentiating in the three variables,
∂J ∂J T ∂J ∇J (A0 ) = = [h1 h2 h3 ]T = h. ∂x1 ∂x2 ∂x3 x=0
(19)
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
The required derivatives can be calculated analytically since J (A) is a quadratic equation, resulting in ⎡ T −1 T ⎤ tr{YT1,I AT BC−1 m (F − B AS)Cn S } ⎢ T −1 T ⎥ ∇J (A0 ) = −2 ⎣ tr{YT2,I AT BC−1 m (F − B AS)Cn S } ⎦ .
1095
(20)
T −1 T tr{YT3,I AT BC−1 m (F − B AS)Cn S } A curve is generated from the gradient at an initial point A0 , taking Y = (h1 Y1,I + h2 Y2,I + h3 Y3,I )/|J (A0 )|, where h/|J (A0 )| is the unitary vector which indicates the direction of the curve (or the axis of the rotation). The cost function, written as J (ξ(θ )) = J¯(θ ), is one-dimensional and is easily minimized by conventional methods. The evaluation of the (1D) function J¯(θ ) is made simpler expressing it as a 2-degree real trigonometric polynomial as shown in Appendix B. Once the minimum is found at θ0 , A1 = ξ(θ0 ), and the scheme is repeated until the θi step is as small as needed. The evolution of the estimation as the algorithm is iterated, can be interpreted using Eq. (17). At each step, the gradient indicates the axis of rotation which produces the largest change of J (A). Then, the 1D minimization indicates how much to rotate about this axis with θi . A global minimum is guaranteed to exist because the cost functional is continuous and SO(3) is a compact set. Moreover, SO(3) is a Riemannian manifold and Eq. (16) is a distance and hence, a Morse–Bott function [20]. Then, the convergence to a single (isolated) critical point is guaranteed [20], although it is not necessarily the global minimum. Actually, very recent results in [25] bound the number of local minima of the weighted orthogonal Procrustes problem to less than 2m , where m is the number of double-differences; prove that there exists a global minimum; and characterize the set of local minima.
5. Experimental results 5.1. Hardware and data processing The attitude determination system was implemented using an independent standard (nondedicated) GPS receiver for each antenna of the array. Data from the receivers is analyzed off-line on a personal computer (PC). This configuration was proved to be useful for attitude determination in [26] and continues being used [33]. It leads to a cheaper and less technologically demanding solution than utilizing dedicated GPS receivers. A four-antenna symmetric plane array was built as shown in Fig. 3. Relatively large ground-planes were used to mitigate multipath effects. The phase measurements of the four GPS receivers were stored simultaneously in a PC to form the double-differences afterwards, as it was explained in Section 2. Since our receivers do not measure the phases at the same time they had to be extrapolated to a common instant before calculating the differences. This extrapolation, similar to the case of pseudo-ranges for position calculation [9], is accomplished in a linear way, using the carrier frequency measurements provided by GPS receivers. For further details of this part of our approach see [27]. The basic assumption for ambiguity resolution is that ambiguities remain constant during the observation period. Another important pre-processing task is the cycle-slip and outlier correction. A cycle-slip occurs when the tracking j loop shifts from an equilibrium to a neighboring one. This causes a change of cycle-ambiguities Nα in (1) to another integer value. Typically, a Costas loop, insensitive to phase changes of 180◦ , is used to demodulate the 50 bps data of the GPS signal and therefore the slips are multiples of half cycles. In addition, other sources of error can randomly
Fig. 3. Four-antenna array.
1096
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
Fig. 4. Summary of data processing tasks.
generate outliers that have to be detected and properly discarded. Our correction method is based on a Kalman filter predicting the evolution of the double-differences of phase and comparing these values with the observed ones. It produces a certain degree of filtering of the double-differences of phase which reduces the variance of the observables but has to be redesigned for medium and high dynamics mobiles. The procedure is based on [28] and the details are fully explained in [30]. Once the double-differences are formed and cycle-slips and outliers are corrected, the ambiguity resolution can be faced. There exist different methods to this end [10]. We use here a method based on least square errors proposed in [31], adapted to work with double-differences and taking into account the statistical correlation between them, i0 , α = 1, . . . , m and as shown in Section 2. The determination of the set of double-difference ambiguities (∇ Nα0 i = 1, . . . , n) involves three statistical tests [29] and the details are given in [32]. When the ambiguities are known, the attitude of the array for each measurement instant is estimated finding the minimum of (16) by means of the algorithm explained in Section 4. A block diagram summarizing the data processing tasks performed to achieve attitude estimation is given in Fig. 4. In order to reduce the computational burden, we assumed that the phase variances of different satellites were equal. In this case, the common variance value becomes just an irrelevant scale factor of the cost function (16), which still gives the ML solution. Therefore, normalizing the cost function by this value, the weighting matrices of (12) become Cm = Im + Im ITm , Cn = In + In ITn . (21) 5.2. Results Data were collected during approximately one hour for different attitudes of the array and processed as explained before. In order to facilitate the test, only changes of yaw angle are shown. We emphasize that the algorithm also estimates pitch and roll. The algorithm was implemented using a Newton–Raphson like (1D) optimization, initialized at t = 0 with the identity matrix and stopped when θi < 0.0001 rad. For t > 0 the initialization used was the previous estimate of the rotation matrix. Under these conditions, we empirically observed that the number of gradient searches was lower than three and the number of Newton–Raphson steps per gradient was always lower than six. The results obtained with the present algorithm are compared to manual measurements of the yaw angle which also have some error. These measurements were obtained using a measuring tape, a laser beam to extend the baselines in order to reduce the measurement errors, and some trigonometry. Applying error propagation to the angle formula it was found that these manually measured angles were known within a ±0.5◦ error band. Table 1 summarizes the estimated angles for different time instants, using the weighting matrices of Eq. (21). In all cases the estimated values were inside the interval of one degree established by the comparison method. Moreover, the standard deviation of the estimates did not exceed 0.1◦ . In Fig. 5 a sequence of yaw estimates is shown when the measured yaw of the array was 60.5◦ ± 0.5◦ . As seen in the table, the results are always within the comparison interval. A second set of estimates is obtained in the same way, but calculating the cost functional (16) without the proper weighting matrices. The cost functional minimized in each case is
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
1097
Table 1 Yaw estimation from measured GPS data Contrast measure
Yaw estimation (◦ ) Min
Mean
Max
σ
15.3 ± 0.5 29.1 ± 0.5 41.1 ± 0.5 60.5 ± 0.5 71.6 ± 0.5 89.7 ± 0.5
14.9 28.8 41.0 60.4 71.3 89.5
14.83 28.74 40.88 60.35 71.17 89.48
15.13 28.85 41.23 60.58 71.49 89.55
0.1 0.04 0.1 0.06 0.1 0.02
Fig. 5. Estimation results for yaw equal to 60.5◦ .
−1/2 −1/2 2 Case 1: J (A) = Cm (F − BT AS)Cn F ,
2 Case 2: J (A) = F − BT AS . F
Case 2 corresponds to the simplest choice of nonoptimal weighting matrices. Choosing weights as in [10] is nonoptimal but permits a simple solution in terms of singular values. However, it is either not applicable to a planar array configuration as used here because B is singular or it is impractical because the weighting factor becomes time varying due to S. As expected, the use of inadequate weighting matrices of Case 2 produces biased estimates, which in this particular case is of 2◦ , approximately. The standard deviation seems to be unaffected. Empirical estimates of the phase variances from the measured data could produce more precise values for the weighting matrices. 6. Conclusions The maximum likelihood criterion for the attitude matrix estimation from double-difference phase measurements of GPS signals was considered in this paper. Double-differences were chosen because they allow the use of independent, nondedicated GPS receivers. The ML criterion leads to a cost function similar to others used in previous works [10,11]. However, our derivation of the ML estimator for double-differences of phase shows the proper values for the weighting matrices in (16). A simple but realistic noise model based on the GPS receiver operation, allowed us to compute the double-differences of phase covariance matrix and from it, the weighting matrices. Minimization of this kind of cost functional is a nonlinear optimization task called the weighted orthogonal Procrustes problem, with no known closed-form solution. Thus, a new steepest descent scheme was devised based on the properties of Lie groups, that guarantees that the solution always stays in SO(3). The experimental results confirm that the proposed scheme improves the estimation quality producing unbiased estimates of the attitude, as opposed to the case of non-proper weighting in [10,11]. The present approach also allows the use of an arbitrary structure array of antennas preserving optimality.
1098
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
It remains to compare with other approaches in terms of computational cost and importance for real time and dynamic attitude applications. A comparison in terms of accuracy can be done by means of the Cramér–Rao bound (CRB) [34]. However, a new form of the CRB has to be developed, along the lines of [35], since the estimated variable is constrained to SO(3). Work in these directions is currently under way by some of the authors. Appendix A The Lemma in this appendix shows that the exponential of a 3 × 3 skew-symmetric matrix is easily calculated. Lemma. Let Y be a skew-symmetric matrix given by 0 −c b Y= c 0 −a . −b a 0 Then 1 − cos( v ) sin( v ) Y2 , Y+ eY = I + v v 2 √ where v = [a, b, c] and v = a 2 + b2 + c2 .
(A.1)
(A.2)
Proof. The matrix exponential eY can be written as a formal series replacing x = Y in the Taylor series of ex . Using the Cayley–Hamilton theorem [36] the n × n matrix Yn can be written as a polynomial of degree n − 1. Then, the convergent series for the exponential reduces to a polynomial h(·) of degree (n − 1) in Y. The coefficients of the matrix polynomial h(Y) satisfy h(A) = eY . Y in (A.1) is a 3 × 3 matrix, so eY will be equated to a 2-degree polynomial h(Y) = β0 I + β1 Y + β2 Y2 . In order to solve for the coefficients βi , i = 0, 1, 2, we use Theorem 3.5 in [36, p. 64] obtaining h(λi ) = β0 + β1 λi + β2 λ2i = eλi ,
i = 1, 2, 3,
(A.3)
where λi , i = 1, 2, 3, is the ith root of the characteristic polynomial of Y, pY (λ) = det(λI − Y). It is easy to see from (A.1) that pY (λ) = λ(λ2 + a 2 + b2 + c2 ), so its roots are given by λ1 = 0, λ2 = +j v , and λ3 = −j v . Thus, solving (A.3) we get β0 = 1, β1 = sin( v )/ v , and β2 = (1 − cos( v ))/ v 2 for the coefficients, and finally 1 − cos( v ) sin( v ) Y e = h(Y) = I + Y2 . Y+ 2 (A.4) v v 2 Appendix B Formula (B.2) is derived to evaluate the functional (16). It shows that the computational cost of calculating the functional is the same as evaluating the second order trigonometric polynomial (B.2). The cost function (16) evaluated at the flow A = ξ(θ ) = A0 eθYi,I is T T θYi,I (B.1) S C−1 J¯(θ ) = tr F − BT A0 eθYi,I S C−1 m F − B A0 e n . Then, expressing the matrix exponential as in (A.2) with v = θ [h1 , h2 , h3 ]/|∇J (A0 )| and v = |θ | we get eθYi,I = I + sin(θ )Yi,I + (1 − cos(θ ))Y2i,I , which replaced in (B.1) gives J¯(θ ) = a sin(θ ) + b cos(θ ) + c sin(2θ ) + d cos(2θ ) + e, where
(B.2)
a = 2 tr Yi,I AT0 F1 − tr Yi,I AT0 B1 A0 S1 − tr Yi,I AT0 B1 A0 Y2i,I S1 , b = 2 tr{Y2i,I AT0 F1 − tr Y2i,I AT0 B1 A0 S1 − tr Y2i,I AT0 B1 A0 Y2i,I S1 , 1 c = tr Yi,I AT0 B1 A0 Y2i,I S1 , d = tr Y2i,I AT0 B1 A0 Y2i,I S1 + tr Yi,I AT0 B1 A0 Yi,I S1 2 −1 −1 T −1 T T with F1 = BCm FCn S , S1 = SCn S , B1 = BC−1 m B , and e an irrelevant constant with respect to the minimization because it does not depend on θ . Notice that the coefficients (B.2) are calculated only once per descent direction.
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
1099
References [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] [24] [25] [26] [27] [28] [29] [30] [31] [32] [33] [34] [35] [36]
M.D. Shuster, A survey of attitude representations, J. Astronaut. Sci. 41 (4) (1993) 439–517. T. Frankel, The Geometry of Physics, Cambridge Univ. Press, Cambridge, 1997. J.R. Wertz, Spacecraft Attitude Determination and Control, Reidel, Dordrecht, 1986. G. Wahba, A least squares estimate of satellite attitude, Problem 65.1, SIAM Rev. 7 (3) (1966) 409. M.D. Shuster, Three-axis attitude determination from vector observations, Amer. Inst. Aeronaut. Astronaut. AIAA-81-4003 4 (1981) 70–77. F.L. Markley, Attitude determination using vector observations and the singular value decomposition, J. Astronaut. Sci. 36 (3) (1988) 245–258. F.L. Markley, Attitude determination using vector observations: A fast optimal matrix algorithm, J. Astronaut. Sci. 41 (2) (1993) 261–280. M.D. Shuster, Maximum likelihood estimator of spacecraft attitude, J. Astronaut. Sci. 37 (1) (1989) 79–88. E.D. Kaplan, Understanding GPS: Principles and Applications, Artech House, Norwood, MA, 1996. C.E. Cohen, Attitude determination using GPS, Ph.D. dissertation, Stanford University, 1992. J.L. Crassidis, F.L. Markley, Attitude determination using global positioning system signals, Amer. Inst. Aeronaut. Astronaut. AIAA-97-3452 (1997) 23–31. I.Y. Bar-Itzhack, P.Y. Montgomery, J.C. Garrick, Algorithms for attitude determination using GPS, in: Proceedings of the 37th Israel Annual Conference on Aerospace Sciences, Tel-Aviv and Haifa, Israel, 1997, pp. 233–244. J.C. Juang, G.S. Huang, Development of GPS based attitude determination algorithms, IEEE Trans. Aerospace Electron. Syst. 33 (3) (1997) 968–976. R.A. Brown, Instantaneous GPS attitude determination, IEEE Aerospace Electron. Mag. 7 (6) (1997) 3–8. T. Bell, GPS-based attitude determination and the orthogonal Procrustes problem, J. Guid. Control Dynam. 26 (5) (2003) 820–822. M.K. Simon, J.K. Omura, R.A. Scholtz, B.K. Levitt, Spread Spectrum Communications, Computer Science Press, Rockville, MD, 1985. H.V. Henderson, S.R. Searle, Vec and vech operators for matrices, with some uses in Jacobians and multivariate statistics, Can. J. Statist. 7 (1) (1979) 65–81. M.T. Chu, N.T. Trendafilov, The orthogonally constrained regression revisited, J. Comput. Graph. Statist. 10 (4) (2001) 746–771. G.H. Golub, C.F. Van Loan, Matrix Computations, second ed., Johns Hopkins Univ. Press, Baltimore, 1991. U. Helmke, J.B. Moore, Optimization and Dynamical Systems, Springer-Verlag, New York, 1994. F.C. Park, J. Kim, C. Kee, Geometric descent algorithms for attitude determination using GPS, in: Proceedings of the 14th Wold Congress of IFAC, Beijing, China, 1999. P.A. Roncagliolo, J.A. Areta, D.R. Fuhrmann, C.H. Muravchik, Attitude estimation with GPS: Steepest descent algorithm on SO(3), in: Proceedings of the IEEE Second South American Workshop on Circuits and Systems, Río de Janeiro (Brasil)–Buenos Aires (Argentina), 2001. D.G. Luenberger, Linear and Nonlinear Programming, second ed., Kluwer Academic, Dordrecht/Norwell, MA, 2003. A. Srivastava, M.I. Miller, U. Grenander, Ergodic algorithms on special Euclidean groups for ATR, in: Systems and Control in the Twenty-First Century: Progress in Systems and Control, vol. 22, Birkhäuser, Boston, MA, 1997. T. Viklands, Algorithms for the weighted orthogonal Procrustes problem and other least squares problems, Ph.D. dissertation, Department of Computer Science, Umea University, Sweden, 2005. G. Lu, M.E. Cannon, G. Lachapelle, P. Kielland, Attitude determination using dedicated and nondedicated multiantenna GPS sensors, IEEE Trans. Aerospace Electron. Syst. 30 (4) (1994) 1053–1058. J.G. García, P.I. Mercader, C.H. Muravchik, Use of GPS carrier phase double differences, Latin Amer. Appl. Res. 35 (2) (2005) 115–120. P.Z. Jia, L.D. Wu, An algorithm for detecting and estimating cycle slips in single frequency GPS, Chin. Astronaut. Astrophys. 25 (4) (2001) 515–521. G. Lu, Development of a GPS multi-antenna system for attitude determination, Ph.D. dissertation, University of Calgary, 1995. P.I. Mercader, J.G. García and C.H. Muravchik, Detección y corrección de saltos de ciclos de fase para estimación de orientación con GPS, in: Proceedings of the XIX Congreso Argentino de Control Automático, Buenos Aires, Argentina, 2004. R. Hatch, Instantaneous ambiguity resolution, in: Proceedings of the IAG Symposium No. 107 on Kinematic Systems in Geodesy, Surveying and Remote Sensing, Springer-Verlag, New York, 1991. J.G. García, P.I. Mercader, C.H. Muravchik, Resolución de la ambigüedad de fase para estimación de orientación con GPS, in: Proceedings of the XIX Congreso Argentino de Control Automático, Buenos Aires, Argentina, 2004. D.S. De Lorenzo, S. Alban, J. Gautier, P. Enge, D. Akos, GPS Attitude determination for a JPALS testbed: Integer initialization and testing, in: IEEE Position Location and Navigation Symposium, Monterey, CA, 2004. H.L. van Trees, Detection, Estimation and Modulation Theory, Wiley, New York, 1968. H. Hendriks, A Cramér–Rao type lower bound for estimators with values in a manifold, J. Multivar. Anal. 38 (1991) 245–261. C.T. Chen, Linear System Theory and Design, second ed., Oxford Univ. Press, New York, 1999.
Pedro A. Roncagliolo received the Electronics Engineer degree from the National University of La Plata (UNLP), Argentina, in 2001. He is currently a Ph.D. student and Professor in the UNLP, Argentina. His research interests are in statistical signal processing with applications to wireless communications and global positioning system (GPS). Javier G. García received the Electronics Engineer degree from the National University of La Plata (UNLP), Argentina, in 2003. He is currently a Ph.D. student and a TA in the UNLP, Argentina. His research interests are in statistical signal processing with applications to global positioning system (GPS) and digital communications.
1100
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
Pablo I. Mercader received the Electronics Engineer degree from the National University of La Plata (UNLP), Argentina, in 2003. He is currently employed by NOKIA as Core Network Engineer in Latin America. He has previously worked with statistical signal processing in applications to global positioning system (GPS). Daniel R. Fuhrmann received the Ph.D. degree from Princeton University in 1984. Since that time he has been with the Department of Electrical Engineering, Washington University, St. Louis, where he is currently a Professor. In the 2000–2001 academic year he was a Fulbright Scholar visiting at UNLP, Argentina. His research interests are in various areas of statistical signal and image processing, including sensor array signal processing, radar systems, space–time adaptive processing, and image processing for genomics applications. Carlos H. Muravchik received the Electronics Engineer degree from the National University of La Plata (UNLP), Argentina, in 1973, the M.Sc. degree in statistics in 1983, and the M.Sc. and Ph.D. degrees in electrical engineering in 1980 and 1983, respectively, all from Stanford University, Stanford, CA, USA. He is currently a Professor in the UNLP, Argentina. His research interests are in statistical signal and array processing with biomedical, control and communications applications, and nonlinear control systems.
Maximum-likelihood attitude estimation using GPS signals P.A. Roncagliolo a,∗,1 , J.G. García a,1 , P.I. Mercader a , D.R. Fuhrmann b,2 , C.H. Muravchik a,3 a Laboratorio de Electrónica Industrial, Control e Instrumentación (LEICI), Facultad de Ingeniería, UNLP, La Plata, Argentina b Department of Electrical Engineering, Washington University, St. Louis, MO, USA
Available online 3 October 2006
Abstract The maximum likelihood estimator (MLE) for a vehicle’s attitude using signals received from GPS satellites is derived in this paper. A statistical model of the measured phase double-differences is developed to form the likelihood function. The attitude is represented by a rotation matrix, a member of the group of 3 × 3 special orthogonal matrices. A steepest descent-like maximization algorithm is derived, that guarantees that the estimates belong to this manifold. Experimental results illustrate the performance and the importance of considering the right correlation between measurements. © 2006 Elsevier Inc. All rights reserved. Keywords: Attitude measurement; Maximum likelihood estimation; Global positioning system; Nonlinear optimization
1. Introduction A new algorithm to determine the attitude of a vehicle based on the phase of GPS signals is presented. Estimation of a mobile’s attitude or orientation is an important problem in many areas, such as satellite control, air and water navigation, and robotics. It consists of finding the relationship between a local coordinate frame attached to the vehicle and another frame considered as a reference. This change of coordinates amounts to a pure rotation and is described by an orthogonal matrix that takes a vector from one frame to the other. There are several ways to represent the rotation [1]; quaternions and Euler angles are among the most used in the aerospace area. In this paper the attitude is represented by a matrix belonging to the class of 3 × 3 special orthogonal matrices denoted SO(3) [2], the group of orthogonal matrices with unit determinant. Traditional systems to determine the attitude are based on specialized sensors such as sun detectors, star trackers, inertial sensors, and gyroscopes [3]. In all cases, the idea is to use the known direction of some signals in the reference coordinate system, like the direction-of-arrival of the light from some easily seen star, and measure theses directions with the sensors in the local coordinate system. Two noncollinear directions (or vectors) is the minimum required to establish the attitude uniquely. Then, a cost function is built, for instance mean squared error, that is minimized by * Corresponding author. Fax: +54 221 4259306.
E-mail address: [email protected] (P.A. Roncagliolo). 1 Supported by National Research Council (CONICET) and ANPCyT. 2 Partially supported by a Fulbright scholarship. 3 Supported by Buenos Aires Province Research Council (CIC-PBA) and ANPCyT.
1051-2004/$ – see front matter © 2006 Elsevier Inc. All rights reserved. doi:10.1016/j.dsp.2006.09.001
1090
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
the desired rotation matrix. Wahba posed the problem in this way in 1965 [4]. Since then, several other algorithms have been developed; among them, [5] estimates the quaternion that corresponds to the attitude, and [6,7] use singular values. The latter were shown to be optimal for the statistical model in [8]. GPS provides accurate 3D navigation suitable for many types of vehicles, i.e., it allows calculating the position and velocity of a mobile. Among other tasks, the receiver must calculate the distance to the visible satellites and obtain the satellites’ time and location from the message they transmit. Since the position of the satellites and of the vehicle are known or estimated, the direction-of-arrival of each satellite signal can also be determined. In order to estimate angular dynamic variables (attitude, its rate of change, and angular acceleration) an array of antennas and one or more receivers are used, where each receiver is kept locked to the phase of the received carrier signals. Distances to the satellites can be calculated when the GPS receiver is locked to the phase and to the modulation of the incoming signal to provide for message decoding. The phase information as recovered by a phase-lock loop (PLL) is noisy and can be modeled as a signal in additive Gaussian noise. The noise variance depends mainly on the carrier-to-noise ratio (CNR) at the loop input and the loop equivalent noise bandwidth [9]. The measurements needed to compute the attitude are the phase differences among the signals received at each antenna and for each satellite. Hence, a single PLL is required for each antenna and tracked satellite. To the best of our knowledge, existing approaches to attitude estimation using GPS either (i) employ single phase differences [10], (ii) convert the phase differences to vector observations of the direction-of-arrival [11,12], or (iii) form double differences with the measured phases [13,14]. Then, they estimate the attitude minimizing a cost functional based on a least-squareserror criterion (coincident with the maximum likelihood (ML) under certain hypotheses). The noise contaminating the phase-difference measurements is assumed to be uncorrelated in the cited references what leads to suboptimum solutions, except for some restricted cases. In this paper, we derive an algorithm that uses double-difference of phase as measurements and estimates the attitude maximizing their likelihood function, under the assumption of known phase noise variances. Double-differences are preferred since, as it will be explained, they are free of several perturbations that corrupt single-differences especially when independent GPS receivers are used for each antenna [13]. The functional to be minimized looks similar to other approaches cost functions, but it is truly a ML procedure on the manifold of rotation matrices, that takes into account the proper noise correlation in the phase double-difference measurements. The connection between the GPS-based attitude determination and the orthogonal Procrustes problem was established in [15]. The orthogonal Procrustes can be solved explicitly, but the weighted orthogonal Procrustes problem (WOPP) requires a numerical solution with an iterative algorithm. Our likelihood function corresponds to a WOPP and its associated cost function is minimized by means of a steepest descent-like algorithm. Our optimization procedure is naturally carried on within the class of SO(3) matrices. Hence, it always produces solutions that preserve the rotation nature of the desired attitude matrix, without need of any kind of projection. The paper is organized as follows. In Section 2 the assumed measurements model is explained and the noise contribution is specially analyzed. Section 3 deals with the maximum likelihood approach and obtaining the cost function that is to be optimized. Section 4 briefly explains the geometry of the space of rotation matrices. Then, this geometric setup is exploited in the optimization phase to produce estimates of the attitude matrix that belong to SO(3). Section 5 presents experimental results obtained with our test platform. We conclude and discuss further work in Section 6. 2. Double difference phase model The basic measurements for attitude estimation using GPS are the carrier phases of the satellites signals received with an array of antennas. The carrier phase of the ith GPS satellite signal tracked by a receiver with antenna α can be expressed as [9] i − dtri + εαi , (1) φαi = ραi − λNαi + φ0i − c τ i + τα + dio where φ0i is the transmitted satellite signal phase, ραi is the propagation distance between the ith satellite and the phase center of the antenna α, λ is the wavelength of the GPS signal, c is the speed of light, Nαi is the unknown number of i is carrier integer cycles in ραi (usually called cycle ambiguity), τ is the associated satellite or receiver clock bias, dio i i the advance of the carrier due to the ionosphere and dtr is the delay of the carrier due to the troposphere, and εα is the measurement error or phase noise mainly due to space and electronic thermal noise, propagation inhomogeneities and
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
1091
Fig. 1. Carrier phase model.
Fig. 2. The phase difference between a pair of antennas.
multipath. Note that the quantities are all scaled to meters (for instance, by multiplying all phases in radians by the factor λ/2π ) and correspond to the same time instant. This situation is depicted in Fig. 1. The signals arrive to the array as a planar wavefront hence, the line of sight (LOS) vectors to the same satellite can be considered parallel. This is because the distance separating the antennas is much smaller than the distance from the vehicle and any of the satellites, as illustrated in Fig. 2 where the signal of the ith satellite is received by the antennas α and β. It is clear that the difference between propagation paths can be expressed as the following inner product, i
ραβ (t) = ραi (t) − ρβi (t) ≈ bTαβ si ,
(2)
where bαβ is called the baseline vector, si is the unitary line of sight vector, both expressed in some common coordinate system, and (·)T denotes matrix transpose. However, both vectors are typically known in different coordinate systems. The expression of si is in the reference coordinate system where the satellite and vehicle positions are determined. The baseline vector bαβ is known in the body system fixed to the vehicle where the antennas are attached to. The relationship between these two systems is the unknown attitude matrix A ∈ SO(3) that rotates one system into the other. The set of matrices SO(3) is a manifold that can be shown to be a Lie group [2], whose elements satisfy AAT = I = AT A and det(A) = 1. The information about the attitude of the array is not directly in the phases, but in their differences, thus, it is useful to form differences with them. Let bαβ be the baseline vector as above and Asi the LOS vector to satellite i, both referenced to the body system. Using the operator (·)iαβ = (·)iα − (·)iβ , the single difference of measured phases is i i i
φαβ = bTαβ Asi + λ Nαβ − c ταβ + εαβ .
(3)
The terms corresponding to the ionosphere and troposphere, like the other common terms, cancel out because they are practically equal given the short distance between antennas, and their residues can be neglected compared with the other error terms. Moreover, the ambiguity term is now bounded by the baseline length, helping to find a solution for it. In contrast, the receiver clock biases are different in general, unless special receiver configurations are used as in [10]. This special configurations lead to an increase in hardware and software complexity and therefore we disregarded them. Another way to eliminate receiver clock biases is to calculate double differences, from differences ij j j of single differences of two satellites. The corresponding operator is ∇ (·)αβ = (·)iα − (·)iβ − (·)α + (·)β , where
1092
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
refers to difference between receivers, or antennas, and ∇ to difference between satellites. The double differences of carrier phase of antennas α and β and satellites i and j is ij
ij
ij
ij
ij
ij
∇ φαβ ≈ ∇ ραβ + λ∇ Nαβ + ∇ εαβ ≈ bTαβ Asij + λ∇ Nαβ + ∇ εαβ ,
(4)
where sij = si − sj . This differentiating process is computationally inexpensive but increases the noise variance, which approximately doubles with each differentiation. Also, the uncertainty of the ambiguity term is doubled in the last differentiation. Another consequence of utilizing double-differences is that the known directions used to estimate the attitude are not the LOS, but differences of them. Therefore, at least three satellites are needed, instead of two in the case of single differences. This is not a real problem since GPS receivers need at least four satellites for 3D ij navigation. Thus, the attitude matrix A can be estimated once ∇ φαβ is computed for a sufficient number of satellites ij and antennas (or receivers), the corresponding double differences of cycle ambiguities ∇ Nαβ are estimated, and a ij model for the measurement noise ∇ εαβ is assumed. The noise in (1) is modeled as an additive Gaussian process with zero mean since it is the output of a tracking loop at high CNR. The phase noise variance depends on the CNR, the loop equivalent noise bandwidth and the level of multipath [10,16]. If the phases φαi are obtained from independent loops, one for each antenna and each visible satellite, independence of these measurements can be assumed. Consider that there are n + 1 visible satellites and m + 1 antennas. The antenna and the satellite taken as a reference to make the differences are labelled with index 0. Then, the nm phase double differences are given by i0 , ∇ φα0
α = 1, . . . , m and i = 1, . . . , n.
(5)
In fact, more double differences can be formed but they do not provide any more information because they are linear combinations of the first nm. According to this model, each phase double difference is a random variable with Gaussian distribution and i0 i0 , = bTαβ Asi0 + λ∇ Nα0 E ∇ φα0 i0 2 2 2 = σαi + σ0i2 + σα0 + σ00 , Var ∇ φα0 2 is the variance of φ i , for α = 0, 1, . . . , m and i = 0, 1, . . . , n. Note that these variances remain constant, where σαi α while longer baselines produce larger phase differences to measure, increasing the signal-to-noise ratio in the attitude estimation. On the other hand, the longer the baselines, the more difficult it is to resolve the cycle ambiguities, in addition to the constructive problems associated with a large array (flexion, vibrations, etc.). The use of a common antenna and a common satellite to form the nm phase differences causes them to be correlated. In the present case, the covariances are easily obtained as j0 i0 2 2 2 ∇ φβ0 = σαi δij δαβ + σ0i2 δij + σα0 δαβ + σ00 , (6) Cov ∇ φα0
where δab is the Kronecker delta (1 if a = b, or 0 if a = b). The presence of this correlation, as it will be proved in Section 5, is essential to the attitude estimation, and must be taken into account in this process. 3. Maximum likelihood estimation The likelihood function of a set of measurements consists of their joint probability distribution taken as a function of the parameters to be estimated. In this case it is a multivariate Gaussian distribution which depends on the attitude matrix A. Assuming that the ambiguities have been estimated in a previous stage, it is convenient to define the corrected double difference phase measurements as i0 i0 − λ∇ Nα0 fαi = ∇ φα0
(7)
for α = 1, . . . , m and i = 1, . . . , n. There are seldom enough grounds to consider different variances for each of the array antennas while they receive the same satellite signals. Thus, the following simplifying assumption is made: 2 σi2 = σαi ,
∀α = 0, . . . , m
for i = 1, . . . , n. Therefore, the mean and covariance of the corrected measurements are j E fαi = bTα0 Asi0 , Cov fαi , fβ = σi2 δij + σ02 (δαβ + 1).
(8)
(9)
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
1093
These nm measurements can be arranged into n vectors according to the satellite to which they correspond, T (10) fi = f1i f2i . . . fmi for i = 1, . . . , n. The means and covariance matrices of these vectors are given by E{fi } = BT Asi0 , Cov fi fTj = σi2 δij + σ02 Im + Im ITm where Im is the m × m identity; Im is the m-dimensional vector I = [1, . . . , 1]T and B = [b10 b20 Defining the matrices S = [s10 s20 . . . sn0 ], D = diag σ12 σ22 . . . σn2 , F = [f1 f2 . . . fn ], Cn = D + σ02 In ITn Cm = Im + Im ITm , it is easily found that E{F} = BT AS,
Cov vec(F) = Cn ⊗ Cm = C,
(11) ...
bm0 ].
(12)
(13)
where vec(F ) stacks the columns of F to form a single column vector, and ⊗ indicates Kronecker product. Then the likelihood function of the nm measurements is vec(F − BT AS)T C−1 vec(F − BT AS) 1 . (14) exp − l(A) = 2 (2π)nm/2 |C|1/2 The value of A maximizing l(A) is the desired estimate. Taking the logarithm of this function, changing its sign and discarding the terms that do not depend on A, the cost function J (A) to minimize is given by J (A) = vec(F − BT AS)T C−1 vec(F − BT AS).
(15)
Using the following properties of the Kronecker product [17] tr{PT QRTT } = vec(P)T (T ⊗ Q)vec(R),
(U ⊗ V)−1 = U−1 ⊗ V−1 ,
when U and V are nonsingular matrices, the function J (A) can be rearranged as −1/2 −1/2 2 T −1 J (A) = tr (F − BT AS)T C−1 = Cm (F − BT AS)Cn F , m (F − B AS)Cn
(16)
R−1/2
where represents a square root of R, its inverse, and · F is the Frobenius norm. The functional J (A) of (16) looks similar to the cost function in [10]. However, (16) is formed with double difference phase measurements. Moreover, (16) has optimal weighting matrices chosen according to the maximum likelihood criteria as opposed to [10] where an ad-hoc value is used that facilitates computations but loses optimality except for special array configurations. This minimization problem appears frequently in data matching contexts and is called Penrose regression or weighted orthogonal Procrustes problem, see [15,18], for example. Unlike the classical orthogonal Procrustes problem which has a unique closed-form solution via the singular value decomposition [19], the WOPP can have more than one local minimum, as discussed at the end of Section 4, and only numerical methods are known to solve it. A relatively new method to deal with this kind of nonlinear optimization problems over manifolds is known to use geometric differential tools. A general approach to the subject can be found in [20]. In particular, this technique was applied to attitude estimation with simple differences in [21,22]. R1/2
4. Optimization in SO(3) The special cost function (16) to be minimized has the particular feature that the solution must belong to SO(3), i.e., it must satisfy (AAT = I = AT A) with no reflections (det(A) = 1). Standard, unrestricted minimization techniques should not be applied since they can attain solutions that are not rotations. This is the main motivation to devise our own optimization algorithm in SO(3). A steepest descent method [23] starts from an initial rotation matrix A0 , calculates the gradient of J (A) at A0 denoted ∇J (A0 ), and moves on SO(3) along this direction ∇J (A0 ) until a minimum of J (A) is found at A1 , i.e., J (A1 ) J (A0 ). This step is repeated computing ∇J (A1 ), finding a minimum of J (A) moving on SO(3) along ∇J (A1 ), say at A2 with J (A2 ) J (A1 ). The procedure continues repeating these steps until a stopping rule is satisfied.
1094
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
This steepest descent method is chosen because (i) the gradient ∇J (A) can be readily computed and (ii) moving along this direction restricted to always stay on SO(3) until a minimum is found, is easily done as explained below. These features derive from the geometric structure of SO(3) which we briefly review in Section 4.1. 4.1. Geometry of the manifold The set of matrices SO(3) is not Euclidean but it has the structure of a manifold, thus it has the property of being locally Euclidean and allows defining a tangent plane in each point of the manifold [2,24]. The curved geometry of the group implies that the sum of two elements does not belong to the set. Moreover, SO(3) is a group whose group operation is the matrix product and whose identity element is the 3 × 3 identity matrix I. It is also a Lie group, and therefore it is parallelizable, i.e., the tangent plane at each point can be defined as a rotated version of the tangent plane in the identity element. The tangent space at the identity point I is denoted TI (SO(3)) and defined by the 3 × 3 skew-symmetric matrices. An orthogonal base for TI (SO(3)) is given by 0 0 0 0 0 1 0 −1 0 Y2,I = 0 0 0 , Y3,I = 1 0 0 . Y1,I = 0 0 −1 , 0 1 0 −1 0 0 0 0 0 Let Ya , Yb ∈ TI (SO(3)) be elements of the tangent space, a metric is defined as Ya , Yb = tr{Ya T · Yb }. Notice that the induced norm is the Frobenius norm. The translation of any element Yi,I on the tangent plane at I to the tangent plane at A ∈ SO(3) is obtained by simply pre-multiplying by the rotation A, Yi,A = AYi,I ∈ TA (SO(3)). A curve, in fact a 1-parameter subgroup, θ → ξ(θ ) ∈ SO(3) can be generated such that (i) its velocity vector dξ(θ )/dθ |θ=0 = Y ∈ TI (SO(3)) and (ii) for a θ > 0 such that ξ(θ ) = A, dξ(θ )/dθ = AY ∈ TA (SO(3)). This curve is built with the help of the so-called exponential map eθY and is the locus of the rotation matrices “parallel” to, or in the direction of Y. As an example, suppose the rotations in R3 about the x-axis are to be described. Then, choose Y = Y1,I and let 1 0 0 θY1,I = 0 cos(θ ) − sin(θ ) = A, ξ(θ ) = e 0 sin(θ ) cos(θ ) It is easily checked that ξ(θ ) satisfies the properties (i) and (ii) required above. The curve ξ(θ ) describes all the rotations about the x-axis and the parameter θ ∈ [0, 2π) denotes the size of the rotation. The example generalizes to any rotation about a given vector. Indeed, suppose that n = [n1 n2 n3 ] denotes a unit vector and θ ∈ [0, 2π), if Y = θ (n1 Y1,I + n2 Y2,I + n3 Y3,I )
(17)
then A = represents a rotation of angle θ about the axis defined by n. This is also seen by noting that the matrix exponential of a skew symmetric matrix is a rotation matrix. Computationally, the evaluation of these matrix exponentials is not complex since they can be expressed as a 2-degree matrix polynomial as is shown in Appendix A. eY
4.2. Steepest descent algorithm The key point of this algorithm is to calculate the gradient of the functional to be minimized and to move on the manifold in the direction it defines. This step is done calculating the curve ξ(θ ) as in Section 4.1 and finding the value of θ that minimizes the functional. A formula for the gradient of the cost function is derived first. Evaluating the functional at a point slightly off A0 ∈ SO(3) is a function of the perturbation, J (A0 + x1 Y1,A0 + x2 Y2,A0 + x3 Y3,A0 ) = J A0 (I + x1 Y1,I + x2 Y2,I + x3 Y3,I ) = J (x1 , x2 , x3 ) = J (x). (18) The gradient of J evaluated at the point A0 is found differentiating in the three variables,
∂J ∂J T ∂J ∇J (A0 ) = = [h1 h2 h3 ]T = h. ∂x1 ∂x2 ∂x3 x=0
(19)
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
The required derivatives can be calculated analytically since J (A) is a quadratic equation, resulting in ⎡ T −1 T ⎤ tr{YT1,I AT BC−1 m (F − B AS)Cn S } ⎢ T −1 T ⎥ ∇J (A0 ) = −2 ⎣ tr{YT2,I AT BC−1 m (F − B AS)Cn S } ⎦ .
1095
(20)
T −1 T tr{YT3,I AT BC−1 m (F − B AS)Cn S } A curve is generated from the gradient at an initial point A0 , taking Y = (h1 Y1,I + h2 Y2,I + h3 Y3,I )/|J (A0 )|, where h/|J (A0 )| is the unitary vector which indicates the direction of the curve (or the axis of the rotation). The cost function, written as J (ξ(θ )) = J¯(θ ), is one-dimensional and is easily minimized by conventional methods. The evaluation of the (1D) function J¯(θ ) is made simpler expressing it as a 2-degree real trigonometric polynomial as shown in Appendix B. Once the minimum is found at θ0 , A1 = ξ(θ0 ), and the scheme is repeated until the θi step is as small as needed. The evolution of the estimation as the algorithm is iterated, can be interpreted using Eq. (17). At each step, the gradient indicates the axis of rotation which produces the largest change of J (A). Then, the 1D minimization indicates how much to rotate about this axis with θi . A global minimum is guaranteed to exist because the cost functional is continuous and SO(3) is a compact set. Moreover, SO(3) is a Riemannian manifold and Eq. (16) is a distance and hence, a Morse–Bott function [20]. Then, the convergence to a single (isolated) critical point is guaranteed [20], although it is not necessarily the global minimum. Actually, very recent results in [25] bound the number of local minima of the weighted orthogonal Procrustes problem to less than 2m , where m is the number of double-differences; prove that there exists a global minimum; and characterize the set of local minima.
5. Experimental results 5.1. Hardware and data processing The attitude determination system was implemented using an independent standard (nondedicated) GPS receiver for each antenna of the array. Data from the receivers is analyzed off-line on a personal computer (PC). This configuration was proved to be useful for attitude determination in [26] and continues being used [33]. It leads to a cheaper and less technologically demanding solution than utilizing dedicated GPS receivers. A four-antenna symmetric plane array was built as shown in Fig. 3. Relatively large ground-planes were used to mitigate multipath effects. The phase measurements of the four GPS receivers were stored simultaneously in a PC to form the double-differences afterwards, as it was explained in Section 2. Since our receivers do not measure the phases at the same time they had to be extrapolated to a common instant before calculating the differences. This extrapolation, similar to the case of pseudo-ranges for position calculation [9], is accomplished in a linear way, using the carrier frequency measurements provided by GPS receivers. For further details of this part of our approach see [27]. The basic assumption for ambiguity resolution is that ambiguities remain constant during the observation period. Another important pre-processing task is the cycle-slip and outlier correction. A cycle-slip occurs when the tracking j loop shifts from an equilibrium to a neighboring one. This causes a change of cycle-ambiguities Nα in (1) to another integer value. Typically, a Costas loop, insensitive to phase changes of 180◦ , is used to demodulate the 50 bps data of the GPS signal and therefore the slips are multiples of half cycles. In addition, other sources of error can randomly
Fig. 3. Four-antenna array.
1096
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
Fig. 4. Summary of data processing tasks.
generate outliers that have to be detected and properly discarded. Our correction method is based on a Kalman filter predicting the evolution of the double-differences of phase and comparing these values with the observed ones. It produces a certain degree of filtering of the double-differences of phase which reduces the variance of the observables but has to be redesigned for medium and high dynamics mobiles. The procedure is based on [28] and the details are fully explained in [30]. Once the double-differences are formed and cycle-slips and outliers are corrected, the ambiguity resolution can be faced. There exist different methods to this end [10]. We use here a method based on least square errors proposed in [31], adapted to work with double-differences and taking into account the statistical correlation between them, i0 , α = 1, . . . , m and as shown in Section 2. The determination of the set of double-difference ambiguities (∇ Nα0 i = 1, . . . , n) involves three statistical tests [29] and the details are given in [32]. When the ambiguities are known, the attitude of the array for each measurement instant is estimated finding the minimum of (16) by means of the algorithm explained in Section 4. A block diagram summarizing the data processing tasks performed to achieve attitude estimation is given in Fig. 4. In order to reduce the computational burden, we assumed that the phase variances of different satellites were equal. In this case, the common variance value becomes just an irrelevant scale factor of the cost function (16), which still gives the ML solution. Therefore, normalizing the cost function by this value, the weighting matrices of (12) become Cm = Im + Im ITm , Cn = In + In ITn . (21) 5.2. Results Data were collected during approximately one hour for different attitudes of the array and processed as explained before. In order to facilitate the test, only changes of yaw angle are shown. We emphasize that the algorithm also estimates pitch and roll. The algorithm was implemented using a Newton–Raphson like (1D) optimization, initialized at t = 0 with the identity matrix and stopped when θi < 0.0001 rad. For t > 0 the initialization used was the previous estimate of the rotation matrix. Under these conditions, we empirically observed that the number of gradient searches was lower than three and the number of Newton–Raphson steps per gradient was always lower than six. The results obtained with the present algorithm are compared to manual measurements of the yaw angle which also have some error. These measurements were obtained using a measuring tape, a laser beam to extend the baselines in order to reduce the measurement errors, and some trigonometry. Applying error propagation to the angle formula it was found that these manually measured angles were known within a ±0.5◦ error band. Table 1 summarizes the estimated angles for different time instants, using the weighting matrices of Eq. (21). In all cases the estimated values were inside the interval of one degree established by the comparison method. Moreover, the standard deviation of the estimates did not exceed 0.1◦ . In Fig. 5 a sequence of yaw estimates is shown when the measured yaw of the array was 60.5◦ ± 0.5◦ . As seen in the table, the results are always within the comparison interval. A second set of estimates is obtained in the same way, but calculating the cost functional (16) without the proper weighting matrices. The cost functional minimized in each case is
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
1097
Table 1 Yaw estimation from measured GPS data Contrast measure
Yaw estimation (◦ ) Min
Mean
Max
σ
15.3 ± 0.5 29.1 ± 0.5 41.1 ± 0.5 60.5 ± 0.5 71.6 ± 0.5 89.7 ± 0.5
14.9 28.8 41.0 60.4 71.3 89.5
14.83 28.74 40.88 60.35 71.17 89.48
15.13 28.85 41.23 60.58 71.49 89.55
0.1 0.04 0.1 0.06 0.1 0.02
Fig. 5. Estimation results for yaw equal to 60.5◦ .
−1/2 −1/2 2 Case 1: J (A) = Cm (F − BT AS)Cn F ,
2 Case 2: J (A) = F − BT AS . F
Case 2 corresponds to the simplest choice of nonoptimal weighting matrices. Choosing weights as in [10] is nonoptimal but permits a simple solution in terms of singular values. However, it is either not applicable to a planar array configuration as used here because B is singular or it is impractical because the weighting factor becomes time varying due to S. As expected, the use of inadequate weighting matrices of Case 2 produces biased estimates, which in this particular case is of 2◦ , approximately. The standard deviation seems to be unaffected. Empirical estimates of the phase variances from the measured data could produce more precise values for the weighting matrices. 6. Conclusions The maximum likelihood criterion for the attitude matrix estimation from double-difference phase measurements of GPS signals was considered in this paper. Double-differences were chosen because they allow the use of independent, nondedicated GPS receivers. The ML criterion leads to a cost function similar to others used in previous works [10,11]. However, our derivation of the ML estimator for double-differences of phase shows the proper values for the weighting matrices in (16). A simple but realistic noise model based on the GPS receiver operation, allowed us to compute the double-differences of phase covariance matrix and from it, the weighting matrices. Minimization of this kind of cost functional is a nonlinear optimization task called the weighted orthogonal Procrustes problem, with no known closed-form solution. Thus, a new steepest descent scheme was devised based on the properties of Lie groups, that guarantees that the solution always stays in SO(3). The experimental results confirm that the proposed scheme improves the estimation quality producing unbiased estimates of the attitude, as opposed to the case of non-proper weighting in [10,11]. The present approach also allows the use of an arbitrary structure array of antennas preserving optimality.
1098
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
It remains to compare with other approaches in terms of computational cost and importance for real time and dynamic attitude applications. A comparison in terms of accuracy can be done by means of the Cramér–Rao bound (CRB) [34]. However, a new form of the CRB has to be developed, along the lines of [35], since the estimated variable is constrained to SO(3). Work in these directions is currently under way by some of the authors. Appendix A The Lemma in this appendix shows that the exponential of a 3 × 3 skew-symmetric matrix is easily calculated. Lemma. Let Y be a skew-symmetric matrix given by 0 −c b Y= c 0 −a . −b a 0 Then 1 − cos( v ) sin( v ) Y2 , Y+ eY = I + v v 2 √ where v = [a, b, c] and v = a 2 + b2 + c2 .
(A.1)
(A.2)
Proof. The matrix exponential eY can be written as a formal series replacing x = Y in the Taylor series of ex . Using the Cayley–Hamilton theorem [36] the n × n matrix Yn can be written as a polynomial of degree n − 1. Then, the convergent series for the exponential reduces to a polynomial h(·) of degree (n − 1) in Y. The coefficients of the matrix polynomial h(Y) satisfy h(A) = eY . Y in (A.1) is a 3 × 3 matrix, so eY will be equated to a 2-degree polynomial h(Y) = β0 I + β1 Y + β2 Y2 . In order to solve for the coefficients βi , i = 0, 1, 2, we use Theorem 3.5 in [36, p. 64] obtaining h(λi ) = β0 + β1 λi + β2 λ2i = eλi ,
i = 1, 2, 3,
(A.3)
where λi , i = 1, 2, 3, is the ith root of the characteristic polynomial of Y, pY (λ) = det(λI − Y). It is easy to see from (A.1) that pY (λ) = λ(λ2 + a 2 + b2 + c2 ), so its roots are given by λ1 = 0, λ2 = +j v , and λ3 = −j v . Thus, solving (A.3) we get β0 = 1, β1 = sin( v )/ v , and β2 = (1 − cos( v ))/ v 2 for the coefficients, and finally 1 − cos( v ) sin( v ) Y e = h(Y) = I + Y2 . Y+ 2 (A.4) v v 2 Appendix B Formula (B.2) is derived to evaluate the functional (16). It shows that the computational cost of calculating the functional is the same as evaluating the second order trigonometric polynomial (B.2). The cost function (16) evaluated at the flow A = ξ(θ ) = A0 eθYi,I is T T θYi,I (B.1) S C−1 J¯(θ ) = tr F − BT A0 eθYi,I S C−1 m F − B A0 e n . Then, expressing the matrix exponential as in (A.2) with v = θ [h1 , h2 , h3 ]/|∇J (A0 )| and v = |θ | we get eθYi,I = I + sin(θ )Yi,I + (1 − cos(θ ))Y2i,I , which replaced in (B.1) gives J¯(θ ) = a sin(θ ) + b cos(θ ) + c sin(2θ ) + d cos(2θ ) + e, where
(B.2)
a = 2 tr Yi,I AT0 F1 − tr Yi,I AT0 B1 A0 S1 − tr Yi,I AT0 B1 A0 Y2i,I S1 , b = 2 tr{Y2i,I AT0 F1 − tr Y2i,I AT0 B1 A0 S1 − tr Y2i,I AT0 B1 A0 Y2i,I S1 , 1 c = tr Yi,I AT0 B1 A0 Y2i,I S1 , d = tr Y2i,I AT0 B1 A0 Y2i,I S1 + tr Yi,I AT0 B1 A0 Yi,I S1 2 −1 −1 T −1 T T with F1 = BCm FCn S , S1 = SCn S , B1 = BC−1 m B , and e an irrelevant constant with respect to the minimization because it does not depend on θ . Notice that the coefficients (B.2) are calculated only once per descent direction.
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
1099
References [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] [24] [25] [26] [27] [28] [29] [30] [31] [32] [33] [34] [35] [36]
M.D. Shuster, A survey of attitude representations, J. Astronaut. Sci. 41 (4) (1993) 439–517. T. Frankel, The Geometry of Physics, Cambridge Univ. Press, Cambridge, 1997. J.R. Wertz, Spacecraft Attitude Determination and Control, Reidel, Dordrecht, 1986. G. Wahba, A least squares estimate of satellite attitude, Problem 65.1, SIAM Rev. 7 (3) (1966) 409. M.D. Shuster, Three-axis attitude determination from vector observations, Amer. Inst. Aeronaut. Astronaut. AIAA-81-4003 4 (1981) 70–77. F.L. Markley, Attitude determination using vector observations and the singular value decomposition, J. Astronaut. Sci. 36 (3) (1988) 245–258. F.L. Markley, Attitude determination using vector observations: A fast optimal matrix algorithm, J. Astronaut. Sci. 41 (2) (1993) 261–280. M.D. Shuster, Maximum likelihood estimator of spacecraft attitude, J. Astronaut. Sci. 37 (1) (1989) 79–88. E.D. Kaplan, Understanding GPS: Principles and Applications, Artech House, Norwood, MA, 1996. C.E. Cohen, Attitude determination using GPS, Ph.D. dissertation, Stanford University, 1992. J.L. Crassidis, F.L. Markley, Attitude determination using global positioning system signals, Amer. Inst. Aeronaut. Astronaut. AIAA-97-3452 (1997) 23–31. I.Y. Bar-Itzhack, P.Y. Montgomery, J.C. Garrick, Algorithms for attitude determination using GPS, in: Proceedings of the 37th Israel Annual Conference on Aerospace Sciences, Tel-Aviv and Haifa, Israel, 1997, pp. 233–244. J.C. Juang, G.S. Huang, Development of GPS based attitude determination algorithms, IEEE Trans. Aerospace Electron. Syst. 33 (3) (1997) 968–976. R.A. Brown, Instantaneous GPS attitude determination, IEEE Aerospace Electron. Mag. 7 (6) (1997) 3–8. T. Bell, GPS-based attitude determination and the orthogonal Procrustes problem, J. Guid. Control Dynam. 26 (5) (2003) 820–822. M.K. Simon, J.K. Omura, R.A. Scholtz, B.K. Levitt, Spread Spectrum Communications, Computer Science Press, Rockville, MD, 1985. H.V. Henderson, S.R. Searle, Vec and vech operators for matrices, with some uses in Jacobians and multivariate statistics, Can. J. Statist. 7 (1) (1979) 65–81. M.T. Chu, N.T. Trendafilov, The orthogonally constrained regression revisited, J. Comput. Graph. Statist. 10 (4) (2001) 746–771. G.H. Golub, C.F. Van Loan, Matrix Computations, second ed., Johns Hopkins Univ. Press, Baltimore, 1991. U. Helmke, J.B. Moore, Optimization and Dynamical Systems, Springer-Verlag, New York, 1994. F.C. Park, J. Kim, C. Kee, Geometric descent algorithms for attitude determination using GPS, in: Proceedings of the 14th Wold Congress of IFAC, Beijing, China, 1999. P.A. Roncagliolo, J.A. Areta, D.R. Fuhrmann, C.H. Muravchik, Attitude estimation with GPS: Steepest descent algorithm on SO(3), in: Proceedings of the IEEE Second South American Workshop on Circuits and Systems, Río de Janeiro (Brasil)–Buenos Aires (Argentina), 2001. D.G. Luenberger, Linear and Nonlinear Programming, second ed., Kluwer Academic, Dordrecht/Norwell, MA, 2003. A. Srivastava, M.I. Miller, U. Grenander, Ergodic algorithms on special Euclidean groups for ATR, in: Systems and Control in the Twenty-First Century: Progress in Systems and Control, vol. 22, Birkhäuser, Boston, MA, 1997. T. Viklands, Algorithms for the weighted orthogonal Procrustes problem and other least squares problems, Ph.D. dissertation, Department of Computer Science, Umea University, Sweden, 2005. G. Lu, M.E. Cannon, G. Lachapelle, P. Kielland, Attitude determination using dedicated and nondedicated multiantenna GPS sensors, IEEE Trans. Aerospace Electron. Syst. 30 (4) (1994) 1053–1058. J.G. García, P.I. Mercader, C.H. Muravchik, Use of GPS carrier phase double differences, Latin Amer. Appl. Res. 35 (2) (2005) 115–120. P.Z. Jia, L.D. Wu, An algorithm for detecting and estimating cycle slips in single frequency GPS, Chin. Astronaut. Astrophys. 25 (4) (2001) 515–521. G. Lu, Development of a GPS multi-antenna system for attitude determination, Ph.D. dissertation, University of Calgary, 1995. P.I. Mercader, J.G. García and C.H. Muravchik, Detección y corrección de saltos de ciclos de fase para estimación de orientación con GPS, in: Proceedings of the XIX Congreso Argentino de Control Automático, Buenos Aires, Argentina, 2004. R. Hatch, Instantaneous ambiguity resolution, in: Proceedings of the IAG Symposium No. 107 on Kinematic Systems in Geodesy, Surveying and Remote Sensing, Springer-Verlag, New York, 1991. J.G. García, P.I. Mercader, C.H. Muravchik, Resolución de la ambigüedad de fase para estimación de orientación con GPS, in: Proceedings of the XIX Congreso Argentino de Control Automático, Buenos Aires, Argentina, 2004. D.S. De Lorenzo, S. Alban, J. Gautier, P. Enge, D. Akos, GPS Attitude determination for a JPALS testbed: Integer initialization and testing, in: IEEE Position Location and Navigation Symposium, Monterey, CA, 2004. H.L. van Trees, Detection, Estimation and Modulation Theory, Wiley, New York, 1968. H. Hendriks, A Cramér–Rao type lower bound for estimators with values in a manifold, J. Multivar. Anal. 38 (1991) 245–261. C.T. Chen, Linear System Theory and Design, second ed., Oxford Univ. Press, New York, 1999.
Pedro A. Roncagliolo received the Electronics Engineer degree from the National University of La Plata (UNLP), Argentina, in 2001. He is currently a Ph.D. student and Professor in the UNLP, Argentina. His research interests are in statistical signal processing with applications to wireless communications and global positioning system (GPS). Javier G. García received the Electronics Engineer degree from the National University of La Plata (UNLP), Argentina, in 2003. He is currently a Ph.D. student and a TA in the UNLP, Argentina. His research interests are in statistical signal processing with applications to global positioning system (GPS) and digital communications.
1100
P.A. Roncagliolo et al. / Digital Signal Processing 17 (2007) 1089–1100
Pablo I. Mercader received the Electronics Engineer degree from the National University of La Plata (UNLP), Argentina, in 2003. He is currently employed by NOKIA as Core Network Engineer in Latin America. He has previously worked with statistical signal processing in applications to global positioning system (GPS). Daniel R. Fuhrmann received the Ph.D. degree from Princeton University in 1984. Since that time he has been with the Department of Electrical Engineering, Washington University, St. Louis, where he is currently a Professor. In the 2000–2001 academic year he was a Fulbright Scholar visiting at UNLP, Argentina. His research interests are in various areas of statistical signal and image processing, including sensor array signal processing, radar systems, space–time adaptive processing, and image processing for genomics applications. Carlos H. Muravchik received the Electronics Engineer degree from the National University of La Plata (UNLP), Argentina, in 1973, the M.Sc. degree in statistics in 1983, and the M.Sc. and Ph.D. degrees in electrical engineering in 1980 and 1983, respectively, all from Stanford University, Stanford, CA, USA. He is currently a Professor in the UNLP, Argentina. His research interests are in statistical signal and array processing with biomedical, control and communications applications, and nonlinear control systems.