Measurement Observers for Pose Estimation on SE(3)

Transcription

1 Measurement Observers for Pose Estimation on SE(3) By Geoffrey Stacey u Supervised by Prof. Robert Mahony 24 September 2010 A thesis submitted in part fulfilment of the degree of Bachelor of Engineering Department of Engineering Australian National University

2 This thesis contains no material which has been accepted for the award of any other degree or diploma in any university. To the best of the author s knowledge, it contains no material previously published or written by another person, except where due reference is made in the text. Geoffrey Stacey 24 September 2010 c Geoffrey Stacey

3 Acknowledgements I would like to thank my supervisors Rob Mahony and Jochen Trumpf for the tremendous amount of assistance they have provided regarding the development of the theory in this thesis. I would also like to thank Mohammad Zamani for his explanation of the numerical integrator adopted for the simulations. Thanks also to my friend Michael Webster and mother Pat Miethke for their assistance in editing this document.

4 Abstract This thesis develops a nonlinear observer for the problem of pose estimation; that is, the determination of the position and attitude of a rigid body from sensor measurements. The observer is derived on the Special Euclidean group, SE(3), and exploits the natural Lie groups geometry of the space in the structure of the observer. The observer uses body fixed frame measurements of known landmarks for the primary position measurement, and implements body fixed frame measurements of velocity, both angular and linear, as feed forward terms. A Lyapunov argument is used to prove local exponential stability of the error system, assuming that there are at least three non-collinear landmark reference points measured. The observer is well-behaved and stable with only one or two landmark reference points measured; however, the pose estimate is not guaranteed to converge to the true value in these cases. The characteristics of the observer and its stability are demonstrated by a series of simulations.

5 Contents 1 Introduction and Statement of Contributions 1 2 Literature Review Approaches for Pose Estimation Typical Sensors used in Pose Estimation Existing Filters for Attitude Estimation Existing Filters for Pose Estimation Measurement Observer for Attitude on SO(3) System Model Measurement Model Error Model Projected System on (S 2 ) n Output Observer for Attitude Estimation Lifting the Attitude Observer to SO(3) Stability Analysis of the SO(3) Observer Measurement Observers for Pose Estimation on SE(3) System Model Measurement Model Error Model Projected System on (R 3 ) n Output Observer Design Lifting the Observer to SE(3) Stability Analysis of the SE(3) Observer Geometry of the Error Trajectories Analysis of the Exceptional Set Simulation Studies Methodology Simulation 1: Three Non-Collinear Reference Points Simulation 2: A Single Reference Point Simulation 3: Two Reference Points Simulations 4A and 4B: Extreme Cases for Three Reference Points Simulation 5: Initial Conditions Outside the Region of Convergence i

6 Contents ii 6 Conclusion 50 A Background Theory regarding SO(3) 51 B Definition of Consistent and Complementary Measurements 53 C Measurement of Reference Directions from an IMU 54 D Review of Stability Theory 57 D.1 Stability in Autonomous Systems D.2 Invariant Sets D.3 Stability in Nonautonomous Systems E Proofs of Lemma and Lemma E.1 Proof of Lemma E.2 Proof of Lemma F Maximising the Region of Exponential Convergence for the SO(3) and SE(3) Observers 64 F.1 Maximising the Region of Exponential Convergence for the SO(3) Observer.. 64 F.2 Maximising the Region of Exponential Convergence for the SE(3) Observer.. 65 G Background Theory regarding SE(3) 66 H Code and Parameters Used for Simulations 69 H.1 Parameters Used for Simulations H.1.1 Simulation 1: H.1.2 Simulation 2: H.1.3 Simulation 3: H.1.4 Simulation 4A: H.1.5 Simulation 4B: H.1.6 Simulation 5: H.2 Code Used for Simulations References 81

7 List of Figures 3.1 Block diagram of the output observer for the case of attitude estimation, given by Equation Block diagram of the SO(3) observer, given by Equation Block diagram of the output observer for the case of pose estimation, given by Equation Block diagram of the SE(3) observer, given by Equation The geometry of pose estimation from a single reference point (a) and two reference points (b) Graphical representation of the Runge-Kutta approximation method. The gradient of f(t) at time t = t 0 is computed, and used to approximate f(t + τ 2 ) (as shown by the dark blue arrow). The result is used to approximate the gradient at f(t + τ ) (shown by the light blue arrow) and used to approximate 2 f(t + τ) (as shown by the red arrow) Error f error ( ˆX, X) = ˆX X for Simulation 1. This simulation had three non-collinear reference points True positions (blue) and estimated positions (red) for Simulation 1. The coloured circles mark the respective initial positions. The attitude estimates and true attitudes are periodically indicated by reference frames in magenta and green, respectively, with dashed lines indicating the x-axes and dotted lines indicating the y-axes. This simulation had three non-collinear reference points, marked by black asterisks Error f error ( ˆX, X) = ˆX X for Simulation 2. This simulation had a single reference point True positions (blue) and estimated positions (red) for Simulation 2. The coloured circles mark the respective initial positions. The attitude estimates and true attitudes are periodically indicated by reference frames in magenta and green, respectively, with dashed lines indicating the x-axes and dotted lines indicating the y-axes. This simulation had a single reference point, marked by a black asterisk Error f error ( ˆX, X) = ˆX X for Simulation 3. This simulation had two distinct reference points iii

