A Robust Estimator for Almost Global Attitude Feedback Tracking

Transcription

1 A Robust Estimator for Almost Global Attitude Feedback Tracking Amit K. Sanyal Nikolaj Nordkvist This article presents a robust and almost global feedback attitude tracking control scheme in conjunction with a robust deterministic estimator that constructs state estimates for feedback. These control and the estimation schemes use the natural and globally unique representation of rigid body attitude provided by rotation matrices. Attitude and angular velocity state estimates are constructed from discrete (possibly multi-rate) measurements and a deterministic filtering scheme. Computational implementation of this estimator-based tracking scheme is carried out with a Lie group variational integrator, which preserves the rotation matrix structure without need for reprojection. Numerical simulation results obtained using this integrator show the robust and almost global tracking properties of this scheme. We also compare the performance of this attitude tracking control scheme with a quaternion observer-based feedback attitude tracking control scheme that has appeared in recent literature. Numerical simulation results are obtained for tracking an oscillating angular velocity spin maneuver with both the attitude tracking schemes for a satellite in circular Earth orbit. These results demonstrate the advantages of our scheme in attitude tracking of continuous rotational motions. I. Introduction Attitude estimation and control of rigid bodies in gravity have applications to motion control of spacecraft, aircraft, underwater vehicles, and mobile robots. Rigid body attitude control has been a benchmark problem in nonlinear control, studied under various assumptions and scenarios in the past. 7 For feedback attitude tracking, attitude and angular velocity estimates are usually constructed from discrete-time measurements in practice. Attitude measurements are obtained indirectly through direction or angle measurements, usually at a slower rate than angular velocity measurements which are directly obtained from rate gyros. Therefore, several attitude estimation schemes estimate only the attitude and sometimes the rate gyro bias from these measurements. 8 Attitude estimates are then propagated between measurements using a filter and measured angular velocities; these estimates are also used for feedback control. In this work, we present an overall attitude and angular velocity estimation scheme that constructs state estimates from multi-rate measurements, for use in an almost global feedback attitude tracking control scheme. Most of the prior research on attitude control and estimation has been carried out using minimal threecoordinate or quaternion representations of the attitude. Any minimal representation necessitates a local analysis since such representations have kinematic or geometric singularities and cannot represent attitude globally. The quaternion representation is ambiguous, since for every possible attitude there are two sets of unit quaternion representations. Since unit quaternions distinguish between principal angle rotations of and π, this leads to continuous controllers that rotate the spacecraft needlessly from principal angle π to, resulting in lack of Lyapunov stability of the desired equilibrium or trajectory (which may be attractive). This was termed the unwinding phenomenon by Bhat and Bernstein, and is observed whenever continuous quaternion feedback is used for feedback attitude tracking. Despite this fact, unwinding remains a poorly known phenomenon as can be witnessed by publications of research papers, many of which have appeared in the past decade, that claim to almost globally asymptotically stabilize or track attitude using continuous quaternion feedback. Some of these publications even claim to almost globally asymptotically track the Assistant Professor, Mechanical Engineering, University of Hawai i at Mānoa, Honolulu, HI 968, aksanyal@hsfl.hawaii.edu Post-doctoral Researcher, Mechanical Engineering, University of Hawai i at Mānoa, Honolulu, HI 968, nikolaj@hawaii.edu of 7

2 desired attitude trajectory without angular velocity feedback. However, these schemes also exhibit unwinding when applied to stabilize or track continuous (but otherwise arbitrary) rigid body attitude motion. 3 As defined in standard texts on nonlinear systems, 4 asymptotic stability requires both Lyapunov stability and attractivity of the desired equilibrium or trajectory in the feedback system. To avoid the unwinding phenomenon, a discontinuous quaternion-based controller has been used. 5 However, discontinuous dynamics entail special difficulties, 6 and may lead to chattering in the vicinity of a discontinuity, especially in the presence of sensor noise or disturbances. Discontinuous controllers are also difficult to implement using actuators like reaction wheels, control moment gyros and magnetic torquerods that are commonly used for attitude control in spacecraft. It is thus of great importance to determine which closed-loop properties can be achieved under continuous feedback control based on state estimates constructed from noisy measurements. This paper combines recent developments in the areas of rigid body attitude estimation and control in the form of an estimator-based feedback attitude tracking control scheme that can be practically implemented in most spacecraft and satellite missions. This scheme is applicable to rigid body systems in an attitudedependent gravity field. Some recent advances on attitude stabilization and tracking in gravity using a global analysis and continuous attitude and angular velocity feedback have been reported. 7 9 In these publications, an attitude feedback control scheme was obtained that results in almost global asymptotic tracking of a given attitude and angular velocity time trajectory; this control scheme has also been shown to be robust under certain disturbances and in the absence of knowledge of inertia. Dynamic attitude and angular velocity estimation for uncontrolled rigid bodies in gravity has also been reported., This robust geometric estimation scheme uses the global attitude description, similar to observers for mechanical systems on Lie groups reported in the past. 3 The combination of this estimation scheme with full state measurements and the feedback tracking scheme was also carried out recently. 4 Here we extend this scheme to the practical case of multi-rate measurements, i.e., when attitude measurements and angular velocity measurements are obtained at different rates. For numerical propagation of state estimates in our estimation scheme and for numerical simulations of our estimator-based feedback tracking scheme, we discretize the equations of motion. Uncertainty in the state estimates are assumed to be bounded by ellipsoids; therefore this scheme is unscented and robust to the stochastic properties of the noise. These ellipsoidal boundaries are propagated using the linearization of the discrete equations of motion about the trajectory of the state estimates. Since we consider tumbling rigid bodies which exhibit global motion in the configuration space SO(3), general purpose integrators like the Runge-Kutta schemes, which rely on use of local coordinates to describe the motion, are not applicable here. Therefore we use a Lie group variational integrator that gives us the discrete equations of motion. This integrator maintains the Lie group structure of the configuration space SO(3), as well as the mechanical structure of the system on discretization. Computations on this Lie group are carried out using the exponential map (and its inverse) relating the Lie group to its corresponding Lie algebra. The idea behind variational integrators is to discretize the variational principles of mechanics; in this case, the Lagrange-d Alembert principle for a system with non-conservative forcing. 5 This paper is organized as follows. In section II, we consider the system model of the dynamics of a rigid body and give a feedback control that exhibits asymptotic convergence to a prescribed trajectory almost globally. Discrete equations of motion for the system are presented in section III, and the linearized discrete equations are also obtained. In section IV we introduce the deterministic estimation scheme using discretetime (noisy) measurements and intersection of uncertainty ellipsoids to filter these measurements. Section V presents the combination of the tracking control with the estimation approach to define an estimator-based control scheme. In section VI, we numerically simulate the performance of our proposed estimation-based attitude tracking control scheme. This numerical simulation is carried out on the model of a satellite in circular Earth orbit with a set of initial conditions that correspond to a tumbling rigid body. In this section, we also compare our scheme with a quaternion-based feedback tracking scheme that has appeared in the recent past. 6 Numerical simulations for this quaternion-based tracking scheme for the same set of initial conditions, show the merits of our scheme and the weaknesses of the quaternion-based scheme due to unwinding. Section VII concludes this paper by summarizing the results. of 7

