Maximum Likelihood Attitude and Position Estimation from Pseudo-Range Measurements Using Geometric Descent Optimization

Maximum Likelihood Attitude and Position Estimation from Pseudo-Range Measurements using Geometric Descent Optimization

A. Alcocer, P. Oliveira, A. Pascoal, and J. Xavier

Landmark Abstract— This paper addresses the problem of estimating j Beacon i, bias ci the attitude and the position of a rigid body when the available measurements consist only of pseudo-ranges between a set of body fixed beacons and a set of earth fixed landmarks. To this dij effect, a Maximum Likelihood (ML) estimator is formulated as an optimization problem on the parameter space Θ = b Rp i SE(3) × corresponding to the attitude and position of the pj rigid body as well as a set of biases present in the pseudo-range equations. Borrowing tools from optimization on Riemannian {B} p manifolds, intrinsic gradient and Newton-like algorithms are derived to solve the problem. The rigorous mathematical setup adopted makes the algorithms conceptually simple and elegant; furthermore, the algorithms do not require the artificial normalization procedures that are recurrent in other {I} R estimation schemes formulated in Euclidean space. Supported by recent results on performance bounds for estimators on Riemannian manifolds, the Intrinsic Variance Lower Bound Fig. 1. Geometry of estimation problem. (IVLB) is derived for the problem at hand. Simulation results are presented to illustrate the estimator performance and to validate the tightness of the IVLB in a wide range of signal to noise ratio scenarios. It is also common to find in the literature work addressing Index Terms— Navigation systems, attitude/positioning systems, maximum likelihood estimation, optimization on Rieman- the problem of estimating the attitude (that is the relative nian manifolds orientation of a reference frame with respect to another reference frame) with vector observations [2],[3]. In some I. INTRODUCTION applications, vector observations can be obtained from range Joint attitude and position estimation systems based on observations by making the planar waveform appoximation pseudo-range measurements are becoming popular and have [4],[5]. Despite the fact that the attitude and positioning received the attention of the research community as an alter- problems are strongly coupled, a simultaneous treatment of native to more complex, expensive, and sophisticated Inertial the problem is seldom encountered (see [6] for an example Navigation Systems. An advantage of such systems is that based on line of sight measurements). they are drift-less and insensible to magnetic disturbances. This paper addresses the problem of simultaneous attitude Examples of applications include multiple GPS receiver and position estimation with pseudo-range measurements. systems, indoor wireless network positioning systems, and To this effect, a Maximum Likelihood (ML) estimator is acoustic systems to determine the attitude/position of a body formulated by solving an optimization problem on a param- underwater. Range measurements are usually obtained by eter space containing the attitude and the position of the measuring the time it takes an electromagnetic or acoustic rigid body as well as a set of bias terms present in the signal to travel between an emitter and a receiver given pseudo-range equations. Intrinsic gradient and Newton-like that the speed of propagation of the signals is assumed algorithms are derived borrowing tools from optimization to be known. Pseudo-range measurements arise when the on Riemannian manifolds. The rigorous mathematical set- emitter and receiver are not synchronized, resulting in an up adopted makes the algorithms conceptually simple and unknown bias term in the range measurements. The range elegant. Furthermore, the algorithms do not require artificial only positioning problem has been extensively treated in the normalization procedures that are recurrent in other estima- literature, see for example [1] and the references therein. tion schemes formulated in Euclidean space.