8 LIST OF FIGURES iv 5.7 Two different views of the true positions (blue) and estimated positions (red) for Simulation 3. The coloured circles mark the respective initial positions. The attitude estimates and true attitudes are periodically indicated by reference frames in magenta and green, respectively, with dashed lines indicating the x- axes and dotted lines indicating the y-axes. This simulation had two reference points, marked by black asterisks Error f error ( ˆX, X) = ˆX X for Simulation 4A. This simulation had a cluster of three non-collinear reference points far away from the true position True positions (blue) and estimated positions (red) for Simulation 4A. The coloured circles mark the respective initial positions. The attitude estimates and true attitudes are periodically indicated by reference frames in magenta and green, respectively, with dashed lines indicating the x-axes and dotted lines indicating the y-axes. This simulation had a three reference points in a far-away cluster, not visible in the range of the graph Error f error ( ˆX, X) = ˆX X for Simulation 4B. This simulation had three non-collinear reference points, but two were very close to each other. This made the scenario similar to that of Simulation True positions (blue) and estimated positions (red) for Simulation 4B. The coloured circles mark the respective initial positions. The attitude estimates and true attitudes are periodically indicated by reference frames in magenta and green, respectively, with dashed lines indicating the x-axes and dotted lines indicating the y-axes. This simulation had three reference points, marked by black asterisks. Two of these were very close to each other Error f error ( ˆX, X) = ˆX X for Simulation 5. This simulation had four non-collinear reference points, but initial conditions such that it converged to a point in the exceptional set, as defined in Section A constant true position (blue) and estimated positions (red) for Simulation 5. The coloured circles mark the respective initial positions. The attitude estimates and true attitudes are periodically indicated by reference frames in magenta and green, respectively, with dashed lines indicating the x-axes and dotted lines indicating the y-axes. This simulation had four non-collinear reference points, marked by black asterisks. The pose estimate converges to a point in the exceptional set, as defined in Section

9 Nomenclature α z i innovation term used to correct the estimate of the observer 3 1 vector representation of the homogeneous vector z i d X f(x) differential of f with respect to X Ṙ derivative of the rotation matrix with respect to time gradŷ f gradient of the function f with respect to Ŷ ˆR estimate of R, X Riemannian metrix, defined on the tangent space of the point X P so(3) S U projection onto so(3) set of points in the surface of the unit sphere exceptional set µ i Gaussian noise in the measurement y i Ω Ω R se(3) angular velocity of a vehicle skew symmetric matrix such that, for all v R 3, Ω v = Ω v the set of real numbers Lie algebra of SE(3) so(3) Lie algebra of SO(3). Set of skew-symmetric matrices stab Z stabiliser of Z τ ξ {A} {B} {E} I k time step used in simulations position of the vehicle inertial reference frame body-fixed reference frame error frame of reference (estimate of frame {A} with respect to the vehicle) identity matrix positive constant for gain of the observer v

10 LIST OF FIGURES vi M R R R π t output space of the system rotation matrix transpose of R rotation matrix representing a rotation through 180 degrees time T X M tangent space of M at the point X U V v X Y y i Z z i A TB C velocity of the vehicle, consisting of angular velocity Ω and translational velocity v Lyapunov function translational velocity of the vehicle matrix representation of the pose of the vehicle (element of SE(3)) matrix composed of the measurements y 1,..., y n stacked horizontally measurement of z i with respect to the body-fixed-frame matrix composed of the references z 1,..., z n stacked horizontally reference point or direction (depending on context) pose or orientation (depending on context) of frame {B} with respect to frame {A}, expressed in frame {C} SE(3) Special Euclidean group for three-dimensional space. Refer to Appendix G SO(3) Special Orthogonal group of three dimensions. Refer to Appendix A EKF extended Kalman filter GNSS global navigation satellite system GPS IMU INS UAV global positioning system inertial measurement unit inertial navigation system unmanned aerial vehicle UKF unscented Kalman filter

11 Chapter 1 Introduction and Statement of Contributions The growing field of robotics has a strong focus on providing assistance in a wide range of tasks, many of which are difficult, dirty or dangerous for humans to undertake directly. Robotic vehicles have been implemented to address a number of such scenarios. Some examples of where robotic systems may be employed include exploring areas unreachable by humans (such as the surface of Mars, or the ocean floor), surveying and repairing structures (such as dam walls), and security systems. Such operations often demand that the vehicle is able to reliably navigate its environment and that its movements are well controlled, particularly to avoid collision. These tasks may be further complicated by the features of the particular environment (such as ocean currents, or the presence of animals), and may also require the vehicle to actively interact with its surroundings. Some fields of robotics attract significant funding, and can ensure reliability and robustness by using high-grade sensors and control technologies. However, other fields can have limited resources and will therefore need to rely on sophisticated control and sensing algorithms, rather than expensive sensing systems and vehicle hardware. Retrieving data (such as images) of the vehicle s surroundings for interpretation by a human driver can be difficult and the data may not have sufficient quality or speed for piloting. Manual control can also lead to human error. The need arises for autonomous navigation and control systems that can cope with noisy sensor measurements. They must also be able to recover from a poor initial estimate of the environment or robot state (or perhaps an event, such as a collision, that has displaced the vehicle in an unknown manner). An accurate estimation of the vehicle s pose (position and orientation) may enable the control system to avoid collisions as well as to plan optimal or safe trajectories. Some of the typical design considerations in developing onboard pose estimation systems include cost, weight, and size, with the latter two having a significant impact on the maneuverability of a flying vehicle. These considerations also apply to onboard computers, and hence place a notable restraint on the computational demands of the algorithms involved. Since a flying vehicle requires rapid updates on its estimated pose, another key concern becomes the efficiency of the estimation process. Cost restraints often place limitations on the quality of the sensors used, and hence the estimation algorithm also needs to be robust against noise present in any measurements. Some examples of the typical sensors employed are inertial measurement units (IMUs), on-board cameras, and global positioning systems

