Robust Static Attitude Determination via Robust Optimization

Transcription

1 Milano (Italy) August 8 - September, Robust Static Attitude Deteration via Robust Optimization Shakil Ahmed Eric C. Kerrigan Department of Electrical and Electronic Engineering, Imperial College London, UK ( shakil.ahmed8@imperial.ac.uk). Department of Aeronautics and Department of Electrical and Electronic Engineering, Imperial College London, UK ( e.kerrigan@imperial.ac.uk). Abstract: We address the problem of robust attitude deteration using a static approach. In contrast to the dynamic approach, a static approach does not depend on the system dynamics. This approach only requires measurements of some vectors, such as the earth magnetic field, sun vector, etc in two different coordinate frames. These vectors, obtained from some sensor or a mathematical model, may not be accurate due to sensor errors and modeling inaccuracies. We consider all such errors as infinity-norm bounded uncertainties and our main focus is to obtain an attitude estimate, which is least sensitive to such uncertainties. We formulate a robust optimization (RO) problem with a quadratic cost and nonlinear constraints and propose a solution using a quaternion approach with an affine uncertainty parameterization. We transform the RO problem into a suboptimal imization problem, which is non-convex butcan be solved with a good initial guess using a nonlinear optimization solver. The results show a significant advantage of the proposed approach for the worst case uncertainties. Keywords: Transformation matrices, attitude algorithms, modeling errors, least square estimation, robust estimation. INTRODUCTION The main focus of this work is on robust attitude deteration in the presence of uncertainties using a static approach, whichis basedonvector information. Onemain advantage of this approach is that it always gives an attitude estimate independent of the dynamic system nonlinearities, making it especially useful for highly nonlinear systems, such as a tumbling satellite, where dynamic approaches based on linear or nonlinear filtering suffer from divergence issues due to lack of good a priori state estimate (Crassidis, Markley, andcheng, 7). Anadded benefit of static estimation in such cases is that it can provide a good state initialization for dynamic filters, reducing the likelihood of divergence. To compute the attitude of an object i.e. its orientation in space with respect to some known reference, two coordinate frames are needed. One, which is fixed to the body of the object, is called the bodyframe, while the secondis called the reference frame. Selection of the reference frame normally depends upon the control system requirements. Formally, attitude of an object is defined as a coordinate transformation that transforms reference coordinates into the body coordinates (Sidi, ). This transformation is obtained through a proper orthogonal transformation matrix C R 3 3, also known as a direction cosine or attitude matrix. Orthogonality of the transformation matrix dictates the constraint C T C = I, while the proper transformation matrix is defined by having the deterant equal to + to preserve the orientation in a rotation, thus imposing the constraint det(c) = +. The vector quantities normally used in static attitude deteration are the earth magnetic field (EMF), sun vector, star vector, etc. Information of these vectors is required both in the body and reference frames in order to detere the attitude or transformation matrix. Normally the vectors in the body frame are measured by some sensor installed on the body of the object, while the same vector information in the reference frame is obtained from some mathematical model. Withonly onepairof information, i.e. a vector quantity in the bodyandreference frame, deteration of the attitude matrix remains an underdetered problem with infinite solutions. Addingasecond pair makes this problem overdetered (Shuster and Oh, 98). A commonly used approach for such problems is to findanoptimalsolutionthatimizes theweightedleast square cost, first proposed by Wahba (965) for satellite applications, given as: C w i b i Cr i subject to C T C = I, det(c) =, where b i R 3 represents i th measurement in the body frame and i =,..,n, n being the total number of sensors, r i R 3 is the corresponding vector in the reference frame obtained from some model, w i R are non-negative weights while represents vector -norm. Many efficient solutions of this problem for satellite applications can be found in the literature, such as Keat (977), Shuster and Oh (98), Markley (988), Mortari (997) and Mortari (). () Copyright by the International Federation of Automatic Control (IFAC) 587