The authors are with Instituto Superior Técnico, Institute for Systems II. PROBLEM FORMULATION and Robotics, Av. Rovisco Pais 1, 1049-001 Lisbon, Portugal. E-mail: Suppose that one is interested in estimating the configura- {alexblau,pjcro,antonio,jxavier}@isr.ist.utl.pt This work was partially supported by Fundação para a Ciência e a tion (that is, position and attitude) of a rigid body in space. Tecnologia (ISR-IST plurianual funding) through the POS-Conhecimento Define a reference frame attached to the rigid body and Program that includes FEDER funds and by projects RUMOS of the an Inertial reference frame{B} . The position of the origin FCT, MAYA of the AdI, and IST-GREX (Contract No. 035223) of the of with respect to {I}can be represented by vector Commission of the European Communities. The work of the first author {B} {I} was supported by a PhD Scholarship from the FCT. p R3, and the relative orientation between and ∈ {B} {I} can be represented by a rotation matrix SO(3), where body beacons can suffer from highly correlated disturbances. SO(3) is the Special Orthogonal group definedR ∈ as This might be due to the fact that the observations originate from signals that have travelled almost through the same SO(3) = R3×3 : T = I , det( ) = 1 . (1) R ∈ R R 3 R propagation channel. In these cases the covariance matrix R is close to being block diagonal. In the above expression I3 stands for the 3 3 identity matrix and det( ) is the matrix determinant operator.× The In order to determine the attitude and the position of the configuration space· of the rigid body can then be identified rigid body from the pseudo-range measurements, the vector p R3 of unknown biases c R needs also to be estimated. It is with the Special Euclidean group SE(3) = SO(3) (as ∈ a set). Elements of SE(3) will be identified with× the pair common in the literature to avoid estimating the biases by T ( , p) or with the vector vec( )T pT (where vec( ) is subtracting pseudo-range equations among them to obtain theR operator that stacks the columnsR of a matrix from left· to range-differences. This approach will not be pursued in this right) depending on what representation is more suitable for paper. Instead, an augmented parameter space Θ will be the computations. considered containing the unknown bias terms. The final parameter space will be defined as the Cartesian product Suppose the rigid body has p beacons and assume that Rp R3 Rp the location of the beacons with respect to is known. Θ = SE(3) = SO(3) . {B} × × × The beacons could be GPS antennas, or underwater acoustic A. ML Estimator Formulation transponders, arranged with a certain known geometry in the The Maximum Likelihood (ML) Estimator determines rigid body. Let us further consider that there are m fixed the triad ( , p, c) Θ that maximizes the likelihood landmarks distributed in the ambient space with known posi- ML function, thatR is, the probability∈ p (y , p, c) of obtaining tions: for example, GPS satellites or surface buoys equipped |R the observationsb y given the parameters ( , p, c) [7] [8]. Ac- with hydrophones. Let b R3, i 1,...,p denote the b b i cording to the Gaussianity assumption onR the disturbances, positions of the p beacons in∈ the rigid∈ body { expressed} in the ML estimator can be found by solving the optimization and let p R3,j 1,...,m denote the positions of{B} the j problem m landmarks∈ expressed∈ { in }(see Fig. 1). {I} Let dij denote the distance between the i’th beacon and , p, c = arg min f( , p, c) (6) the j’th landmark. Let c R; i 1, ,p be an unknown R ML (R,p,c)∈ Θ R i beacon dependent bias term.∈ Then∈ { · the · · pseudo-ranges} are R where f :Θb b bis given by defined as → 1 f( , p, c) = (y ρ)T R−1(y ρ) (7) ρij = dij + ci = bi + p pj + ci (2) R 2 − − kR − k and, from (2), ρ = ρ( , p, c). Note that the cost function f with i 1,...,p , j 1,...,m , and stands is not differentiable whenR some of the ranges d vanish, that for the∈ Euclidean { norm.} The∈ { observations} y ,k defined · k by ij ij is, when the position of a beacon coincides with the position y = ρ + w consist of the pseudo-ranges ρ corrupted ij ij ij ij of a landmark. It is realistic to assume that this situation by an additive zero mean Gaussian disturbances w . Note ij never occurs in practice. that the terms beacon and landmark are used to illustrate the problem but they do not impose any particular role (in III. OPTIMIZATION ON RIEMANNIAN terms of emitter/receiver). They should be simply viewed as SUBMANIFOLDS pseudo-range measuring devices. In order to determine the ML Estimator, the function It is convenient to stack all the ij elements in a more f needs to be minimized over the parameter space Θ. At compact form by defining the vectors this point, it is not obvious how to continue and effectively T mp solve this constrained optimization problem. The problem d , [d dp ] [d m dpm] R , (3) 11 · · · 1 · · · 1 · · · ∈ does not admit a closed form solution so it is necessary to h i resort to an iterative scheme. There has been some work and ρ, y, w Rmp in a similar way. Define also a vector T on generalizing the classic gradient descent and Newton c = [c ∈c ] Rp containing all the bias terms. With 1 p methods to Riemannian manifolds [9] [10]. The key idea this arrangement,· · · the∈ observations can then be written in a involved in [9] is to generalize the classic gradient and more compact form as Newton-like directions and to perform line searches along ρ = d + (1m Ip)c (4) geodesics, as depicted in Fig. 2. ⊗ y = ρ + w, E wwT , R Rmp×mp (5) In many cases, such as the one considered in this paper, ∈ the parameter space Θ can be shown to be an embedded where denotes the Kronecker product of matrices, 1m is Riemannian submanifold of some ambient Euclidean space a m ⊗1 vector of ones, and R is the covariance matrix Rn. For instance, when Θ is characterized as the regular level of w×. Note that no assumption is made on the structure set of some smooth function i.e., Θ = x Rn : h(x) = 0 . of R, thus allowing for a number of interesting mission It turns out that many intrinsic objects{ in Θ∈, such as intrinsic} scenarios and configurations. For instance, the measurements gradients and Hessians, can be easily obtained from the of the distances between an Earth fixed landmark and all the more familiar corresponding extrinsic objects in Rn. In what Riemannian submanifold of the ambient Euclidean space Dk × θk R3 3 R3 Rp. There are many references in the literature γ k(t) about×SE(3)× and SO(3), the reader is referred to [15], [16], [17], and [18]. θ +1 k Let θ = ( , p, c) be an element of Θ. The tangent space Θ R TθΘ can be identified with the linear subspace 3×3 3 p Fig. 2. Geometric descent optimization. Intrinsic gradient or Newton ( S, v, u) R R R : S K(3, R) , (9) directions followed by geodesic line searches. R ∈ × × ∈ where K(3, R) stands for the set of 3 3 skew symmetric matrices with real entries (recall that× a matrix S is skew symmetric if S + ST = 0). Recall also that sometimes it follows it is assumed that the reader is familiar with the will be convenient to identify TθΘ with vectors of the form T concepts of Riemannian geometry [11], [12]. vec( S)T vT uT in R12+p. Let (Θ, g) be an embedded Riemannian submanifold of A RiemannianR metric g is a smooth assignment of an inner Rn Rn . The tangent space at θ Θ , denoted TθΘ, can be product to each tangent space. The parameter space Θ can ∈ ⊂ Rn identified with some linear subspace of . This means that be made an embedded Riemannian submanifold by providing the tangent vectors of Θ, which are usually quite abstract it with the canonical Riemannian metric inherited from its objects, can be identified with very familiar objects: vectors ambient Euclidean space. Let two tangent vectors ∆1, ∆2 in Rn. For each θ Θ Rn, the tangent space T Rn Rn ∈ θ TθΘ have the form can be split into the∈ direct⊂ sum T Θ N Θ where N≃Θ is θ θ θ vec(Ω ) vec(Ω ) the orthogonal complement of the linear⊕ subspace T Θ in 1 2 θ v v (10) Rn [12, p.125]. ∆1 = 1 , ∆2 = 2 .  u   u  Let f : Rn R be a smooth function and let f : 1 2 →     Θ R, be the restriction of f to Θ, that is, f = f . Then g(∆1, ∆2), also denoted ∆1, ∆2 , becomes → |Θ h i The extrinsic gradient of at is defined as the usual T e f θ ∆ , ∆ = ∆ ∆ . (11) vector of partial derivatives ∂ ∂ T . 1 2 1 2 f θe = [ ∂x1 f θ ∂xn f θe] h i Moreover, according to the∇ above| discussion,| · · the · extrinsic| Note that, when restricted to SE(3), the induced Riemannian gradient can be decomposed ase the sum ofe two vectors ine Rn metric of (11) is in fact equivalent to the scale-dependent belonging to TθΘ and NθΘ. The intrinsic gradient of f at θ, left-invariant metric of [17] (with c = 2, and d = 1). denoted gradf θ is a tangent vector in TθΘ. It provides a way With this choice of metric, the geodesics of Θ have a of locally approximating| functions and obtaining generalized simple closed form expression (see [17], [18] for a related (intrinsic) gradient descent search directions. The extrinsic discussion). Let θ = ( , p, c) Θ and ∆ = (Ω, v, u) R ∈ ∈ Hessian of f at θ is a map Hessf : Rn Rn R that can TθΘ. The geodesic emanating from θ in the direction of ∆ be obtained from the usual matrix of second× → order partial is given by γ : R Θ, → derivatives of f. The intrinsic Hessiane of f at θ is also a γ(t) = ( exp T Ωt , p + vt, c + ut) (12) map Hessf : TθΘ TθΘ R. R R × → The next propositionse summarize fairly well known re- where exp is the matrix exponential. Note that γ(0) = θ and sults, the proofs of which are straightforward exercises in γ˙ (0) = ∆. Riemannian geometry (see [13] or [14]): The second fundamental form, which will be used to Proposition 1 (Intrinsic Gradient): The intrinsic gradient determine the intrinsic Hessian, can also be simply computed (see [13] for details). Let ∆ , ∆ TθΘ be tangent vectors of f at θ, denoted gradf θ TθΘ, is exactly the projection 1 2 | ∈ at some point θ Θ as in (10).∈ The second fundamental of the extrinsic gradient f θ onto the tangent space TθΘ. ∈ ∇ | form II : TθΘ TθΘ NθΘ can be found to be Proposition 2 (Intrinsic Hessian): The intrinsic Hessian × → of the smooth function f ateθ satisfies II(∆ , ∆ ) = ( symm(ΩT Ω ), 0, 0) (13) 1 2 −R 1 2 Hessf(∆1, ∆2) = Hessf(∆1, ∆2) + II(∆1, ∆2), f θ where symm is the operator that extracts the symmetric part h ∇ | i 1 T (8) of a matrix, that is symm(A) = 2 (A + A ). e e for any tangent vectors ∆ , ∆ TθΘ. In the above expres- V. INTRINSIC GRADIENT AND NEWTON 1 2 ∈ sion, II : TθΘ TθΘ NθΘ is the second fundamental ALGORITHMS × → form, which relates the geometries of Θ and Rn [12]. In order to determine the maximum likelihood (ML) These propositions will provide a simple way of determining estimate of the attitude and position of a rigid body, a intrinsic gradient descent and Newton-like search directions constrained optimization problem on the parameter space for the problem at hand. Θ needs to be solved. The geometric descent optimization algorithms proposed in this paper are of iterative nature. IV. THE GEOMETRY OF THE PARAMETER SPACE At each iteration a gradient or Newton-like search direction The parameter space Θ was defined as the Cartesian k is determined. Then, an intrinsic geodesic line search is 3 p D product SO(3) R R . It will be regarded as an embedded performed along k to update the estimated parameter. × × D A. Intrinsic gradient and Newton directions k = N (Newton descent). A line search can be performed D According to Proposition 1, the intrinsic gradient of along the geodesic a smooth function defined on an embedded Riemannian T γk(t) = ( exp( Rt), p + pt, c + ct). (19) submanifold of some Euclidean space can be found by R R D D D determining the extrinsic gradient (which is the usual vector Ideally, the line search procedure aims at finding the optimal of partial derivatives as if the function were defined on the ∗ stepsize tk satisfying ambient Euclidean space f : R3×3 R3 Rp R, instead of × × → ∗ f :Θ R) and projecting it orthogonally onto the tangent tk = arg min f(γk(t)). (20) → t∈R space of the parameter space.e The derivation of the extrinsic gradient for the problem at hand is done in detail in [13]. This optimization subproblem can be hard to solve. Alterna- Let θ = ( , p, c) be the point in Θ at which to evaluate the tively, it is common to solve this problem only approximately extrinsic gradient.R Define the matrices using for instance the Armijo rule [19, p.29]. The Armijo mi rule selects tk = β s, where mi 0, 1, 2,... is the first bT (p p )T bT T + (p p )T ∈ { } 1 ⊗ − 1 1 R − 1 integer satisfying . .  .   .  mi mi T T T T T f(θk) f(γk(β s)) σβ s gradf θk , k (21) F = b (p pj) , C = b + (p pj ) , − ≥ − h | D i  i ⊗ −   i R −   .   .  for some constants s > 0, and β,σ (0, 1).  .   .  ∈  T T   T T T  b (p pm)  b + (p pm)   p ⊗ −   p R −  C. Algorithm implementations: an outline    (14) d . . . 0 The theoretical results of the previous sections lead to the 11 following implementations of the proposed intrinsic gradient . .. . D = diag(d) =  . . .  (15) and Newton algorithms: 0 ... d  mp   where F Rmp×9, C Rmp×3, and D Rmp×mp. Then p c the extrinsic∈ gradient has∈ the form ∈ 1) Start at initial estimate θ0 = ( 0, 0, 0) Θ. Set k = 0. R ∈ T −1 vec(GR) F D 2) Determine a search direction k: D , T −1 −1 f θ Gp = C D R (y ρ). (16) a) Intrinsic gradient: take k = gradf θk ∇ |   −  T  − D − | Gce (1m Ip) according to (18).   ⊗ e e   b) Intrinsic Newton: If N, f θk < 0 (descent According to Proposition 1 the intrinsic gradient can be h ∇ | i e condition) take k = N, otherwise take found by solving the projection problem D k = gradf θk . D − | 3) Line search along the geodesic (19), using gradf θ = arg min ∆ f θ, ∆ f θ . (17) γk(t) ∆∈ Tθ Θ | h − ∇ | − ∇ | i Armijo rule to determine step size tk. Using some algebraic manipulationse and the facte that ∆ = 4) update estimate θk+1 = γk(tk). Set k = k + 1. T T T T [vec( S) v u ] for some skewsymmetric matrix S and 5) If gradf θk ǫ, stop. Otherwise return to 2. k | k ≤ vectorsR v, u, it can be shown that (see [13] and also [9] for a related result): VI. SIMULATION RESULTS