12 2 (GPSs). Attitude estimation for a vehicle is a sub-problem of pose estimation and has seen a significant amount of attention in recent years. It has commonly been addressed by the use of an Extended Kalman Filter (EKF) or variations of this such as the Unscented Kalman Filter (UKF) (as in [11]). However, such approaches have encountered various problems regarding efficiency and errors due to the linearisation of the noise model [32]. More recently, nonlinear observers have been applied to this problem, with remarkable success [17, 18, 27, 30, 20]. Whilst variations of the Kalman Filter are still popular for the problem of pose estimation (as used in [22]), nonlinear observers appear to be a promising alternative approach to this problem. The paper [14] considers the more general problem of designing a nonlinear observer for an invariant system on a finite-dimensional connected Lie group. This thesis seeks to apply the theory given in [14] to the problem of pose estimation, developing an observer on the Special Euclidean group, SE(3) (this is the set of matrices representing rigid body transformations in 3 dimensional space). The resulting observer will be expressed solely in terms of measurements to known reference points. It is proven that the derived observer is locally exponentially stable. Additionally, this thesis includes an analysis of the geometry involved with the observer, and of the set of points for which the observer will not converge. The geometrical interpretation is useful for understanding the constraints on the setup of reference points. The study of the exceptional set is important for understanding how the observer could potentially fail. The properties of the observer are demonstrated through the use of simulations. The work in this thesis is organised as follows. Chapter 2 provides a review of existing literature relevant to both attitude and pose estimation, and discusses the merits of the various available approaches. In Chapter 3, the similar but simpler problem of attitude estimation is considered. The method presented in [14] is applied to develop an observer on the Special Orthogonal group (SO(3)), which is the set of rotation matrices. A proof of local exponential convergence is provided for the observer, which is achieved through the use of Lyapunov stability analysis. Chapter 4 proceeds to develop an observer for pose estimation on SE(3). Local exponential stability is proved following a very similar method to that used for the SO(3) case. This chapter also contains a discussion regarding the geometry of the problem of pose estimation, which provides insight to the expected behaviour of the observer. It concludes with an analysis of the set on which the estimate does not converge to the true value. The performance of the observer is then demonstrated by a series of simulations in Chapter 5. Chapter 6 brings the work in this thesis to a conclusion and discusses ideas for future developments.

13 Chapter 2 Literature Review This chapter examines the existing work related to pose estimation. It begins with a brief discussion of how the problem can be approached, and in doing so establishes the need to understand the particular problem of attitude estimation. This divides into two sections: research regarding the sensors commonly employed in attitude and pose estimation; and an overview of numerous studies regarding the filters used for attitude estimation, and how these make use of the measurements available. The final section of this chapter considers existing work on pose estimation. 2.1 Approaches for Pose Estimation Primarily, there are two strategies for pose estimation. One is to consider the problem as a whole, estimating the position and attitude as a single six degree-of-freedom variable in the Special Euclidean group SE(3) [1]. The alternative is to treat it as two separate sub-problems, estimating position and attitude independently (as in [31, 29]). The first approach uses all measurement information explicitly in the full estimation of pose, undertaken in a consistent and coherent manner. This has the advantage of balancing errors in measurements across the full state estimate. The second approach splits the measurements into some that contribute to attitude and some that contribute to position. This has the advantage of isolating part of the state estimate (typically the attitude estimate), from potentially large errors in some of the measurements (typically target positions). The most appropriate approach for a particular application is very dependent on prior knowledge regarding the nature of the measurement errors. It also depends on the desired properties of the observer. For example, for an unmanned aerial vehicle (UAV) flying high above the earth, it is often highly desirable that the attitude estimate is of high quality, while a position error of some metres is of little concern. In contrast, for a small quadrotor vehicle manoeuvring in a cluttered three dimensional environment with many obstacles, the combined pose of the vehicle is crucial position error can lead to an immediate collision, whilst attitude error can lead to uncontrolled acceleration and an equally destructive outcome. The focus of this thesis is on the development of observers on the full state in SE(3). However, the full pose estimation problem has much in common with the pure attitude subproblem of the separated case. Hence, this sub-problem will also be covered in detail to provide a simpler example of how the theory may be applied, and to provide context for the full pose case. The problem of position estimation is much simpler and of less interest, so no