3 II. Attitude Dynamics and Trajectory Tracking A. Equations of Motion for Attitude Dynamics Here we describe the rigid body attitude kinematics and dynamics model using rotation matrices for attitude representation. We consider the attitude dynamics of a rigid body in the presence of control moments and a potential that is dependent only on the attitude. The configuration space for attitude dynamics is the special orthogonal group SO(3), also known as the rotation group. Therefore the state evolves in the 6 dimensional tangent bundle TSO(3). We denote by so(3) the space of 3 3 real skew-symmetric matrices, which is the Lie algebra of SO(3). We obtain the diffeomorphism TSO(3) SO(3) so(3) by left trivialization, and the vector space isomorphism ( ) : R 3 so(3), defined below, identifies so(3) and R 3. For the remainder of the paper, we therefore represent the attitude state as an element of SO(3) R 3. Let R SO(3) be the attitude and Ω R 3 the angular velocity of the body measured in the body-fixed frame. The attitude kinematics equation is where ( ) : R 3 so(3) is the vector space isomorphism given by Ω = Ω Ω Ω 3 Ṙ = RΩ, () Ω 3 Ω = Ω 3 Ω. Ω Ω Note that ( ) is also the cross-product in R 3 : x y = x y. We denote by ( ) : so(3) R 3 the inverse of the map ( ) : R 3 so(3). Let U : SO(3) R be a gravity potential dependent on the attitude; this could be due to uniform gravity or central gravity (for example, a satellite in circular Keplerian orbit). The attitude dynamics is given by J Ω = JΩ Ω + M g (R) + τ, () where τ is the control torque. M g (R) is the moment due to the gravity potential U(R) given by M g (R) = ( U ) TR R T U R R, ( ) where the partial derivative U R R3 3 U is defined such that R ij an additional disturbance moment due to atmospheric drag, was considered in our disturbance rejecting feedback tracking scheme. 8 A similar dynamics model, without control torques, was also used to study the attitude estimation problem., B. Trajectory Tracking = U R ij. This dynamics model, with In this subsection, we introduce the attitude and angular velocity trajectory tracking problem. We specify the desired state trajectory by the desired attitude R d (t) and the desired angular velocity Ω d (t), for some interval of time t [,T], where T >. Further, Ω d (t) and Ω d (t) are bounded during this time interval, and the rate of change of the desired attitude satisfies Ṙ d (t) = R d (t)ω d (t). (3) We first define the attitude and angular velocity tracking errors as follows Q(t) := R T d (t)r(t), ω(t) := Ω(t) Q T (t)ω d (t). (4) These definitions and equation (3) lead to the attitude error kinematics equation Q = Qω, (5) 3 of 7