1 T Simulations were performed to validate the proposed vec(GR) vec(GR GR ) 2 − R R algorithms. A simulation setup was chosen that is similar gradf θ , Gp = G . (18)  p  to a multi-antenna GPS attitude/positioning system. Four |  G  e e c Gc (m = 4) earth ﬁxed landmarks were located at positions    e  The Newton-like search direction is the unique tangent vector corresponding to a favorable GPS satellite constellation. That e is, N TθM satisfying Hessf(X, N) = X, gradf for ∈ −h i all tangent vectors X TθM (assuming that Hessf is 0.966 0 0.259 0 nonsingular at θ). It can∈ be found by determining the usual [p1 p2 p3 p4] = Rsat 0.483 0.837 0.259 0 matrix of second order partial derivatives and solving a −0.259 0.259 0.259 1 certain linear system of equations. Its derivation is omitted  (22) due to space constraints. A similar derivation is done in detail 7 in [13] and [14]. where Rsat = 2.656 10 m. Three (p = 3) body ﬁxed · beacons were located at positions [b1 b2 b3] = 5 I3 m. The B. Intrinsic geodesic line search 2 observation error covariance was set to R = σ Imp, with Let θk Θ be the parameter estimate at iteration k. σ = 0.1 m. In order to illustrate the attitude estimation errors ∈ Let k = ( R, p, c) Tθk Θ be the search direction (in Fig. 3 and Fig. 4) exponential coordinates for SO(3) D D D D ∈ T at iteration k, i.e, k = gradf θk (gradient descent) or were used. That is, the vector θ = [θ θ θ ] is used to D − | 1 2 3 Gradient descent: Estimation errors Newton descent: Estimation errors 4 2 θ1 2 θ1 θ2 0 θ2 0 θ3 θ3