14 2.2 Typical Sensors used in Pose Estimation 4 literature review of position estimation is provided. 2.2 Typical Sensors used in Pose Estimation This section discusses some of the sensors available for estimating the pose of a mobile robotic vehicle. These sensors are typically attached to the vehicle and provide information regarding the environment around it. Sensors commonly used for this application include inertial measurement units (IMUs, which contain accelerometers, rate gyros and magnetometers), cameras, and GPSs. This section discusses the properties of these sensors in order to achieve a better understanding of how filters for pose (or attitude) estimation might be implemented. Inertial measurement units are popular devices used to obtain on-board measurements of a vehicle s angular velocity and acceleration. Some applications can be found in [22, 20, 31, 17, 4]. They are favoured for their high data rate, low weight, and low cost [31, 17, 25]. A measurement of attitude can be obtained from the accelerometers and magnetometers, by using the directions of gravity and the earth s magnetic field as reference. Change in pose can theoretically be found by the integration of the acceleration and angular velocity readings. However, the rate gyros are often subject to bias (meaning that the integration accumulates error over time), and measurements from the accelerometers and magnetometers typically include high-frequency noise [7]. Magnetometers can also suffer from the interference of local magnetic fields [7, 25]. The problems of stochastic noise and bias estimation are often addressed separately; some examples of where bias is considered are [20, 16, 17, 27]. Cameras are typically used to obtain measurements or bearings to external landmarks, by application of computer vision techniques (as in [1, 23, 22, 4]). Image-flow analysis can be applied to measure velocity [23]. Whilst cameras are capable of collecting substantial information, processing the images is computationally expensive, and can limit the rate at which the position or attitude estimates are updated[6, 1]. However, cameras can be used to aid the inertial navigation from an IMU, providing low-rate adjustments to the estimate [22]. Global positioning systems employ satellites to measure the position of the vehicle. However, their reliability is extremely dependent on the environment; if the vehicle is in a canyon, surrounded by high-rise buildings, in a tunnel or otherwise concealed from the sky then the measurement is often highly inaccurate or unavailable [25, 1, 4]. A global position system is sometimes employed to correct position estimates from IMUs, adjusting for errors that might accumulate from the integration of biased velocity measurements [31, 11]. The IMU allows the estimate to be updated at a higher rate and can maintain reasonable accuracy for periods in which the GPS signal is unusable (such as when the vehicle passes under a bridge) [31]. 2.3 Existing Filters for Attitude Estimation A significant body of research related to the problem of pose estimation has been carried out over the past twenty years. Much of this work has addressed the sub-problem of attitude estimation estimating the orientation of a vehicle from measurements obtained by on-board

15 2.3 Existing Filters for Attitude Estimation 5 sensors. This problem is now reasonably well understood. Many of the applications for this problem are similar to those of pose estimation, with a focus on spacecraft and unmanned aerial vehicles (UAVs) such as in [7, 17, 4, 23, 20]. Much of the theory here is also applicable to the larger problem of pose estimation. The set of all possible unique orientations for a vehicle in three-dimensional space can be modelled by the Special Orthogonal Group (SO(3)), which consists of all three by three rotation matrices. This group is compact; that is, it is closed and bounded as a subset of R 3 3. This property is related to the fact that a 360 rotation returns an object to its initial orientation. The group SO(3) is also related to the set of unit quaternions, the surface of a unit sphere in R 4 (quaternions are discussed in [21]). The curved and compact nature of SO(3) makes the problem of attitude estimation a nonlinear one, complicating the development of successful filters. The Kalman filter cannot be applied to nonlinear problems, but some success has been found using Extended Kalman Filter (EKF) techniques ([22]) and Unscented Kalman Filters (UKF) ([11]). Other approaches include particle filters ([10]), and, with an increasing focus in later years, nonlinear observers (as in [17, 18, 7, 27, 30, 20]). The basic idea of the Kalman filter is to use a model of a system, along with the previous state estimate and new measurements, to predict the system s state in the following time step. It uses the covariance of the noisy measurements to propagate a weighted average of the predictions, along with an estimate of the uncertainty involved. However, this propagation assumes the system to be linear. The goal of an Extended Kalman filter is to provide a similar approach for nonlinear systems, and is discussed in [32, 25]. It uses a nonlinear model of the system to predict the state in the next time step, and then propagates the noise distribution through a linearisation of this system model. The linearisation process contains a significant computational cost in computing the matrix Jacobian. The linearisation of the noise distribution also leads to errors in the state estimate. In [22], an EKF is developed that utilises inertial measurements, and multiple camera poses with stationary landmarks (of unknown location), in order to produce an estimate of the system s state. This work achieves computational complexity that is linear with the number of reference features identified, and cubic with the number of states in the state vector. A method called delayed linearisation (whereby several time steps of data are collected before producing an estimate of the Jacobian) is used to reduce the effect of linearisation inaccuracies. However, while reduced, the issues of computational complexity and inaccuracies from locally linear approximations are still present in the result. The approach of an unscented Kalman filter (UKF) is to take sample points from around the mean of the state measurements, and to apply the nonlinear transformation to these. A method known as the unscented transformation is then implemented to acquire an estimate of the mean and covariance in the new state. This saves the computational expense of explicitly calculating the Jacobian (as is done for the EKF), and gives better results in highly nonlinear systems [32, 11]. A drawback of this approach is that it assumes the noise to be roughly Gaussian, and can fail if the distribution is skewed or multimodal [2, 32]. This UKF approach has seen application in [11]. Particle filters provide a way of handling problems in which the noise is truly non-