2 Milano (Italy) August 8 - September, A general approach used in solving the imization problem () is to convert it into an equivalent imization problem. For this, consider the cost function used in () for imization and expand it as w i b i Cr i = w i {b T i b i +ri T r i } w i b T i Cr i. Neglecting the constant term as it will not change argument of the optimization problem, an equivalent imization problem can be written as C w i b T i Cr i subject to C T C = I, det(c) =. To solve this imization problem the most popular approach is based on Davenport s q-method (Keat, 977; Shuster and Oh, 98). Two important steps of the q- method are given now, which will be used for solving the problem addressed in this work. Step : Find an equivalent formulation of () in terms of a quaternion. This new formulation, firstreported in Keat (977), states that the imization of () is equivalent to the following problem (see Appendix A for derivation): q q T Kq subject to q T q =, where q R 4 represents a 4-parameter quaternion, while K R 4 4 is a symmetric, indefinite and traceless matrix given as [ ] B K := T + B tr(b)i z z T, (4) tr(b) where B R 3 3 := n w ib i r T i and z R 3 := n w i(b i r i ), while represents cross product and tr( ) represents trace operator. Step : Using the new formulation, the non-concave imization problem can be converted into an eigenvalue problem. For this we first add the constraint q T q = using a Lagrange multiplier λ in (3) that gives () (3) f(q,λ) = q T Kq λ(q T q ), (5) To obtain a stationary point, we solve f/ q = and f/ λ = and obtain the following expression, that has the same formulation as the standard eigenvalue problem Kq = λq. (6) Four eigenvectors of the symmetric matrix K are possible solutions of (6); however, the eigenvector corresponding to the imum eigenvalue is the solution that will solve (3) (Shuster and Oh, 98),(Markley and Mortari, ), i.e. Kq opt = λ (K)q opt, (7) where q opt is the solution to (3) and λ (K) is the imum eigenvalue of the matrix K. Most of the work in static satellite attitude deteration is based on this result and many efficient algorithms have been proposed, such as QUEST (Shuster and Oh, 98), ESOQ (Mortari, 997), ESOQ (Mortari, ). A survey paper by Markley andmortari () provides ageneraldescriptionofmany of these algorithms. The use of a quaternion in the new formulation imparts many benefits. It not only reduces the number of optimization variables from nine to four, but also avoids the constraint det(c) = + of () being inherent in the definition of quaternion. However, the main benefit obtained is the transformation of the optimization problem into an eigenvalue problem. This classical approach, based on () does not directly address the issue of robustness in the measured and model vectors. Mostly, sensitivity analysis have been presented for different algorithms with analytical expressions of the imum error covariance under stochastic variation in the measurement vector, such as Shuster and Oh (98), Shuster (6), Markley (988), Markley (8). However, modeling errors in the reference vectors are generally not considered. These errors could be significant. In the case of the earth magnetic field, which is a commonly used sensor especially in satellite applications, errors between sophisticated models and the actual field can be significant (McLean et al., December, 4; Roithmayr, 4). The use of simple models, such as low order IGRF (Roithmayr, 4), which are normally preferred due to less computational cost, results in a less accurate earth magnetic field vector in the reference frame, leading to errors in the attitude estimate. Attitude inaccuracy is further increased due to sensor errors, which are mainly due to noise and installation issues. The magnetic field sensing in the post launch tumbling phase is also a big source of uncertainty. We consider all such errors as - norm bounded uncertainties in the input vectors. The main contribution of this paper is consideration of the measurement and modeling uncertainties in the problem formulation for static attitude deteration. We formulate a robust optimization (RO) problem for the uncertain inputs. The formulated - RO problem is relaxed and transformed to a suboptimal imization problem using a suitable affine uncertainty parameterization. The resultingnon-convex imization problem with nonlinear cost and constraints is solved using a nonlinear optimization solver with a good initial guess.. ROBUST PROBLEM DESCRIPTION To formulate a robust attitude deteration problem, we will represent an uncertain measurement vector in the body frame with b i B i R 3 and an uncertain reference vector with r i R i R 3, i =,...,n, where B i and R i are bounded uncertainty sets. To formulate the problem of detering the best uncertainty immunized transformation matrix for attitude deteration, we will use the weighted least square approach as used in () for the noal problem. We define the robust problem as C bi B i, r i R i, i =,..., n w i bi C r i subject to C T C = I,det(C) =. In order to take advantage of using a quaternion to simplify the optimization problem, as a first step, we (8) 588