−2 −2 Attitude [exp.coord] Attitude [exp.coord] 0 10 20 30 40 50 0 1 2 3 4 5 6 7 5 5 1 p 0 p1 2 0 p p2 −5 p3 p3 Position [m] Position [m] −5 −10 0 10 20 30 40 50 0 1 2 3 4 5 6 7 10 10 c1 c1 5 5 c2 c2 3 0 c3

0 c Bias [m] Bias [m]

−5 −5 0 10 20 30 40 50 0 1 2 3 4 5 6 7 Iteration k Iteration k

Fig. 3. Intrinsic gradient algorithm estimation errors: Attitude, Position Fig. 4. Intrinsic Newton algorithm estimation erros: Attitude, Position and and Bias. The attitude error is represented as the entries of vector θ˜, the Bias. ˜ ˆ T exponential coordinates of Rk = R R. The position and bias errors are ˜ = k ˜ ˜ = ˜ shown as the entries of vectors p p − pk and c c − ck, respectively. The algorithm has a very slow convergence. Gradient Newton f

0 −θ3 θ2 represent the orthogonal matrix = exp θ3 0 −θ1 . R −θ2 θ1 0 0 10 The actual and initial parameters were: 0 5 10 15 20 25 30 35 40 45 1 0 0 0.032 0.542 0.704 − = 0 1 0 , ˆ0 = 0.710 0.604 0.584 k