16 2.3 Existing Filters for Attitude Estimation 6 Gaussian. The general idea of this approach is to take samples, called particles from the state distribution and, at each time step, update their locations by applying drift and diffusion processes. They are then allowed to drift towards local maxima in the state distribution before being sampled again and redistributed according to the estimate [10]. Particle filtering has seen good success in highly nonlinear problems with non-gaussian noise, but it is suboptimal and computationally expensive due to the diffusion and resampling processes [2]. In [10], a particle filter is implemented and its performance is compared to that of a UKF. The results suggest that the UKF converges to a lower error than a particle filter, because the particle filter continuously introduces new variations, but otherwise the particle filter converges much faster. The result of the UKF is dependent upon the choice of certain parameters, which can be difficult to select. The authors of [10] suggest in the conclusion that it may be worth applying a particle filter to acquire a rapid convergence and then to increase the accuracy of the result by switching to a UKF. The problems encountered with the above approaches have resulted in a growing interest in nonlinear observers. An early attempt to design a nonlinear observer for angular velocity was undertaken in [26]. Since then, much work has been done to derive nonlinear observers for attitude estimation (for example, [17, 18, 27, 30, 20]). These observers typically use an IMU to obtain measurements of various reference directions, such as gravity and the Earth s magnetic field. The estimate of attitude is then derived from these measurements, using either the quaternion or the matrix representations of SO(3). For the attitude to be properly estimated, at least two (linearly independent) stationary reference directions are required. If only a single stationary reference direction is provided, the attitude can be determined down to a rotation about this axis [17]. One of the very attractive properties of these observers is that they are often accompanied by proofs for almost global stability, with local exponential convergence. This means that, apart from a few unstable points (forming a set of measure zero) [17], any initial estimate of attitude will converge to the true value. Noise in the measurements and slight changes in attitude can be enough to cause the estimate to diverge from the unstable points. The unstable points result from the geometry of SO(3), and it is proven in [3] that global stability cannot be achieved. An interesting complication for nonlinear observers is the presence of bias in the measurement noise. It is considered in [20, 16, 17, 27], usually as an extension of the basic observer. Proof of convergence of the bias estimates to the true bias values is typically achieved using La Salle or Barbalat type arguments in a Lyapunov stability analysis, as in [17]. Many nonlinear observers achieve good reliability and robustness to noise from what has been termed a complementary filter design [20, 16, 17, 18, 7]. This design combines the gyro measurement of angular velocity (which is valid for high frequencies, but not for low frequencies due to the bias) with a measurement of attitude derived from acceleration and magnetometer readings (which is valid at low frequencies but suffers from high-frequency noise in its computation). The result is an estimate of attitude that is valid over the entire frequency domain. Further work on complementary filters can be found in [16, 17, 7]. The result of this

17 2.4 Existing Filters for Pose Estimation 7 work is an observer termed the explicit complementary filter, which saves computation of the attitude measurement and instead derives the attitude estimate in terms of the direct vectorial measurements, as obtained from an IMU. In [18], the observability of a system with time-varying reference directions is considered. This work determines a condition for the attitude of a vehicle to be fully observable, given a single time-varying reference direction. 2.4 Existing Filters for Pose Estimation The problem of pose estimation is a fundamental problem in robotics, and is considered in [2, 29, 31, 22, 9, 24]. The classical approach is to use extended Kalman filtering techniques, with more recent work using some nonlinear filtering techniques such as unscented Kalman filters and particle filtering. An example of where such filters have been employed is [22], which uses a single filter for pose estimation rather than developing separate filters for position and attitude. In the case where a GPS signal is available, the problem is often considered as a data fusion problem for a global navigation satellite system (GNSS) and an inertial navigation system (INS), as discussed in [13]. There is an extensive body of research into this problem using the classical approaches, as in [22, 9]. A related problem is where the position information is obtained by a location measurement of global beacons. This problem is more specific to robotic applications, and has also been treated in depth using classical techniques [24, 9, 15]. More recently, nonlinear observers are being considered for the problem of pose estimation. An example of work that considers the attitude and position components of pose estimation separately is [31]. This approach used the integration of a GPS and an inertial navigation system (INS) to develop a nonlinear observer, and achieved global exponential stability. The paper [29] also approaches the problem of pose estimation by developing nonlinear position and attitude observers separately. It proves almost global asymptotic stability of the errors, with exponential convergence for any closed ball within the region of attraction. The work extends to the case of bias in the velocity measurements, and achieves exponential convergence of the bias estimates to the true values. Little work has been found that develops a single nonlinear observer for full pose estimation. In [1], an observer on SE(3) is presented that uses measurements of velocity and bearings to landmarks. A proof of local asymptotic stability is provided, but bias is not considered. Later, [2] presented a number of observers for the problem of pose estimation. These included both the approach considering position and attitude estimation separately, and the approach of using a single observer for pose estimation. The observers here achieved almost global asymptotic stability with local exponential stability. A substantial contribution to the theory associated with pose estimation observers is given by [14]. The reference [14] considers the more general problem of designing a nonlinear observer for an invariant system on a finite-dimensional connected Lie group. The paper approaches the problem by projecting the system onto an observable quotient space, and developing an observer for this simplified case. It proceeds by lifting the derived filter into the original space to acquire the desired observer.

18 Chapter 3 Measurement Observer for Attitude on SO(3) This chapter provides background theory by deriving a nonlinear attitude observer on the Special Orthogonal group SO(3). Background theory regarding SO(3) can be found in Appendix A. This chapter begins by presenting a model of the system, and of the measurements available to the attitude observer. It also provides a model of the error present in the attitude estimate. This chapter then proceeds to derive a filter using the theory developed in [14]. This derivation was provided in [14] as an example of how the theory can be applied. This chapter also proves local exponential convergence of the attitude estimate to the true attitude, by use of a Lyapunov argument. The derivation of the SO(3) filter, as well as the proof of its convergence, follow similar methods to those applied for the problem of pose estimation in the next chapter. 3.1 System Model This section discusses the way in which this thesis models the orientation of a moving vehicle. Let {A} denote the inertial reference frame, and let {B} denote a reference frame that is fixed to the body of the vehicle. The rotation matrix representing the orientation of frame {B} with respect to the inertial frame {A} shall be denoted R = A R B SO(3). Refer to Appendix A for background theory regarding rotation matrices. Remark In this thesis, the notation A TB C is used to indicate that T describes the pose (or the orientation, alone) of frame {B} with respect to frame {A}, and that it expresses this in frame {C}. If frame {A} is the same as frame {C}, then the notation is simplified to A T B, as in the case above. Often, once the frames {A} and {B} have been established, this will be abbreviated to T. Let Ω = A Ω B B be the angular velocity of the body-fixed-frame {B} with respect to the inertial reference frame {A}, expressed in frame {B}. The term (Ω) denotes the skewsymmetric matrix such that, for any vector v R 3, Ω v = Ω v, i.e., Ω = 0 Ω 3 Ω 2 Ω 3 0 Ω 1 Ω 2 Ω 1 0. (3.1)