4 where we have used the important identity (Fη) = Fη F T, for all F SO(3) and all η R 3. The angular velocity error dynamics is determined by J ω=j ( ω Q T Ω d Q T Ωd ) ( ω + Q T Ωd ) J ( ω + Q T Ωd ) + M g (R d Q) + τ. (6) The trajectory tracking error kinematics (5) and dynamics (6) depend on Q, ω, Ω d, Ω d, the moment due to the potential M g = M g (R d Q), and the control moment τ. We propose the control torque 8 τ = Lω + JQ T Ωd + ( Q T ) JQ Ω T d Ωd 3 + Φ (tr(k KQ)) k j e j QT e j M g (R d Q). j= (7) Here L = L T >, K = diag(k,k,k 3 ) with k 3 > k > k >, and e i is the ith canonical unit vector. The function Φ : R + R + is a C function that satisfies Φ() = and Φ (x) > for all x R +. Furthermore, we let Φ ( ) α( ) where α( ) is a Class-K function. 4 This ensures that Φ(tr(K KQ)) is a Morse function on SO(3) whose critical points are non-degenerate and hence isolated, according to the Morse lemma. 7 It has been previously shown 8 that (Q,ω) = (I,) is an almost globally asymptotically stable equilibrium of the error dynamics (5)-(6) with control torque (7). By almost globally we mean that the domain of attraction is the whole state space (TSO(3) = SO(3) R 3 ), except for a subset of measure zero. III. Discrete Approximation of the Dynamics To approximate the flow of the equations of motion ()-() we use the following integrator along with its linearization, which have been described in our recent work: 4 F k J J F T k = h(jω k ), (8) R k+ = R k F k, (9) JΩ k+ = F T ) k JΩ k + h (M g (R k+ ) + τ k. () Here J = trace[j]i J is a modified inertia matrix, h is the fixed step size (meaning that t k+ = t k + h), and τ k := ( ) τ(r k,ω k,t k ) + τ(r k+,ω k,t k+ ). Using the angular velocity Ω k at time t k and (8), we obtain F k by solving the implicit equation (8). This F k along with the rotation matrix R k at time t k, is then used to solve for R k+ using (9). This R k+ along with R k and Ω k are then used in equation () to solve for Ω k+. This gives a forward time map (R k,ω k ) (R k+,ω k+ ) and the only implicit relation to be solved is equation (8). A simple Taylor series analysis shows that this is a first order method. The implicit equation (8) can be solved efficiently using the Newton-Raphson method with F k = exp(f k ) and solving for f k; these analytical developments are described in greater detail in our recent work. 4 Linearizing (8)-() using δr k = R k Σ k, Σ k R 3, gives [ ] [ ] Σ k+ Σ = A k k, () δω k+ δω k 4 of 7

5 with A k = [ A k C k B k D k ], A k =F T k, B k =hf T k {tr(f k J )I F k J } J, C k =hj M k+ F T k, D k =J (h [ F T k (JΩ k ) + M k+ F T k ]{ tr(fk J )I F k J } ) J + F T k J. The M k are defined by the first variation of the gravitational potential. Since the potential is a function of attitude only its variation is given by δm g = M k Σ k, where M k = M(R k ) R 3 3. IV. State Estimation from Body Vector and Angular Velocity Measurements A. Uncertainty Ellipsoids A nondegenerate uncertainty ellipsoid in R n is defined as { } E R n(x,p) = y R n (y x) T P (y x). where x R n, and P R n n is a symmetric positive definite matrix. We call x the center of the uncertainty ellipsoid, and we call P the uncertainty matrix that determines the size and the shape of the uncertainty ellipsoid. The size of an uncertainty ellipsoid is measured by tr(p). It equals the sum of the squares of the semi principal axes of the ellipsoid. A nondegenerate uncertainty ellipsoid centered at (R,Ω ) SO(3) R 3 is induced from a nondegenerate uncertainty ellipsoid in R 6 [ E(R,Ω,P) = {R SO(3), Ω R 3 log(r T ] } R) E R 6(,P), Ω Ω where P R 6 6 is a symmetric positive definite matrix. Equivalently, an element (R,Ω) E(R,Ω,P) can be written as R = R exp(ζ ), Ω = Ω + δω, for some x = [ζ T,δΩ T ] T R 6 satisfying x T P x. A degenerate uncertainty ellipsoid in R n is a generalization of nondegenerate uncertainty ellipsoids defined as } E R n(x,m) = {y R n (y x) T M(y x). where x R n, and M R n n is a symmetric positive semidefinite matrix. If M is in fact definite we notice that E R n(x,m) = E R n(x,m ). We call x the center of the uncertainty ellipsoid, and we call M the degenerate uncertainty matrix that determines the size and the shape of the uncertainty ellipsoid. A degenerate uncertainty ellipsoid centered at (R,Ω ) SO(3) R 3 is induced from a degenerate uncertainty ellipsoid in R 6 [ E(R,Ω,M) = {R SO(3), Ω R 3 log(r T ] } R) E R 6(,M), Ω Ω where M R 6 6 is a symmetric positive semidefinite matrix. 5 of 7

6 B. Uncertainty Ellipsoid Based on Measurements We assume that the angular velocity Ω is measured directly; denoting by Ω its measured value. The angular velocity measurement error δ Ω is assumed to be bounded by the ellipsoid δ Ω E R 3(,T), where Ω = Ω + δ Ω. () We assume that there are m fixed points in the inertial reference frame, no two of which are co-linear, that are measured in the body frame. We denote the known direction of the ith point in the inertial reference frame as e i S where S denotes the two sphere (embedded in R 3 ). The corresponding vector is represented in the body fixed frame as b i S. Since we only measure directions, we normalize e i and b i so that they have unit lengths. The e i and b i are related by a rotation matrix R SO(3) that defines the attitude of the rigid body; e i = Rb i, for all i {,,...,m}. We assume that e i is known accurately and we assume that b i is measured in the body fixed frame. Let the measured direction vector be b i S, which contains measurement errors. We assume that the error in measuring b i is given by ν i as where ν i is assumed bounded by b i = exp ( (ν i ) ) bi, ν i E R 3(,S i ). (3) Based on the measured vector b i we obtain an estimate of the rotation matrix R SO(3). The vector estimation errors are given by e i R b i, i =,...,m. The weighted Euclidean norm of these errors is given by the error functional, J ( R) = m w i (e i R b i ) T (e i R b i ), i= = tr [(E R B) T W(E ] R B), where E = [ e,e,...,e m] R 3 m, B = [ b, b,..., b m ] R 3 m, and W = diag ( w, w,...,w m) R m m has a weighting factor for each measurement. We assume that m in this paper. If m =, we can take the cross product of the two measured unit vectors b b = b 3 and treat that as a third measured direction, with the corresponding unit vector in the inertial frame taken to be e 3 = e e. The attitude determination problem then consists of finding R SO(3) such that the error functional J is minimized: R = arg min J (C) (4) C SO(3) The problem (4) is known as Wahba s problem. 8 It was first solved 9 shortly after it was posed. A solution, known as the QUEST algorithm, 3 is expressed in terms of quaternions. A solution without using generalized attitude coordinates has also been found. A necessary condition for optimality of (4) is given as L T R = RT L, (5) where L = EW B T R 3 3. The unique minimizing solution to the attitude determination problem (4), which satisfies (5), is given by R = SL, S = Q L (R L R T L ) Q T L, (6) 6 of 7

7 where i= L = Q L R L, Q L SO(3), (7) and R L is upper triangular and invertible; this is the QR decomposition of L. The symmetric positive definite (principal) square root is used in (6). Using existing results we have that the uncertainty matrix in the case of simultaneous body vector measurements and angular velocity measurements is given by ( m P = tr ( K i S i (K i ) T) + tr (T)) m H K i S i (K i ) T H T tr ( K i S i (K i ) T) + H TH T tr, (8) (T) i= where ( ) K i = w i (tr RT L I 3 3 R T ) ( L tr( bi (e i ) T ) R I 3 3 b i (e i ) T ) R, (9) with H = [I 3 3, 3 3 ] T and H = [ 3 3, I 3 3 ] T. If body vectors are measured, but no velocity measurement is available, then the degenerate uncertainty ellipsoid this measurement defines has degenerate uncertainty matrix ( m M = tr ( K i S i (K i ) T)) i= H m K i S i (K i ) T tr ( K i S i (K i ) T) i= H T vector measurements only. () If instead angular velocity is measured, but no attitude information is available, the corresponding degenerate uncertainty ellipsoid is given by the following degenerate uncertainty ellipsoid Hence the measured uncertainty ellipsoid is given by calculated as follows. M = H T H T velocity measurement only. () E( R, Ω, M), Body vector and velocity measurements: M = P is given by (8), R is given by (6)-(7), and Ω is measured directly. Body vector measurements only: M is given by (), R is given by (6)-(7), and Ω is redundant in specifying E( R, Ω, M) so it can be chosen arbitrarily. Angular velocity measurement only: M is given by (), Ω is measured directly, and R is redundant in specifying E( R, Ω, M) so it can be chosen arbitrarily. Remark Note that in a practical implementation E and S i may have different values for different measurements. This comes from the fact that at different attitudes we may have different directions available for observation and the quality of a measurement of a certain direction can depend on the attitude as most direction measuring instruments have limited field of view. Also T may change between measurements. C. Flow-propagated Uncertainty Ellipsoid At some instant t = t let the system state be in the ellipsoid (R,Ω) E(R,Ω,P ). As in our previous publication on the subject we will use the discrete equations of motion from section III to propagate the center of this ellipsoid; and the linearized equations to propagate the uncertainty matrix. Using the linearized equations to propagate the uncertainty matrix ensures that the resulting uncertainty 7 of 7

8 bounds remain ellipsoidal. The validity of the linearized equations to update the uncertainty matrix depends on the size of E(R,Ω,P ), i.e. tr(p ); the smaller the ellipsoid is, the better this approximation. Following this approach we get that at t N = t + Nh the state lies in the flow-propagated uncertainty ellipsoid (R,Ω) E(R f,ω f,p f ), where R f = R N and Ω f = Ω N are given by equations (8)-(), with initial condition (R,Ω ). As described in previous work the updated uncertainty matrix P f = P N is given by the discrete dynamics where A k is the linear flow matrix given by equations (). P q = A q A q A P (A q A q A ) T, () Figure. Minimal trace intersecting ellipsoids in D. Two nondegenerate ellipsoids intersecting (left) and a degenerate ellipsoid intersecting a nondegenerate ellipsoid (right). The symbol + is used to represent the center of an ellipsoid. D. Intersection of Measurement and Flow Uncertainty Ellipsoids as Filter Let E R n(x,p ) and E R n(x,p ) be two ellipsoids with nonempty intersection. We consider the problem of finding an ellipsoid E R n(ˆx, ˆP) satisfying 3 see Figure (left). The solution to (3) is given as 3 where and q = q solves the equation E R n(x,p ) E R n(x,p ) E R n(ˆx, ˆP), min tr( ˆP); (3) ˆx = x + L(q )(x x ), (4) ˆP = β(q ) ( (I L(q ))P (I L(q )) T + q L(q )P L(q ) T), (5) L(q) = P (P + q P ), (6) β(q) = + q (x x ) T P L(q)(x x ), (7) tr(u P U(Λ + qi) ) tr(u P U(Λ + qi) ) = β (q) β(q), (8) 8 of 7

9 using the eigendecomposition P P = UΛU. A sufficient condition for (8) to have a solution is ( (x x ) T P (x x ) ) tr(p ) tr(p P P ), and if this condition is violated q = is an optimal solution. We use the Newton-Raphson method, with initial guess q =, to numerically solve (8); Figure was obtained using this approach. Time evolution Flow ellipsoid + Filtered ellipsoid Measurement ellipsoid Figure. The filtering procedure. Since the flow-propagated uncertainty ellipsoid is nondegenerate, but the measurement-based uncertainty ellipsoid may be degenerate, the generalization of (3) that we need is E R n(x,p ) E R n(x,m ) E R n(ˆx, ˆP), min tr( ˆP). (9) This is illustrated in Figure (right). The previously published results 3 remain valid for this situation too. Rewriting the results in these terms gives where ˆx = x + L(q )(x x ), (3) ˆP = β(q ) ( I L(q ) ) P, (3) L(q) = q ( P + qm ) M, (3) β(q) = + q (x x ) T P L(q)(x x ), (33) and q = q solves the equation ( tr (P + qm ) ( I qm (P + qm ) ) ) M P ( (I ) ) = β (q) tr q(p + qm ) M P β(q). As in our previous publication we will use this approach as filter to extract the uncertainty ellipsoid at time t N = t + Nh from the intersection of the flow-propagated ellipsoid (from time t to t N = t + Nh) 9 of 7

10 and the measurement-based ellipsoid (at time t N = t + Nh). Formulated in the language of uncertainty ellipsoids on SO(3) R 3,we are thus interested in solving E(R f,ω f,p f ) E( R, Ω, M) E( ˆR, ˆΩ, ˆP), min tr( ˆP) The filtered states ( ˆR, ˆΩ) form our updated state estimates at time t N. This filtering procedure is illustrated in Figure. We need to convert this problem of intersecting uncertainty ellipsoids on SO(3) R 3 to an equivalent problem of intersecting uncertainty ellipsoids on R 6 in order to use the described solution approach. We choose the center of the flow-propagated ellipsoid as origin of R 6, i.e., E R 6(,P f ). The center of the measurement-based ellipsoid in R 6, with origin in the center of the measurement based ellipsoid, is the Euclidean difference between the centers of the two ellipsoids in SO(3) R 3. Therefore [ ] ζ x =, δω where Thus we find the intersection R = R f exp(ζ ), Ω = Ω f + δω. E R 6(,P f ) E R 6(x, M) E R 6(ˆx, P), min tr( ˆP) (34) Using (3)-(33) with x =, x = x, P = P f, and M = M, we find the filter-updated uncertainty ellipsoid in R 6 as the solution to (34). The updated uncertainty ellipsoid in SO(3) R 3 is obtained by translating this ellipsoid as E( R, Ω, P), where the center in SO(3) R 3 is obtained from ˆx = [ˆζ T,δˆΩ T ] T R 6 as R = R f exp(ˆζ ), Ω = Ω f + δ Ω. (35) V. Attitude Feedback Control using State Estimates based on Multi-rate Measurements In this section we construct a control algorithm where the tracking control is based on that described in section II but instead of the true state (of which we have no exact knowledge) we base it on the estimated state as given in section IV, with the filter applied at each measurement to improve the accuracy of the estimate. These measurements are assumed multi-rate, i.e., body vector measurements and angular velocity measurements may not be available at the same frequency. We propose the following control algorithm based on state estimates obtained from estimated uncertainty ellipsoids at each measurement. Algorithm Let t j R be the initial time and t j+ > t j the time of the next measurement. Choose the time step h > such that (t j+ t j )/h is an integer. Let R(t j ) and Ω(t j ) be the initial estimate of the state with initial estimated uncertainty matrix P(t j ). Let the control torque (7) be based on the estimated state ( R(t), Ω(t)), i.e. τ = L ω + J Q T Ωd + ( QT Ωd ) J QT Ωd + Φ (tr(k K Q)) 3 k j e Q T j e j M g (R d Q), where Q := R T d (t) R(t) and ω := Ω(t) Q T (t)ω d. The time evolution of the estimated state ( R(t), Ω(t)) is defined as follows: j= of 7

11 . Propagate: Use (8)-() to find the estimated state ( R(t), Ω(t)) at discrete instances in the time interval [t j,t j+ ]; take R(t j ) and Ω(t j ) as initial conditions. Using the linearized equations () along with () find the flow-propagated uncertainty matrix P(t) for discrete t [t j,t j+ ]; take P(t j ) as initial condition. The flow uncertainty ellipsoid at time t j+ is E( R(t j+ ), Ω(t j+ ), P(t j+ )).. Measure: Use the measurement data to calculate the measurement-based uncertainty ellipsoid at time t j+ as described in section B. 3. Filter: Calculate the intersection between the flow-propagated uncertainty ellipsoid and the measurementbased uncertainty ellipsoid at time t j+, as described in Section D. The center of this ellipsoid replaces R(t j+ ) and Ω(t j+ ) and its uncertainty matrix replaces P(t j+ ). This algorithm can then be used repeatedly using as initial time the time of the last measurement. VI. Numerical Results A. Simulation Example and Parameters This section presents some numerical simulation results of this estimator-based control scheme applied to the practical example of a rigid satellite in circular Earth orbit. For this system, the gravity-gradient torque is given by 3 and M k is given by M k = 3ω M g = 3ω (R bl ) T e 3 JR bl e 3, ( ( J(R bl k ) T e 3 ) ( (R bl k ) T e 3 ) + ( (R bl k ) T e 3 ) J ( (R bl k ) T e 3 ) ), where ω is the orbital angular velocity (assumed constant) and R bl is the rotation matrix transforming from body fixed frame to LVLH frame (local vertical local horizontal frame). In this simulation we take ω =. s (corresponding to an altitude of 35 km). The discrete dynamics of R bl is given by 3 R bl k+ = e hωe R bl k F k. The principal moment of inertia matrix for the satellite is J = diag(4.,3.85,4) kg m which corresponds to a small satellite about 5 kg in mass. The controller parameters for this simulation are taken to be L=diag(.6,.7,) kg m s, K =.5 diag(,,3), and Φ(x) = x. The attitude time trajectory to be tracked is given by c θ c ψ c θ s ψ s φ s θ c φ c θ s ψ c φ + s θ s φ R d (t) = s ψ s φ c ψ c φ c ψ (36) s θ c ψ s θ s ψ s φ c θ c φ s θ s ψ c φ + c θ s φ where c α = cos(α(t)) and s α = sin(α(t)). This attitude profile is obtained from a modified -3- Euler angle representation with θ(t)=.5t., ψ(t)=.7t +.3, φ(t)=.3t +.5. (37) The resulting desired angular velocity time profile is θs ψ + φ Ω d (t) = θs φ c ψ ψc φ. (38) θc φ c ψ + ψs φ Note that the angular rates are constant, so Ω d (t) may be obtained easily from the Ω d (t) given above. of 7

12 B. Simulation Results and Observations With these simulation parameters for this rigid satellite, we implement the Lie group variational integrator (8)-() to obtain the time evolution of the true state (R(t),Ω(t)) with the control law (7) based on estimated states ( R(t), Ω(t)) as given by control algorithm. We assume that the initial tracking errors are Q() = , ω() = rad/s. (39) This initial angular velocity error (with a norm of around 5 /s) corresponds to an initially tumbling satellite at orbit insertion, after its release from a spin-stabilized launch vehicle. Spin-stabilized launch vehicles for Earth satellites could be spinning at a rate of to.5 rev/s; after orbit insertion, a yo-yo mechanism is supposed to reduce the spin rate of the satellite to about 5 to % of this value. One of the failure modes of analysis in such satellites deals with failure of this mechanism to fully deploy. Our numerical simulation results use a time step size of h. s for the variational integrator (the exact number is /549 of the period of an orbit which is approximately s) in the discrete equations of motion. We take simultaneous body vector and angular velocity measurements each seconds, corresponding to / of the period of an orbit. Angular velocity measurements are made ten times as frequent as the body vector measurements, i.e. every.548 seconds. The total duration of the simulation is / of the period of an orbit. We emulate direction measurements numerically with the measured inertial directions given by E = and with errors randomly chosen in the ellipsoids given by S i = (5π/8) I and T = (π/8 s ) I; see () and (3). These ellipsoids correspond to measurement error bounds of 5 in attitude and /s in angular velocity about each body axis, which are worse than errors expected from coarse attitude sensors and rate gyros. The weight matrix is chosen as W = I 5 5 for simplicity. We choose an initial error in the state (that is estimated state minus true state, i.e., R() R() T and Ω() Ω()) randomly from the ellipsoid with uncertainty matrix diag(π,π,π,(5π/8),(5π/8),(5π/8) ). This initial uncertainty matrix corresponds to error bounds of π radians in attitude about any body axis and 5 /s in angular velocity about any body axis. 3.5 log(r T d R).5.5 ω (s ) Figure 3. Time plots of the attitude tracking error norm (left) and the angular velocity tracking error norm (right). In Figure 3 the norm of the error in tracking the true attitude R(t) and the tracking error in the true angular velocity Ω(t) are plotted versus time. We see that after some time the tracking errors remain of 7

13 log( R T R)..8.6 tr ( P) Figure 4. Time plots of the attitude estimation error norm (left) and the trace of the state estimate uncertainty matrix (right). bounded and small. Since every measurement has the same (non-zero) error distribution, such a result is the best possible that can be achieved. It is important to note that even with measurement errors we avoid unwinding. The unwinding phenomenon is a result of the inherent ambiguity of the quaternion representation that is caused by the fact that the unit quaternions double cover SO(3). Since we use an element of SO(3) (a rotation matrix) directly to represent the attitude we get a unique representation of attitude and thus unwinding is prevented..8 8 τ (Nm) t τ dτ (Nms) Figure 5. Time plots of the norm of the control torque (left) and its time integral (right). The performance of the estimation approach is illustrated in Figure 4. Since the error bounds on the measurements here are defined as constants such a non converging behavior of the estimation error is expected; stricter bounds will produce better estimation. Figure 4 (right) shows the time evolution of the size, given by the trace of the uncertainty matrix P, of the flow-propagated uncertainty ellipsoid according to algorithm (and thereby implicitly ()). In Figure 5 (left) we have plotted the norm of the applied control torque as given by algorithm. After initial transients, the torque remains bounded since it has to mainly compensate for the measurement errors. Figure 5 (right) plots the time integral of this norm over the maneuver duration, which is a measure of the control energy expended. 3 of 7

14 C. Comparison with a quaternion observer-based feedback tracking scheme We now compare the performance of our deterministic estimator-based attitude tracking scheme with a continuous quaternion observer-based tracking scheme that has appeared in the recent past. 6 The purpose of this comparison is to show that continuous unit quaternion feedback controllers can waste considerable amount of time and control energy trying to overcome unwinding, even though they may be attracted to a desired attitude trajectory in the space of rigid body rotations SO(3). Prior work has described the unwinding phenomenon, which occurs due to the two-to-one map from the unit quaternion space S 3 to the space of rotations SO(3). Unwinding results in lack of Lyapunov stability of the desired equilibrium or state trajectory, which can lead to loss of control effort and time in reaching the desired trajectory. A quaternion is represented by q = [q q T v ] T R 4 where q R is the scalar component and q v R 3 is the vector component of the quaternion. We denote by q p the quaternion product of two quaternions, given by q p = [q p q T v p v (q p v + p q v + q v p v ) T ] T. If [ ] T denotes the identity element in the group of quaternions, then the inverse of a quaternion is given by q = [q q T v ] T / q, such that q q = [ ] T. For this simulation, we use the following equations describing the quaternion kinematics, observer and control law: 6 q = q q Ω where q Ω = [ Ω T ] T, (4) q = q q β where q β = [ β T ] T, β = Γ q v, (4) τ = α q e v α q v + JQ T (q e ) Ω d + Ω d JΩ d M g (q), (4) where q and q denote the actual and observed unit quaternions, α,α > are controller gains and Γ = Γ T > is an observer gain matrix. The attitude tracking error is represented by the unit quaternion q e = (q d ) q, where q d denotes the desired unit quaternion. The unit quaternion representing the observer error is q = ( q) q e = [ q q v ] T. We first convert the desired attitude trajectory R d (t) in(36) to a corresponding continuous desired quaternion trajectory q d (t) as the quaternion product of the component rotations of the -3- Euler angle representation used to obtain R d (t). Note that there are other algorithms that would do this transformation 33 such that the scalar component of the obtained quaternion is always positive; this however would give rise to a non-c trajectory in S 3. The initial tracking errors are as given in (39), while the desired trajectory on TSO(3) is given by (36)-(38). Then we use a Runge-Kutta solver to numerically integrate the equations for the feedback system and the continuous observer, while projecting onto the space of unit quaternions S 3 at each time step. The results of applying this scheme with α =.9, α =.84 and Γ =.7I are shown in figures 6 to 8. In Figure 6, the unit quaternion tracking errors q e = (q,q e,q e,q e 3) e and observer errors q = ( q, q, q, q 3 ) are given in components. Note that the time responses in these plots suggest that while the quaternion tracking error q e and estimation error q for the feedback system are attracted to (,,,), they are repelled from the error quaternions q e = q = (,,,) which are unstable for the feedback dynamics. Since any antipodal quaternion pair (q and q) corresponds to the same physical attitude, this results in homoclinic orbits in SO(3) at Q(q e ) = R d (q d ) T R(q) = I. Therefore, this scheme leads to unwinding. Moreover, a control law that assigns different control values and/or exhibits different behavior for the two quaternions q e and q e (corresponding to the same physical attitude tracking error), is not well-defined on TSO(3). Figure 7 shows the time response of the corresponding attitude tracking error and angular velocity tracking error for this quaternion tracking scheme. The effects of unwinding of this quaternion-based tracking scheme on the state space of attitude motion TSO(3) is clearly visible in these plots. Note that while the initial tracking errors are identical in figures 7 and 3, the errors in Figure 7 show larger oscillations in these errors. Note that the unit quaternion tracking error pair ±q e = ±(,,,) in Figure 6 corresponds to the identity matrix (R T d R)(qe ) = Q(q e ) = I for attitude tracking error in Figure 7. To conclude, we show the time profile of the norm of the control torque in Figure 8. Although the control effort in Figure 5 is initially larger than the control effort in Figure 8 (due to the large initial angular velocity), the control effort for the quaternion observer-based tracking scheme shown in Figure 8 keeps oscillating significantly longer as it 4 of 7

15 q e q e q q q e q e 3 q q Figure 6. Time plots of the tracking error quaternion components (left) and the estimate error quaternion components (right) for the quaternion-based tracking scheme log(r T d R).5 ω (s ) Figure 7. Time plots of the corresponding attitude tracking error norm (left) and the angular velocity tracking error norm (right) for the quaternion-based tracking scheme. follows the oscillating attitude and angular velocity tracking errors due to unwinding. This leads to a much larger control effort (given by the time integral of the control torque norm during the maneuver) for the quaternion observer-based tracking scheme, as shown in the time plots on the right sides of figures 5 and 8. Though the states are eventually attracted to the desired attitude and angular velocity profile on TSO(3), this feedback scheme wastes significantly more time and effort than our feedback scheme, which is designed to achieve almost global asymptotic stability on TSO(3). VII. Conclusion In this paper, an estimator-based attitude and angular velocity feedback tracking control scheme that uses state estimates for feedback was presented. This scheme can be applied to attitude tracking control of rigid bodies for all possible initial conditions, since it is based on a global description of attitude motion. Estimated states were updated from discrete measurements with a filter applied at each measurement. This approach combined our earlier developments in almost global feedback attitude tracking control, 8 with deterministic attitude and angular velocity estimation based on uncertainty ellipsoids, after adapting this estimator to the usual case when attitude and angular velocity measurements were obtained at different 5 of 7

16 τ (Nm) t τ dτ (Nms) Figure 8. Time profile of the norm of the control torque (left) and its time integral (right) for the quaternionbased tracking scheme. rates. As measured directions were used to obtain the attitude, an earlier reported method was applied to solve the attitude determination problem. For the propagation of estimated states a Lie group variational integrator was used to maintain the structure of numerically propagated attitude states. A linearization of this integrator about the estimated state trajectory was used to propagate the uncertainty matrix. This estimator is robust to the statistical distribution of uncertainty in measured states and initial conditions, since it uses deterministic bounds on these uncertainties. Numerical simulations confirmed that the proposed estimator-based control algorithm can track the desired attitude and angular velocity trajectory for an initially tumbling rigid body, based on noisy measurements with known error bounds. A comparison of this scheme to a continuous quaternion observer-based feedback tracking scheme (assuming perfect knowledge of quaternion) was carried out for tracking the same desired attitude maneuver. This comparison clearly shows the weaknesses of continuous quaternion-based feedback for tracking arbitrary attitude maneuvers. The continuous quaternion-based tracking scheme was shown to exhibit unwinding when tracking the desired attitude and angular velocity profile, which resulted in wasted time and control effort in tracking. In conclusion, the results in this paper demonstrate the advantages of using the natural representation of physical attitude (the rotation matrices) when tracking arbitrary attitude maneuvers using continuous state feedback. References Ahmed, J., Coppola, V. T., and Bernstein, D., Adaptive Asymptotic Tracking of Spacecraft Attitude Motion with Inertia Matrix Identification, AIAA Journal of Guidance, Control, and Dynamics, Vol., 998, pp Bhat, S. P. and Bernstein, D. S., A Topological Obstruction to Continuous Global Stabilization of Rotational Motion and the Unwinding Phenomenon, Systems & Control Letters, Vol. 39, No.,, pp Bloch, A. M., Nonholonomic Mechanics and Control, No. 4 in Interdisciplinary Texts in Mathematics, Springer Verlag, 3. 4 Dixon, W. E., Behal, A., Dawson, D. M., and Nagarkatti, S. P., Nonlinear Control of Engineering Systems: A Lyapunov- Based Approach, Birkhäuser Verlag, Boston, MA, 3. 5 Koditschek, D. E., The Application of Total Energy as a Lyapunov function for Mechanical Control Systems, Proc. of AMS-IMS-SIAM Joint Summer Research Conf. at Bowdin College, Maine, Wen, J. T. and Kreutz-Delgado, K., The Attitude Control Problem, IEEE Transactions on Automatic Control, Vol. 36, No., 99, pp Wie, B., Space Vehicle Dynamics and Control, AIAA Education Series, Reston, Crassidis, J. L. and Markley, F. L., A minimum model error approach for attitude estimation, Journal of Guidance, Control and Dynamics, Vol., No. 6, 997, pp Crassidis, J. L. and Markley, F. L., Unscented filtering for spacecraft attitude estimation, AIAA Journal of Guidance, Control, and Dynamics, Vol. 6, No. 4, 3, pp Rehbinder, H. and Hu, X., Drift-free attitude estimation for accelerated rigid bodies, Automatica, Vol. 4, No. 4, 4, pp of 7

17 Shuster, M. D., Kalman filtering of spacecraft attitude and the QUEST model, Journal of the Astronautical Sciences, Vol. 38, No. 3, 99, pp Vasconcelos, J. F., Silvestre, C., and Oliveira, P., A Nonlinear GPS/IMU Based Observer for Rigid Body Attitude and Position Estimation, IEEE Conf. on Decision and Control, Cancun, Mexico, Dec. 8, pp Chaturvedi, N. A., Sanyal, A. K., and McClamroch, N. H., Rigid body attitude control Understanding global dynamics and performance, IEEE Control Systems Magazine, 9, (submitted). 4 Khalil, H. K., Nonlinear Systems, Prentice Hall, Englewood Cliffs, NJ, nd ed., Crassidis, J. L., Vadali, S. R., and Markley, F. L., Optimal Variable-Structure Control Tracking of Spacecraft Maneuvers, AIAA Journal of Guidance, Control, and Dynamics, Vol. 3,, pp Cortes, J., Discontinuous Dynamic Systems, IEEE Control Systems Magazine, Vol. 8, 8, pp Chaturvedi, N. A. and McClamroch, N. H., Almost global attitude stabilization of an orbiting satellite including gravity gradient and control saturation effects, American Control Conference, Minneapolis, MN, June 6. 8 Sanyal, A. K. and Chaturvedi, N. A., Almost Global Robust Attitude Tracking Control of Spacecraft in Gravity, AIAA Conf. on Guidance, Navigation and Control, Honolulu, HI, Aug Sanyal, A. K., Shen, J., Bloch, A. M., and McClamroch, N. H., Stability and Stabilization of Relative Equilibria of Dumbbell Bodies in Central Gravity, AIAA Journal of Guidance, Control, and Dynamics, Vol. 8, No. 5, 5, pp Sanyal, A., Fosbury, A., Chaturvedi, N., and Bernstein, D., Inertia-free Spacecraft Attitude Trajectory Tracking with Disturbance Rejection and Almost Global Stabilization, AIAA Journal of Guidance, Control, and Dynamics, Vol. 3, 9, pp Sanyal, A. K., Optimal Attitude Estimation and Filtering Without Using Local Coordinates, Part I: Uncontrolled and Deterministic Attitude Dynamics, American Control Conference, Minneapolis, MN, June 6, pp Sanyal, A. K., T. Lee, M. L., and McClamroch, N. H., Global Optimal Attitude Estimation using Uncertainty Ellipsoids, Systems & Control Letters, Vol. 57, No. 3, 8, pp Maithripala, D. H. S., Dayawansa, W. P., and Berg, J. M., Intrinsic Observer-Based Stabilization for Simple Mechanical Systems on Lie Groups, SIAM Journal on Control and Optimization, Vol. 44, No. 5, 5, pp Nordkvist, N. and Sanyal, A. K., Attitude Feedback Tracking with Optimal Attitude State Estimation, American Control Conference, Baltimore, MD, June, p. to appear. 5 Marsden, J. and West, M., Discrete mechanics and variational integrators, Acta Numerica, Vol.,, pp Tayebi, A., Unit Quaternion-based Output Feedback for the Attitude Tracking Problem, IEEE Transactions on Automatic Control, Vol. 53, No. 6, 8, pp Milnor, J., Morse Theory, Princeton University Press, Princeton, NJ, Wahba, G., A least squares Estimate of Satellite Attitude, Problem 65-, SIAM Review, Vol. 7, No. 5, 965, pp Farrell, J. L., Stuelpnagel, J. C., Wessner, R. H., Velman, J. R., and Brock, J. E., A least squares Estimate of Satellite Attitude, Solution 65-, SIAM Review, Vol. 8, No. 3, 966, pp Shuster, M. D. and Oh, S. D., Three-axis attitude determination from vector observations, AIAA Journal of Guidance and Control, Vol. 4, No., 98, pp Maksarov, D. G. and Norton, J. P., State bounding with ellipsoidal set description of the uncertainty, International Journal of Control, Vol. 65, No. 5, 996, pp Lee, T., Leok, M., and McClamroch, N. H., Attitude Maneuvers of a Rigid Spacecraft in a Circular Orbit, American Control Conference, Minneapolis, MN, June 6, pp Bar-Itzhack, I. Y., New method for extracting the quaternion from a rotation matrix, AIAA Journal of Guidance, Control, and Dynamics, Vol. 3, No. 6,, pp of 7