R   R  −  k 0 0 1 0.363 0.363 0.404 θ 0 | Gradient 10

− − f    (23) Newton grad

0.738 3.991 k −5 p = 0.358 , pˆ0 = 2.993 (24) 10  0.075 −3.299 0 5 10 15 20 25 30 35 40 45 − − Iteration k  160.331  164.155 Fig. 5. Intrinsic gradient and Newton algorithms: performance evaluation. c = −33.937 , cˆ = −24.403 (25)   0   Value of the cost function f (top) and norm of the intrinsic gradient 13.113 10.778 kgradf|θ k (bottom). − − k     Fig. 5 shows the evolution of the cost function together with the norm of the gradient for the intrinsic gradient and Newton methods. Fig. 3 and Fig. 4 show the evolution of the attitude, such a limitation [7]. When the state space is not Euclidean, position, and bias estimation errors for both methods. The as in our case, one should resort to generalizations of the gradient method shows a very poor (linear) convergence rate CRB as the Intrinsic Variance Lower Bound (IVLB) [20]. whereas the Newton method achieves quadratic convergence The IVLB uses the intrinsic (geodesic) Riemannian distance when close to the minimum. to quantify the estimation errors. More specifically, it sets a Note that the cost function has local minima that depend lower bound on the intrinsic variance of unbiased estimators. highly on the number and position of the beacons and Let θ be the true parameter and θ an estimate of it. The landmarks, as well as the intensity of the measurement noise. intrinsic variance is defined as This is an important issue, as well as on many optimization b 2 var θ = E dΘ(θ,θ) (26) problems, that prevents the methods from being global. Pre- n o n o liminary experiences suggest that local minimum solutions where dΘ( ) is the intrinsicb (geodesic)b distance function in can be easily detected as they yield final likelihoods some Θ. Considering· the canonical metric in (11) it becomes orders of magnitude different from the global minimum. 2 2 2 2 dΘ(θ ,θ ) = dSO ( , ) + p p + c c , 1 2 (3) R1 R2 k 1 − 2k k 1 − 2k VII.PERFORMANCEBOUNDS (27) where θ = ( , p , c ), θ = ( , p , c ), and Given a set of noisy pseudo-range measurements, there is 1 R1 1 1 1 R2 2 2 a fundamental limitation on the size of the estimation errors T tr 1 2 1 that can be achieved. When the state space is Euclidean, the dSO ( , ) = √2 arccos R R − (28) (3) R1 R2 2 Cramér-Rao Bound (CRB) is the classic tool to determine ! 1 10 ML results showed that the intrinsic gradient algorithm has a IVLB 0 slow convergence. However, the intrinsic Newton algorithm 10 converged in a few iterations and attained performances