19 3.2 Measurement Model 9 The system being modelled may now be represented as, Ṙ = RΩ, (3.2) where Ṙ denotes the time derivative of the rotation matrix R. 3.2 Measurement Model This section develops a model of the measurements available to the observer. One measurement is of the angular velocity Ω as defined previously. The other measurements are of stationary reference directions, and are also expressed with respect to frame {B}. Appendix C reviews how these measurements are typically obtained using an IMU. Suppose there exists a stationary reference direction z = A z S 2 that is known a-priori with respect to the inertial frame {A}. The direction z is an element of the unit sphere, and may be represented as a 3 1 unit vector that lies in the reference direction. In a similar way, let y = B y S 2 be a (noiseless) measurement of this reference direction, made by the sensors on the vehicle. Since the sensors will measure this direction with respect to the body-fixed-frame {B}, y may be found by applying the appropriate rotation to z, y = R z. (3.3) This type of measurement is termed a complementary measurement with respect to the system dynamics described by (3.2). The right hand side of this equation is termed right-invariant [18]. Both of these terms are defined and explained in Appendix B. In order for the attitude to be fully observable, it is necessary for there to be more than one stationary reference direction ([18, 17]). The reference directions expressed with respect to frame {A} shall be denoted z 1,..., z n, and the measurements of the reference directions with respect to frame {B} shall be denoted y 1,..., y n. Furthermore, let Z be the matrix formed by stacking the reference directions horizontally, and Y be the matrix formed by similarly stacking the measurements, i.e., Z = ( z 1 z n ), and Y = ( y 1 y n ). It is straightforward to see that the form of (3.3) still holds, i.e., Y = R Z, (3.4) since R is applied to each column separately. The derivative of Y with respect to time is then given by, Ẏ = ( ẏ 1 ẏ n ). The measurements of Ω and Y are taken from onboard sensors (as discussed in Appendix

20 3.3 Error Model 10 C), and as such will include some degree of noise in reality. It shall be assumed that this noise is Gaussian, and that it has zero mean (bias correction is beyond the scope of this thesis). The noise for each measurement y i shall be denoted by µ i, and is modelled by adding it to the true value. Adjusting equation (3.3) to account for noise gives, y i = R z + µ i. Note that this noise must be such that y i is a unit vector. Similarly, the noise µ Ω in the measurement of angular velocity can be modelled by, Ω y = Ω + µ Ω, where Ω y is the measurement of the true value Ω. Note that the theory in this chapter will be carried out using the true values, Y (as given in (3.4)) and Ω. It is assumed that the results are still valid if there is Gaussian noise present in the measurements. 3.3 Error Model This section defines the error in the attitude estimate. Let the reference frame {E} be the estimate of the inertial reference frame with respect to frame {B} (i.e. the estimate of where the inertial reference frame lies with respect to the vehicle). Then, the estimate ˆR = E R B of the rotation matrix A R B is a map from frame {B} to frame {E}. The error in the attitude estimate is a function E( ˆR, R) : {A} {E} and shall be defined by, E( ˆR, R) := ˆRR. (3.5) Here, E( ˆR, R) may be thought of as the rotation from the inertial reference frame {A} to frame {E}. Hence, the attitude estimate ˆR is equal to R if and only if E( ˆR, R) = I (recalling that the matrix I represents a rotation of zero). The goal of this chapter is therefore to design an observer such that R converges to I. 3.4 Projected System on (S 2 ) n The next few sections apply the theory in [14] to develop an attitude observer on SO(3). The approach used in [14] involves projecting the system onto its output space, which will be done in this section. It is much simpler to derive an observer on this output space than on the full state space. Once an output observer has been developed, it may be lifted to the full state space described by (3.2) and (3.4). Consider the system described by, Ṙ = RΩ, (3.6)

21 3.5 Output Observer for Attitude Estimation 11 with complementary observations, Y = h Z (R) = h(r, Z) = R Z. (3.7) The output space of this system is the space of all possible values for Y. Since each of the n columns of Y is a 3 1 vector describing a point on the unit sphere, the output space is (S 2 ) n. In the above expression, h(r, Z) is termed a right group action, which means that it satisfies the following two properties, for all homogeneous points Z (S 2 ) n : (i) h(i, Z) = Z, where I is the identity transformation on SO(3), and, (ii) h(r 2, h(r 1, Z)) = h(r 1 R 2, Z), for all R 1, R 2 SO(3). The first property of h follows from the fact that the identity transformation on SO(3) is given by the 3 3 identity matrix I. The second property can be checked by computation, h(r 2, h(r 1, Z)) = h(r 2, R1 Z) = R 2 R 1 Z (3.8) = h(r 1 R 2, Z). The fact that h is a right group action implies that the projected dynamics can be expressed as a function of Y and Ω. Theorem 3.3 of [14] states that a system of the form Ẋ = Xu, with complementary observations y = h(x, y 0 ), will project to the system ẏ = T X h y0 (Xu), X h 1 y 0 (y) on M (where M is the output space of the system). Here, T X denotes the tangent space of M at the point X. This theorem may be applied to the system described by (3.6) and (3.7). The result is that the system can be projected onto its output space, with dynamics given by, Ẏ = T R h Z (RΩ ) = Ω R Z (3.9) = Ω Y. 3.5 Output Observer for Attitude Estimation This section demonstrates how the theory in [14] can be applied to obtain an observer on the output space of the projected system given by (3.9).