3 Milano (Italy) August 8 - September, reformulate (8) introducing the quaternion q using the same approach used to derive (3). We get { } w i ( b T i q b i + r i T r i ) q T Kq bi B i, r i R i, i =,..., n subject to q T q =, where (9) [ B + K := BT tr( B)I ] z z T tr( B), B := w i bi r i T, z := w i ( b i r i ). () In this formulation, similar to (4), K R 4 4 is also a symmetric indefinite matrix, while B R 3 3 and z R 3 depend on uncertain input vectors. In contrast to (), the first term in (9) is a function of optimization variables and cannot be neglected in the optimization problem formulation. 3. UNCERTAINTY MODELING To develop a tractable method of solving (9), we define an affine uncertainty parameterization of the uncertainty sets B i and R i. A general description of such an affine parameterization is given now (Ben-Tal, Ghaoui, and Nemirovski, 9). Let β i := [β i,β i,β i3 ] T and ρ i := [ρ i,ρ i,ρ i3 ] T are vectors of perturbation variables used for uncertainty parameterization, then B i := { b i = b i + R i := { r i = r i + β ilˆbl : β i γ bi }, l= ρ ilˆr l : ρ i γ ri }, () l= where γ bi and γ ri represent uncertainty bounds for the input vectors in the body frame and reference frame, respectively. This type of uncertainty is called an interval uncertainty and correspondingperturbation set represents a box (Ben-Tal et al., 9). The interval uncertainty can be considered a suitable representation for a mix of different type of errors, such as sensor noise, having a stochastic interpretation, biases andmisalignments, which are constant and modeling inaccuracies, etc. However, bounds for each measurement or model vector should be carefully chosen, as unnecessary large values may result in large residual between the non-robust and the robust solution for noal cases. As we considered all vectors in the body and reference frame to be unit vectors, we choose ˆb l and ˆr l as ˆb = ˆr = [ ] T,ˆb = ˆr = [ ] T,ˆb 3 = ˆr 3 = [ ] T. With this choice, γ bi and γ ri represent the imum possible change in each value of b i and r i as a percentage of the input vector magnitude. Although, each perturbation variable in the body and reference frame has different bound, we can use normalization to obtain a single perturbation variable for uncertainty parameterization. For this, we normalize β il and ρ il with γ bi and γ ri, respectively. The new normalized perturbation vector is δ := [δ,δ,δ 3 ], such that δ. Using this normalization, we can represent the above mentioned sets B i and R i as B i = { b i = b i + R i = { r i := r i + δ l bil : δ }, l= δ l r il : δ }, () l= where b il := γ biˆbl and r il := γ riˆr l. 4. SOLUTION OF THE ROBUST PROBLEM To solve (9), we transform the robust - problem into aimization problem. Inthis regard, firstly, we will derive an expression for the uncertain matrix K, similar to the K matrix of (3), using the presented uncertainty model. This will provide a basis for the subsequent results that describe the simplified but a suboptimal formulation obtained for the robust problem (9). Using the uncertainty model described in Section 3, matrix K defined in (), can be written as K = K + (δ l Kl + δl K l s + δ l δ k Kc jk ), (3) l= where the superscripts s and c represent matrices with square and cross terms in δ,k = l+ if k 3 or k = l+ n if k > 3. The matrices K l, K s l, K c lk R4 4 are given as: [( Bl + K l := B l T tr( B ) ] l )I z l z l T tr( B, l ) [( K l s st Bs := l + B l tr( B ) ] l s )I z l s z l st tr( B, l s ) [( K lk c ct Bc := lk + B lk tr( B ) ] lk)i c z lk c tr( B. (4) lk) c z ct lk The other terms used in these expressions are givens as: B l := B c lk := z l := z lk := w i (b i r il T + b il ri T ), B l s := w i ( b il r ik T + b ik r il), T w i bil r il, T w i {(b i r il ) + ( b il r i )}, z l s := w i {( b il r ik ) + w i ( b il r il ), w i {( b ik r il ). (5) It can be observed that Kl depends on measurements, but Ks l and K lk c depend on uncertainty modeling and remain fixed for different measurement vectors, provided the uncertainty description remains same. We now present two results using (3), that will be used to reformulate (9) in terms of the proposed uncertainty model. 589

4 Milano (Italy) August 8 - September, Lemma. The formulation given in (9) is equivalent to q subject to q T q =, { q T Kq+ δ (pt δ + δ T Qδ)} (6) where p R 3 and Q R 3 3 depend on q and are defined as c q T K q p := c q T K q, c 3 q T K3 q c s q T Ks q qt Kc q qt Kc 3 q Q := qt Kc q c s q T Ks q qt Kc 3 q qt Kc 3 q. qt Kc 3 q c s 3 q T Ks 3 q In this equation, c l, c s l are constants that depend on the uncertain vectors and are given as: c l := c s l := w i (b T b i il + b T ilb i + ri T r il + r ilr T i ), w i ( b T il b il + r il T r il ). (7) Proof. We start with the cost given in (9). Expanding the first term, we get w i ( b T b i i + r i T r i ) = w i (b T i b i + ri T r i ) + (δ l c l + δl c s l ) (8) Using (8) and (3) and joining similar terms in δ l we can write the required expression. The robust problem (6) is always feasible because the solution of the noal problem (3) always exists (Shuster and Oh, 98). However, finding the optimal imum value of p T δ + δ T Qδ in (6) is difficult as the term is nonconcave in δ (Q is positive semidefinite). Based on this fact, we detere an upper bound on the imum of (p T δ + δ T Qδ) over δ. The result is given in the following lemma. Lemma. An upper bound on the imization term appearing in (6) is l= δ (pt δ + δ T Qδ) p + 3λ (Q) (9) Proof. The left hand side in (9) can be written as δ (pt δ + δ T Qδ) δ pt δ + δ δt Qδ () Using the Hölder dual norm (Boyd and Vandenberghe, 4), the first term on the right hand side in () is given as δ pt δ = p. () For the second term appearing in (), since Q is a symmetric matrix, we can write the imum eigenvalue of Q as (Boyd and Vandenberghe, 4) λ (Q) = sup δ T Qδ δ. () Hence, we first replace the -norm in the second term on the right hand side of () with the -norm using the inequality δ 3 δ for δ R 3. We can write δ δt Qδ δ δ T Qδ 3 3λ (Q), Using () and (3), we can write (9). (3) Lemma gives an upper bound on the imum value of the quadratic term p T δ+δ T Qδ and can be used to simplify (6). Theorem. The - problem (6) can be approximated with the following imization problem q,u,u,u 3 q T Kq + u + u + u 3 + 3λ (Q) subject to q T q =, u j p j u j,j =,,3, where p j,j =,,3 are elements of vector p. (4) Proof. In formulation (6) given in Lemma, replacing the term with the upper bound given in Lemma and using the fact that a set of n+ linear inequalities u j x j u j, j u j,j =,...,3 represent the nonlinear inequality j x j (Ben-Tal, Ghaoui, and Nemirovski, 9, definition.3.), we can write (4). The argument q of the approximate robust problem (4) approaches the argument of the noal problem (3) when the bound on the uncertainty approaches zero. This becomes evident if, instead of taking δ, we assume a bound α on each element of the normalized vector δ. In that case we can write (9) as δ α (pt δ + δ T Qδ) α p + 3α λ (Q). (5) If α tends to zero, the right hand side of (5) will also tend to zero, making the robust problem equivalent to (3). Since the cost and constraints in (4) are nonlinear, we solve it using a nonlinear optimization solver with a good initial guess. This initial guess for q is obtained from the non-robust form of the problem using the q-method or QUEST (Shuster and Oh, 98), etc. 5. SIMULATION RESULTS We consider the problem of attitude estimation for a low cost CubeSat (Heidt et al., ), a pico class of satellite having three units each of dimension cm 3 and weight less than kg. CubeSats are mainly used for scientific missions and currently most of the satellites are beingdeveloped inuniversities. Beinglow cost, interestin suchsatellites is increasingfor awiderclass ofapplications; however, there are many challenges from an estimation and control point of view to successfully achieve a given objective. The CubeSat under consideration is assumed to be moving in a circular orbit having a radius of 65 km from the center of the earth. We are considering that only two measurements are available in the satellite, namely the earth magnetic field and the sun vector. For the earth magnetic field, two magnetometers are installed, one inside the satellite, which is mainly used in the post- 58

5 Milano (Italy) August 8 - September, launch phase when the satellite is recovering from launch disturbances, while the second is installed on an extended boom, which is deployed once the satellite has de-tumbled and achieved an equilibrium. The sun vector is sensed by a pair of sun sensors installed on the satellite body. Both of these measurements are in the body frame. We used the orbit frame as the reference frame. For the reference earth magnetic field vector the first order IGRF model (Roithmayr, 4) is used, while the reference sun vector is obtained using a simplified sun model based on sun ephemeris (Wertz, 978). Sensor measurements are not accurate due to many factors, such as sensor noise, misalignments due to installation errors, biases, etc. Especially in the post-launch tumbling phase measurement errors further increase due to the use of an internal magnetometer installed on-board the satellite, which suffers from interaction with the magnetic field generated by surrounding electronics. Similarly, the reference vectors for both measurements are also not exact since the mathematical models used to obtain measurement vectors in the reference frame are based on loworder approximations. For simulation purpose, we consider that the bound on uncertainty in the measurements and reference vectors i.e. γ bi and γ ri, respectively to be % of the input magnitude. We take equal weights, such that n w i =. We need to solve the optimization problem formulated in (4). Since this formulation includes nonlinear objective function and constraints, we used the nonlinear optimization solver (fcon) of MATLAB. Two plots are presented to see the effectiveness of the proposed methodology. Firstly, the problem was solved for a given uncertain data set for one instant. To obtain the uncertain data set, we added a uniformally distributed random error in the range of ±γ bi and ±γ ri in each measured and model vectors obtained from a simulation without error. The vectors with added uncertainty are given as: b = [ ] T, r = [ ] T, b = [ ] T, r = [ ] T. Figures presents a comparison between the non-robust and the robust approach for one time instant. For this comparison, anon-robustsolution was obtained satisfying (3), while a robust solution was found satisfying (4). Then, two components ofthenormalized uncertainty vectorδ, i.e. δ and δ are varied, while keeping δ 3 constant for simulation purpose, and a variation in the cost J := n b i C r i is calculated for both solutions. This cost is plotted along the z-axis for both solutions, while x- and y-axis represent δ and δ. It can be observed from both plots that the cost J obtained using the noal solution has large variation when δ and δ are close to set bounds, while the J obtained using the robust solution shows the best immunization against uncertainty. However, the cost obtained for the robust solution has an offset when compared with the cost value obtained using the noal solution without error. Figure presents a performance comparison of the robust and non-robust approaches in the presence of uncertain- J δ.5.5 δ non-robust robust Fig.. Robust and non-robust design comparison for given set of uncertain vectors at one instant. φ ψ θ Time (sec).5 non-robust robust actual Fig.. Comparison of attitude angles obtained using nonrobust and robust algorithms. The dotted line shows the data without errors while the other two cases include errors within the uncertainty bound ±γ bi or ±γ ri. ties, using in-orbit data obtained from nonlinear closedloop simulation for the satellite. The error free data obtained from simulation was corrupted by adding uniformly distributed random errors in the range of ±γ bi and ±γ ri in corresponding vectors. The simulation was run with a sample time of second with initial roll, pitch and yaw body rates of.5,.5 and deg/s and roll, pitch and yaw angles of 3,, deg, respectively. It can be observed that due to the uncertainties in the input information, the non-robust approach gives large errors in the estimated attitude angles, while the robust approach gives much better performance, limiting the imum attitude error to a smaller band. 6. CONCLUSIONS We presented a formulation for robust attitude deteration with -norm bounded uncertainty in the input vectors. The robust optimization problem, using a weighted 58

6 Milano (Italy) August 8 - September, least square approach, has a nonlinear cost and constraints. The formulated - optimization problem was transformed into a suboptimal imization problem due to the use of an upper bound and solved using a nonlinear optimization solver with a good initial guess obtained from the noal solution. The results showed that the robust approach has significant benefit over the noal approach in the situations when inputs are uncertain with boundeduncertainty. Thebenefitis generallyimum in the worst case scenarios. However, the robust solution for the case without error has a larger residual than the nonrobust solution, which is expected in the robust optimal solutions. However, the main issue is increased computational burden in solving the optimization problem. Further work will focus on finding a computationally efficient solution of this problem. ACKNOWLEDGEMENTS The first author is thankful to the Higher Education Commission of Pakistan for supporting his PhD studies. REFERENCES Ben-Tal, A., Ghaoui, L.E., and Nemirovski, A. (9). Robust Optimization. Princeton University Press, USA. Boyd, S. and Vandenberghe, L. (4). Convex Optimization. Cambridge University Press. Crassidis, J.L., Markley, F.L., and Cheng, Y. (7). Survey of nonlinear attitude estimation methods. Journal of Guidance, Control and Dynamics, 3(), 8. Heidt, H., Suari, J.P., Moore, A.S., Nakasuka, S., and Twiggs, R.J. (). Cubesat: A new generation of pico-satellite for education and industry low cost space experimentation. In 5th Annual/USU Conference on Small Satellite. SSC-VIIIb-5. Keat, J.E. (977). Analysis of least-squares attitude deteration routine DOAOP. Technical Report NASA/CSC/TM-77/634. Markley, F.L. (988). Attitude deteration using vector observations and the singular value decomposition. Journal of Astronatutical Sciences, 36(3), Markley, F.L. (8). Optimal attitude matrix from two vector measurements. Journal of Guidance, Control and Dynamics, 3(3), Markley, F.L. and Mortari, D. (). New developments in quaternion estimation from vector observation. In Advances in the Astronautical Sciences, volume 6, p McLean, S., Macmillan, S., Maus, S., Lesur, V., Thomson, A., and Dater, D. (December, 4). The US/UK world magnetic model for 5-. Technical Report NOAA, NESDIS/NGDC-. Mortari, D. (997). ESOQ: A closed form solution to the Wahba problem. Journal of the Astronautical Sciences, 45(), Mortari, D. (). Second estimator of the optimal quaternion. Journal of Guidance, Control and Dynamics, 3(5), Roithmayr, C.M. (4). Contributions of spherical harmonics to magnetic and gravitational fields. Technical Report NASA/TM Shuster, M.D. and Oh, S. (98). Three-axis attitude deteration from vector observation. Journal of guidance and Control, 4(), Shuster, M.D. (6). The generalized wahba problem. Journal of Astronautical Sciences, 54(), Sidi, M.J. (). Spacecraft Dynamics and Control. Cambridge University Press, UK, nd edition. Wahba, G. (965). A least square estimate of spacecraft attitude. SIAM Review, 7(3), 49. Wertz, J.R. (978). Spacecraft AttitudeDeteration and Control. D. Reidel Publishing Company, Holland. Appendix A. DAVENPORT TRANSFORMATION To derive the Davenport transformation, consider the cost function given in () w i b T i Cr i = tr(w i b T i Cr i ). (A.) Now we use two properties of the trace. Firstly trace is invariant under cyclic permutations, and secondly, tr ( i A i) = i tr(a i) A R n n. Using these properties we can write w i b T i Cr i = tr(cb T ), (A.) where B := n w ib i ri T. Now we represent C using the quaternion q R 4 := [ ] T, q T q 4 where q := [q q q 3 ] T, written as (Shuster and Oh, 98) C = (q4 q T q)i + qq T + q 4 Q. (A.3) In the above equation Q := q, where represents vector cross product, is given as [ q3 ] q Q = q 3 q. (A.4) q q Substituting (A.3) in (A.), we get w i b T i Cr i = (q4 q T q)tr(b T ) + tr(qq T B T ) + q 4 tr(qb T ). (A.5) The second term on the right hand side of the above equation can be written as tr(qq T B T ) = q T (B T + B)q. The last term can be written as (A.6) q 4 tr(qb T ) = q 4 (q T z + z T q), (A.7) where z := n w i(b i r i ). Substituting (A.6) and (A.7) in (A.5), we get w i b T i Cr i = [ ] [ ] q T B T + B tr(b)i z q q 4 z tr(b)][ T q 4 = q T Kq, (A.8) which is the required form of (A.), where K R 4 4 is a symmetric, traceless matrix given as [ ] B K := T + B tr(b)i z z T. (A.9) tr(b) 58