−1 10 very close to the Intrinsic Variance Lower bound (IVLB). Hence, the intrinsic Newton algorithm is suitable for real

−2

} 10 time implementation. Future work will include the study of ˆ θ

{ nonlinear ﬁlters which take into account the dynamics of −3 var 10 the rigid body. Another topic of interesting research is the inclusion of other sensorial data such as bearing, Doppler, −4 10 and vision information. Testing the algorithms with real experimental data is also a subject that warrants further −5 10 efforts.

−6 10 EFERENCES 0 1 2 3 4 5 6 R 10 10 10 10 10 10 10 2 [1] J. O. Smith and J. S. Abel, “Closed-form least-squares source location SNR = 1/σ estimation from range-difference measurements,” IEEE Transactions Fig. 6. IVLB compared to Monte carlo Newton ML at different SNR on Acoustics, Speech, and Signal Processing, vol. ASSP-35 Issue: 12, scenarios. pp. 1661–1669, December 1987. [2] G. Wahba, “A Least-Squares Estimate of Spacecraft Attitude,” SIAM Review, Vol. 7, No.3, p. 409, July 1965. [3] A. Nadler, Y. Bar-Itzhack, and H. Weiss, “On Algorithms for Attitude is the canonical metric on SO(3) (as a submanifold of Rn×n) Estimation using GPS,” in Proceedings of the 2000 IEEE Conference on Decision and Control, Sidney, Australia, December 2000, pp. 3321– [17][15]. 3326. The (extrinsic) Fisher Information Matrix for the problem [4] X. Yu, K. Ling, and N. Nagaragan, “LEO Satellite Attitude Determi- at hand, that is used in the computation of the IVLB (see nation Based on GPS,” in Proceedings of the 1999 IEEE Conference on Decision and Control, Phoenix, Arizona USA, December 1999, pp. [20] and [14] for a similar derivation), can be computed as 1998–1992. [5] J. L. Crassidis and F. L. Markley, “New Algorithm for Attitude De- T I(θ) = Eθ ( θ log p(θ y)( θ log p(θ y) termination Using Global Positioning System Signals,” AIAA Journal ∇ | ∇ | of Guidance, Control, and Dynamics, Vol. 20, No. 5, pp. 891–896, T −1 T −1 T F D F D Sept.-Oct. 1997. = CT D−1 R−1 CT D−1 . (29) [6] J. L. Crassidis, R. Alonso, and J. L. Junkins, “Optimal Attitude  T   T  and Position Determination from Line-of-Sight Measurements,” The (1 Ip) (1 Ip) m ⊗ m ⊗ Journal of the Astronautical Sciences, Vol. 48, Nos. 2-3, pp. 391–408,     April-Sept. 2000. It can also be used as a measure of the observability of [7] H. L. V. Trees, Detection, Estimation, and Modulation Theory (Part 12+p×6+p the problem. Let Uθ R be a matrix whose I). John Wiley and Sons, 1968. ∈ columns form an orthonormal basis for TθΘ. If the matrix [8] S. M. Kay, Fundamentals of Statistical Signal Processing: Estimation T 6+p×6+p Theory. Upper Saddle River, New Jersey, USA: Prentice-Hall, 1993. U I(θ)Uθ R is singular or badly conditioned, θ ∈ [9] A. Edelman, T. A. Arias, and S. T. Smith, “The Geometry of this indicates that with the present estimation set up, in Algorithms with Orthogonality Constraints,” SIAM Journal on Matrix terms of the number of landmarks/beacons and their relative Analysis and Applications, vol. 20, no. 2, pp. 303–353, 1998. [10] J. Manton, “Optimization Algorithms Exploiting Unitary Constraints,” geometry, it is not possible to determine all of the unknown IEEE Trans. Signal Processing, 50(3):635–650, March 2002. parameters. [11] W. M. Boothby, An Introduction to Differentiable Manifolds and The IVLB for the same simulation setup of Section VI Riemannian Geometry. New York: Academic Press, 1975. [12] M. do Carmo, Riemannian Geometry. Boston: Birkhäuser, 1992. is shown in Fig. 6 and compared with experimental Monte [13] A. Alcocer, P. Oliveira, A. Pascoal, and J. Xavier, “Estimation of Carlo ML Newton runs at different signal to noise ratio Attitude and Position from Range only Measurements using Geometric 2 (SNR) conditions, where SNR 1/σ , and R = σImp. Descent Optimization on the Special Euclidean Group,” January 2006, ≃ Tech. Report, ISR, Lisbon. At each point, 100 Monte Carlo ML algorithm runs were [14] ——, “Estimation of Attitude and Position from Range only Mea- performed initialized at the true value of the parameter. surements using Geometric Descent Optimization on the Special The SNR range plotted corresponds to standard deviations Euclidean Group,” in International Conference on Information Fusion, of σ 10−3, 1 m. It is hard to distinguish between the FUSION’06, Florence, Italy., July 2006. ∈ [15] M. Moakher, “Means and Averaging in the Group of Rotations,” SIAM experimental ML performance and the IVLB. This shows Journal on Matrix Analysis and Applications 24/1, pp. 1–16, 2002. the good performance of the estimator and the tightness of [16] R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation. Boca Raton, FL, USA: CRC Press, 1994. the derived bound. [17] F. C. Park, “Distance Metrics on the Rigid Body Motions with Appli- cations to Mechanism Design,” Transactions of the ASME Journal of VIII.CONCLUSIONSANDFUTUREWORK Mechanical Design, Vol.117(1), pp:48–54., March 1995. [18] M. Žefran, V. Kumar, and C. B. Croke, “On the Generation of Smooth In this paper intrinsic gradient and Newton like algo- Three-Dimensional Rigid Body Motions,” IEEE Trans. Robotics and rithms were derived to solve the problem of simultaneous Automation, 14(4):576–589, August 1998. position and attitude estimation with pseudo-range only [19] D. P. Bertsekas, Nonlinear Programming. Belmont, MA: Athena Scientiﬁc, 1995, 2nd edition 1999. measurements. The algorithms are relatively simple and [20] J. Xavier and V. Barroso, “Intrinsic Variance Lower Bound (IVLB): avoid the need for any normalization procedure since the an extension of the Cramér-Rao bound to Riemannian manifolds,” in iterates evolve naturally on the parameter space. Simulation IEEE Int. Conf. on Acoust., Sp. and Sig. Proc. (ICASSP), March 2005.