22 3.5 Output Observer for Attitude Estimation 12 Let the estimate of Y and the derivative of this estimate be denoted by, ( Ŷ = ŷ 1 ŷ n ), and Ŷ = ( ŷ1 ŷn ), (3.10) respectively. Furthermore, let ˆR denote the estimate of R as discussed in Section 3.3. Hence, the estimate of Y may be expressed as, Ŷ = ˆR Z, (3.11) and its dynamics may be derived in a similar fashion to those in (3.9), Ŷ = T ˆRh Z ( ˆRΩ ) = Ω ˆR Z (3.12) = Ω Ŷ. In [14, Equation 5.1], the suggested form of the output observer is, Ŷ = T ˆXh Z ( ˆXu) gradŷ f(ŷ, h Z(X)). (3.13) Here, f : M M is a cost function on Ŷ and Y (noting that h Z(X) = Y ), where the output space of the system (given by (S 2 ) n ) has been denoted M. The function f is used to measure the error between the estimate and the true value of Y. The chosen cost function must satisfy the following conditions: (i) f has a unique global minima at Ŷ = Y (ii) f is invariant with respect to the right action of SO(3) on M M, i.e. for all R SO(3), S stab(z), and Z M, f(h( ˆR, Z), h(r, Z)) = f(h( ˆRS, Z), h(rs, Z)). Remark The stability proof in [14] also requires f to be a Morse-Bott function with respect to Ŷ. However, this thesis will analyse the stability of the observer by a Lyapunov argument and hence has no such requirement. The set stab(z) is the stabiliser of Z, and is a subset of SO(3). The stabiliser is the component of the true attitude that cannot be observed given the measurements Y. The second condition here is required so that the cost function s characteristics are preserved when it is lifted to the full state space. The term gradŷ f in equation (3.13) denotes the gradient of f with respect to Ŷ. This is the direction in the tangent space of M that locally results in the most rapid increase of f. It is defined by the following statements, for all X M: (i) grad f(x) T X M

23 3.5 Output Observer for Attitude Estimation 13 (ii) for all Z T X M, grad f(x), Z X = d X f(x) Z. (3.14) The inner product here is computed according to a Riemannian metric on M. A Riemannian metric on M is a positive definite symmetric tensor, which defines an inner product on the tangent space of M at each point X M. It enables the length of tangent vectors, and the angle between them, to be defined. The Riemannian metric, Y : T Y M T Y M R used in this paper will be given by, V, W Y = tr(v W ), where V, W T Y M. In (3.14), d X f(x) denotes the derivative of f with respect to X. The chosen cost function f : M M R is given by, f(ŷ, Y ) = k Ŷ Y 2 2 = k (3.15) 2 tr((ŷ Y ) (Ŷ Y )), where k > 0 is a positive constant. Here, k is a constant gain used to control how rapidly the estimate is corrected. A higher value for k will cause faster convergence but will also make the system more susceptible to noise. It is straightforward to see that this cost function is positive, and equal to zero if and only if Ŷ = Y. To check that this cost function satisfies condition (ii), above, let R SO(3) and S stab(z) SO(3). Then, for all Z M, f(h( ˆRS, Z), h(rs, Z)) = k 2 S ( ˆR Z R Z) 2 = k 2 ˆR Z R Z 2 (3.16) = f(h( ˆR, Z), h(r, Z)), as required. The second line follows since the determinant of S is equal to 1. The gradient of this cost function can be found by applying the definition from (3.14), gradŷ f(ŷ, Y ), Z Ŷ = tr((d Ŷ f(ŷ, Y )) Z) ( [ ] ) k = tr dŷ 2 (Ŷ Y ) (Ŷ Y ) Z = tr(k(ŷ Y ) Z). (3.17) Now, since the gradient must lie in the tangent space TŶ M, the gradient is given by the projection of k(ŷ Y ) onto T Ŷ M, denoted P TŶ M. This is achieved by left-multiplying each column i of (Ŷ Y ) by (I ŷ iŷ i ), so, gradŷ f(ŷ, Y ) = k ( (I ŷ 1 ŷ 1 )(ŷ 1 y 1 ) (I ŷ n ŷ n )(ŷ n y n ) ).

24 3.6 Lifting the Attitude Observer to SO(3) 14 Since ŷ i is in the kernel of this projection, the above expression may be simplified to, gradŷ f(ŷ, Y ) = k ( ( = k ( = k ( = k ) (I ŷ 1 ŷ1 )y 1 (I ŷ n ŷn )y n ) (y 1 ŷ 1 y1 ŷ 1 ) (y n ŷ n yn ŷ n ) ) (y 1 ŷ1 ŷ 1 y1 )ŷ 1 (y n ŷn ŷ n yn )ŷ n (ŷ 1 y 1 ) ŷ 1 (ŷ n y n ) ŷ n ) (3.18) The second line uses the fact that ŷi y i = yi ŷ i, and the third line follows because the magnitude of ŷ i is 1 (and hence, ŷi ŷ i = 1). The last line can be confirmed by computation. Substituting (3.12) and this result into (3.13) gives the output observer, ( Ŷ = Ω Ŷ + k A diagram of this observer is shown in figure 3.1. (ŷ 1 y 1 ) ŷ 1 (ŷ n y n ) ŷ n ). Figure 3.1: Block diagram of the output observer for the case of attitude estimation, given by Equation Lifting the Attitude Observer to SO(3) This section completes the derivation of an attitude observer on SO(3). This is done by applying the final part of the method proposed in [14], which involves lifting the cost function defined in (3.15) to SO(3). The attitude observer then results from a similar process to that used to obtain the output observer. The lifted cost function, f, may be found by applying the relations Ŷ = ˆR Z and Y =

25 3.6 Lifting the Attitude Observer to SO(3) 15 R Z to (3.15), f( ˆR, R) = k 2 ˆR Z R Z 2 = k 2 = k 2 n tr(( ˆR z i R z i ) ( ˆR z i R z i )) n tr(( ˆR R)( ˆR R )z i zi ). (3.19) The differential of f with respect to ˆR, in the direction ˆRΩ, may now be computed as, d ˆR f( ˆR, R)[ ˆRΩ ] = k 2 = k 2 = k = k n tr(d ˆR(( ˆR R)( ˆR R ))z i zi ) n tr(( ˆRΩ )( ˆR R )z i zi + ( ˆR R)(RΩ ) z i zi ) n tr(zi ( ˆRΩ )( ˆR R )z i ) n tr((ŷ i y i )ŷi Ω ). (3.20) Now, applying the definition of the gradient in Equation (3.14), k n tr((ŷ i y i )ŷi Ω ) = grad ˆR f( ˆR, R), ˆRΩ ˆR = ˆRkα, ˆRΩ ˆR = k tr(α Ω ). (3.21) Here, α so(3) is an innovation term used to correct the estimate. It must be an element of so(3) because the gradient must lie in the tangent space of SE(3) (which is of the form SO(3) so(3)). It can be seen by inspection that, n α = P so(3) ((ŷ i y i )ŷi ) n = P so(3) ŷ i (ŷi yi ). (3.22) The projection onto so(3) can be applied because Ω is an element of so(3), and hence only the so(3) component of other factors in the trace will affect its value. Applying the projection

26 3.7 Stability Analysis of the SO(3) Observer 16 onto so(3), P so(3) (A) = A A 2 (derived in Appendix A), gives, α = 1 2 = 1 2 = 1 2 n (ŷ i (ŷi yi ) (ŷ i (ŷi yi )) ) n (y i ŷi ŷ i yi ) n (ŷ i y i ). (3.23) Hence, the gradient is given by, grad ˆR f( ˆR, R) = ˆRkα = ˆRk 2 n (ŷ i y i ). The observer proposed by the theory in [14] can now be written, ˆR = ˆRΩ grad ˆR f( ˆR, R) ( ) = ˆR Ω k n (ŷ i y i ). 2 (3.24) This observer is shown in Figure 3.2. Figure 3.2: Block diagram of the SO(3) observer, given by Equation Stability Analysis of the SO(3) Observer This section applies Lyapunov stability theory (see Appendix D) to prove that the derived observer on SO(3) is locally exponentially stable, given certain assumptions. While the above observer was developed as an example in [14], it was not accompanied by the Lyapunov stability analysis presented here. This proof uses the same method that will be adopted for the SE(3) observer in the next chapter. The proof is achieved by the use of a candidate Lyapunov function V, which is expressed in terms of the error function given in (3.5). The proof requires V to be positive definite, which means that V must be nonnegative and equal

27 3.7 Stability Analysis of the SO(3) Observer 17 to zero if and only if the error function is the identity. This in turn implies that V is zero if and only if ˆR = R (and hence that the attitude estimate has converged to the true value). Note that V is only positive definite if there are at least two linearly independent stationary reference directions in the matrix Z. Each of these directions is represented by a 3 1 unit vector z i S 2 (which is embedded in R 3 ), and are stacked horizontally to form Z. The following lemma shows the desired property of V. Lemma Define the error function E( ˆR, R) to be given by, E( ˆR, R) := ˆRR. Assume there exist at least two linearly independent reference directions in the matrix Z. Then, the function V defined by, V ( ˆR, R) := 1 2 E( ˆR, R)Z Z 2, (3.25) is positive definite. Furthermore, if there do not exist two or more linearly independent reference directions in Z, then V is not positive definite. The proof of this Lemma is given in Appendix E.1. Following is the proof of local exponential convergence for the derived observer. Lyapunov argument proceeds by showing that, whenever 0 < V < r (for some r > 0), V is decreasing. Since it is nonnegative, it must be therefore be approaching zero. This implies (as shown above) that the error E is converging to I. The condition that V < 0 is necessary for asymptotic stability, and this condition is checked before considering the stronger result of exponential stability. For exponentially fast convergence, it is required that V cv, for some c > 0. The open ball of radius r is limited by a set U (termed the exceptional set) on which V = 0, but V 0. The problem is that if this set intersects the open ball, then it is possible for a trajectory within the open ball to meet this set and cease to approach zero. The exceptional set consists of the points at which ˆR is a 180 degree rotation of R, and is given substantial consideration in [17] (which derived an equivalent attitude observer to the one given here). Theorem Consider the system described by (3.2), with ˆR (described by (3.24)) denoting the estimate of the time varying rotation R. Suppose that there exist measurements Y of stationary reference directions Z, as given in (3.4). Let Ŷ denote the estimate of these as defined in (3.10). Assume also that there are at least 2 linearly independent reference directions in Z. Then, the error E, defined in (3.5), is locally exponentially stable to the identity rotation, I. For all initial conditions ˆR 0 SO(3) in a neighbourhood of R 0, the trajectory ˆR(t) converges exponentially to R(t). The region of local exponential stability is restricted to the open ball on which V < m, where m is the minimum value of V such that E is a 180 degree rotation. Proof. Define the error E( ˆR, R) of the estimate ˆR by, The E( ˆR, R) = ˆRR. (3.26)