<%BANNER%>

Vision-Based State Estimation for Uninhabited Aerial Vehicles Using the Coplanarity Constraint

University of Florida Institutional Repository

PAGE 1

VISION-BASED STATE ESTI MATION FOR UNINHABITED AERIAL VEHICLES USING THE COPLANARITY CONSTRAINT By THOMAS PHILIP WEBB A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLOR IDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY UNIVERSITY OF FLORIDA 2007

PAGE 2

2007 Thomas Philip Webb

PAGE 3

iii ACKNOWLEDGMENTS I wish to thank my supervisory comm ittee chair (Dr. Andy Kurdila) for his guidance, direction, and support. He is, w ithout doubt, a worldwide le ader in this field and I have been privileged to benefit from hi s extensive experience in both research and education. I am grateful for his time and effo rt despite his many other responsibilities. I thank Dr. Rick Lind for his advice and valuable c ontributions to this research. I also wish to thank Dr. Warren Dixon and Dr. Haniph Latchman for serving on my committee and providing their valuable input. I would like to express my appreciation to Dr. Chad Prazenica, a researcher and visiting professor at the UF Research and Engineering Education Facility. He has coauthored papers with me, run numerous si mulations for me in the vision laboratory, spent valuable time discussing ideas and tec hniques, and provided much encouragement. I could not have completed this effort without his kind support. I wish to thank my employer, JE Sverdrup Technology, TEAS Group (Eglin Air Force Base, Florida) for providing my tuit ion and books during my studies through the Edwin “Bud” George Fellowship. I thank my loving wife, Libby, for the support and encouragement she has given me during this long and seemingly endless ende avor. Finally, I tha nk God for the ability and opportunity to make any modest contribution I may have accomplished toward understanding and harn essing his creation.

PAGE 4

iv TABLE OF CONTENTS page ACKNOWLEDGMENTS.................................................................................................iii LIST OF TABLES.............................................................................................................vi LIST OF FIGURES..........................................................................................................vii ABSTRACT....................................................................................................................... xi CHAPTER 1 INTRODUCTION........................................................................................................1 1.1 Previous Work in this Field...................................................................................2 1.2 Current Research Effort.........................................................................................5 1.2.1 Objective......................................................................................................5 1.2.2 Outline of Tasks Accomplished..................................................................6 1.2.3 Contributions...............................................................................................6 2 THEORY AND CONCEPTS.......................................................................................8 2.1 Coplanarity Constraint...........................................................................................8 2.2 Scale Factor Ambiguity.......................................................................................10 2.3 Implicit Extende d Kalman Filter.........................................................................10 2.4 Development of Measurement Model.................................................................13 2.5 Estimation Algorithm Summary..........................................................................23 3 APPLICATION WITH STATIS TICAL DYNAMIC MODEL.................................27 3.1 Simulation Description........................................................................................27 3.1.1 Statistical Model........................................................................................27 3.1.2 Simulation of Focal Plan e and Vision Processing Output.........................28 3.2 Simulation Results...............................................................................................29 3.2.1 Results for No Measurement Noise...........................................................30 3.2.2 Results in the Presence of Measurement Noise.........................................31 3.3 Sensitivity of Algorithm to Measurement Noise.................................................31 3.4 Observations from Applica tion with Statistical Model.......................................33

PAGE 5

v 4 APPLICATION WITH A UAV DYNAMIC MODEL..............................................39 4.1 Dynamic Model Description................................................................................39 4.2 Observability Considerations...............................................................................44 4.3 Simulation Description........................................................................................47 4.4 Results With No Measurement Noise..................................................................48 4.5 Results With M easurement Noise.......................................................................49 4.6 Degree of Observability.......................................................................................52 4.6.1. Modal Observability.................................................................................52 4.6.2 The Measur ement Matrix..........................................................................53 4.6.3 Modification of the IEKF Measurement Matrix.......................................55 4.7 Results With the Modified IEKF With Measurement Noise...............................56 5 ROBUSTNESS AND SENSITIVITY ASSESSMENT.............................................68 5.1 Robustness to Modeling Uncertainties................................................................68 5.2 Sensivity to Design Parameters...........................................................................72 5.2.1 Camera Depression Angle.........................................................................72 5.2.2 Camera Field of View...............................................................................73 5.2.3 Measurement Interval................................................................................74 5.2.4 Number of Feature Points..........................................................................76 5.2.5 Feature Point Location..............................................................................77 5.3 Effect of Measurement Noise..............................................................................79 5.4 Overall Comments on Robustness and Sensitivity..............................................80 6 IEKF CLOSED LOOP PERFORMANCE...............................................................107 6.1 Simulation with Computer Generated Feature Points.......................................107 6.2 Simulation With Synthetic Imagery..................................................................109 7 CONCLUSION.........................................................................................................121 7.1 Summary............................................................................................................121 7.2 Follow On Work................................................................................................122 APPENDIX A UAV TRUTH MODEL DESCRIPTION.................................................................125 B AUGMENTATION TO LINEARIZED AIRCRAFT EQUATIONS OF MOTION..................................................................................................................134 LIST OF REFERENCES.................................................................................................139 BIOGRAPHICAL SKETCH...........................................................................................142

PAGE 6

vi LIST OF TABLES Table page 5-1 Straight and level traj ectory Monte Carlo sets...........................................................84 5-2 Vertical S trajectory Monte Carlo sets.......................................................................84 5-3 Horizontal S traject ory Monte Carlo sets...................................................................85 5-4 Camera angle Monte Carlo sets..................................................................................87 5-5 Camera field of view Monte Carlo sets......................................................................90 5-6 Measurement interval Monte Carlo sets.....................................................................93 5-7 Number of feature points Monte Carlo sets................................................................96 5-8 Feature point locatio n Monte Carlo sets...................................................................101 5-9 Measurement noise Monte Carlo sets.......................................................................104 A-1 Aerodynamic and mass parameters ........................................................................131

PAGE 7

vii LIST OF FIGURES Figure page 1-1 Conceptu al model.........................................................................................................7 2-1 The perspective projection camera model..................................................................24 2-2 Geometry of the coplanarity constraint......................................................................25 2-3 Measurement model geometry...................................................................................26 3-1 Trajectory for statistical dynamic model numerical simulation.................................34 3-2 Focal plane measurement...........................................................................................34 3-3 Feature point movement on focal plant fo r initial run with no measurement noise...35 3-5 Estimation results with 03 pixel measurement noise.................................................36 3-6 Estimation results with 14 pixel measurement noise.................................................36 3-7 Estimation results with .47 pixel m easurement noise added at 6 seconds.................37 3-8 Focal plane for results with meas urement noise added at 6 seconds..........................37 3-9 Estimation results with extended baseline, .47 pixel measurement noise..................38 4-1 Eight foot wingspan class aircra ft simulated for this effort........................................57 4-2 Straight and level trajectory simulation, no measurement noise................................58 4-3 Errors from straight and level traj ectory simulation, no measurement noise.............58 4-4 Vertical S trajectory simu lation, no measurement noise............................................59 4-5 Errors from vertical S trajector y simulation, no measurement noise.........................59 4-6 Horizontal S trajectory simu lation, no measurement noise........................................60 4-7 Errors from horizontal S trajector y simulation, no measurement noise.....................60 4-8 Errors from straight and level tr ajectory, one pixel measurement noise....................61

PAGE 8

viii 4-9 Errors from vertical S traj ectory, one pixel measurement noise..............................61 4-10 Errors from horizontal S traject ory, one pixel measurement noise..........................62 4-11 Monte Carlo runs, stra ight and level trajectory with measurement noise................62 4-12 Straight and level trajectory erro rs with measurement noise, increased measurement interval...............................................................................................63 4-13 Modal observability for straight a nd level trajectory, no measurement noise..........63 4-14 Measurement matrix values fo r straight and level, no noise....................................64 4-15 Measurement matrix values for straight and level, one pixel measurement noise...64 4-16 Errors for modified IEKF, straight and level trajectory with measurement noise...65 4-17 Errors for modified IEKF, vertical S trajectory with measurement noise................65 4-18 Errors for modified IEKF, horizontal S trajectory with measurement noise............66 4-19 Modified IEKF Monte Carlo, straig ht and level with measurement noise...............66 4-20 Modified IEKF Monte Carlo, ve rtical S with measurement noise...........................67 4-21 Modified IEKF Monte Carlo, hori zontal S with measurement noise.......................67 5-1 Six run Monte Carlo set errors, cas e 1, straight and le vel trajectory.........................82 5-2 Case 1 straight and level trajec tory, ensemble mean error and bounds.....................82 5-3 Case 1 vertical S trajectory, ensemble mean error and bounds.................................83 5-4 Case 1 horizontal S trajectory, ensemble mean error and bounds.............................83 5-5 Straight and level tr ajectory, case statistics...............................................................85 5-6 Vertical S trajectory, case statistics...........................................................................86 5-7 Horizontal S trajectory, case statistics.......................................................................86 5-8 Focal plane history for straight a nd level trajectory with camera looking down, intermediate baseline position, and level.................................................................87 5-9 Straight and level trajecto ry, camera angle statistics.................................................88 5-10 Vertical S trajectory, camera angle statistics............................................................88 5-11 Horizontal S trajectory, camera angle statistics........................................................89

PAGE 9

ix 5-12 Straight and level trajector y, camera FOV statistics................................................91 5-13 Vertical S trajectory, camera FOV statistics............................................................91 5-14 Horizontal S trajectory, camera FOV statistics........................................................92 5-15 History of statistics from .5 second m easurement interval ensemble, straight and level trajectory..........................................................................................................92 5-16 Straight and level trajectory, measurement interval statistics..................................94 5-17 Vertical S trajectory, meas urement interval statistics..............................................94 5-18 Horizontal S trajectory, meas urement interval statistics..........................................95 5-19 Straight and level tr ajectory, number of featur e points statistics.............................97 5-20 Vertical S trajectory, number of feature point s statistics..........................................97 5-21 Horizontal S trajectory, numbe r of feature points statistics.....................................98 5-22 Focal plane history for straight and level trajectory with area constraints...............99 5-23 Focal plane history for straight and level trajectory with area constraints.............100 5-24 Straight and level trajectory, feature point location statistics.................................102 5-25 Vertical S trajectory, feat ure point location statistics.............................................102 5-26 Horizontal S trajectory, f eature point location statistics.........................................103 5-27 Straight and level trajectory, measurement noise statistics....................................105 5-28 Vertical S trajectory, m easurement noise statistics................................................105 5-29 Horizontal S trajectory, m easurement noise statistics............................................106 6-1 Closed loop simulation............................................................................................114 6-2 Autopilot for closed loop simulation, computer generated feature points..............114 6-3 Estimated and actual states, computer generated feature points.............................115 6-4 Horizontal trajectory, com puter generated feature points.......................................115 6-5 Vertical trajectory, com puter generated feature points...........................................116 6-6 Focal plane image from synthetic imagery simulation............................................116 6-7 Autopilot for closed loop simulation, synthetic imagery........................................117

PAGE 10

x 6-8 Estimated and actual stat es, synthetic feature points...............................................117 6-9 Horizontal trajectory synthetic feature points........................................................118 6-10 Vertical trajectory, s ynthetic feature points............................................................118 6-11 Estimated and actual states, synt hetic feature points, new guidance......................119 6-12 Horizontal trajectory, synthe tic feature points, new guidance...............................119 6-13 Vertical trajectory, syntheti c feature points, new guidance....................................120 A-1 UAV truth model....................................................................................................133

PAGE 11

xi Abstract of Dissertation Presen ted to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy VISION-BASED STATE ESTI MATION FOR UNINHABITED AERIAL VEHICLES USING THE COPLANARITY CONSTRAINT By Thomas Philip Webb May 2007 Chair: Andrew J. Kurdila Major: Aerospace Engineering We developed and evaluated a vision-b ased state estimation algorithm for uninhabited aerial vehicles (UAVs) using the implicit extended Kalman filter (IEKF) and the coplanarity constraint (also known as the epipolar or essentia l constraint). The coplanarity constraint, a wellknown property in the structur e from motion (SFM) field, has advantages for this application in that th e feature point locations in three dimensional space do not have to be known and tracked and that feature points can be discarded and acquired as necessary. This reduces the com putational load which is important for real time applications such as aircraft control. A dvantages of the IEKF are that, in principle, the current estimate uses all previous inform ation, not just the current observations, and that the estimate will propagate forward in an orderly fashion in the case of interrupted or reduced measurements. The dynamics of the aircraft are included in the process model which improves the observability of the states and resolves the SFM scale factor ambiguity. The algorithm was implemented in a numerical simulation and exhibited

PAGE 12

xii divergence problems in the presence of measurement noise. These effects were particularly evident in the velocity estimat es. The problems were eliminated by zeroing out the small velocity dependent terms in the measurement matrix. The algorithm was exercised in a Monte Carlo fashion and found to be robust to errors in the process model and to measurement noise. Sensitivities to filter and focal plane implementation parameters including camera depression angle, field of view, measurement interval, and feature point location and number were also assessed. The modified estimator was then employed in a closed loop UAV simulation to pr ovide feedback to a simple autopilot. The simulation demonstrated that the state es timates provided were sufficiently accurate to allow control of the UAV through successful waypoint navigation. This simulation used feature points generated at random locatio ns in the field of view. A second closed loop simulation was successfully run using synthetic imagery from the University of Florida’s vision laboratory and a Lucas-Ka nade feature point tracking algorithm.

PAGE 13

1 CHAPTER 1 INTRODUCTION Interest in autonomous flight of small uni nhabited aircraft has been increasing over the past few years. Also, the demand and expectation for smaller and smaller vehicles has increased as the size and weight of sensors, actuators, microprocessors, and propulsion systems has decreased [1, 2]. Many of these aircraft employ a video camera as a sensor in accomplishing their mission (e .g., information gathering, surveillance, route documentation) or for manual control for some phases of flight using a remote human pilot. Dual use of video for state estim ation in a flight control system as well as for mission accomplishment is attractive in that payload space and weight can be conserved allowing for a smaller aircraft for a given mission. This research explores one approach for using visual information as the sole measurement for estimating an adequate number of aircraft states with enough accuracy to drive an autopilot. A conceptual model is shown in Figure 1-1. Multiple use of components for different functions in air vehicle design is certainly not a new idea. A remarkable example in the extreme is the Gossomer Condor in which designer Paul MacCready combined the propuls ion power source, sensors, flight control computer, control actuators, and flight recorder into a single “off the shelf” package in the person of Bryan Allen [3].

PAGE 14

2 1.1 Previous Work in this Field Vision-based state estimation for control of aircraft has been around as long as the airplane since vision is the sense most used by a human pilot. Of course flying creatures have been doing this for much longer. The University of Florida has successfully flown micro air vehicles (MAVs) using an autopilot based on a vision-based horizon trac king scheme [4]. The Robotics Institute, Carnegie Mellon University has much experi ence with “structure from motion” (SFM) research and has applied that to aerial vehicles in recent year s [5]. Georgia Institute of Technology has recently flown a glider that su ccessfully homes to a target of known size, shape, and color using vision-only navigation, guidance, and control [6]. The technique used in my research described is drawn from the body of SFM work. The computer vision problem of SFM has b een described as “estimating 3-D motion and structure of a rigid object from a sequence of monocular images” [8]. This is equivalent to estimating the motion of a moving camera and the structure of a fixed environment from a sequence of images. SFM methods can generally be categorized as causal or noncausal. Causal methods are those that, at a given point in time, us e only current and past information. These methods can be recursiv e in nature. In noncausal schemes, batch processing is normally used after the fact to include all information for an estimate at any given time of interest. Any algorithm used for real-time control would obviously have to be causal [9]. Soatto and Perona [8] explore and comp are the performance of a number of recursive methods implemented using a form of the extended Kalman filter (EKF). The basic measurement used in all these methods is the movement of “feature points” from frame to frame on the camera focal plane. A feature point is a fixed point in the

PAGE 15

3 environment that is identified and tracked as the camera moves. The methods examined use various constraints imposed on feature point image movement on the focal plane by geometry. Also, some methods attempt to esti mate the positions of the feature points in the environment (the structure) at the same time as the camera motion variables are estimated, while other methods are able to decouple these operations. If feature point location (s tructure) is not important, the decoupled methods are attractive for real-time implementation becau se they are less demanding computationally since the distance to feature points is not required to be known initially and updated. Also, new feature points can be added and old ones discarded at will. These schemes are able to deduce camera angular rates and directio n of velocity (or, fo r the discrete case, incremental changes in angular orientati on and direction of position change). The magnitude of the translational motion cannot be determined directly without some reference length. This is the well-known scal e factor ambiguity that comes from the fact that the camera model used cannot distinguish whether a feature point is close with slow relative motion or further away with faster relative motion [5, 8, 10]. Azarbayejani and Pentland [11] develope d and demonstrated an EKF method of estimating structure, camera motion, and camera focal length. It carries a state in the filter for each feature point, corresponding roughly to scaled range, as well as the incremental rotations and scal ed translations. Chiuso et al. [9] improved upon this general approach by handling the problem of losing and adding feature points (feature point occlusion). The method allows for th e use of an arbitrary dynamic model for the motion variables, however the examples published included only statistical random walk models.

PAGE 16

4 Soatto formalized the implicit EKF (IE KF) to implement recursive decoupled methods using two different implicit geometri c constraints; the c oplanarity constraint (also known as the epipolar or essential constr aint) [12] and the subspace constraint [13]. These methods are attractive for use in estima ting motion variables in real time since, as mentioned previously, filter states for the feature points do not have to be carried. Motion parameters estimated are formulated as angular rates and dire ction of velocity. Again, statistical models are implemented in the examples. A causal, but non-EKF method has been develo ped by Kanade et al. [5]. It links two-frame SFM solutions of the essential constraint computed by a conventional nonrecursive computer vision algorithm. Drawbacks to this method for the current application appear to be that since there is no dynamic mode l, there is no mechanism for incorporating available information about the dy namic characteristics of the plant. Also, the current state estimate uses only informa tion from the current and last focal plane measurement while the EKF blends in information from all previous measurements. Even so, this method was shown to have pr omising results using vision data obtained during an actual model aircraft flight of short duration with good initialization. Kehoe et al. [14, 15] developed a me thod based on application of nonlinear optimization techniques applied to the optical flow of tracked feature points. This method is also not filter based and does not use any model information. The technique was demonstrated using simulation. Gurfil and Rotstein [16] added an aircraft dynamic model to the subspace constraint IEKF method mentioned earlier [13]. Their numerical simulation results demonstrated estimation of aircraft angle of a ttack, sideslip, and angular rates for a single

PAGE 17

5 trajectory using camera focal plane measuremen ts with a limited amount of measurement noise. They did not attempt to use the state estimates in aircraft control. 1.2 Current Research Effort 1.2.1 Objective The objective of this research effort is to develop and demonstrate a causal, visionbased state estimation algorithm for aut onomous uninhabited aerial vehicles (UAVs) capable of providing enough states of sufficient accuracy to reliably control the vehicle in an urban, dynamic environment. The a pproach used is based on the coplanarity constraint IEKF method mentioned earlier [12] This method is attractive as a departure point for several reasons. It is decoupled from the structure which decreases computational demands and allows feature point s to be efficiently discarded and added as necessary as the field of view changes. It is not dependent on a discernable horizon which is likely to be missing in an urban envi ronment. If feature point measurements are reduced or even lost completely for a time, the Kalman Filter w ill still provide state estimates that gracefully degrade which contri butes to the robustness and integrity of the control system. And finally, th e IEKF lends itself to the incorporation of a specific plant model which provides more information to the process allowing the estimation of additional states. It is important to note that we assume d that the feature poi nt correspondence and tracking problem is solved. That is, the lo cation of feature points on the focal plane from frame to frame is known within a certain level of accuracy. That is not to say that the location of the feature points in space is known. This vision processing problem is certainly not trivial and is the subject of much ongoing research. The Lucas-Kanade algorithm [5] is one proven method for this task.

PAGE 18

6 1.2.2 Outline of Tasks Accomplished A simulation of a small UAV was develope d and “truth” data sets for various trajectories were compiled. The IEKF wa s augmented with a simplified dynamic model representing the aircraft and a rigidly attached focal plane. The filter was exercised on the trajectories using randomly chosen f eature points moving on a simulated focal plane in accordance with the particular trajectory being run. The filter exhibited some divergence problems in the velocity states when measurement noise was added. A modification to the filter measurement model was made which seemed to alleviate the problem. Filter parameters were tuned during this process and state estimation accuracy was evaluated. Robustness of the filter to initialization e rrors, vehicle mo deling errors, and measurement noise was addressed using Mo nte Carlo simulation ru ns. Sensitivity of its performance to camera orientation, field of view, measurement frequency, and feature point distribution and density was investigated An autopilot tailored to the estimated states was designed and a closed loop simula tion including the IEKF, aircraft simulation, and focal plane simulation was developed to de monstrate the suitabil ity of the filter for use in autonomous waypoint navigation for th e UAV. The filter was also exercised using synthetic video produced by the University of Florida Research and Engineering Education Facility MAV vision laboratory. So me of this research has been documented previously in references [17] and [18]. 1.2.3 Contributions This research effort makes several contri butions to the state of the art in visionbased state estimation and control for autonomous UAVs. The IEKF based on the coplanarity constraint was applied to an ag ile aerial vehicle. A specific dynamic model was used in the IEKF which expanded its capab ilities in two respects First, the plant

PAGE 19

7 dynamics allow the estimation of states in addition to angular rates and direction of velocity. In this case, they are the Euler angl es of pitch and roll. Second, the scale factor ambiguity is resolved for the velocities. A final contribution is that this effort demonstrates that an autonomous UAV can be productively controlled using information derived solely from focal plane measur ements in a no-horizon environment. Figure 1-1. Conceptual model. Visual Environment Vision Tracker Estimation Filter Autopilot UAV Dynamics Focal Plane Guidance Environment / Hardware

PAGE 20

8 CHAPTER 2 THEORY AND CONCEPTS 2.1 Coplanarity Constraint The fundamental geometric constraint we used as a measurement is the coplanarity constraint, also called the ep ipolar constraint or the e ssential constraint. This relationship, credited to H. C. Longuet-Higgi ns [19], uses the perspective projection or pinhole camera model (Figure 2-1). The coplanarity constraint is formulated based on the principl e that the position vectors of a feature point re lative to the camera, projecte d onto the focal plane at two instants in time, must be coplanar with the translation vector of the origin of the camera frame. This is shown in Figure 2-2, where X and X’ denote the position vectors of feature point P in the camera reference frame, x and x ’ represent the corresponding position vectors of the image of P on the focal plane, and T indicates the translation vector of the origin of the camera frame. The development of this form of the constraint equation is found throughout the SFM literature [12] and is repeated here for completeness. Since the scalar triple pr oduct of three coplanar vectors is zero, the coplanar constraint for this featur e point is expressed as (')0 XTRX (2-1) and, equivalently for the focal plane, (')0 xTRx (2-2) since x is co-linear with X and x ’ is co-linear with X ’. Here, the common coordinate system is arbitrarily chosen to be that of the camera at time t. R is the orthonormal

PAGE 21

9 rotation matrix that transforms camera coordinates at time t+ t to camera coordinates at time t. To handle multiple feature points, it is convenient to make the following definitions. []T x f where and are the coordinate components in th e focal plane as shown in Figure 2-1 and f is the camera focal length. If the co mponents are normalized to the focal length then let [1]Tx and '[''1]Tx Also, a skew-symmetric matrix is formed from T to compute the cross product, 32 31 210 0 0 tt Ttt tt and the matrix E is defined as [] ETR (2-3) E belongs to a special class of matrices called “essential ma trices” described in the SFM literature [10, 12]. Equation 2-2 can now be written as [1][''1]0T iiiiE for the i th feature point. Carrying out the multiplication and making the subsequent definitions yields 0iCe (2-4) where e is the 9x1 column vector compos ed of the stacked columns of E and [''''''1]iiiiiiiiiiiiiC (2-5) The Ci rows are stacked to form the constraint for all feature points Ce = 0 (2-6)

PAGE 22

10 Note that C contains only the observations from th e focal plane, each row for a different feature point, and e contains only camera movement parameters. 2.2 Scale Factor Ambiguity Scale factor ambiguity was mentioned in chapter 1. For the coplanarity constraint this limitation can be observed by considering the translation vector T in Figure 2-2. The coplanarity constraint provides inform ation only about the orientation of T not its magnitude, since the triple product will be zero for any magnitude of T In applying the constraint to solve for motion, an additional constraint is placed on T In the literature the usual additional constraint is to set the magnitude of T to unity realizing that the direction of translation is what is produced. A similar condition will be imposed for this work. This ambiguity also extends to the lengths of the position vectors from the camera to the feature point, X and X ’ since it is the compone nts of x and x’ measured relative to the focal plane that are used. Scale ambiguity is the price paid for be ing able to decouple structure from motion which is arguably a good tradeoff when one is only interested in camera motion. Scale can be restored at any time if the magnitude of T is known or the magnitude of X or X ’ is known for any single feature point since the three form the sides of a triangle whose angles are known. 2.3 Implicit Extended Kalman Filter A brief review of the discrete extended Kalman filter (EKF) algorithm is presented followed by an explanation of the modificati on to the measurement update that leads to the implicit extended Kalman filter (IEKF). The EKF algorithm is derived and can be f ound in any of a number of texts, for example Gelb et al. or Grewal and Andrews [ 20, 21]. For the discrete case the results are:

PAGE 23

11 Dynamic model: 111(,),(0,)kkkkkk x fxuwwNQ (2-7) Where x is the state vector, u is an independent know n control input vector, w is a white random vector sequence with properties denoted by matrix N(0,Q) meaning normal distribution, zero mean, and covariance matrix Q The symbol f represents a known vector function and k is shorthand for k t. Measurement model: (),(0,)kkkkkkzhxNR (2-8) Note that this gives z, the measurement, explicitly. State propagation: 11ˆˆ (,)kkk x fxu (2-9) Where “–” denotes a value prior to the measurement update and “+” is the value post update. The “^” indicates that the value is an estimate. Measurement prediction: ˆ ˆ ()kkkzhx (2-10) State covariance propagation: 1111 T kkkkkPPQ (2-11) where the state transition matrix: 11 ˆkk k xxf x (2-12) Gain computation: 1 TT kkkkkkkkPHHPHR (2-13) where the measurement matrix: ˆkk k xxh H x (2-14)

PAGE 24

12 State update: ˆˆ ˆ ()kkkkk x xkzz (2-15) Covariance update: kkkkPIkHP (2-16) In the IEKF, the explicit measurement mode l is replaced with an implicit constraint involving measurements. For this case the copl anarity constraint, Equation 2-6, is used. This leads to the following modifi cations as given by Soatto [12]. The quantity Ce becomes the predicted “pseudo measurement”, ˆkz. The measurement,kz, is replaced by zero, the true value of the constraint. The state update, Equation 2-15, now becomes: 1111ˆˆˆ (,,',',....,,,',')()kkkNNNNkk x xkCex (2-17) where N is the number of feature points. The measurement matrix, Hk is more specifically defined as: ˆ()kk xxCee HC xx (2-18) Also, it is useful to express the measurement noise covariance term, k R for the EKF, in terms of the covariance of the measur ement error of the positions of the feature points on the focal plane, Rk (which is not the same as the covariance of the error in Ce). To first order, this is: T kkkk R DRD (2-19) where: 11112222cov44T k NNNN R NxN

PAGE 25

13 denotes the measurement error of the particular parameter N is the number of feature points 11112222()k T NNNNCe D (2-20) For this IEKF the gain, Equation 2-13, now becomes: 1 TTT kkkkkkkkkkPHHPHDRD (2-21) 2.4 Development of Measurement Model In this section, the specifics of the measurement model for the coplanarity constraint in the IEKF are developed in terms of the state variables. In reference [12], Soatto described two approaches with regards to the state variables used in the IEKF. The first assumed the usual qua ntities of linear a nd angular velocities which he referred to as the “local coordinates estimator.” The second used th e components of the essential matrix E as the state variables and was referred to as the “essential estimator.” The essential estimator results in a linear Kalman filter, however a nonlin ear transformation is required to obtain the desired state variables The local coordinates estimator yielded slightly better results according to Soa tto [12] and is a more straight-forward implementation. That is the approach used in this work and described here. In section 2.1 it was shown that the camera frame to frame movement information is contained in the essential matrix, E, which is comprised of the translation vector, T and the rotation matrix, R. The objective then is to express these in terms of aircraft motion variables. T and R are determined by the linear and a ngular velocities of the camera with respect to the environment, denoted as vectors Vc and c, respectively. These in turn are related to the velocity vector of the aircraft center of mass, Vb, and the aircraft angular

PAGE 26

14 velocity vector, b The six components of Vb and b comprise the state vector for purposes of this section. The geometry for th is is shown in Figure 2-3. The right-handed set of unit vectors ˆ ˆˆ {,,}cccijk describes the camera fixed reference frame and likewise the right-handed set ˆ ˆˆ {,,}bbbijk describes the aircraft body fixed frame. In the development that follows, R will be determined as a function of c and T will be determined as a function of Vc and c. Then c and Vc will be expressed in terms of b and Vb. It is convenient to make the following definitions for the velocity components: ˆ ˆˆ [,,][,,] ˆ ˆˆ [,,][,,] ˆ ˆˆ [,,][,,] ˆ ˆˆ [,,][,,]T ccccccc T ccccccc T bbbbbbb T bbbbbbbpqrijk Vuvwijk pqrijk Vuvwijk (2-22) and for the skew symmetric matrix: 0 []0 0cc ccc ccrq rp qp (2-23) Note: As stated previously, these velocitie s are with respect to the environment even though they are coordinated in different frames. The rotation matrix R can be computed using the well known exponential expression (e.g., from chapter two of reference [22]): []ctRe (2-24) where t is the time increment between images. This assumes that c is constant during t which is not too bad an assumption if t is not too long. One popular method of evaluating this matrix exponential is

PAGE 27

15 Rodrigues’ formula (see again chapter two of reference [21]). For this work the more general truncated Taylor series is used. 23 []([])([]) [] 2!3!ct cc ctt ReIt (2-25) In computing the translation vector T, recall that it is to be expressed in terms of the camera frame of reference at the first of th e two images. In general, the camera is rotating as it is translating which means the reference frame for [,,]cccuvw is rotating as well. Defining as the time from the first image of the pair during t, an infinitesimal T on this interval is: []()ccc cc ccuu dTRvdevd ww (2-26) In carrying out the integration it is assumed that the values for [,,]cccuvw as well as for c are constant. 2 [] 00 22334([]) ([]) 2! [][][] () 2!3!4!ccc tt c ccc cc c ccc c cuu TevdIdv ww u ttt Itv w (2-27) Equations 2-25 and 2-27 give R and T in terms of camera relative velocities. For the IEKF measurement model, these need to be written in terms of aircraft relative velocities. This requires know ledge of the orientation and displacement of the camera

PAGE 28

16 reference frame with respect to the aircraft body reference frame. Let the orientation of the camera frame with respect to the aircraft frame described by the usual ordered set of Euler angles ,,ccc about the 3,2,1 axes, respectively, and the corresponding rotation matrix that transforms from the aircraft frame to the camera frame be denoted by Rbc. Referring to Figure 2-3, these can be thought of as the camera gimbal angles. Providing for a changing orientation, let the angular rate of the camera with respect to the aircraft be defined in the aircraft frame asˆ ˆˆ [,,][,,]T bcbcbcbcbbbpqrijk. This would represent the camera slew rates. Let the displacement from the origin of the aircraft frame to that of the camera frame be defined in the aircraft frame as ˆ ˆˆ [,,][,,]T bcbcbcbcbbbdxyzijk Again, it is assumed that the values for ,,,,cccbcbcd are known at all times. The camera relative velocities can now be written as the following: 100cos0sincossin0 0cossin010sincos0 0sincossin0cos001cccc bccccc ccccR (2-28) cbbcbbc T cbcbbcbbc cbbcbbcuuxpx vRvyqy wwzrz (2-29) where the dot denotes the time rate of change of the coordinate. cbbc T cbcbbc cbbcppp qRqq rrr (2-30) The expressions in 2-29 and 2-30 can be s ubstituted into 2-25 and 2-27 to obtain R and T in terms of aircraft motion variables which are the IEKF state variables.

PAGE 29

17 At this point several simplifying assumptions will be made. The camera is assumed to be mounted rigidly to the aircraft, i.e. the camera orientation and position relative the aircraft frame is fixed so that ,,,cccbcd are constant and bc is zero. Also, the camera is assumed to be mounted at the aircraft center of mass so that dbc is zero. Finally, the camera is mounted in the aircra ft vertical plane at a fixed elevation (depression) angle so that c and c are zero. These assumptions simplify the computations and reduce the potential for errors but do not detract from the overall objectives and results of this effort. If at anytime it is desirable to remove any of these assumptions, it would be a straight forward pr ocess to include the appropriate terms and reformulate the implementation used here. With these assumptions and the appropriate substitutions: 0(sincos) (sincos)0(cossin) (cossin)0bcbcb cbcbcbcbc bbcbcprq prpr qpr (2-31) cossin sinsincbcbc cb cbcbcuuw vv wuw (2-32) From R and T the essential matrix E can be calculated and the predicted pseudo measurement from section 2.3 can be expresse d in terms of the available state variable estimates. 1111ˆˆˆˆˆˆ ˆ (,,',',....,,,',')(,,,,,)kNNNNkbbbbbbkzCeuvwpqr (2-33) The measurement matrix, H, must also be expressed in terms of the aircraft state variable estimates. As indicated in E quation 2-18 repeated below, determining H requires taking the partial derivatives of E with respect to the aircraft state variables,

PAGE 30

18 ˆ()kk xxCee HC xx (2-18 repeated) where ,,,,,T bbbbbb x uvwpqr (2-34) H is an Nx6 matrix with N corresponding to the number of feature points being tracked during a given frame interval. Matrix C is the Nx9 matrix described earlier and / ex is the 9x6 Jacobian matrix of partial derivatives of e with respect to x Each of these six columns is formed by stacking the columns of the partial derivative of the essential matrix E with respect to a different aircraft stat e variable. This requires the partials of T and R as indicated in Equation 2-35. ˆ ˆ iii xx xxT ER RT xxx (2-35) These partials are approximated using only th e first two terms of the series given in Equations 2-25 and 2-27. The result is the following: 2 22 2cossin/2 cos/2sin/2 cos/2sinccc cccc b cccttq T trtp u tqt (2-36) 2 2/2 /2c b ctr T t v tp (2-37) 2 22 2sincos/2 sin/2cos/2 sin/2cosccc cccc b cccttq T trtp w tqt (2-38) 2 22 2sin/2 cos/2sin/2 cos/2cc cccc b cctv T twtu p tv (2-39)

PAGE 31

19 2 20c b ctw T q tu (2-40) 2 22 2cos/2 sin/2cos/2 sin/2cc cccc b cctv T twtu r tv (2-41) 0bbbRRR uvw (2-42) 0sin0 sin0cos 0cos0c cc b ct R tt p t (2-43) 00 000 00bt R q t (2-44) 0cos0 cos0sin 0sin0c cc b ct R tt r t (2-45) The iT x terms in Equation 2-35 are formed in the usual way with the skew-symmetric form of the vectors iT x The velocity components in th e expressions are intentionally left in camera frame coordinates for compactness. It is now necessary to impose the additional constraint on T discussed in section 2.2 due to the scale factor ambiguity. For this work the constraint will be that the magnitude of the aircraft velocity, |Vb|, will remain constant during the state update. This means that only two components of the velocity vect or can vary independently. For this work vb

PAGE 32

20 and wb are chosen to be the independent components and ub is chosen as the dependent component. The reason for these choices will be come apparent in a later chapter. This additional constraint is then: 222 2 2 22ˆˆˆbbbb bbbbVuvw uVvw (2-46) The partials affected by this are ,,bbbT uvw Equations 2-36 through 2-38. For this adjustment the partial derivative of ub with respect to vb and wb are required. 1 2 1 22 22 2 22() ()b bbbb b b bbbb bu vVvw v u wVvw v (2-47) An assumption is made here that ub is positive. This should always be the case for purposes of this effort using a fixed wing aircraft If in a later application this is not the case (e. g. for a helicopter or balloon) th en the signs on the expressions in 2-47 will reverse for a negative ub. Equation 2-37 now becomes: 1 222 2 2222 22/2cossin/2 cos/2sin/2() /2cos/2sinb bbbb cccc ccccbbbb ccccu TTT vvuv trttq ttrtpvVvw tptqt (2-48) Equation 2-38 becomes:

PAGE 33

21 1 222 2 222222 22sincos/2cossin/2 sin/2cos/2cos/2sin/2() sin/2coscos/2sinb bbbb cccccc ccccccccbbbb ccccccu TTT wwuw ttqttq trtptrtpwVvw tqttqt (2-49) and Equation 2-36 becomes: ˆ0b xxT u (2-50) The remaining component of the measurement model is the expression for the NxN pseudo measurement noise covariance matrix, R from Equations 2-19 and 2-20 repeated below. T kkkk R DRD (2-19 repeated) 11112222()k T NNNNCe D (2-20 repeated) It is assumed that the value of the 4Nx4N covariance matrix, R, for the error of the feature point measurements is known or can be approximated. The Nx4N matrix, D, can be rewritten as: 1 1111 2 222200 ,,, 00 ,,, 0 00 ,,,N NNNNCe Ce D Ce (2-51) where each 1x4 component correspondi ng to a different feature point:

PAGE 34

22 ,,,,,, 0000100 0000010 1000000 0001000T i T i iiiiiiii T ii ii T ii iiCe C e e (2-52) The assumption will be made that the measur ement errors for each feature point location have the same variance, 2 and are uncorrelated with each other which implies that: 2 2 44 20 0NxNRI (2-53) R can now be expressed as the diagonal matrix: 2 11 11111111 20 ,,,,,, 0 ,,,,,,TT T T NN NNNNNNNNRDRDDD CeCe CeCe (2-54) At this point it might be useful to re view the assumptions, approximations, and simplifications that have been made in this section: The linear and angular velo cities are constant during each measurement interval. The camera is mounted rigidly to the UAV at the center of mass. The camera frame of reference differs from that of the aircraft frame only by a rotation of c about the number two axis, the camera elevation angle. In obtaining the measurement matrix, H the partial derivatives of the essential matrix, E with respect to the state variables, x are approximated by using only the first two terms of the given series.

PAGE 35

23 The component of UAV velocity in the ˆbi direction, bu is positive. (The UAV is moving forward.) All of the focal plane measurement errors are characterized by the same variance and they are all uncorrelated with each other. It is felt that none of these serio usly detract from the overall objectives and results of this effort. 2.5 Estimation Algorithm Summary This section provides an outline of how th e concepts and expressions described in this chapter are assembled to form the ma in loop of the recursive state estimator. 1. Propagate state estimate from last update using (2-9). The measurement model, f(x,u) has yet to be defined. 2. Compute state transition matrix using (2-12). 3. Propagate state error covariance using (2-11). 4. Compute the essential matrix with (2-3) and (2-23 through 2-32). 5. Acquire the focal plane measurements from the vision processing module. 6. Form the predicted pseudo measurement using (2-5 and 2-6). 7. Compute the measurement matrix with (2-18) and (2-35 through 2-50). 8. Compute the measurement noise co variance with (2-52 through 2-54). 9. Compute the Kalman gain with (2-13). 10. Update the state using (2-15). 11. Update the state error covariance using (2-16).

PAGE 36

24 Figure 2-1. The perspectiv e projection camera model. Feature Point Center of Projection Focal Plane

PAGE 37

25 Figure 2-2. Geometry of the coplanarity constraint. P X ’ X x x ’ T Time t Time t+ t

PAGE 38

26 Aircraft Body Fixed Frame (b) Camera Fixed Frame (c) ˆbiˆbj ˆbkˆciˆcj ˆckb bV cV c Figure 2-3. Measurement model geometry.

PAGE 39

27 CHAPTER 3 APPLICATION WITH STATISTICAL DYNAMIC MODEL This chapter describes a numerical simu lation of the estimator using a simple statistical dynamic model inst ead of the more complex UAV dynamics. The aim of this simulation is to reproduce results similar to Soatto et al. [8, 12] and to gain basic insight into the suitability and usefulness of the copl anarity constraint as an IEKF measurement. For this simulation the camera simply move s around a circular path at constant speed with the lens pointed toward the center, observing a cloud of f eature points located inside the circle. The camera rotates at constant rate so as to point in the same direction relative to the trajectory. The aim is slightly offset from the center in order to provide a velocity along two of the camera axes. This arrangement is depicted in Figure 3-1. A description of the simulation to include the dynamic m odel, the focal plane, the feature point generation method, and the vision processing output is given. Results from the filter simulation along the given trajectory are examined. 3.1 Simulation Description 3.1.1 Statistical Model The state vector is defined as in Equation 2-34 from the last chapter. ,v,,,,T k k x uwpqr (3-1) The subscripts on the components have been omitted. They will refer to body axes unless stated otherwise.

PAGE 40

28 The form of the dynamic model was given in the previous chapter in Equation 2-7 as 111(,)kkkk x fxuw The statistical model used here and in Soatto et al. [12] is a first order random walk. 11,(0,)kkkkk x xwwNQ (3-2) The model assumes a random increment is adde d to each component of the state at each step. This model is typically used when little is known about the dynamics of the physical system. Referring to Equation 2-9, the state propagation is simply 1 kk x x (3-3) and the state error covariance pr opagation from Equation 2-11 is 11 kkkPPQ (3-4) The value of the proc ess noise covariance, Q is used as a tuning factor in the Kalman filter implementation to compensate for syst em modeling uncertainti es and nonlinearities as well as disturbances. 3.1.2 Simulation of Focal Plane and Vision Processing Output The measurement is the location of the proj ection of each feature point on the focal plane, [,,1]T iiix, as given in the last chapter and illustrated in Figure 3.2. The fixed position of each feature point in the environment is known as well as the position and orientation of the camera along the actual or “t ruth” trajectory at any time. To simulate a particular feature point meas urement, the position vector, X, from the camera to the feature point is obtained in earth fixed coor dinates, then transformed to body coordinates using the appropriate rotation matrix tra nsformations. An additional rotation transformation is done to go from body axes to camera axes. In this simulation c the camera elevation angle, is 90 degrees so that the camera is looking in the direction of the

PAGE 41

29 velocity component u This position vector in the camera frame is then normalized to the third component, the ˆck component, which also norma lizes it to the focal length, f as discussed in chapter 2. This pr oduces the desired parameters and for each feature point. It is noted here that the estimato r has no knowledge of the camera location and orientation or the location of the feature points in space. Measurement noise is simulated by adding a random white Gaussian component to each and from a random number generator. Th e focal plane is chosen to be 320 x 240 pixels (4:3 aspect ratio), a value that is typical throughout the literature for readily available hardware. Pixel density is specified in order to relate measurement noise to a physical characteristic. The standard deviati on of the noise is defined for this work in terms of pixel widths and is a key parameter that can be varied and is specified for any simulation run. 3.2 Simulation Results The IEKF and measurement simulation algorithms as described previously were implemented in Matlab. The truth trajectory for this simulation (computed apriori) was a 10 second duration arc. The state vect or for the trajectory was constant at x = [-12.48, 124.38, 0, 0, 0 -.1]T with units of feet/second and radians/second. This combination of velocity and angular rate results in the cam era following a circular path of radius 1250 feet with the camera always pointed .1 radians to the left of the center of the circle, see again Figure 3-1. (Since u was negative, the signs on equa tion 2-47 were reversed.) A cloud of 20 randomly placed feature points was lo cated at the center of the circle which provided the measurements for the estimator. A field of view of 60 degrees horizontal was chosen for the camera. The 4:3 aspect rati o resulted in a verti cal field of view of

PAGE 42

30 approximately 46.8 degrees. The measuremen t noise value input to the IEKF via the variance matrix R in Equation 2.53 was the equivalent of one pixel standa rd deviation for the 60 degree field of view or = .0036. The update rate chosen for the filter was 20 Hertz (hz). Based on preliminary runs using trial and error, a value of Q = diag[0 100 100 .1 .1 .1] was chosen for the process noise covariance and P = diag[0 100 100 .01 .01 .01] was chosen to initialize the state estimation error covariance. 3.2.1 Results for No Measurement Noise An initial run was made with no measurem ent noise; that is, a lthough the filter ran assuming noise equivalent to = .0036, there was no noise actually being added to the simulated focal plane measurements. Figure 33 depicts the location of the feature points on the focal plane each time a measurement was taken during the 10 second run. This shows how the feature points move on the focal plane. The estimated states in the filter were inten tionally initialized with an error to allow observation of the convergence of the filter. The initial values were ˆ [12.48,110,5,.1,.1,0]x which equates to initial errors of 14.38 and 5 ft/sec for v and w respectively, and .1 rad/sec for p q and r The results are shown in Figure 3-4. Initializing the state u with no error in effect scales th e other velocities. This is due to several factors. Scale factor ambiguity, the choice of v and w as the independent velocity variables, and the additional constraint that the magnitude of velocity is constant over the update were discussed in the last chapter. In addition, with this simple model, there is no interdependency among any of the state variables. The measurements shape the direction of velocity through th e coplanarity cons traint while the u component influences the magnitudes since it is not altere d in either state propa gation or the update.

PAGE 43

31 Referring to Figure 3-4 it can be seen th at the angular rate estimates converge rapidly, within less that one second, to their true values. Th e velocity component estimate w corrects almost as quickly to its tru e value of zero. The v component of velocity is much more sluggish bu t appears to be slowly converging 3.2.2 Results in the Presence of Measurement Noise For the next run everything remained the same except the equivalent of .03 pixel width standard deviation of noise was added to the measurement. The results are shown in Figure 3-5. The noise is evident in the plots of the state estimates however the values do converge to the truth values as before. Another run was made with the measuremen t noise increased to the equivalent of .14 pixel standard deviation. Results are show n in Figure 3-6. Initial convergence for the angular rates is still good however v appears to diverge fr om the beginning. After about four seconds divergence can also be seen in r. A final run was made with initially no meas urement noise then at six seconds, noise equivalent to .47 pixel standard deviation wa s abruptly added. The results for this are shown in Figure 3-7. As expected from the f irst run, the filter starts out well but when the noise is added at the six second point, the f ilter fails. A depiction of the focal plane is shown in Figure 3-8 to indicate in a qualitati ve fashion the magnitude of the measurement noise. The point at which the noise begins on the feature point traces can be observed if closely compared to Figure 3-3. Intuitively, this amount of noise would not be unreasonable, particularly on a 320x240 pixel focal plane. 3.3 Sensitivity of Algorit hm to Measurement Noise The failure of the filter with a reasonable amount of noise added to the focal plane positions was unexpected as this had not been demonstrated in either of the sets of

PAGE 44

32 simulation examples from Soatto et al. [8, 12]. However reference [7], the companion paper to reference [8], briefly discusses that SFM becomes extremely sensitive to noise with short baselines. Ma et al. [22] say that “one needs to exercise caution to make sure that there is sufficient parallax for the algor ithm to be well conditioned” in their chapter Reconstruction from Two Calibrated Views which deals with using the coplanarity constraint. Baseline and parallax refer to how much translation of the camera there is between frames and how much feature point mo vement on the focal plane there is due to this translation. However references [12] a nd [22] both indicate th at when translation is almost zero, it is the direction of velocity that becomes ill-conditioned, the angular rates can still be computed. This seems to be apparent in Figure 3-6, at least during the initial part of the run. Kehoe et al. had si milar findings in their work [14, 15]. Increasing the baseline or parallax w ould obviously be desirable for state estimation. Baseline can be extended in severa l ways. An obvious way is to increase the time between measurements. The disadvantag es to this are less measurement updates and some degradation due to the assumption that linear and angular velocities are constant over the update interval. Also, a longer interval be tween feature point correlations on the focal plane from frame to frame (correspondence) makes it more difficult for the feature point tracking algorithm [7]. Another avenue is to pick feature points that move more on the focal plane in response to the anticipated direction of translation. For example for motion in the dir ection that the camera is looking, feature points on the periphery would have more m ovement than those in the center. A disadvantage is that in this case, these featur e points are in view for a much shorter period

PAGE 45

33 of time. Orienting the focal plane parallel (or more parallel) to the direction of motion where feasible should also improve the baseline. To observe the effect of using a longer baseline, an additional run was made with the update rate changed from 20 hz to 4 hz. The propagated state and error covariance were still computed at 20 hz. Noise equi valent to .47 pixel standard deviation was applied to the measurements from the beginni ng. The results are shown in Figure 3-9. The filter no longer fails and appears to conve rge for all states except v, which may be slowly converging. The r estimate has large variations but appears to be centered around the correct value. 3.4 Observations from Application with Statistical Model In this chapter implementation of the IKEF with the coplanarity constraint has been demonstrated, basically paralleling the work of Soatto et al. [8, 12]. However the extreme sensitivity of the algorithm to noise, particularly for the velocity estimates, was not apparent in their examples. The simulati ons here have shown a lack of robustness in the presence of noise that may affect the us efulness of this algorithm as applied to the state estimation task in UAVs. The problem a ppears to be related to baseline length for the measurements. Gurfil and Rotstein [16], referred to in chapter 1, mentioned similar problems in the presence of noise in their wo rk using the subspace constraint with aircraft dynamics as the model. In the following chapters the method will be applied with a specific model that includes the dynamics of the UAV.

PAGE 46

34 Figure 3-1. Trajectory for statistic al dynamic model numerical simulation. Figure 3-2. Focal plane measurement. u v Feature Point Camera Center of Projection Focal Plane ˆcj ˆci ˆckx X

PAGE 47

35 Figure 3-3. Feature point movement on focal plant for initial run with no measurement noise. 0 2 4 6 8 10 -14 -13 -12 -11 Body Velocitiesu (ft/s) Estimated Actual 0 2 4 6 8 10 105 110 115 120 125 v (ft/s) 0 2 4 6 8 10 -2 0 2 4 6 w (ft/s)Sec 0 2 4 6 8 10 -0.05 0 0.05 0.1 0.15 Body Angular Ratesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 0.1 0.15 q (rad/s) 0 2 4 6 8 10 -0.2 -0.15 -0.1 -0.05 0 r (rad/s)Sec Figure 3-4. Estimation results for initial run with no measurement noise.

PAGE 48

36 0 2 4 6 8 10 -14 -13 -12 -11 Body Velocitiesu (ft/s) Estimated Actual 0 2 4 6 8 10 105 110 115 120 125 v (ft/s) 0 2 4 6 8 10 -2 0 2 4 6 w (ft/s)Sec 0 2 4 6 8 10 -0.05 0 0.05 0.1 0.15 Body Angular Ratesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 0.1 0.15 q (rad/s) 0 2 4 6 8 10 -0.2 -0.15 -0.1 -0.05 0 r (rad/s)Sec Figure 3-5. Estimation results with .03 pixel measurement noise. 0 2 4 6 8 10 -14 -13 -12 -11 Body Velocitiesu (ft/s) Estimated Actual 0 2 4 6 8 10 0 50 100 150 v (ft/s) 0 2 4 6 8 10 -10 -5 0 5 w (ft/s)Sec 0 2 4 6 8 10 -0.05 0 0.05 0.1 0.15 Body Angular Ratesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 0.1 0.15 q (rad/s) 0 2 4 6 8 10 -0.15 -0.1 -0.05 0 0.05 r (rad/s)Sec Figure 3-6. Estimation results with .14 pixel measurement noise.

PAGE 49

37 0 2 4 6 8 10 -14 -13 -12 -11 Body Velocitiesu (ft/s) Estimated Actual 0 2 4 6 8 10 -50 0 50 100 150 v (ft/s) 0 2 4 6 8 10 -15 -10 -5 0 5 w (ft/s)Sec 0 2 4 6 8 10 -0.2 -0.1 0 0.1 0.2 Body Angular Ratesp (rad/s) 0 2 4 6 8 10 -0.1 -0.05 0 0.05 0.1 q (rad/s) 0 2 4 6 8 10 -0.3 -0.2 -0.1 0 0.1 r (rad/s)Sec Figure 3-7. Estimation results with .47 pi xel measurement noise added at 6 seconds. Figure 3-8. Focal plane fo r results with measurement noise added at 6 seconds.

PAGE 50

38 0 2 4 6 8 10 -14 -13 -12 -11 Body Velocitiesu (ft/s) Estimated Actual 0 2 4 6 8 10 100 110 120 130 v (ft/s) 0 2 4 6 8 10 -10 0 10 20 w (ft/s)Sec 0 2 4 6 8 10 -0.05 0 0.05 0.1 0.15 Body Angular Ratesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 0.1 0.15 q (rad/s) 0 2 4 6 8 10 -0.2 -0.1 0 0.1 r (rad/s)Sec Figure 3-9. Estimation results with exte nded baseline, .47 pixel measurement noise.

PAGE 51

39 CHAPTER 4 APPLICATION WITH A UAV DYNAMIC MODEL In this chapter a more complex dynamic m odel, that of a typical fixed-wing model aircraft, is implemented in the estimator. It is shown that the additional information provided by the dynamic model allows more stat es to be estimated than the five from previous efforts that were demonstrated in the last chapter. Numerical results along several trajectories are examined. A modification to the IEKF is implemented to overcome deficiencies evident in the presence of measurement noise. 4.1 Dynamic Model Description The UAV simulated is an eight foot wingspan class radio controlled model aircraft pictured in Figure 4-1. The aircraft has four control inputs; ailerons ( a), rudder ( r), elevator ( e), and throttle ( T). A high fidelity 12 state nonlinear model of the aircraft is given in reference [23] which was derived from the general non-linear equations of motion for a rigid body. Mass properties were measured on the actual model and aerodynamic coefficients were measured in a wind tunnel. This high fidelity model was coded and used as the truth model. A brief de scription of this model is given in appendix A. For the filter model, it was desired to us e something simpler than the truth model and representative of what might be reasonabl y expected to be obtai nable for a vehicle in the field. The model chosen was the “perturb ed state model” which is a linearization of the truth model about a nominal steady state or “trimmed” flight condition. In this

PAGE 52

40 technique the deviati ons of the original state variables and co ntrol inputs from the trimmed conditions are the new perturbati on variables and controls such that: ss ss x xx uuu (4-1) Here x is the familiar state vector, xss is comprised of the consta nt steady state values, and x is the perturbation state vector. The control inputs, u are handled in a similar fashion. This method is quite frequently used in aircraft control system development [24]. (It is envisioned that, in practice, this model c ould also come from system identification analysis of flight test data for the des ired type UAV flown remotely by hand at the trimmed condition since no high fidelity model is likely to exist.) Regardless of the type and fidelity of the model used to propagate the state, recall from the previous chapters that a linear representation is required to propagate the state error covariance. The IEKF state vector was expanded to include two additional states; the aircraft pitch attitude ( ) and roll attitude (). These are two of the th ree usual Euler angles that describe the orientation of the aircraft body axes with respect to the local level inertial frame with , being the ordered rotations about the 3,2,1 axes, respectively. This is similar to the rotations described in Equati on 2-28. The justification for adding these states will be discussed later. The re sulting IKEF state vector is now given as: [,,,,,,,]Txuvwpqr (4-2) and the control vector is defined as: [,,,]T areTu 1 (4-3) 1 u is used to denote both the scalar for the component of velocity along the body i axis and the vector of controls to the UAV to conform to popular convention. Differentation can be made by context.

PAGE 53

41 For this study, the case of no wind is assu med. The trimmed flight condition used is straight and level at a velocity of 73.3 f t/s (50 mph) at an altitude of 7500 ft, standard day. For this condition the steady state va lues computed from the truth model are xss=[72.75, 0, 9.19, 0, 0, 0, .126, 0]T where units are ft/s, rad/s and rad as appropriate and uss=[0, 0, -2.82, 1.11]T where units are degrees of de flection and horsepower. The equations of motion from appendix A were linearized about this condition using the methods given in Roskam [24]. This con tinuous representation wa s then converted to discrete form by using well known techniques from state-space linear systems theory as given in, for example, Brogan [25]. The inte rval chosen for the discrete form was .05 seconds as this is the minimum filter measurem ent interval expected to be used. (This linearization and discretization process coul d have also been done using numerical linearization routines from Matlab toolboxes.) The resulting linear discrete perturbation model is given below: 1kkk x AxBu (4-4) where the perturbation state vector [,,,,,,,]Txuvwpqr (4-5) the perturbation control vector [,,,]T areTu (4-6) the system or state transition matrix

PAGE 54

42 .988 0 .0309 0 -.371 0 -1.591 0 0 .954 0 .419 0 -3.48 0 1.58 -.0122 0 A .791 0 2.71 0 -.170 0 0 -.0213 0 .638 0 .0957 0 -.0187 .000644 0 -.000664 0 .829 0 -.000458 0 0 .00802 0 -.0119 0 .976 0 .00647 .0000166 0 -.0000179 0 .0456 0 1 0 0 -.000550 0 .0340 0 .00839 0 1 (4-7) and the control matrix 00.00289.2782 .0999.023600 00-.0334.0297 .412.019700 00-.00926 .00008 .00217-.0048500 00-.000239.00000136 .0110.00051300 B (4-8) Those readers familiar with conventional ai rcraft dynamics will recognize that the resulting equations decouple into two independent sets of dynamics known in the literature as the “longitudinal” set and the “l ateral-directional” set [25]. The linearized equations of motion about this condition produce the conventional aircraft dynamic modes. The longitudinal set is comprised of two oscillatory m odes, the “phugoid” and “short-period” governing motion in the vertic al plane and about the pitch axis. The lateral-directional set consists of an osci llatory mode, “Dutch-roll”, and two aperiodic modes, “roll” and “spiral” concerning moti on in the horizontal plan e and about the roll and yaw axes. In the dynamics for this partic ular flight condition, the spiral mode has an unstable pole at .076 (about 9 seconds time to double amplitude) indicating the aircraft

PAGE 55

43 will tend to roll off into a tightening “spiral” if perturbed from trim with no correcting control inputs. For state propagation in the IEKF this linear representation, Equation 4-4, was augmented as follows: 1 1(cos1) ()/ ()/ ()/ (cos1) ()()zzyyxx kkk xxzzyy yyxxzz ss krvqw u pwru v qupvg w qrIII p xAxBu prIII q pqIII r qr qr kt (4-9) where ,,1.7,6.8,9.3xxyyzzIII slug-ft2 are the vehicle moments of inertia about the roll, pitch, and yaw axes, respectively, g is the acceleration of gravity, and t is the integration interval, .05 seconds in this case. These ki nematic higher order non-linear terms increase the accuracy of the IEKF model as it strays from its steady state condition and yet require very little knowledge of the specific vehi cle. The derivation is in Appendix B. The following summarizes the steps in the IEKF used to propagate the aircraft states and error covariance from k-1 to k : Obtain xk-1 by subtracting xss from the previous updated estimate of xk-1 (Equation 4-1). Obtain uk-1 by subtracting uss from the previous value of uk-1 (Equation 4-1). Compute the propagated value of xk using Equation 4-9. Obtain the propagated value of xk by adding xk and xss. Compute the propagated value of the e rror covariance using Equation 2-11 which becomes 1 T kkPAPAQ where Q the process noise matrix, is a constant for this application.

PAGE 56

44 It is assumed that the control input vector, u which in general is not a constant, is known to the filter with a high degree of accuracy. This is a reasonable assumption since these commands are either sent to the aircraft or generated on board by an autopilot. Therefore, control uncertainty does not enter in to the error statistics computation. Also, since the trimmed values of the states are know n constants, it is only the uncertainty in the perturbed states that affects the error covariance. Recall from chapter two that Q is used to account for modeling errors and uncertainties as well as process noise and in practice is a tuning f actor in the filter. 4.2 Observability Considerations In the last chapter it was demonstrated that the IEKF with the coplanarity constraint was able to estimate, at least to some degree, five parameters related to camera motion, angular rates and direction of velocity, usi ng a very simple system model that provided almost no information about the dynamics of the motion. The additional information provided in the more complex UAV dynamic model allows the estimation of additional states. Computing and examining the observability matrix from linear systems theory (see reference [25]) gives insight into this. For a linear system given by: 1 111kkk kkk x AxBu zHx (4-10) where z is the measurement and H is the measurement matrix, the observability matrix is given by:

PAGE 57

45 2 1nH HA O HA HA (4-11) Where n is the number of states. The system is fully observable if O has full rank, n For a cursory look at observability usi ng the UAV dynamic model given in the last section, assume that the five states v, w, p, q, r can be measured and that the measurement equation is 11 101000000 00100000 00010000 00001000 00000100kk ku v w p zHx q r (4-12) Although the real, time varying measurement ma trix, expressed in Equations 2-18, 2-5, and 2-35 through2-50, is much more complex, this simplification is useful for a preliminary assessment of observability based on the results of the last chapter. The measurement matrix H from Equation 4-12 can be used with the UAV system matrix A from Equation 4-7 to compute the observability matrix O using Equation 4-11. When this is done, O has rank eight indicating that the system is completely observable. In fact, if the measurement is reduced to just the angular velocities, i.e.

PAGE 58

46 11 100010000 00001000 00000100kk ku v w p zHx q r (4-13) the resulting observability matrix has rank eigh t indicating that all eight states are still observable. A similar exercise can be done with a syst em matrix for the statistical model used in the last chapter but augmented with the addi tional two Euler angle states. In this case the system is described in Equation 4-13. (Note that some information has actually been added by using the kinematic relationship betw een the angular rates and the Euler angles for small angles over the .05 sec interval.) 110000000 01000000 00100000 00010000 00001000 00000100 0000.05010 000.050001kk ku v w p xAx q r (4-14) When the observability matrix is computed using the system matrix A above and the measurement matrix H from Equation 4-12, the rank is five indicating incomplete observability. This section has shown that the info rmation provided by the more complex dynamic UAV model should result in increased observability of the aircraft states.

PAGE 59

47 4.3 Simulation Description The IEKF simulation described in the last chapter was modified for the UAV dynamic model. The major modifications were to the system model in the filter used to propagate the state estimate and error covarian ce and the addition of the new states to the filter, as discussed in the previous sections. The measurement matrix, Hk, was augmented with zero columns to accommodate the new st ates. A provision was also made to add new feature points along the way at random locati ons in the field of view to replace those that dropped from view. The simulation runs were started with 15 to 20 feature points with new ones added whenever the number dr opped below ten. The field of view was unchanged at 60 degrees horizontal and 47 de grees vertical, 320 x 240 pixels, and the filter measurement noise covariance value for th e Kalman gain computation was also left unchanged at the equivalent of one pixel standard deviation, =.0036. The camera elevation angle was set at 60 degrees (30 degrees down from the longitudinal axis of the aircraft) so that the camera was looking “ahead and slightly down”. The update rate for the filter remained at 20 Hertz (hz). Based on preliminary runs using trial and error, a value of Q = diag[0 0 0 .49 .0025 .49 0 0] was c hosen for the process noise covariance and P = diag[100 1 1 .01 .01 .01 .01 .01] was chosen to initialize the state estimation error covariance. Three 10 second truth trajecto ries were used to assess f ilter performance. These trajectories were generated using the truth simulation (appendix A) driven by a simple autopilot to achieve the desired flight path s. The data needed from these truth simulations were the true values for the states to be estimated, the true aircraft position and orientation values for feature point meas urement generation, and the aircraft control inputs. The first trajectory was simply strai ght and level at the trim condition with the

PAGE 60

48 trimmed controls. The second was a vertical S maneuver which started at the trimmed flight condition with an initial pitc h down command from the elevator ( e) then a pitch up at 2.5 seconds followed by reversals at 2.5 second intervals. The third was a horizontal S maneuver initiated from the trim fli ght condition which used elevator ( e), rudder ( r), and aileron ( a) inputs to command horizontal turns in alternate directions at 2.5 second intervals. The throttle control ( T) remained constant at the trim setting for all trajectories. The truth values for each of th ese three trajectories are included in the plots for the next section. 4.4 Results With No Measurement Noise Runs with the filter simulation were made using the three trajectories with no noise added to the focal plane measurements (alt hough the filter still a ssumed that there was noise equivalent to =.0036). In each case the estimated states were intentionally misinitialized at a value different from trim to aid in observing filter convergence. Filter initialization values were ˆ [67.75, .5, 9.69, 0, 0, 0, .126, 0] x for all three cases which equates to errors of 5, .5, and .5 ft/s for u v, and w respectively. These are within the one sigma bounds for the initial state estimate error covariance, P given earlier. Results of the runs are shown in the figures at the end of the chap ter. For each state estimated, three values are shown; the actual or truth value, the estimated value from the filter, and a propagated value. This propagate d value is computed external to the filter, open loop, using the same dynamic model that is in the filter, starting with the same initial conditions and applying the same true c ontrol history. This is what the IEKF would produce with no measurements. The purpo se of this is to help assess how much the measurements are contribu ting to filter performance.

PAGE 61

49 Figure 4-2 shows the results for the strai ght and level trajectory. The estimated states seem to be converging for all stat es and provide better response than the propagated states in all cases except for perhaps w The estimates for the angular velocities appear to be particularly “tight”. Also, the propagated value for the roll angle phi is diverging as a result of the unstable spiral mode however the measurements keep the estimated value in check. Figure 4-3 shows the results for the straight and level trajectory in a different way. Here the just the errors (value – truth) are pl otted. Also included are the filter estimates for the one sigma error bounds (square root of the covariance) for each of the estimated states. In this case these uncertainties are overly pessimistic because the expected measurement noise is not present. Figures 4-4 and 4-5 show the results fo r the vertical S trajectory with no measurement noise and Figures 4-6 and 4-7 show results for the horizontal S trajectory. The updated estimated states appear to gene rally be more accurate than the propagated states although in some of th e velocities for the horizontal S trajectories the propagated states are actually better for a portion of the trajectory. In all cases the angular rate estimates appear to benefit significantly from the measurements and divergence in the roll angle estimate is overcome. 4.5 Results With Measurement Noise The three runs from the last section were repeated with Gaussian measurement noise added to the feature point measurement locations on the focal plane using the same procedure as with the statistical model simulations in chapter three. The equivalent of one pixel standard deviation ( =.0036) was used for the noise which is the same value used in the IEKF gain calculation. The estimated states were initialized with the same

PAGE 62

50 errors as in the previous section. The thre e sets of error plots are shown in Figures 4-8 through 4-10. The results are disappointing although not en tirely unexpected given the results for the statistical model with measurement noise sh own in the last chapter. The estimates for the velocities and Euler angles seem to de velop extreme bias-like errors that are well outside the expected bounds indicated by the predicted error covariances and substantially larger than for th e propagated states. The angul ar rate estimates appear to be somewhat better. Although they are ve ry noisy, the errors are approximately contained within the predicte d one sigma error bounds and th e mean error is close to zero. In an attempt to ensure that the single se t of filter initialization errors chosen was not dominating the filter performance, a Mont e Carlo ensemble of six runs was made for the straight and level case. Random draws for initialization errors for each of the eight states were made using a standard deviati on corresponding to the initialization covariance values in P The results of this set of runs ar e shown in Figure 4-11 and indicate similar results regardless of the f ilter initialization errors. As discussed in the last chapter for the statistical model, these problems seem to be a result of ill-conditioning due to short m easurement baseline (lack of parallax). It was demonstrated that lengthening the baseline by increasing the measurement interval would mitigate the problem. To investigate whethe r that also applies with the UAV dynamic model filter, the straight and level case was run with the measurement interval increased from .05 sec to .2 sec. The results are shown in Figure 4-12.

PAGE 63

51 With the longer baseline the state estimate s do not diverge as they did with the shorter baseline. However, the estimates are generally inferior to the propagated state values and the usefulness of the estimates fo r vehicle control is doubt ful. It is possible that some retuning of the filter might impr ove the performance but a more robust remedy for this problem is desired. To this e nd some possible weaknesses in the algorithm are examined. There are several factors in the implem entation of the IEFK into the true EKF framework that “stretch” the basic assumptions made in deriving the original Kalman filter. The EKF formulation assumes the measurement noise to be additive but this is not really the case for the IEKF as indicated in the following equations from chapter 2. Conventional EKF: ()kkkkzhxw (2-8 repeated) IEKF: ˆ ˆ (,,',')()kiiiikkzCex (2-33 repeated) In the IEKF the noisy measurements are mixe d in a nonlinear fashion with the estimated states to form the pseudo measurement. This could lead to higher order terms that are not negligible or zero mean as assumed for the EKF. A second factor is that the noise in the focal plane measurements is not truly “white” or uncorrelated over time as stipulated for the EKF. The “current” feature point locations on the focal plane that are a part of the current measurement, along with their errors, w ill also be a part of the next measurement as the “last” location. Soatto et al. address this in reference [12] and conclude that for their application, the small amount of correlation over just two steps has no discernable effect. In most successful Kalman filter appl ications, factors such as these are absorbed into the process noise as part of the tuning process. For the IEKF implementation being studied in this effort, the ill-conditioning e xperienced, particularly with the shorter

PAGE 64

52 baselines, appears to accentuate these factor s and leads to the type of errors observed when a reasonable amount of measurement noise is present. In the following sections, this ill-conditioning and its effects on filter pe rformance are explored in more detail and a robust solution is implemented. 4.6 Degree of Observability In practice it has been noted that low observability of all or some states of a system leads to high gains in a Kalman filter which is essentially ill-conditio ning [26]. In section 4.2 it was shown that the eight states for the case at hand were observable in the absolute sense but “how observable” or the degree of observability was not addressed. 4.6.1. Modal Observability A measure for degree of modal observability is given in references [26] and [27] which can be extended to the discrete case. For the linear system given earlier in this chapter: 1 111 kkk kkk x AxBu zHx (4-10 repeated) If the system has distinct eigenvalues then it can be decomposed into its modal form with a similarity transformation: 11 1 111kk kkk kkkxMq qMAMqMBu zHMq (4-15) where 12[,,]n M mmm is the matrix of the right eigenvectors of A and 1 M AMis a diagonal matrix with the eigenvalues on the diagonal. If Hk+1 is defined by rows as 11,12,1,1[,,]T kkknkHhhh then a measure of the degree of observability is:

PAGE 65

53 ,1 ,,,1 ,1 2 2cosikj oijk ikjhm hm (4-16) for the observability between measurement i and mode j cos o is a measure of the orthogonality between hi and mj. A value approaching zero means little observability and a value approaching one means high observability. The value of this measure was computed fo r a run with the estimation filter on the straight and level trajectory with no measurement noise. The results are shown in Figure 4-13 for a typical feature point. The larger va lues vary with different feature points but the two lower values are very close to the same for all feature points observed. The legend delineates the values with respect to the modes described earlier in the section on the UAV model; short period, Phugoid, roll, Dutc h roll, and spiral. For the oscillatory modes, the value shown was computed for a single component in the complex conjugate pair. The measure indicates low observabili ty for both longitudinal modes, the short period and Phugoid. This is an indicator that the filter can develop high gains and therefore is susceptible to conditioning problems [26]. 4.6.2 The Measurement Matrix Examining the behavior of the values in the measurement matrix during a run provides insight into the relative observability of the particular states and how that is related to measurement noise. Recall from Equation 2.18 that the elements of the measurement matrix, Hk, reflect how the pseudo measurem ents change with respect to each state variable. ˆ()kk xxCee HC xx (2-18 repeated)

PAGE 66

54 Figure 4.14 depicts the actual values of the measurement matrix during an estimation filter run for the straight and le vel trajectory with no measurement noise. Different feature points will have different va lues that change as the feature points move on the focal plane. Depicted are the values for a single representative feature point that form a single row of Hk. The legend delineates which st ate each value corresponds to. The partial derivative terms with respect to u , and are always zero as explained in chapter two and earlier in this chapter. In the lower plot the vertical scale has been expanded. The units for the angular rates are “per rad/sec” and for the velocities are “per ft/sec”. Note that the values for the angular rates are much la rger than for the velocities. This indicates that the pseudo measurement, the coplanarity constrain t, is much more a function of angular rates than of velocity, at least for these circumstances. Also note that the curves are smooth. It was demonstrated earlier that the filter performance was good for this case. Figure 4-15 shows the measurement matrix values obtained for the same feature point when the equivalent of one pixel meas urement noise is added. As demonstrated earlier, the filter did no t perform well. For this run the filter was modified so that the state updates were not added to the state estima tes. Also, the initialization errors were removed. This resulted in the estimated states matching the true states, thus the measurement matrix was calculated using errorfree state estimates so any errors are due solely to the measurement noise. Note that the curves now appear noisy, apparently due to the measurement noise. The noise seems to have a minor effect on the relatively large values for the angular rates which are still ve ry close to those of Figure 4-14 percentage wise. The values for the velocities, howeve r, are extremely erratic and bear little

PAGE 67

55 resemblance to those of Figure 4.14. (A sm oothed mean value may a pproach the curve in Figure 4.14 but even that is doubtful.) The poor performance of the filter in the presence of measurement noise seems to be caused by two factors working together. First, the measurement matrix is a function of the noisy measurements as well as the stat es. This is not the case in the standard EKF as was pointed out in the last section. S econd, the pseudo measurement has a relatively weak dependence on the velocities (direction of velocity, actually). This allows the measurement matrix and the computed gains rela ted to the velocities to be corrupted by the measurement noise to the extent that th e resulting velocity estimates are worthless. The errors also propagate through to the es timated Euler angles. The angular rate estimates seem to be more robust (see Figur e 4-11) due to a healthy dependence of the pseudo measurement on angular rates. Another way of looking at this is that the angular rate estimates are better conditioned due to their greater observabi lity making them more resistant to the measurement errors. The velocity estimates are poorly conditioned due to weak observability making them more vulnerable to the measurement errors. This ill conditioning is worse with shorter baselin es (due to slower UAV speed or a higher measurement rate, for example). 4.6.3 Modification of the IEKF Measurement Matrix Several factors suggest a possible soluti on to the problems experienced by the IEKF in the presence of measurement noise. The noise appears to significantly corr upt the part of the measurement matrix relating the pseudo measurement to the velo cities (more properly, the direction of velocity).

PAGE 68

56 These components of the measurement matrix, the partial derivatives with respect to v and w are very small values. The UAV model is completely observable ju st from knowledge of the angular rates (reference section 4.2). The updated angular rate states appear to st ill be fairly well behaved in the presence of measurement noise. The solution suggested is to neglect the components of the measurement matrix relating to velocity altogether, that is, set these values to zero in Hk. 4.7 Results With the Modified IEKF With Measurement Noise The modification to the IEKF described in the last section was made. The two columns in Hk relating the pseudo measurement to v and w were set to zero. Runs on the three trajectories were repeated with the same conditions as in section 4.5. The results are shown in Figures 4-16, 4-17, and 4-18. A comparison with Figures 4-8, 4-9,and 4-10 shows that estimated values from the modified IEKF are clearly better. The errors are less and appear to be bounded for the most pa rt by the filter predic ted covariances. In some cases the estimated states have slightly more error than the propagated states at certain points in the trajectory. The angular rates are especial ly noisy but the averages of the estimated values appear to track better th an the propagated values, particularly during the maneuvering profiles. Further filtering of these state estimates might be beneficial prior to use for autopilot feedback. A series of six runs was made along each of the three trajectories varying the initial errors in a Monte Carlo fashi on as was done previously for th e straight and level case in section 4.5. The results are shown in Figures 4.19, 4.20, and 4.21. These results also show much better result s after the modification to the IE KF. The angular rates are noisy but all the states seem to converge. So me momentary deviations are noted during

PAGE 69

57 maneuvers especially on the horizontal S wh ich involves control i nputs on all three aircraft axes. In this chapter, the IEKF using the copl anarity constraint has been applied to a specific set of UAV dynamics. It was shown that the extra information provided by the UAV model allowed the estimation of additional st ates over that of th e simple statistical model used in previous work in this area. Ill-conditioning problems in the velocity updates were evident in the presence of measurement noise. These were overcome by setting the small noisy velocity partial derivatives terms in the measurement matrix to zero. Performance using this modified IEKF was demonstrated on three different trajectories. Figure 4-1. Eight foot wingspan class a ircraft simulated for this effort [23].

PAGE 70

58 0 2 4 6 8 10 65 70 75 Body Velocitiesu (ft/s) Estimated Actual Propagated 0 2 4 6 8 10 -0.5 0 0.5 v (ft/s) 0 2 4 6 8 10 9 9.5 10 w (ft/s) 0 2 4 6 8 10 -0.02 0 0.02 Body Angular Ratesp (rad/s) 0 2 4 6 8 10 -0.02 -0.01 0 0.01 q (rad/s) 0 2 4 6 8 10 -0.02 0 0.02 r (rad/s) 0 2 4 6 8 10 0.05 0.1 0.15 Euler Angles Sectheta (rad) 0 2 4 6 8 10 -0.01 0 0.01 Euler Anglesphi (rad)Sec Figure 4-2. Straight and level traje ctory simulation, no measurement noise. 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -4 -2 0 2 4 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 q (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 4-3. Errors from straight and leve l trajectory simulation, no measurement noise.

PAGE 71

59 0 2 4 6 8 10 65 70 75 80 Body Velocitiesu (ft/s) Estimated Actual Propagated 0 2 4 6 8 10 -0.5 0 0.5 v (ft/s) 0 2 4 6 8 10 6 8 10 12 w (ft/s) 0 2 4 6 8 10 -0.02 0 0.02 Body Angular Ratesp (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 q (rad/s) 0 2 4 6 8 10 -0.02 0 0.02 r (rad/s) 0 2 4 6 8 10 0 0.2 0.4 Euler Angles Sectheta (rad) 0 2 4 6 8 10 -0.01 0 0.01 Euler Anglesphi (rad)Sec Figure 4-4. Vertical S trajectory simulation, no measurement noise. 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -2 0 2 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 4-5. Errors from vertical S tra jectory simulation, no measurement noise.

PAGE 72

60 0 2 4 6 8 10 60 70 80 90 Body Velocitiesu (ft/s) Estimated Actual Propagated 0 2 4 6 8 10 -10 0 10 v (ft/s) 0 2 4 6 8 10 6 8 10 12 w (ft/s) 0 2 4 6 8 10 -2 0 2 Body Angular Ratesp (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 0.2 q (rad/s) 0 2 4 6 8 10 -1 0 1 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 0.2 Euler Angles Sectheta (rad) 0 2 4 6 8 10 -1 0 1 Euler Anglesphi (rad)Sec Figure 4-6. Horizontal S trajectory simulation, no measurement noise. 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -5 0 5 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 4-7. Errors from horizontal S tra jectory simulation, no measurement noise.

PAGE 73

61 0 2 4 6 8 10 -20 -10 0 10 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -3 -2 -1 0 1 v (ft/s) 0 2 4 6 8 10 -1 0 1 2 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 4-8. Errors from straight and le vel trajectory, one pixel measurement noise. 0 2 4 6 8 10 -40 -20 0 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -4 -2 0 2 v (ft/s) 0 2 4 6 8 10 0 2 4 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 0.4 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 4-9. Errors from vertical S trajectory, one pixel measurement noise.

PAGE 74

62 0 2 4 6 8 10 -30 -20 -10 0 10 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -5 0 5 10 15 v (ft/s) 0 2 4 6 8 10 -1 0 1 2 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 0.2 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 0.2 Euler Angle Errorsphi (rad)Sec Figure 4-10. Errors from horizontal S trajectory, one pixel measurement noise. Figure 4-11. Monte Carlo runs, straight a nd level trajectory with measurement noise. 0 2 4 6 8 10 -40 -20 0 Velocity Errors, Body Axesu (ft/s) Erro r Cov. Est. 0 2 4 6 8 10 -5 0 5 10 v (ft/s) 0 2 4 6 8 10 -1 0 1 2 3 w (ft/s) 2 4 6 8 10 -0.2 0 0.2 A ngular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 0.4 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.2 0 0.2 Euler Angle Errorsphi (rad)Sec

PAGE 75

63 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -10 0 10 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.2 0 0.2 Euler Angle Errorsphi (rad)Sec Figure 4-12. Straight and level trajectory errors with measurement noise, increased measurement interval. 0 20 40 60 80 100 120 140 160 180 200 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Filter Step kcos Theta oModal Observability for Feature Point 2 SP Phu roll DR Spiral Figure 4-13. Modal observability for straight and level trajectory, no measurement noise.

PAGE 76

64 0 20 40 60 80 100 120 140 160 180 200 -0.15 -0.1 -0.05 0 0.05 0.1 H Matrix Row for Feature Point 2h u v w p q r 0 20 40 60 80 100 120 140 160 180 200 -1.5 -1 -0.5 0 0.5 1 1.5 x 10-4 h ExpandedFilter Step k Figure 4-14. Measurement matrix values for straight and level, no noise. 0 20 40 60 80 100 120 140 160 180 200 -0.15 -0.1 -0.05 0 0.05 0.1 H Matrix Row for Feature Point 2h u v w p q r 0 20 40 60 80 100 120 140 160 180 200 -5 0 5 x 10-4 h ExpandedFilter Step k Figure 4-15. Measurement matrix values for straight and level, one pixel measurement noise.

PAGE 77

65 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -4 -2 0 2 4 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 4-16. Errors for modified IEKF, stra ight and level trajectory with measurement noise. 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -2 0 2 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 4-17. Errors for modified IEKF, ve rtical S trajectory with measurement noise.

PAGE 78

66 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Estimated Propagated Cov. Est. 0 2 4 6 8 10 -2 0 2 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 4-18. Errors for modified IEKF, horiz ontal S trajectory with measurement noise. Figure 4-19. Modified IEKF Monte Carlo, st raight and level with measurement noise. 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Erro r Cov. Est. 0 2 4 6 8 10 -2 0 2 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 A ngular Rate Errors, Body Axes p (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errors phi (rad)Sec

PAGE 79

67 Figure 4-20. Modified IEKF Monte Carlo, vertical S with measurement noise. Figure 4-21. Modified IEKF Monte Carlo horizontal S with measurement noise. 0 2 4 6 8 10 -20 0 20 Velocity Errors, Body Axesu (ft/s) Error Cov. Est. 0 2 4 6 8 10 -5 0 5 v (ft/s) 0 2 4 6 8 10 -2 0 2 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 A ngular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errors phi (rad)Sec 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Error Cov. Est. 0 2 4 6 8 10 -5 0 5 v (ft/s) 0 2 4 6 8 10 -2 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 A ngular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errors phi (rad)Sec

PAGE 80

68 CHAPTER 5 ROBUSTNESS AND SENSITIVITY ASSESSMENT For practical use, the modified IEKF developed in the last chapter must be tolerant of inaccuracies in the system model that is used in the filter. These inaccuracies are unavoidable and come from simplifications in the governing equations and from uncertainties in parameters such as UAV aerodynamics, mass properties, and trim conditions. Some of these parameters may vary from flight to flight for a given set of hardware. In this chapter the robustness of th e modified IEKF filter developed in the last chapter is assessed using a Monte Carlo analysis. It is also desirable to gain some insight into how some other parameters that are con trollable by the designer affect performance. These are items such as camera depression a ngle, camera field of view, feature point selection, etc. Filter performance sensitivity to some of these parameters is examined using the same Monte Carlo framework. Fi nally, the effect of varying amounts of measurement noise on filter pe rformance is investigated. 5.1 Robustness to Modeling Uncertainties Robustness of the IEKF to uncertainties in the system model was assessed using Monte Carlo runs on the same truth trajectories used in the last chapter. An attempt was made to choose the Monte Carlo variables and their bounds in a realistic fashion. (These are constants in the system model but are changed for each individual run using draws from a random number generator.) Five gr oups of parameters were chosen to vary. Steady-state or trimmed f light condition. The UAV might be trimmed at a condition that is not exactly what is in the filter model. The values given in chapter four were used as the mean with standard deviations of 5 ft/s for uss, 1 ft/s for vss

PAGE 81

69 and wss, and .02 radians for ss and ss. The steady-state angular rates were not varied as it was assumed the UAV would alwa ys be trimmed to fly in a straight path. Steady-state or trimmed controls. The trim med flight control settings might not be precisely known or might be offset in the UAV controls. The values given in chapter four were used as the mean w ith standard deviations of .1 deg for deflections and .1 horsepower for throttle. Aerodynamic parameters. The aerodynamics of the system model are contained in the A and B matrices given in Equations 47 and 4-8. Aerodynamic coefficients are present in the terms in the upper left 6x6 partition of the A matrix and the upper left 6x3 partition of the B matrix. (The ot her terms in these matrices have to do with kinematics, gravity, and thrust.) This is shown in Roskam [24]. The aerodynamic uncertainties were modeled by varying these parameters individually by a five percent standard deviation around the nominal. This was done with the continuous form of the matrices and then the effect over .05 seconds was added to the discrete matrices in Equations 4-7 and 4-8. This was an approximation since all the terms varied are not strictly “only aerodynamic” but their major effects are. Mass properties. There are likely to be uncertainties in the UAV mass and moments of inertia. These uncertainties were modeled similar to aerodynamic uncertainties by varying the A and B matrices. The first three rows in these matrices come from “acceleration equals force divided by mass” equations so these terms in the matrices were varied togeth er corresponding to an approximate five percent standard deviation va riation in mass about the nomin al value. Again, this was done with the continuous form of the matrices and then the effect over .05 seconds was added to the values in Equati ons 4-7 and 4-8. The moment equations were handled in a similar fashion. The fourth row was varied for a five percent standard deviation in roll moment of iner tia, the fifth row for pitch inertia, and the sixth row for yaw inertia. The higher orde r terms in Equation 4-9 were not varied from the nominal moment of inertial values. Longitudinal center of mass location. There is likely to be uncertainty in the center of mass location for the UAV and it may also vary if components are shifted from flight to flight. An allowance for an uncer tainty of .1 foot standard deviation along the longitudinal axis of the vehicle from the nominal location was provided. This was done again using the A and B matrices. The pitch and yaw moment equations, rows five and six, were changed togeth er by adjusting each moment component by an amount determined using the change in center of mass location and the appropriate term from the force equations. For example the pitching moment due to w term, A(5,3), was adjusted using the normal force due to w term, A(3,3), through a lever arm of the change in cente r of mass and rescaled with mass divided by pitch moment of inertia. Again, this was done using the continuous equations first.

PAGE 82

70 In all, 39 Monte Carlo parameters were chosen. This list is certainly not all inclusive. For instance camera inaccuracies and atmospheric turbulence are not included. However this assortment should allow enough perturbation from the baseline conditions to assess robustness. To provide insight into the effects of th e different types of uncertainties, seven cases were run against th e three trajectories for a total of 21 sets. Each set consisted of 50 runs. The cases are described below. Case 1. None of the model parameters were varied. The initial state estimates were varied and measurement noise was applied as in the Monte Carlo runs in chapter 4. Case 2. Same as case 1 plus the st eady-state flight condition Monte Carlo parameters. Case 3. Same as case 1 plus the steady-state control Monte Carlo parameters. Case 4. Same as case 1 plus the aerodynamic Monte Carlo parameters. Case 5. Same as case 1 plus the mass properties Monte Carlo parameters. Case 6. Same as case 1 plus the lo ngitudinal center of mass Monte Carlo parameter. Case 7. Case l plus all 39 Monte Carlo parameters at once. The modified IEKF was used with the same setup as described in section 4.3 with regards to field of view, camera depression angle, process and measurement noise parameters, focal plane, measurement interval and feature points. Actual measurement noise was one pixel standard deviation, =.0036, for all runs. A suitable metric was needed to characte rize each Monte Carlo set. In Figure 5-1 the estimation errors for a set of six runs are shown for case 1 on the straight and level trajectory to demonstrate the process. The error plots are informative for six runs, however when 50 runs are made the overlaid plots would be useless.

PAGE 83

71 For the 50 run sets, or ensembles, the mean error and its standard deviation about the mean are computed at each .05 second inte rval. Plots of this are shown for case one on all three trajectories in Figur es 5-2, 5-3, and 5-4. Mean values close to zero and small standard deviations are desirable. Ther e is some trending in the mean during the maneuvers on the vertical and horizontal trajectories but no divergence. The metric chosen to characterize each set was the average of the error means and standard deviations over the last five seconds. This was to allow the filter a chance to converge from the erroneous initial conditions and also to average out the eff ects of the trajectory maneuvers (since the maneuvers were not rando m). Results for the 21 sets are tabulated in Tables 5-1, 5-2, and 5-3. In order to ma ke the results more readable, bar graphs of the results are plotted in Figures 5-5, 5-6, and 5-7. Note that the absolute values of the mean instead of the actual values from the table are plotted. An additional finding is that none of the 1050 runs had divergences, that is, errors increasing without bound. Overall, the results i ndicate that the modified filter is robust to model inaccuracies. The angular rates are very insensitive to model variations. This is because of the high observability of the angular rates in the measurement matrix as discussed in the last chapter. Deviations in the trim conditions, case two for the state variables and case three for controls, seemed to have more effect than the errors in aerodynamics, mass properties, and center of mass location, cases four, five, and six. The states that seem the most vulnerable are u and These are longitudi nal states. Recall from section 4.6.1 that these modes were predicted to have low observability. Notice from the tabulated values of the means the correlation in signs. When u error is positive, error is negative. This is inherent in the dynamics of the model since the UAV tends to

PAGE 84

72 slow down as pitch increases and speed up as pitch decreases. The results for case seven indicate good robustness to all th e error sources applied at once. 5.2 Sensivity to Design Parameters Certain parameters in the implementation of the estimation scheme are chosen by the designer. Sensitivity of the filter perf ormance to some of these parameters was assessed. The parameters examined were camera depression angle, camera field of view, filter measurement interval, and numbe r and location of feature points. 5.2.1 Camera Depression Angle Recall that the camera angle is refere nced to the UAV vertical body axis, i.e. c=0 when aligned with the body z axis (or approximately down when the UAV is at trim). Three camera angles were chosen for evaluation, c=0 degrees, c=60 degrees (the baseline configuration), and c=82.8 degrees which at trim conditions correspond to 82.8, 22.8, and 0 degrees down from the horizon, resp ectively. To demonstrate the effect this has on feature point movement, three focal plane histories with the different camera angles are shown in Figure 5-8 for the strai ght and level trajector y with no measurement noise. Fifty run Monte Carlo sets were performed for each of the three camera angles on each of the three trajectories for a total of nine sets. The only Monte Carlo variables used were the initial state variables and the meas urement noise which corresponds to case one in the last section. The same metrics were co mputed as in the last section. The results are presented in similar fashion in Tabl e 5-4 and Figures 5-9, 5-10, and 5-11. The angular rates show a consistent trend for all three trajectories. The estimation for yaw rate, r is worse when the camera is looking down and best with the camera on the horizon. Estimation for roll rate, p is best when the camera is looking down. Both

PAGE 85

73 roll and pitch rate estimation, p and q are a little worse at the in termediate camera angle. Side velocity, v, follows the same trend as r The other variables do not seem to have a consistent trend through out all three trajectories. None of the state estimates diverged during any of the 450 runs. Overall, the para meter of camera depression angle appears to have a definite but not overpowering effect on filter estimation performance for this application. 5.2.2 Camera Field of View The focal plane specifics remain the same at 320 x 240 pixels with aspect ratio 4:3. Recall that the baseline field of view is 60 degrees horizontal by approximately 47 degrees vertical. The design parameter to be varied here is field of view while using the same focal plane. This is equivalent to changing the focal length of the lens used in the camera. When field of view is narrowed with the same focal plane, an increase in resolution is gained which shoul d translate into more accurate feature point coordinates. For the for the focal plane simulation algorithm used in this effort, the measurements are normalized to the focal length as explained in chapters two and three. The increased resolution is realized by reducing the measurem ent noise standard deviation to keep it at the equivalent of one pixel. The opposite is done when the field of view is widened. For this assessment, three fields of vi ew were chosen; 30 degrees, 60 degrees (the baseline), and 90 degrees horizontal. Th e measurement noise applied had standard deviations of .001675, .0036, and .00625, respecti vely, to keep the noise at the equivalent of one pixel. The filter assumed value for measurement noise remained unchanged. As in the previous section, 50 run Monte Carlo sets were done for each field of view on each of the three trajectories for a total of nine sets. Monte Carlo variables remained the same

PAGE 86

74 and c was set back to the baseline value of 60 degrees. The results are presented in previous fashion in Table 5-5 and Figures 5-12, 5-13 and 5-14. Consistent trends noted over the three tra jectories are that the pitch and yaw rate errors appear smaller for the narrower fields of view but the roll angle errors are smaller at the wider fields of view. Over all, the re sults seem to show surprisingly little variation in performance as the field of view varies. 5.2.3 Measurement Interval The effect of increasing the measurement interval was investigated. This was explored in chapters three a nd four with the unmodified f ilter as a way of increasing feature point parallax to help with ill-condi tioning. Three measurement intervals were assessed; the baseline value of .05 seconds .25 seconds, and .5 seconds. The Monte Carlo variables and filter configuration remain ed the same with the field of view at 60 degrees. Three sets of 50 Monte Carlo runs were done on each of the trajectories for a total of nine sets. To see the effect of increased sample interval, a time history of statistics from the .5 second measurement inte rval set of runs on the straight and level trajectory is shown in Figure 5-15. The “saw tooth” effect on the one sigma bounds for the angular rates is typical behavior as th e uncertainty grows between updates. This cyclic behavior propa gates through to v and w The overall results for the nine sets are presented in previous fashion in Tabl e 5-6 and Figures 5-16, 5-17, and 5-18. The results for the straight and level and vertical S cases are unexpected at first glance. The overall performance of the filter as indicated by the erro r standard deviations seems to improve with less frequent updates fo r most states which is counterintuitive. More measurements mean more information which should result in better estimates. However, for the horizontal S case, the perfor mance shows an overall deterioration with

PAGE 87

75 increased update interval which would be expected. These results are not fully understood but a plausible explanation has to do with filter tuning which, as explained in chapter three, is the selection of the process noise covariance matrix, Q Q is what drives the estimated filter uncertainty, P through the system model (Equation 2-11) to offset modeling uncertainties and non-linearities as well as disturbances. The Kalman gain matrix, k is determined based on P and the measurement noise (Equations 2-13 and 2.21). It can be thought of as a weighti ng on the measurement residual based on the estimated uncertainty in the current estimate as compared to the assumed noise in the pseudo-measurement. A high uncertainty in the estimate and a relatively low measurement noise leads to larger gains a nd updates while the converse leads to smaller gains and updates. For k to be optimum, accurate values of process and measurement noise must be used. Q and the feature point measurement noise, R were constant for these sets. It can be argued that the “real” uncertainty is highest for the horizontal S profile since the UAV is maneuvering the most and therefore further from the trim conditions on which the model is based. If the filter is tuned for the maneuvering case, then it may be mistuned for the non-maneuveri ng case resulting in gains that are proper for the maneuvering case but too high for th e non-maneuvering case. This might explain why the results make sense for the horizontal S case and not for the other two. Also note that for the single state variable the trends are correct for all three trajectories (that is, the estimate gets worse as measurement interv al increases). Recall that this is the variable that most reflects the one unstable mode in the process model. It requires updates to prevent divergence and might not be as sensitive to the higher gains in the in the non-maneuvering case.

PAGE 88

76 5.2.4 Number of Feature Points More feature points result in more measurements at each update which should, as a general rule, increase the filter performan ce. Non-filter based methods employing the coplanarity constraint require a minimum of eight points or five points [5, 12, 19] to obtain a solution. As mentioned in chapter on e, an advantage of this filter based method is that it will provide estimates that gracef ully degrade when feature point measurements are reduced or even lost completely for a time Soatto reported in his work that filter performance tended to “saturate”, that is reach a point of limited improvement, after about 20 feature points [12]. The effect of varying the number of feat ure points used for the measurements was investigated. Runs were made using five, ten, twenty, and forty feature points. The Monte Carlo variables and filter configuration remained the same. The 60 degree field of view and .05 second measurement interval fr om the baseline configuration were used. Each run was started with the desired numb er of feature points. As feature points dropped from view, new ones were generated at random positions. At some points in the runs, the number of feature points dipped be low the desired value momentarily because only one new feature point was a llowed to be added at each measurement. This was true more for the maneuvering profiles at the highe r numbers of feature points which tended to drop points more rapidly. Four sets of 50 Monte Carlo runs were done on each of the trajectories for a total of 12 sets. The results are presented in previous fashion in Table 5-7 and Figures 5-19, 5-20, and 5-21. The performance of the filter appears to im prove with the increase in feature points, at least for the lateral-directional states, v, p r and The accuracy of the longitudinal

PAGE 89

77 state estimates does not seem to be affected as much. The saturation in performance mentioned earlier does not seem to occur, that is, performance improvement does not seem to be leveling off with 40 feature points. None of the estimat es diverged during any of the 600 runs. 5.2.5 Feature Point Location Insight into the observability of p q and r with respect to feature point location can be obtained by examining the values in measurement matrix H for different feature point locations. Figure 5-22 shows f eature point traces on the focal plane for several feature points for a filter run on the st raight and level trajectory with no measurement noise. The triangle at the top of the focal plane depicti on in Figure 5-22 is the point from which all the feature points appear to emanate or be moving away from which is constant during this run since the vehicle is in a trimmed, straight, steady state flight condition. This point is known as the focus of expansion (F OE) in optic flow literature [14, 15]. The FOE also depicts the direction of translationa l motion of the vehicle. The position of the feature point with respect to the FOE is what seems to be relevant. Also shown in Figure 5-22 is a plot of the changing values in H that relate the pseudo-measurements from a specific feature point to the state variables p q and r (Recall that the components of H are the partial derivatives of the coplanarity constraint for each featur e point with respect to the state variables and that these are set to zero for all states except p q and r .) The five numbered segments on the plot correspond to the five numbered feature point traces on the focal plane depiction. A larger magnitude of h indicates a greater observability of the particular state variable for that feature point location. Figure 5-22 seems to indicate the following:

PAGE 90

78 q (pitch rate) observability increases as the magnitude of the horizontal distance, from the FOE increases. Segments 2, 4, and 5 have smaller absolute values for h and the corresponding feature points are clos er horizontally to the FOE than for 1 and 3. r (yaw rate) observability increases as the magnitude of the vertical distance, from the FOE increases. Segments 3 and 5 have smaller absolute values for h and the corresponding feature points are closer ver tically to the FOE than for 1,2, and 4. p (roll rate) observability increases as th e overall distance from the FOE increases. Segments 4 and 5 have smaller absolute values for h and the corresponding feature points are closer the the FO E than for 1, 2, and 3. It should be noted that for the run shown, th e standard set-up was used so the FOE was not aligned exactly with the body axis system used to define the angular rates. However it is only off in pitch by the angle of attack of the vehicle, approximately seven degrees, which should not affect the validity of the above subjective observations. An attempt was made to assess how featur e point location on the focal plane affects filter performance. It seems to be a reasona ble assumption that filter performance would be related to feature point location in accordance with the preceding discussion on observability. To investigate this, Monte Carlo runs were made with restrictions on the focal plane as to location of feature points. Four configurations were chosen as illustrated in Figure 5-23; the full 60 degree fiel d of view, a vertical strip centered on the FOE of width .2, a horizontal strip fr om top of the field of view at =.433 to =.233 (including the FOE at =.42), and a restricted box around th e FOE with of .433 and .1 and +/.2. The Monte Carlo variables and filt er configuration remained the same as in the previous section and runs were made us ing all three trajectories. The runs were started with approximately 10-15 feature points. As feature points dropped from view, new ones were generated at random positions in the selected area of th e focal plane to try to maintain 10 points in view. At some points in the runs, the number dipped below 10

PAGE 91

79 during the maneuvering profiles, part icularly with the restricted fields of view. Four sets of 50 Monte Carlo runs were done on each of the trajectories for a to tal of 12 sets. The results are presented in previous fashion in Table 5-8 and Figures 5-24, 5-25, and 5-26. The results are mixed. For roll rate, p performance is worse on all trajectories for feature points restricted to clos e to the FOE. Roll angle, is also worse for this case. This is what was predicted in the earlier discussion. For pitch rate, q the poorest performance was expected for the vertical st rip case. This was true on the horizontal S trajectory but not on the other two trajectories. For yaw rate, r performance actually seemed better on two of three trajectories fo r the horizontal strip case which should have been the worst. Filter performance for the ot her state variables did not seem to show any discernable trends with regard s to feature point location. One possible problem in this assessment is that the FOE moves around on the focal plane for the maneuvering trajectories which could not be taken into account. 5.3 Effect of Measurement Noise The effect of measurement noise, that is the error in the feature point locations on the focal plane provided to the filter, was i nvestigated by varying th e standard deviation of the random noise that was added to the co mputed feature point locations in the Monte Carlo simulations. For all the previous runs in this chapter, the st andard deviation used has been the equivalent of one pixel. For th ese runs, standard devi ations equivalent to .25, .5, 1, and 1.5 pixels were used. Otherwis e the baseline parameters were used, i.e. the full focal plane at 60 degrees field of view with a measurement interval of .05 seconds, c of 60 degrees, and approximately 10 feature points. The value of measurement noise used by the filter for computing the Kalman gain s was left at the equivalent of one pixel

PAGE 92

80 for all runs. This simulates the case where the filter assumes a value for measurement noise but does not really “know” what the no ise is. Four sets of 50 Monte Carlo runs were made for the four values of measurement noise on each of the three trajectories for a total of 12 sets. The results are presented in previous fashion in Table 5-9 and Figures 5-27, 5-28, and 5-29. The results are generally as expected. The filter performance gets progressively worse as the noise level is in creased. This is true for p q r v, and on all three trajectories. The states u w and do not seem to be as affected by the noise, perhaps because these states are not as observable and th e noise tends to be filtered out. This is similar is to the results noted for the sensitivit y to number of feature points. None of the estimates diverged during any of the 600 runs. 5.4 Overall Comments on Robustness and Sensitivity The filter design seems to be robust and very tolerant of errors in the UAV process model. Monte Carlo results indicated that th e filter was tolerant of modeling errors due to misinitialization, measurement noise, incorrect trimmed condition and controls, aerodynamic parameters, and mass properties. No filter divergences were encountered during the 1050 runs. The filter seemed most sensitive to errors in the trimmed states and controls. The effects of variation of several pa rameters in filter and focal plane implementation on filter performance were assessed. Parameters examined were camera depression angle, field of view, measurement interval, and number and location of feature points. The Monte Carlo results indicated less than expected sensitivity to these factors. Number of feature points (the more, the bett er) seemed to have the most consistent

PAGE 93

81 results. Also, u w and seemed to be least affected during all these assessments. Again, no filter divergences were noted. Increasing the measurement noise level had the expected detrimental effect on filter performance. Overall, the filter showed a great degree of robustness to errors and inaccuracies in the system model and variations in filter implementation. However the effects of the various changes in filter design parameters we re difficult to differentiate, at least by the Monte Carlo methods and metrics used in this ch apter. This could be in part due to the stable nature of the particul ar UAV model for which the filter was implemented. Another factor could be that the large amount of noi se that feeds through to the angular rate estimates masks some of the trends that might be present.

PAGE 94

82 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Error 0 2 4 6 8 10 -5 0 5 v (ft/s) 0 2 4 6 8 10 -2 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.5 0 0.5 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 q (rad/s) 0 2 4 6 8 10 -0.5 0 0.5 r (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.2 0 0.2 Euler Angle Errorsphi (rad)Sec Figure 5-1. Six run Monte Carlo set errors case 1, straight a nd level trajectory. 0 2 4 6 8 10 -10 0 10 20 Velocity Errors, Body Axesu (ft/s) Error One Sigma Bounds 0 2 4 6 8 10 -2 0 2 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 5-2. Case 1 straight and level trajectory, ensemble mean error and bounds.

PAGE 95

83 0 2 4 6 8 10 -10 0 10 20 Velocity Errors, Body Axesu (ft/s) Error One Sigma Bounds 0 2 4 6 8 10 -2 0 2 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 5-3. Case 1 ver tical S trajectory, ensemble mean error and bounds. 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Error One Sigma Bounds 0 2 4 6 8 10 -5 0 5 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.2 0 0.2 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.05 0 0.05 q (rad/s) 0 2 4 6 8 10 -0.2 0 0.2 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 5-4. Case 1 horizontal S trajec tory, ensemble mean error and bounds.

PAGE 96

84 Table 5-1. Straight and level trajectory Monte Carlo sets. Average of mean error Case u v w p q r ft/s ft/s ft/s rad/s rad/s rad/s rad rad 1 -0.04335 -0.0195 0.018962 -0.00013 0.000682 -3.37E-05 -0.00011 -0.00063 2 -1.6186 -0.01527 0.025485 -0.00013 0.000238 -0.00016 0.018332 -0.00349 3 -0.80384 -0.29529 0.057816 -0.00033 0.000701 -0.00011 0.000472 -0.00755 4 -0.95018 0.031218 0.068606 -0.00029 0.000453 -0.00034 0.005701 -0.00044 5 -0.58888 -0.03818 0.046013 -0.00019 0.000598 -0.00017 0.002837 -0.00074 6 0.49949 -0.02877 -0.00398 -0.00014 0.000816 -0.00012 -0.00786 -0.00115 7 -2.7942 0.076492 0.1051 -6.10E-05 0.001071 -0.00027 0.005477 0.004862 Average of standard deviation of error 1 3.7724 0.47367 0.19498 0.11372 0.024872 0.13119 0.039212 0.008291 2 6.1804 1.1113 0.9574 0.11387 0.024342 0.13156 0.045087 0.02205 3 4.1802 2.2906 0.22039 0.1134 0.024514 0.13065 0.050022 0.059248 4 4.3554 0.5206 0.23638 0.11404 0.024892 0.13389 0.045329 0.008675 5 4.1598 0.47511 0.22646 0.11294 0.024465 0.13041 0.046088 0.007784 6 5.105 0.50105 0.2609 0.11551 0.024682 0.13335 0.051796 0.008558 7 8.265 2.7318 0.95319 0.1127 0.024138 0.13058 0.063676 0.066345 Table 5-2. Vertical S trajectory Monte Carlo sets. Average of mean error Case u v w p q r ft/s ft/s ft/s rad/s rad/s rad/s rad rad 1 1.421 0.0381 -0.14139 0.000192 0.000838 7.55E-05 -0.01822 0.001268 2 2.5879 0.14814 -0.31278 0.000468 0.00135 0.000618 -0.02446 -0.0005 3 1.516 -0.09106 -0.15493 8.04E-05 0.000649 0.000357 -0.02015 -0.0023 4 1.1249 -0.01991 -0.13966 -0.00076 0.000421 -0.00057 -0.01526 -0.0006 5 1.105 -0.01391 -0.12363 3.25E-05 0.000372 0.000244 -0.01609 -0.00065 6 1.4917 -0.01935 -0.13931 -8.75E-05 0.001124 -0.00011 -0.023 -0.00053 7 3.1734 0.17221 -0.37537 0.000698 0.0006 0.000596 -0.0265 0.007965 Average of standard deviation of error 1 4.2084 0.45093 0.27757 0.11863 0.026223 0.11071 0.043465 0.010757 2 7.0845 1.1267 1.184 0.11797 0.02647 0.10963 0.054424 0.019862 3 4.8097 2.7291 0.32119 0.11952 0.026548 0.1116 0.051041 0.070892 4 3.9335 0.43276 0.2939 0.12023 0.025823 0.11124 0.042417 0.00977 5 4.4422 0.43519 0.28523 0.11898 0.026631 0.11121 0.045047 0.010223 6 4.8925 0.44398 0.30731 0.12032 0.026206 0.11341 0.050393 0.009408 7 7.6246 2.8276 1.0144 0.1212 0.026668 0.1124 0.058297 0.065056

PAGE 97

85 Table 5-3. Horizontal S tra jectory Monte Carlo sets. Average of mean error Case u v w p q r ft/s ft/s ft/s rad/s rad/s rad/s rad rad 1 3.47 0.21437 -0.28201 -0.00011 -0.00212 6.33E-05 -0.03259 0.00711 2 3.008 0.27835 -0.4086 -0.00035 -0.00239 -0.00038 -0.0358 0.005282 3 2.8316 0.026012 -0.26288 -0.00127 -0.00248 -0.00076 -0.02785 -0.00181 4 3.1572 0.18199 -0.22469 -0.00082 -0.00234 -0.00073 -0.02843 0.003626 5 2.4691 0.29688 -0.24576 -0.00094 -0.0026 -0.00078 -0.02429 0.006344 6 2.4493 0.20664 -0.24868 -0.00027 -0.00321 0.000338 -0.02937 0.006169 7 2.3303 -0.50543 -0.15205 -0.00158 -0.00243 0.000192 -0.02967 -0.00253 Average of standard deviation of error 1 5.2436 1.1673 0.34853 0.13531 0.026771 0.11883 0.050474 0.032863 2 7.3113 1.6049 1.2454 0.13187 0.026863 0.11615 0.05478 0.038959 3 4.6546 2.3222 0.43665 0.1328 0.026843 0.11618 0.049777 0.066782 4 5.1832 1.2399 0.33208 0.12947 0.026154 0.11283 0.046634 0.033341 5 4.2636 1.2429 0.31771 0.13265 0.026852 0.11604 0.04356 0.034434 6 6.0214 1.3711 0.37397 0.13341 0.027145 0.11943 0.059269 0.034022 7 8.4666 2.4958 1.0198 0.13085 0.026609 0.11552 0.058652 0.067813 1 2 3 4 5 6 7 0 5 10 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 1 2 3 4 5 6 7 0 1 2 3 v (ft/s) 1 2 3 4 5 6 7 0 0.5 1 w (ft/s) 1 2 3 4 5 6 7 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Case 1 2 3 4 5 6 7 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 1 2 3 4 5 6 7 0 0.01 0.02 0.03 q (rad/s) 1 2 3 4 5 6 7 0 0.1 0.2 r (rad/s) 1 2 3 4 5 6 7 0 0.05 0.1 Euler Angle Error Statisticsphi (rad)Case Figure 5-5. Straight and level trajectory, case statistics.

PAGE 98

86 1 2 3 4 5 6 7 0 5 10 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 1 2 3 4 5 6 7 0 1 2 3 v (ft/s) 1 2 3 4 5 6 7 0 0.5 1 1.5 w (ft/s) 1 2 3 4 5 6 7 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Case 1 2 3 4 5 6 7 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 1 2 3 4 5 6 7 0 0.01 0.02 0.03 q (rad/s) 1 2 3 4 5 6 7 0 0.1 0.2 r (rad/s) 1 2 3 4 5 6 7 0 0.05 0.1 Euler Angle Error Statisticsphi (rad)Case Figure 5-6. Vertical S trajectory, case statistics. 1 2 3 4 5 6 7 0 5 10 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 1 2 3 4 5 6 7 0 1 2 3 v (ft/s) 1 2 3 4 5 6 7 0 0.5 1 1.5 w (ft/s) 1 2 3 4 5 6 7 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Case 1 2 3 4 5 6 7 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 1 2 3 4 5 6 7 0 0.01 0.02 0.03 q (rad/s) 1 2 3 4 5 6 7 0 0.1 0.2 r (rad/s) 1 2 3 4 5 6 7 0 0.05 0.1 Euler Angle Error Statisticsphi (rad)Case Figure 5-7. Horizontal S tra jectory, case statistics.

PAGE 99

87 Figure 5-8. Focal plane hist ory for straight and level trajectory with camera looking down, intermediate baseline position, and level. Table 5-4. Camera angle Monte Carlo sets. c u v w p q r deg ft/s ft/s ft/s rad/s rad/s rad/s rad rad Average of mean error Straight and Level Trajectory 0 -0.05409 0.007032 0.021851 9.75E-06 -0.0004 5 -0.00019 -0.00591 -3.68E-05 60 -0.04335 -0.0195 0.018962 -0.00013 0.000682 -3.37E-05 -0.00011 -0.00063 82.8 -0.30927 0.014068 0.025078 -0.0002 0.000201 1.81E-05 0.002525 0.000485 Vertical S Trajectory 0 1.4645 0.035481 -0.13992 0.000203 -0.00084 -0.0008 -0.02906 -1.87E-06 60 1.421 0.0381 -0.14139 0.000192 0.000838 7.55E-05 -0.01822 0.001268 82.8 1.9163 -0.04465 -0.18002 8.13E-05 -1.68E -05 0.000126 -0.02353 -0.00089 Horizontal S Trajectory 0 3.5958 0.12057 -0.35776 -0.0005 -0.00871 -0.00041 -0.05065 0.003908 60 3.47 0.21437 -0.28201 -0.00011 -0.00212 6.33E-05 -0.03259 0.00711 82.8 3.6006 0.24286 -0.29331 -0.00075 -0.00241 -0.00057 -0.03297 0.005555 Average of standard deviation of error Straight and Level Trajectory 0 4.0577 0.74843 0.087288 0.03742 0.017928 0.16092 0.034328 0.012611 60 3.7724 0.47367 0.19498 0.11372 0.024872 0.13119 0.039212 0.008291 82.8 4.477 0.36341 0.23067 0.083895 0.023681 0.066755 0.048836 0.006345 Vertical S Trajectory 0 4.1344 0.74597 0.21869 0.035919 0.018287 0.15869 0.043088 0.013405 60 4.2084 0.45093 0.27757 0.11863 0.026223 0.11071 0.043465 0.010757 82.8 4.8849 0.36355 0.32594 0.087702 0.024275 0.062332 0.05171 0.007588 Horizontal S Trajectory 0 5.5306 1.3797 0.42478 0.042608 0.02244 0.17705 0.063435 0.02951 60 5.2436 1.1673 0.34853 0.13531 0.026771 0.11883 0.050474 0.032863 82.8 5.5214 1.1121 0.35725 0.11398 0.025551 0.062243 0.050303 0.02991

PAGE 100

88 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 0.5 1 1.5 v (ft/s) 0 0.2 0.4 w (ft/s) 0 60 82.8 0 0.02 0.04 0.06 Euler Angle Error Statisticstheta (rad)theta c (deg) 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.01 0.02 0.03 q (rad/s) 0 0.1 0.2 r (rad/s) 0 60 82.8 0 0.02 0.04 Euler Angle Error Statisticsphi (rad)theta c (deg) Figure 5-9. Straight and level tra jectory, camera angle statistics. 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 0.5 1 1.5 v (ft/s) 0 0.2 0.4 w (ft/s) 0 60 82.8 0 0.02 0.04 0.06 Euler Angle Error Statisticstheta (rad)theta c (deg) 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.01 0.02 0.03 q (rad/s) 0 0.1 0.2 r (rad/s) 0 60 82.8 0 0.02 0.04 Euler Angle Error Statisticsphi (rad)theta c (deg) Figure 5-10. Vertical S trajectory, camera angle statistics.

PAGE 101

89 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 0.5 1 1.5 v (ft/s) 0 0.2 0.4 w (ft/s) 0 60 82.8 0 0.02 0.04 0.06 Euler Angle Error Statisticstheta (rad)theta c (deg) 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 3 0 0.01 0.02 0.03 q (rad/s) 0 0.1 0.2 r (rad/s) 0 60 82.8 0 0.02 0.04 Euler Angle Error Statisticsphi (rad)theta c (deg) Figure 5-11. Horizontal S trajectory, camera angle statistics.

PAGE 102

90 Table 5-5. Camera field of view Monte Carlo sets. FOV u v w p q r deg ft/s ft/s ft/s rad/s rad/s rad/s rad rad Average of mean error Straight and Level Trajectory 30 -0.31156 -0.0279 0.030548 9.32E-04 0.000526 0.00069 0.004317 -3.69E-05 60 -0.04335 -0.0195 0.018962 -0.00013 0.000682 -3.37E-05 -0.00011 -0.00063 90 -0.49075 0.04095 0.040698 -0.00023 0.000489 -1.79E-04 0.00516 0.000907 Vertical S Trajectory 30 1.0152 0.014569 -0.12665 -0.00023 8.79E-05 -0.00019 -0.01303 1.28E-03 60 1.421 0.0381 -0.14139 0.000192 0.000838 7.55E-05 -0.01822 0.001268 90 0.19782 0.016915 -0.07034 3.90E-04 -6.23E-06 0.000406 -0.00972 0.001038 Horizontal S Trajectory 30 3.6404 0.15804 -0.29471 -0.00094 -0.005 -0.0004 -0.03771 0.004198 60 3.47 0.21437 -0.28201 -0.00011 -0.00212 6.33E-05 -0.03259 0.00711 90 2.0183 0.23992 -0.23619 0.000888 -0.00309 0.000442 -0.02196 0.006824 Average of standard deviation of error Straight and Level Trajectory 30 4.1761 0.38241 0.18471 0.10272 0.012675 0.074763 0.03999 0.014126 60 3.7724 0.47367 0.19498 0.11372 0.024872 0.13119 0.039212 0.008291 90 4.2079 0.52844 0.23354 0.0976 0.040227 0.1354 0.04389 0.007823 Vertical S Trajectory 30 4.5502 0.44637 0.26825 0.10974 0.013229 0.076213 0.046607 0.015009 60 4.2084 0.45093 0.27757 0.11863 0.026223 0.11071 0.043465 0.010757 90 4.0655 0.50502 0.27554 0.11046 0.044138 0.12852 0.043138 0.008866 Horizontal S Trajectory 30 4.7846 1.2683 0.34994 0.11676 0.015528 0.08146 0.049422 0.043436 60 5.2436 1.1673 0.34853 0.13531 0.026771 0.11883 0.050474 0.032863 90 4.7543 1.1743 0.33938 0.15517 0.045513 0.16016 0.046234 0.029832

PAGE 103

91 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 1 2 v (ft/s) 0 0.2 0.4 w (ft/s) 30 60 90 0 0.02 0.04 0.06 Euler Angle Error Statisticstheta (rad)FOV (deg) 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 0.06 q (rad/s) 0 0.1 0.2 r (rad/s) 30 60 90 0 0.02 0.04 Euler Angle Error Statisticsphi (rad)FOV (deg) Figure 5-12. Straight and level trajectory, camera FOV statistics. 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 1 2 v (ft/s) 0 0.2 0.4 w (ft/s) 30 60 90 0 0.02 0.04 0.06 Euler Angle Error Statisticstheta (rad)FOV (deg) 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 0.06 q (rad/s) 0 0.1 0.2 r (rad/s) 30 60 90 0 0.02 0.04 Euler Angle Error Statisticsphi (rad)FOV (deg) Figure 5-13. Vertical S trajectory, camera FOV statistics.

PAGE 104

92 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 0.5 1 1.5 v (ft/s) 0 0.2 0.4 w (ft/s) 30 60 90 0 0.02 0.04 0.06 Euler Angle Error Statisticstheta (rad)FOV (deg) 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 0.06 q (rad/s) 0 0.1 0.2 r (rad/s) 30 60 90 0 0.02 0.04 0.06 Euler Angle Error Statisticsphi (rad)FOV (deg) Figure 5-14. Horizontal S trajectory, camera FOV statistics. 0 2 4 6 8 10 -10 0 10 Velocity Errors, Body Axesu (ft/s) Mean Error One Sigma Bounds 0 2 4 6 8 10 -2 0 2 v (ft/s) 0 2 4 6 8 10 -1 0 1 w (ft/s) 0 2 4 6 8 10 -0.1 0 0.1 Angular Rate Errors, Body Axesp (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 q (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 r (rad/s) 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorstheta (rad)Sec 0 2 4 6 8 10 -0.1 0 0.1 Euler Angle Errorsphi (rad)Sec Figure 5-15. History of statistics from .5 second measurement interval ensemble, straight and level trajectory.

PAGE 105

93 Table 5-6. Measurement in terval Monte Carlo sets. Meas. Int. u v w p q r sec ft/s ft/s ft/s rad/s rad/s rad/s rad rad Average of mean error Straight and Level Trajectory .05 -0.04335 -0.0195 0.018962 -1.30E-04 0.000682 -3.37E-05 -0.00011 -6.32E-04 .25 0.36708 0.014268 -0.00394 -0.00052 0.000456 -6.17E-05 -0.00427 0.000534 .5 0.35028 -0.02364 -0.00378 0.000899 0.000611 -5.24E-04 -0.00184 -0.00287 Vertical S Trajectory .05 1.421 0.0381 -0.14139 0.000192 8.38E-04 7.55E-05 -0.01822 1.27E-03 .25 1.6899 -0.02697 -0.15935 0.001091 0.000159 3.05E-04 -0.02185 -0.00037 .5 0.3658 0.013302 -0.17809 -6.79E-04 1.62E-03 0.000633 -0.02886 0.001314 Horizontal S Trajectory .05 3.47 0.21437 -0.28201 -0.00011 -0.00212 6.33E-05 -0.03259 0.00711 .25 5.253 -0.11559 -0.2295 -0.00213 -0.00426 -2.80E-04 -0.05496 -0.0089 9.0158 -0.31269 -0.62523 -0.00082 -0.01036 0.001441 -0.12214 -0.04675 Average of standard deviation of error Straight and Level Trajectory .05 3.7724 0.47367 0.19498 0.11372 0.024872 0.13119 0.039212 0.008291 .25 4.5599 0.4527 0.21097 0.028428 0.008933 0.026972 0.046274 0.011827 .5 4.1267 0.40317 0.14975 0.024123 0.008953 0.01697 0.039022 0.016677 Vertical S Trajectory .05 4.2084 0.45093 0.27757 0.11863 0.026223 0.11071 0.043465 0.010757 .25 4.3849 0.44102 0.29411 0.030754 0.014504 0.024704 0.04678 0.012733 .5 4.3005 0.40752 0.26463 0.02554 0.013562 0.015265 0.044135 0.018106 Horizontal S Trajectory .05 5.2436 1.1673 0.34853 0.13531 0.026771 0.11883 0.050474 0.032863 .25 6.2988 1.8361 0.40453 0.14157 0.020585 0.086663 0.062197 0.047443 .5 10.085 3.7906 0.75333 0.28766 0.03162 0.1937 0.13389 0.12544

PAGE 106

94 0 5 10 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 2 4 v (ft/s) 0 0.5 1 w (ft/s) .05 .25 .5 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Meas. Int. (sec) 0 0.2 0.4 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 q (rad/s) 0 0.1 0.2 r (rad/s) .05 .25 .5 0 0.05 0.1 Euler Angle Error Statisticsphi (rad)Meas. Int. (sec) Figure 5-16. Straight and level trajectory, measurement interval statistics. 0 5 10 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 2 4 v (ft/s) 0 0.5 1 w (ft/s) .05 .25 .5 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Meas. Int. (sec) 0 0.2 0.4 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 q (rad/s) 0 0.1 0.2 r (rad/s) .05 .25 .5 0 0.05 0.1 Euler Angle Error Statisticsphi (rad)Meas. Int. (sec) Figure 5-17. Vertical S trajectory, measurement interval statistics.

PAGE 107

95 0 5 10 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 2 4 v (ft/s) 0 0.5 1 w (ft/s) .05 .25 .5 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Meas. Int. (sec) 0 0.2 0.4 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 q (rad/s) 0 0.1 0.2 r (rad/s) .05 .25 .5 0 0.05 0.1 Euler Angle Error Statisticsphi (rad)Meas. Int. (sec) Figure 5-18. Horizontal S trajectory, measurement interval statistics.

PAGE 108

96 Table 5-7. Number of feat ure points Monte Carlo sets. Points u v w p q r ft/s ft/s ft/s rad/s rad/s rad/s rad rad Average of mean error Straight and Level Trajectory 5 -1.1036 0.045415 0.07658 2.69E-03 0.000684 1.66E-03 0.011307 3.65E-03 10 1.0585 0.026367 -0.02681 -0.00089 0.00143 -9.44E-04 -0.01353 -6.32E-05 20 -0.43518 -0.04356 0.030619 -0.00023 0.000327 -2.48E-04 0.005283 -0.00024 40 0.33326 -0.04722 -0.0068 2.73E-05 0.000273 8.76E-05 -0.00477 -0.00012 Vertical S Trajectory 5 1.3543 -0.02903 -0.1056 -0.00304 2.44E-03 -3.49E-03 -0.02238 -7.12E-03 10 1.0349 -0.009 -0.12156 -0.00046 0.000709 -6.39E-04 -0.01388 -0.00166 20 1.39 -0.02878 -0.14869 -1.98E-04 1.99E-04 6.47E-05 -0.01779 -0.00103 40 1.0232 0.015037 -0.13109 -1.16E-04 1.29E-04 -0.00015 -0.01314 0.000574 Horizontal S Trajectory 5 2.7938 0.15172 -0.22433 -0.00056 -0.00317 -6.15E-04 -0.03397 0.000312 10 2.4306 0.23446 -0.24816 -0.00151 -0.00308 -1.36E-03 -0.0256 0.003865 20 3.3663 0.23351 -0.28082 -0.00083 -0.00194 -0.00048 -0.03314 0.004657 40 3.2593 0.24159 -0.27839 0.000278 -0.00134 0.000426 -0.02987 0.006363 Average of standard deviation of error Straight and Level Trajectory 5 4.0595 0.58544 0.21928 0.16663 0.025959 0.14822 0.040753 0.017429 10 4.5136 0.53012 0.21942 0.12853 0.025647 0.13683 0.047761 0.0114 20 3.9757 0.3999 0.20985 0.080272 0.021654 0.088534 0.043492 0.007079 40 4.4665 0.29376 0.23053 0.052272 0.019514 0.055599 0.048001 0.00428 Vertical S Trajectory 5 4.814 0.87363 0.28925 0.18223 0.026391 0.19033 0.048561 0.031095 10 4.7361 0.53589 0.29367 0.13557 0.026794 0.14397 0.049243 0.013376 20 4.3564 0.41424 0.2892 0.087385 0.023531 0.092877 0.04866 0.008692 40 4.555 0.33539 0.29938 0.061939 0.020659 0.064797 0.048862 0.006027 Horizontal S Trajectory 5 4.6304 1.4858 0.33839 0.18219 0.026487 0.17091 0.053294 0.050077 10 4.3139 1.1829 0.32739 0.13134 0.027269 0.12421 0.041594 0.036095 20 4.9068 1.1093 0.3421 0.099008 0.025747 0.093693 0.048119 0.02999 40 5.2347 1.0826 0.33997 0.073283 0.022785 0.066902 0.050466 0.028762

PAGE 109

97 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 0.5 1 1.5 v (ft/s) 0 0.2 0.4 w (ft/s) 5 10 20 40 0 0.02 0.04 0.06 Euler Angle Error Statisticstheta (rad)Number Feature Points 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.01 0.02 0.03 q (rad/s) 0 0.1 0.2 r (rad/s) 5 10 20 40 0 0.02 0.04 0.06 Euler Angle Error Statisticsphi (rad)Number Feature Points Figure 5-19. Straight and level trajector y, number of feature points statistics. 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 0.5 1 1.5 v (ft/s) 0 0.2 0.4 w (ft/s) 5 10 20 40 0 0.02 0.04 0.06 Euler Angle Error Statisticstheta (rad)Number Feature Points 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.01 0.02 0.03 q (rad/s) 0 0.1 0.2 r (rad/s) 5 10 20 40 0 0.02 0.04 0.06 Euler Angle Error Statisticsphi (rad)Number Feature Points Figure 5-20. Vertical S trajectory, number of feature points statistics.

PAGE 110

98 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 0.5 1 1.5 v (ft/s) 0 0.2 0.4 w (ft/s) 5 10 20 40 0 0.02 0.04 0.06 Euler Angle Error Statisticstheta (rad)Number Feature Points 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.01 0.02 0.03 q (rad/s) 0 0.1 0.2 r (rad/s) 5 10 20 40 0 0.02 0.04 0.06 Euler Angle Error Statisticsphi (rad)Number Feature Points Figure 5-21. Horizontal S trajectory, number of feature points statistics.

PAGE 111

99 Figure 5-22. Focal plane history for straight and level trajectory with area constraints. -0.5 0 0.5 -0.4 -0.2 0 0.2 0.4 Feature Point 7 Movement on Focal Plane nu mu 1 2 3 4 5 0 1 2 3 4 5 6 7 8 9 10 -0.1 -0.05 0 0.05 0.1 H matrix row for fea pt7 Sec h 1 2345 p q r

PAGE 112

100 Figure 5-23. Focal plane history for straight and level trajectory with area constraints.

PAGE 113

101 Table 5-8. Feature point location Monte Carlo sets. Location u v w p q r ft/s ft/s ft/s rad/s rad/s rad/s rad rad Average of mean error Straight and Level Trajectory Full 1.0585 0.026367 -0.02681 -8.86E-04 0.00143 -9.44E-04 -0.01353 -6.32E-05 Vert -0.03771 0.047367 0.016575 -3.32E-05 0.000462 -2.19E-04 -0.00135 1.16E-03 Horz 0.1804 0.017794 -0.00091 -0.00019 0.000215 -1.22E-04 -0.00122 -0.00051 FOE 0.49852 0.019019 -0.00106 -2.76E-04 0.00105 -8.47E-05 -0.0093 6.81E-05 Vertical S Trajectory Full 1.0349 -0.009 -0.12156 -0.00046 7.09E-04 -6.39E-04 -0.01388 -1.66E-03 Vert 0.53179 -0.01403 -0.08728 0.000646 -0.0005 1 5.64E-04 -0.01898 0.000216 Horz 0.25376 0.008169 -0.08339 -4.38E-04 3.13E-04 1.70E-04 -0.00667 0.001468 FOE 1.6902 0.05147 -0.13353 2.19E-03 1.53E-03 0.000535 -0.02569 0.004838 Horizontal S Trajectory Full 2.4306 0.23446 -0.24816 -0.00151 -0.00308 -1.36E-03 -0.0256 0.003865 Vert 2.3616 0.053697 -0.23995 0.000705 -0.0067 7 1.90E-03 -0.01957 0.001281 Horz 3.2013 0.22619 -0.27286 -0.00109 -0.0029 -0.00124 -0.02949 0.00296 FOE 1.9702 -0.03905 -0.1694 -0.00356 -0.00241 -0.0012 -0.02192 -0.00716 Average of standard deviation of error Straight and Level Trajectory Full 4.5136 0.53012 0.21942 0.12853 0.025647 0.13683 0.047761 0.0114 Vert 4.312 0.45849 0.2086 0.13144 0.025284 0.095002 0.044759 0.012867 Horz 4.9751 0.34897 0.25527 0.09772 0.022295 0.078043 0.053158 0.006755 FOE 4.1542 0.4012 0.20855 0.25745 0.02486 0.10337 0.043967 0.016756 Vertical S Trajectory Full 4.7361 0.53589 0.29367 0.13557 0.026794 0.14397 0.049243 0.013376 Vert 3.6289 0.48195 0.22946 0.14963 0.022567 0.10445 0.038136 0.016458 Horz 4.3796 0.67786 0.29283 0.10658 0.023551 0.11498 0.046793 0.020515 FOE 5.3293 0.89113 0.32633 0.27859 0.025398 0.11317 0.057896 0.037785 Horizontal S Trajectory Full 4.3139 1.1829 0.32739 0.13134 0.027269 0.12421 0.041594 0.036095 Vert 4.9553 1.7465 0.4331 0.18602 0.031474 0.12109 0.055234 0.069633 Horz 4.8676 1.339 0.35108 0.14936 0.026237 0.098039 0.047684 0.043053 FOE 4.3073 1.8108 0.36474 0.30508 0.027774 0.12051 0.050017 0.077889

PAGE 114

102 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 1 2 v (ft/s) 0 0.2 0.4 w (ft/s) Full Vert Horz FOE 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Fea. Pt. Location 0 0.2 0.4 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 q (rad/s) 0 0.1 0.2 r (rad/s) Full Vert Horz FOE 0 0.05 0.1 Euler Angle Error Statisticsphi (rad)Fea. Pt. Location Figure 5-24. Straight and level traject ory, feature point location statistics. 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 1 2 v (ft/s) 0 0.2 0.4 w (ft/s) Full Vert Horz FOE 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Fea. Pt. Location 0 0.2 0.4 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 q (rad/s) 0 0.1 0.2 r (rad/s) Full Vert Horz FOE 0 0.05 0.1 Euler Angle Error Statisticsphi (rad)Fea. Pt. Location Figure 5-25. Vertical S trajectory, feature point location statistics.

PAGE 115

103 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 1 2 v (ft/s) 0 0.2 0.4 w (ft/s) Full Vert Horz FOE 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Fea. Pt. Location 0 0.2 0.4 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 q (rad/s) 0 0.1 0.2 r (rad/s) Full Vert Horz FOE 0 0.05 0.1 Euler Angle Error Statisticsphi (rad)Fea. Pt. Location Figure 5-26. Horizontal S trajectory, feature point location statistics.

PAGE 116

104 Table 5-9. Measurement noise Monte Carlo sets. Noise u v w p q r pixels ft/s ft/s ft/s rad/s rad/s rad/s rad rad Average of mean error Straight and Level Trajectory .25 0.52093 0.091307 -0.02099 8.03E-05 0.000216 -4.58E-05 -0.00477 8.77E-04 .5 -0.15087 0.042528 0.017164 1.37E-04 0.000163 5.46E-05 0.000619 2.47E-04 1 -0.04335 -0.0195 0.018962 -0.00013 0.000682 -3.37E-05 -0.00011 -0.00063 1.5 -0.56687 0.004622 0.05875 2.61E-05 0.001406 -6.68E-05 0.005823 -8.31E-04 Vertical S Trajectory .25 1.6473 -0.02979 -0.16208 0.000168 -1.87E-05 1.29E-04 -0.02194 -9.96E-05 .5 1.4927 -0.08372 -0.1571 0.000307 0.000224 4.27E-04 -0.01741 -0.00072 1 1.421 0.0381 -0.14139 1.92E-04 8.38E-04 7.55E-05 -0.01822 0.001268 1.5 2.4117 0.03425 -0.17723 1.66E-04 2.70E-03 0.000413 -0.02894 0.002549 Horizontal S Trajectory .25 3.4132 0.24907 -0.28643 -0.00101 -0.00284 -5.74E-04 -0.03266 0.005787 .5 3.1922 0.24805 -0.27552 -0.00095 -0.00288 -6.56E-04 -0.0336 0.00547 1 3.47 0.21437 -0.28201 -0.00011 -0.0021 2 6.33E-05 -0.03259 0.00711 1.5 2.3481 0.2218 -0.22164 -0.00276 -0.00243 -0.00219 -0.02548 -0.00022 Average of standard deviation of error Straight and Level Trajectory .25 3.9337 0.30588 0.19416 0.028733 0.006186 0.033442 0.041389 0.003897 .5 4.5037 0.34115 0.22649 0.057106 0.012325 0.066288 0.047206 0.00521 1 3.7724 0.47367 0.19498 0.11372 0.024872 0.13119 0.039212 0.008291 1.5 3.9188 0.65849 0.22124 0.17293 0.037168 0.20073 0.040811 0.011996 Vertical S Trajectory .25 4.6498 0.23576 0.2893 0.030485 0.007606 0.028373 0.051528 0.004784 .5 4.0046 0.32638 0.26408 0.060987 0.013699 0.056084 0.042646 0.006541 1 4.2084 0.45093 0.27757 0.11863 0.026223 0.11071 0.043465 0.010757 1.5 4.6091 0.6061 0.30294 0.17924 0.038409 0.16856 0.047575 0.01465 Horizontal S Trajectory .25 4.9726 1.0192 0.32635 0.038278 0.00814 0.029343 0.046361 0.027301 .5 4.7674 1.0538 0.3222 0.068812 0.013988 0.058153 0.049836 0.028611 1 5.2436 1.1673 0.34853 0.13531 0.026771 0.11883 0.050474 0.032863 1.5 4.803 1.3838 0.33529 0.20124 0.040022 0.17981 0.048395 0.041788

PAGE 117

105 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 1 2 v (ft/s) 0 0.2 0.4 w (ft/s) .25 .5 1 1.5 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Meas. Noise (pixels) 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 q (rad/s) 0 0.1 0.2 r (rad/s) .25 .5 1 1.5 0 0.02 0.04 Euler Angle Error Statisticsphi (rad) Meas. Noise (pixels) Figure 5-27. Straight and level trajectory, measurement noise statistics. 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 1 2 v (ft/s) 0 0.2 0.4 w (ft/s) .25 .5 1 1.5 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Meas. Noise (pixels) 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 q (rad/s) 0 0.1 0.2 r (rad/s) .25 .5 1 1.5 0 0.02 0.04 Euler Angle Error Statisticsphi (rad) Meas. Noise (pixels) Figure 5-28. Vertical S trajectory, measurement noise statistics.

PAGE 118

106 0 2 4 6 Velocity Error Statisticsu (ft/s) Mean (abs value) Std Dev 0 1 2 v (ft/s) 0 0.2 0.4 w (ft/s) .25 .5 1 1.5 0 0.05 0.1 Euler Angle Error Statisticstheta (rad)Meas. Noise (pixels) 0 0.1 0.2 Angular Rate Error Statisticsp (rad/s) 0 0.02 0.04 q (rad/s) 0 0.1 0.2 r (rad/s) .25 .5 1 1.5 0 0.02 0.04 Euler Angle Error Statisticsphi (rad) Meas. Noise (pixels) Figure 5-29. Horizontal S trajector y, measurement noise statistics.

PAGE 119

107 CHAPTER 6 IEKF CLOSED LOOP PERFORMANCE In this chapter the modified IEKF st ate estimator described previously is implemented in a closed loop simulation to provide state estimates for use in a UAV control system. The estimator, guidance, and control loop are closed around the same truth model that was used to generate the trajectories used previously in this effort and was described in reference [23] and appendix A. A rudimentary autopilot was developed and used to drive the UAV controls ba sed on commands from a waypoint guidance module and feedback from the IEKF state es timator. The ability of the system to successfully fly a course using the IEKF state estimates as feedback to the autopilot is demonstrated using computer generated random feature points corrupted with noise as in the previous chapters. A second simulation is done at a different flight condition using synthetic imagery generated in the Universi ty of Florida Research and Engineering Education Facility MAV vision laboratory. 6.1 Simulation with Computer Generated Feature Points Figure 6-1 shows a block diagram of the closed loop simulation used in this chapter. A simple proportional waypoint guida nce scheme was developed to generate the commands for the autopilot. The guidance re quires position and tr ack angle information from some sort of navigator, for instance, a GPS receiver. A seeker based guidance source could have been used just as well to generate the guidance commands since the object of the simulation was to demonstrate the ability to control the UAV using state estimates from the IEKF and this occurs downstream of the guidance. The commands

PAGE 120

108 from the guidance are commanded roll angle, c, and commanded pitch, c. The angle c is generated in the guidance module to command the UAV to turn to track to the next waypoint. The angle c. is generated to command the UAV to climb or descend to the next waypoint altitude. At the end of the waypoint list, the guidance module loops back to the beginning. The focal plane visi on tracker module generates the focal plane measurements as was done in previous chapte rs except that outputs from the truth model are used instead of the preflown trajectories. The autopilot depiction from Simulink, the simulation tool used, is shown in Figure 6-2. A simple proportional scheme was also used here. Only two out of the eight estimated states were actually used in the autopilot, and The throttle and rudder were left at their trimmed values. The elev ator and ailerons were commanded away from their trimmed values in response to the gui dance commands and feedback from the state estimator. Not shown are control limiters th at limit the maximum elevator and aileron deflections to +/15 degrees. For this simulation, the UAV was trimmed at the same conditions as in chapter four, straight and level flight at 73.3 ft/s and 7500 ft, standard day. The filter and camera parameters were in the baseline configur ation of 60 degree horizontal field of view, c of 60 degrees, .05 second measurement interv al, and number of feature points at approximately ten. The feature point locati ons on the focal plane were corrupted with noise equivalent to one pixel standard de viation on a 320 by 240 pixel focal plane. The simulation was initialized at the trimmed c onditions with the UAV tracking north. Three waypoints were commanded in succession. The simulation was run for 100 seconds. The results are shown in Figures 6-3, 6-4, and 6-5.

PAGE 121

109 The estimated and true states are shown in Figure 6-3. As could be expected from the previous chapters, the angul ar rate estimates are noisy bu t their means track the true values very well. The estimates for roll angle, and the velocities v and w agree surprisingly well with the true values. Also, as might be expected, performance is not as good for longitudinal velocity, u and pitch angle, As Figures 6-4 and 6-5 indicate, the state estimates for and were good enough to keep the UAV under control and allow it to over fly the waypoints and climb, descend, and maintain altitude in response to the guidance. 6.2 Simulation With Synthetic Imagery A closed loop simulation using synthetic imagery from the University of Florida Research and Engineering Facility MAV vi sion laboratory was conducted. The vision lab contains a data base that generates scenery of the University of Florida campus in Gainesville. The lab also develops and main tains feature point track ing algorithms. The Lucas-Kanade algorithm [5, 22] mentioned in chapter one was used for this simulation. This algorithm detects and chooses “good” f eature points on the focal plane using image gradients in a small window of pixels. Each feature point is then tracked frame to frame by minimizing the intensity difference for its window between successive frames using image spatial and temporal gradients. This basic technique has proven to be efficient and work well in most situations. A representative focal plane image from this simulation is shown in Figure 6-6. The image resolution was intentionally degr aded to correspond to 320 by 240 pixels. The blue dots or crosses in the image indicate the current feature points. The simulation allowed up to 20 featur e points to be used at any given time.

PAGE 122

110 For this simulation it was decided to fly th e UAV at a slower airspeed. This was done to give the aircraft a smaller turning ra dius which would be more realistic in the urban environment afforded by the vision lab da tabase. It also provided an opportunity to assess the estimator suitability with a ve hicle with somewhat different flight characteristics. For this simulation the UAV was trimmed to fly at 50 ft/sec instead of 73.3 ft/sec and at sea level instead of 7500 feet elevation. This necessitated that the process model in the IEKF be adjusted. Reca ll from chapter four that the process model consists of a linearization of the truth model about the trimmed condition augmented with a few nonlinear terms, Equations 4-4 through 49. The adjustment was to replace the system and control ( A and B ) matrix values with new ones obtained by a linearization of the truth model about the new straight and level trimmed conditions. The new values are shown in Equations 6-1 and 6-2 which replace E quations 4-7 and 4-8. These values were obtained by using the numerical linearization and discretation tools in Matlab and the truth model for a step size of .05 seconds. It was not necessary to alter the nonlinear augmentation shown in Equation 4-9. .9878 0 .04343 0 -.4244 0 -1.576 0 0 .9598 0 .4447 0 -2.326 0 1.548 A -.02001 0 .8181 0 1.833 0 -.2586 0 0 -.01487 0 .684 0 .1019 0 -.01264 .001039 0 .0001977 0 .8527 0 .004357 0 0 .007277 0 -0.003025 0 .9823 0 .005773 2.673e-005 0 5.027e-006 0 .04622 0 1 0 0 -.0003638 0 .04162 0 .0126 0 .9998]; (6-1)

PAGE 123

111 00.002091.4108 .07364.0123100 00-.01667-.004362 .2476.010200 00-.005416 .0002209 -.004895-0.00311200 00-.0001393.736e-006 .006558.000257300 B (6-2) At the new trimmed condition the UAV has th e spiral mode pole at .213 giving it a time to double amplitude of 3.25 seconds compared to 9 seconds for the original condition which indicates significantly less roll stabil ity. The steady state values in the process model were changed to reflect the new trimmed conditions, xss=[49, 0, 9.9, 0, 0, 0, .2, 0]T where units are ft/sec, rad/s ec and rad as appropriate and us s=[0, 0, -4.88, .584]T where units are degrees of deflection and horsepower. The tuning on the IEKF remained the same, that is, the initial state error covariance, process noise, and measurement noi se assumed by the filter were the same. As mentioned earlier, the simulation allowed up to 20 feature points to be used at any given time. The other filter and camera parame ters were in the base line configuration of 60 degree horizontal field of view, c of 60 degrees, .05 second measurement interval, and a 320 by 240 pixel focal plane. No noise was applied to the measurements since any errors associated with image processing were already present. Camera and lens errors were not modeled. Figure 6-1 is still representa tive of the simulation. The Simulink simulation was imported into the vision lab with the focal pl ane and vision tracker module being replaced with synthetic imagery generated from the database and the visi on tracking algorithm as discussed earlier. At each filter measur ement the UAV truth model provides a position

PAGE 124

112 and orientation so that the correct image is constructed and pr esented to the vision tracking algorithm “on the fly”. The focal plane measurements are calculated and provided to the state estimator. The guidance module was initially left the same as used in the previous simulation. Some values were changed in the autopilot because of the new trimmed conditions. The new depiction is shown in Figure 6-7. The simulation was initiali zed at the trimmed conditi ons with the UAV tracking northeast. Waypoints were commanded so th at the UAV would fly a figure eight at 300 feet above sea level. Ground elevation in th e area is approximately 50 feet above sea level. The simulation was run for 100 seconds The results are shown in Figures 6-8, 6-9, and 6-10. Figure 6-8 shows that the angular rate esti mates track accurately and are much less noisy than the computer generated feature point measurements in the previous simulation. This seems to indicate less measurement noise than simulated previously and that the Lucas-Kanade feature point tracking algorithm performs well in this situation. The estimates for roll angle, and the velocities v and w seem to track fairly well as before. There is a noticeable error in of a few degrees during the st eady turns that was not there in the previous simulation. Agai n, the poorest estimation occurs for u and Notice that the errors seem to be related as pointed out in a previous chapter. The u estimates are high when the estimates are low. Figures 6-9 and 6-10 show that the UAV is able to fly through the commanded waypoints but does not perform particularly well and has some track stability problems. The vehicle tends to over shoot and oscillat e in heading while tracking to a waypoint. Altitude control is somewhat erratic with the vehicle tending to “dump” the nose as it

PAGE 125

113 rolls out of the smaller turns. Altitude is slowly deteriorating during the 100 second flight. This poor tracking pe rformance can be attributed more to non-optimal guidance and the decrease in roll stability at the slower trimmed condition rather than state estimation errors. In fact, the extra maneuve ring exercises the filter more and provides an opportunity to observe performance under a diffe rent set of conditions. At any rate, the state estimates were still accu rate enough to allow the auto pilot to control the UAV in a useful fashion. A second run was made after slightly modifying the guida nce module. The gain on the track error was cut in half to better matc h the UAV turn rate at the slower speed. The results are shown in Figures 6-11, 6-12, and 613. The plots indicate that the adjustment in the guidance track error gain greatly im proves the tracking performance of the UAV. The altitude tracking is also improved slight ly. The state estimator seems to perform approximately the same. The use of imagery and a currently availa ble feature point tracking algorithm in a closed loop simulation is a step closer to a flight demonstration and provides confidence that the IEKF implementation is a viable and useful technique for state estimation for this application.

PAGE 126

114 UAV Truth Model Focal Plane Vision Tracker State Estimator Navigation (e.g. GPS) Autopilot Guidance Focal plane coordinates Controls Commands Position, Track State Estimates Waypoints Figure 6-1. Closed loop simulation. Figure 6-2. Autopilot for closed loop simu lation, computer generated feature points.

PAGE 127

115 0 20 40 60 80 100 50 100 150 Velocities, Body Axesu (ft/s) Estimated Actual 0 20 40 60 80 100 -10 0 10 v (ft/s) 0 20 40 60 80 100 5 10 15 w (ft/s) 0 20 40 60 80 100 -2 0 2 p (rad/s)Angular Rates, Body Axes 0 20 40 60 80 100 -0.5 0 0.5 q (rad/s) 0 20 40 60 80 100 -1 0 1 r (rad/s) 0 20 40 60 80 100 -10 0 10 20 Euler Anglestheta (deg)Sec 0 20 40 60 80 100 -40 -20 0 20 40 Euler Anglesphi (deg)Sec Figure 6-3. Estimated and actual states computer generated feature points. -1500 -1000 -500 0 500 1000 1500 2000 2500 3000 3500 -2500 -2000 -1500 -1000 -500 0 500 1000 Trajectory East (ft)North (ft) o = Waypoint Figure 6-4. Horizontal trajectory, computer generated feature points.

PAGE 128

116 0 10 20 30 40 50 60 70 80 90 100 6900 7000 7100 7200 7300 7400 7500 7600 Trajectory Time (sec)Altitude (ft) First Waypoint is at 7500 ft Other Waypoints are at 7000 ft Figure 6-5. Vertical trajectory, co mputer generated feature points. Figure 6-6. Focal plane image from synthetic imagery simulation.

PAGE 129

117 Figure 6-7. Autopilot for closed lo op simulation, synthetic imagery. 0 20 40 60 80 100 40 60 80 Velocities, Body Axesu (ft/s) Estimated Actual 0 20 40 60 80 100 -20 0 20 v (ft/s) 0 20 40 60 80 100 5 10 15 w (ft/s) 0 20 40 60 80 100 -2 0 2 p (rad/s)Angular Rates, Body Axes 0 20 40 60 80 100 -0.5 0 0.5 q (rad/s) 0 20 40 60 80 100 -1 0 1 r (rad/s) 0 20 40 60 80 100 -50 0 50 Euler Anglestheta (deg)Sec 0 20 40 60 80 100 -50 0 50 Euler Anglesphi (deg)Sec Figure 6-8. Estimated and actual states, synthetic feature points.

PAGE 130

118 1600 1800 2000 2200 2400 2600 2800 3000 3200 3400 1400 1600 1800 2000 2200 2400 2600 2800 Trajectory East (ft)North (ft) o = Waypoint Figure 6-9. Horizontal trajecto ry, synthetic feature points. 0 10 20 30 40 50 60 70 80 90 100 240 250 260 270 280 290 300 310 Trajectory Time (sec)Altitude (ft) Altitude commanded is 300 feet. Figure 6-10. Vertical trajectory, synthetic feature points.

PAGE 131

119 0 20 40 60 80 100 40 60 Velocities, Body Axesu (ft/s) Estimated Actual 0 20 40 60 80 100 -20 0 20 v (ft/s) 0 20 40 60 80 100 5 10 15 w (ft/s) 0 20 40 60 80 100 -2 0 2 p (rad/s)Angular Rates, Body Axes 0 20 40 60 80 100 -0.5 0 0.5 q (rad/s) 0 20 40 60 80 100 -1 0 1 r (rad/s) 0 20 40 60 80 100 -50 0 50 Euler Anglestheta (deg)Sec 0 20 40 60 80 100 -50 0 50 Euler Anglesphi (deg)Sec Figure 6-11. Estimated and actual states, synthetic feature points, new guidance. 1600 1800 2000 2200 2400 2600 2800 3000 3200 3400 1400 1600 1800 2000 2200 2400 2600 2800 Trajectory East (ft)North (ft) o = Waypoint Figure 6-12. Horizontal trajectory, s ynthetic feature points, new guidance.

PAGE 132

120 0 10 20 30 40 50 60 70 80 90 100 240 250 260 270 280 290 300 310 Trajectory Time (sec)Altitude (ft) Altitude commanded is 300 feet. Figure 6-13. Vertical trajectory, synt hetic feature points, new guidance.

PAGE 133

121 CHAPTER 7 CONCLUSION 7.1 Summary In this research effort, a vision-based st ate estimation algorithm for UAVs has been developed and demonstrated through simulation. Previous work using the coplanarity constraint for the measurement in an implicit extended Kalman filter has been expanded by incorporating the UAV dynamic model into th e filter. This ha s allowed increased capabilities in two respects. The filter is able to estimate more states and the scale ambiguity present in the coplanarity constrain t, as well as other vision-based methods, is resolved. First, the derivations of the coplanarity c onstraint and the IEKF were reviewed in detail. The measurement model for the camera focal plane was developed for the UAV application. An overall description of th e state estimation algorithm was provided. Second, the algorithm using a statistical ra ndom walk process model was applied in a simple example similar to work done previously by Soatto et al. [5]. The algorithm exhibited a high sensitivity to noise in the focal plane measurements for the velocity estimates. Based on other literatu re, this problem is thought to stem from lack of parallax which refers to how much feature point m ovement on the focal plane there is due to translational motion which stems from multiple factors. Third, the UAV truth model used in this effort was presented. A simplified dynamic model, specific to this UAV but read ily adaptable to others, was developed for use in the IEKF. It was shown that the information from this process model allowed the

PAGE 134

122 estimation of additional states. The filter wa s exercised on several truth trajectories and found to suffer from the same sensitivity to measurement noise as the statistical model had for the velocity measurements. The more specific process model used in the IKEF allowed a modification to be made which grea tly reduced this sensitivity and provided a much more robust result. Next, robustness of the filter to errors in the process model was assessed using Monte Carlo simulation runs on several different trajectories. The filter was shown to be robust to variations in 39 model parameters Filter performan ce in the presence of varying levels of measurement noise was ex amined and appeared to have satisfactory margin. Sensitivity to several design parameters was also investigated. Finally, the IKEF was incorporated into a closed loop simulation with the UAV truth model and used as the sole source of st ate feedback for the autopilot. The UAV was required to autonomously fly a course consisti ng of several waypoints. This was done for two cases. One case was at the original condition using randomly generated feature points. The other case was at a slower tr immed airspeed with significantly different flight characteristics and using measuremen ts from realistic synthetic imagery with feature points tracked using an off-the-shelf vision tracking algorithm. The results showed that the state estimates were adequa te to allow successful negotiation of the course. 7.2 Follow On Work There are several areas for follow on work to this effort that would seem to be of value. The amount of noise present in the angular rate estimates indicates that some further tuning of the filter would be beneficial.

PAGE 135

123 Investigation of the use of this state estimation scheme with a different type of UAV should be done. This effort was ba sed on an eight foot wingspan class conventional UAV with stable flight characte ristics (except for the spiral mode). An application with a smaller MAV class aircraft with its faster and perhaps less stable dynamics would provide a useful study. Some sensitivities of this state estimation method that were investigated in this effort may have been masked by the stability of the UAV dynamics. More study on the nature and characteristics of the focal plane measurement errors could be done. A starting point for this is work done by others, for example Causey et al. [28]. Also, additional advantage of the visi on lab’s capabilities co uld be taken by using an appropriate camera to image the projections produced in the lab. The most obvious extension of this effort would be the use of the estimator in a UAV of some sort in actual autonomous fli ght. The necessary computation capability could initially be located remotely on the ground for convenience and flexibility and later sized to fit on the aircraft. A logical steppi ng stone in that direc tion would be extensive use of the vision lab in hard ware-in-the-loop simulations. An important and perhaps crucial aspect that should be looked at is the effect of wind. If not taken into account, it is possi ble that wind could great ly limit the usefulness of the state estimator presented in this effort. The UAVs and MAVs for which the estimator is envisioned typically travel at re latively slow airspeeds. A steady or slowly changing wind might have a velocity signifi cant in comparison to the vehicle trimmed airspeed. The effect of this on the state estimator was not investigated. This could have large ramifications because the process model in the estimator is based extensively on

PAGE 136

124 aerodynamic forces which assume the velocity is relative to the ai r mass. The visionbased measurements using the coplanarity cons traint are based on motion with respect to the feature point environment or ground speed. This mismatch needs to be investigated. It might be necessary for the filter to estimat e the wind velocity components. This could possibly be done by first implementing a “p seudo heading” state by integrating the angular rates. It might then be possible to estimate wind components relative to this heading. The heading would drift since it is not observable but the process could work if the drift is slow enough. Also, some sort of new focal plane measurement might be devised that would make the heading observable.

PAGE 137

125 APPENDIX A UAV TRUTH MODEL DESCRIPTION The twelve state UAV truth model used in th is effort is taken from reference [23]. It is representative of an eight foot wi ngspan class commercially available radio controlled aircraft. The model is based on the six, body fixed equations of motion for a rigid body augmented with six kinematic equations. The three velocity equations of motion in v ector form for the center of gravity (c.g.) of the vehicle in body fixed axes are: BB BBB IBdVdV FmmV dtdt (A-1) The three rotational equations of motion in vector form in body fixed axes are: BB BBB IBdHdH M H dtdt (A-2) Terms are defined as follows: The subscript “B” means coordinated in the body fixed axis system. Id dt means a derivative with respect to inertial axes. Bd dt means a derivative with respect to body axes. Body axes are defined as right hand orthogona l with one out the aircraft nose, two out the right wing, and three thr ough the bottom of the aircraft. Inertial axes are defined as right hand orthogonal with one north, two east, and three down. The surface of the earth is considered to be inertial.

PAGE 138

126 Velocity ,,T BVuvw No wind conditions are assumed. Angular velocity ,,T B p qr. Forces BATG F FFF for forces acting through the c.g. due to aerodynamics, thrust, and gravity, respectively. [,,]T AAxAyAzFFFF [,,]T TTxTyTzFFFF [,,]T GGxGyGzFFFF Moments BAT M MM for moments about the c.g. due to aerodynamics and thrust, respectively. [,,]T AAAAMLMN, [,,]T TTTTMLMN. Mass m. Angular momentum []BBBHI The inertia tensor 0 []00 0xxxz Byy xzzz I I II I I assumes the x-z plane of symmetry. Equations (A-1) and (A-2) can now be rewritten as: ()/ ()/ ()/AxTxGx AyTyGy AzTzGzuvrwqFFFm vwpurFFFm wuqvpFFFm (A-3) 22[()()]/ [()()]/ [()()]/xzyyzzATxx zzxxxzATyy xzxxyyATzzprpqIIIrqI qIIprrpII rpqrIIIpqI LL MM NN (A-4) (The dot denotes the time derivative of the component.) The Euler angles defining the orientation of the body axis system with respect to the inertial axis system, i.e. going from inertial to body, are , about the three, two, one axes, respectively and in that order. Using the appropriate transformation matrices the gravity force, mg in the down direction, is written in body axes as:

PAGE 139

127 sin cossin coscosGx GGy GzFmg FFmg Fmg (A-5) where g is the acceleration of gravity. The thrust force, T, is assumed to act only in th e body x direction and through the c.g. so that: 0 0Tx TTy TzFT FF F (A-6) and 0 0 0T TT TM L M N (A-7) T is controlled mainly by the power input from the engine to the propeller. The following relationship is used: 550/pTNBHPV (A-8) where T is thrust in pounds, Np is propeller efficiency (a constant of .7 is used), BHP is engine horsepower (a control input), and V is true airspeed in ft/sec and 2221/2()Vuvw (A-9) The aerodynamic forces of lift (L), drag (D), and side force (S) are defined and calculated in the wind axis system which has its number one axis into the relative wind. This differs from the body axis system by the sideslip angle () and the angle of attack (). In going from wind to body axes the angles are defined as negative about the three axis and positive about the two axis in that orde r. In the wind axes the aerodynamic

PAGE 140

128 force vector is [-D, S, -L]T. Using the appropriate transformation matrices the aerodynamic force is written in body axes as: coscoscossinsin sincos sincossinsincosAx AAy AzFDSL FFDS FDSL (A-10) The angles and can be computed as: 1 1tan(/) sin(/)wu vV (A-11) The aerodynamic moments, [,,]T AAAAMLMN, are already defined in the body axis system as indicated in Equation A-4. The aerodynamic forces and moments are determined as follows in the conventional manner using nondimensional co efficients obtained from wind tunnel testing or estimated as stated in reference [23]. L Y DLCQs SCQs DCQs Al Am AnCQsb CQsc CQsb L M N (A-12) The “Ci” terms are nondimensional coefficients that are computed from other coefficients and derivatives, s is wing area, b is wing span, and c is wing chord. Q is dynamic pressure. 2/2QV (A-13) And air density, is computed from altitude above sea level, h in feet, using the model: 64.2559.002377[1(6.8710)]h slgs/ft3 (A-14) The nondimensional coefficients are computed as follows:

PAGE 141

129 0 2 0(/2)(/2) (/2)(/2)e rLLLLLqLe Yyyrypyr DDLCCCCcVCqcVC CCCrbVCpbVC CCkC (A-15) 0(/2)(/2) (/2)(/2) (/2)(/2)ar e arlllplrlalr mmmmmqme nnnpnrnanrCCCpbVCrbVCC CCCCcVCqcVC CCCpbVCrbVCC (A-16) The values for the aerodynamic parameters, dimensions, and mass properties from reference [23] that were used are given in Table A-1. Table A-1 completes the six body fixed equa tions of motion. The remaining six kinematic equations transform the body axis line ar and rotational veloci ties into inertially referenced velocities and Euler angular rates so they can be integrated to determine inertial position (no wind) and or ientation of the vehicle. The Euler angle rates must be equivalent to the angular velocity of the vehicle, however each Euler angle rate is in a different coordinate system as defined earlier. If each Euler angle rate is transformed to th e body axis system the equality in Equation (A-17) must hold. 1000100cos0sin0 00cossin0cossin0100 00sincos00sincossin0cos p q r (A-17) This can be reduced to: 10sin 0cossincos 0sincoscos p q r (A-18) The Euler angle rates are then given by:

PAGE 142

130 110sin1sintancostan 0cossincos0cossin 0sincoscos0sin/coscos/cos p p qq rr (A-19) The inertially referenced velocities, or position derivatives are determined by transforming the velocities from the body axes. 100cos0sincossin0 0cossin010sincos0 0sincossin0cos001TNu Ev Dw (A-20) Where N is north displacement, E is east displacement, and D is down displacement. D is negated and referenced to sea level to get h to be used in Equation A-14 to compute density. The position derivatives are now coscossincoscossinsinsinsincossincos sincoscoscossinsinsincossinsinsincos sincossincoscos N u E v h w (A-21) The set of differential equations for the twelve state variables of the UAV truth model have been described. These equati ons are integrated numerically. The model must be provided with initial conditions for a ll twelve states and with four control inputs (Figure A-1).

PAGE 143

131 Table A-1. Aerodynamic and mass parameters from reference [23]. Symbol Parameter Value Units 0LC Zero alpha lift coefficient -.13 Non dim. LC Lift due to alpha derivative 4.0 /radian LC Lift due to alpha dot deriv. 1.66 Non dim. LqC Lift due to q deriv. 4.16 Non dim. eLC Lift due to elevator deriv. .005 /degree yC Side force due to beta deriv. -.46 /radian yrC Side force due to r deriv. .18 Non dim. ypC Side force due to p deriv. -.004 Non dim. ryC Side force due to r deriv. .00235 /degree 0 D C Zero lift drag coef. .061 Non dim. k Drag due to lift factor .086 Non dim. lC Roll moment due to beta deriv. -.078 /radian lpC Roll mom. due to p deriv. -.36 Non dim. lrC Roll mom. due to r deriv. .096 Non dim. alC Roll mom. due to aileron deriv. .025 /degree rlC Roll mom. due to rudder deriv. .00105 /degree 0mC Zero alpha mom. coef. .0093 Non dim. mC Pitch mom. due to alpha coef. -.29 /radian mC Pitch mom. due to alpha dot deriv. -4.0 Non dim. mqC Pitch mom. due to q deriv. -11.0 Non dim. emC Pitch mom. due to elevator deriv. -.01 /degree nC Yaw mom. due to beta deriv. .17 /radian npC Yaw mom. due to p deriv. -.03 Non dim. nrC Yaw mom. due to r deriv. -.046 Non dim. anC Yaw mom. due to aileron deriv. -.002 /degree rnC Yaw mom. due to rudder deriv. -.00144 /degree s Reference (wing) area 15.6 feet2 c Wing chord 1.784 feet b Wind span 8.73 feet m Aircraft mass .9325 slugs xx I Roll moment of inertia 1.7 slugfeet2 yy I Pitch moment of inertia 6.8 slugfeet2 zz I Yaw moment of inertia 9.3 slugfeet2

PAGE 144

132 Table A-1. Continued Symbol Parameter Value Units xz I x-z plane inertia product 0 slugfeet2 a Aileron deflection (+ for rt. t.e. up) control input degrees r Rudder deflection (+ for t.e. left) control input degrees e Elevator deflection (+ for t.e. dwn.) control input degrees /2 p bV Non-dimensional roll rate computed Non dim. /2 rbV Non-dimensional yaw rate computed Non dim. /2 cV Non-dimensional alpha dot computed Non dim. /2 qbV Non-dimensional pitch rate computed Non dim.

PAGE 145

133 Figure A-1. UAV truth model. Initial Conditions On States Controls States a r e B HP (.3) (.4) (.19) (.21) u vEquationA w p qEquationA r EquationA N EEquationA h u v w p q r N E h Numerical Integration

PAGE 146

134 APPENDIX B AUGMENTATION TO LINEARIZED AIR CRAFT EQUATIONS OF MOTION Linearized equations of motion for a conven tional fixed wing aircraft are frequently used in stability, control, and state estimation studies and analyses. These equations are usually in the form of pertur bations about a particular stra ight and level, steady-state flight condition since the actual dynamics are nonlinear by nature. These linear representations are obtained in a variety of ways such as applying analytical or numerical techniques to complex nonlinear models or using wind tunnel or fli ght test data or a combination of methods. In some cases a hi gher fidelity aircraft model than the linear perturbed state approximation is desired. This may occur if the linear model is the only model available or, even if a complex nonlinear model is also available, a less computationally expensive model is desired at the expense of some fidelity. This appendix shows how to augment the linear model with some simple nonlinear terms that will provide a more accurate representation of aircraft states particularly as conditions diverge from the identified steady-state flight condition, such as in sustained turns. The equations developed here are base d on information in Roskam [24]. The states and controls ar e defined with steady-state and perturbed components in the conventional fashion in Equations B-1 and B-2. The “ ss ” refers to the known steadystate condition and the “ ” refers to the perturbation. The states u v and w are the velocity components coordina ted along the body fixed x, y, z axes, respectively; p q and r are the angular rates about the body fixed x, y, z axes, respectively; is the aircraft

PAGE 147

135 pitch attitude; and is the aircraft roll attitude. For the control vector, subscripts a r e and T refer to the aileron, rudder, el evator, and throttle, respectively. ss ss ss ss ss ss ss ss ssu uu v vv w ww p p p xxx q qq r rr (B-1) ass aa rss rr ss ess ee Tss TT (B-2) The nonlinear rigid body equations of motion of the aircraft as given in reference [24] and also developed in Appendix A are: 22sin()/ cossin()/ coscos()/ [()()]/ [()()]/ [()()]/AxTx AyTy AzTz xzyyzzATxx zzxxxzATyy xzxxyyATzzuvrwqgFFm vwpurgFFm wuqvpgFFm prpqIIIrqI qIIprrpII rpqrIIIpqI LL MM NNcossin sintancostan qr pqr (B-3) where g is the acceleration of gravity, m is aircraft mass, the I terms are inertia terms, the F terms are aerodynamic and thrust forces, and L,M,N are aerodynamic and thrust moments about the center of gravity.

PAGE 148

136 The first step in linearizing these equations is to substitute in the steady-state and perturbation state variables. The steadystate condition is straight and unaccelerated flight so 0sssssspqr and 0ssx If the steady-state condition is also assumed to be coordinated flight (no aerodyna mic side force) then 0ssssv Making the substitution and dropping out the zero terms, Equation B-3 becomes: ()sin()()/ ()()cos()sin()/ ()cos()cos()/ [()()]/ [(ssssAxTx ssssssAyTy ssssAzTz xzyyzzATxx zzuvrwwqgFFm vwwpuurgFFm wuuqvpgFFm prpqIIIrqI qI LL22)()]/ [()()]/ cossin sintan()costan()xxxzATyy xzxxyyATzz ssssIprrpII rpqrIIIpqI qr pqr MM NN (B-4) The forces and moments can also be broken down into steady-state and perturbation values. Applyi ng this and making some trigono metric substitutions for the sum of angles Equation B-4 becomes: ()/(sincoscossin) ()/(coscossinsin)sin /(coscossinsin)cos (xssxssssss yssyssssssss zsszssssssuFFmwqvrwqg vFFmwpurwpurg wFFmuquqvpg p L22//()// (/()/()/ (//()// cossin sintan(ssxxxzxxyyzzxxxzxx ssyyzzxxyyxzyy sszzxzzzxxyyzzxzzz ssIrIIrqIIIpqII qIprIIIrpII rIpIIpqIIIqrII qr p L) MM) MM) )costan()ssqr (B-5) The steady-state moments and forces, including gravity, will sum to zero since the aircraft is in equilibrium. These compone nts from Equation B-5 are shown in Equation B-6.

PAGE 149

137 0/sin 0/ 0/cos 0/ 0/ 0/ x ssss yss z ssss ssxx ssyy sszzFmg Fm Fmg I I I L M N (B-6) Removing the steady-state terms given in Equation B-6 from B-5 and rearranging leaves Equation B-7. /cossinsincos1 /coscossinsinsinsin /sinsincoscoscoscos1 /xssssss yssssssss zssssss xxuFmwqgvrwqg vFmwpurgwpurg wFmuqguqvpg pI L 22/()// /()/()/ //()// (cos1)sin sintan()costan(xzxxyyzzxxxzxx yyzzxxyyxzyy zzxzzzxxyyzzxzzz ssrIIrqIIIpqII qIprIIIrpII rIpIIpqIIIqrII qqr pq M N )ssr (B-7) The equations in B-7 are sti ll exact (and nonlinear) at this point. As in reference [24], the linearization approxi mation is achieved by assuming small perturbations about the steady-state flight condition. Sine or ta ngent of a perturbation angle is approximated by the angle (in radians) and cosine of a perturbation angle is approximated by one. Higher order terms (that is two or more perturbation variables multiplied together) are neglected. The perturbation forces and moments are approximated by the first term in their Taylor series using the partial derivatives with respect to the state variables and controls. The terms that are included (approximately) in the linearization are those in brackets [] in Equation B-7. The result is Equa tion B.8. Note that the residual nonlinear terms are relatively easy to obtain. They are mostly kinematic and do not contain aerodynamic and thrust parameters. The items of aircraft specific information that must be obtained or estimated are inertia ratios and steady-state pitch attitude.

PAGE 150

138 sincos1 sinsinsin coscoscoss ss a ss r e Tuu vrwqg vv wpurg ww uqvpg pp xAB qq rr 22s1 ()// ()/()/ ()// (cos1)sin sintan()costan()yyzzxxxzxx zzxxyyxzyy xxyyzzxzzz ssssrqIIIpqII prIIIrpII pqIIIqrII qr qr (B-8) These residual terms were obtained starti ng with the relatively high fidelity rigid body nonlinear equations of motion. However, as stated in the beginning, these terms were developed so that they can be added to a set of linearized equations that already exist. These terms will allow greater accuracy at conditions that diverge from the steadystate conditions such as sustained turns. As applied in this effort, the terms were further simplified to those expressed in Equation B-9. In this application, the inertia term Ixz is zero. cos1 ()/ ()/ (a yyzzxx r zzxxyy e Tvrwq uu wpur vv uqvpg ww rqIII pp xAB prIII qq pq rr )/ (cos1) ()()xxyyzz ssssIII qr qr (B-9)

PAGE 151

139 LIST OF REFERENCES [1] Office of the Secretary of Defense, United States, Unmanned Aerial Vehicles Roadmap, 2000-2025, Washington, D.C., April 2001. [2] Pines, D., Bonorquez, F., “Challenges Facing Future Micro-Air-Vehicle Development,” Journal of Aircraft Vol. 43, No. 2, March-April 2006, pp. 290-305. [3] Brown, D., Inventing Modern America: From the Microwave to the Mouse The M. I. T. Press, Cambridge, MA, 2002. [4] Ettinger, S., Nechyba, M., Ifju, P., and Waszak, M., “Vision Guided Flight Stability and Control for Micro Air Vehicles,” IEEE International Conf erence on Intelligent Robots and Systems Lausanne, Switzerland, October 2002, Vol. 3, pp. 2134-2140. [5] Kanade, T., Amidi, O., Qi Fa Ke, “Real-Time and 3D Vision for Autonomous Small and Micro Air Vehicles,” IEEE Conference on Decision and Control, Atlantis, Paradise Island, Bahamas, 14-17 December 2004, vol. 2, pp. 1655-1662. [6] Proctor, A. and Johnson, E., “Vision-Only Aircraft Flight Control Methods and Test Results,” AIAA 2004-5351, AIAA Guidance, Navigation, and Control Conference Providence, RI, August 2004. [7] Soatto, S., and Perona, P., “Reducin g Structure From Motion: A General Framework for Dynamic Vision, Part 1: Modeling,” IEEE Transactions on Pattern Analysis and Maching Intelligence Vol. 20, No. 9, September 1998, pp. 933-942. [8] Soatto, S., and Perona, P., “Reduc ing Structure From Motion: A General Framework for Dynamic Vision, Part 2: Implementation and Experimental Assessment,” IEEE Transactions on Pattern Analysis and Machine Intelligence Vol. 20, No. 9, September 1998, pp. 933-942. [9] Chiuso, A., Favaro, P., Jin, H., Soatto, S., “Structure from Motion Causally Integrated Over Time,” IEEE Transactions on Pattern Analysis and Machine Intelligence Vol. 24, No. 4, April 2002, pp. 523-535. [10] Maybank, S., Theory of Reconstruction from Image Motion Springer-Verlag, New York, 1993.

PAGE 152

140 [11] Azarbayejani, A., and Pentland, A., “R ecursive Estimation of Motion, Structure, and Focal Length,” IEEE Transactions on Pattern Analysis and Machine Intelligence Vol. 17, No. 6, June 1995. [12] Soatto, S., Frezza, R. and Perona, P., “Motion Estimation via Dynamic Vision,” IEEE Transactions on Automatic Control Vol. 41, No. 3, March 1996, pp. 393413. [13] Soatto, S. and Perona, P., “Recurs ive 3-D Motion Estimation Using Subspace Constraints,” International Journal of Computer Vision Vol. 22, No. 3, 1997, pp. 235-259. [14] Kehoe, J., Causey, R., Arvia, A., and Li nd, R., “Partial Aircraft State Estimation from Optical Flow Using Non-Mode l-Based Optimization,” ,ACC.2006-1657154, IEEE American Control Conference Minneapolis, MN, 14-16 June 2006. [15] Kehoe, J., Watkins, A., Causey, R., and Lind, R., “State Estimation using Optical Flow from Parallax-Weighted Feature Tracking,” AIAA2006-6721, AIAA Guidance, Navigation, and Control Conference Keystone, CO, 21-24 August 2006. [16] Gurfil, P. and Rotstein, H., “Partial Ai rcraft State Estimation from Visual Motion Using the Subspace Constraints Approach,” Journal of Guidance, Control, and Dynamics Vol. 24, No. 5, Sep-Oct 2001, pp. 1016-1028. [17] Webb, T., Prazenica, R., Kurdila, A., and Lind, R., “Vision-Based State Estimation for Autonomous Micro Air Vehicles,” AIAA2004-5349, AIAA Guidance, Navigation, and Control Conference Providence, RI, 16-19 August 2004. [18] Webb, T., Prazenica, R, Kurdila, A., a nd Lind, R., “Vision-Based State Estimation for Ininhabited Aerial Vehicles,” AIAA2005-5869, AIAA Guidance, Navigation, and Control Conference San Francisco, CA, 15-18 August 2005. [19] Longuet-Higgins, H., “A Computer Algorithm for Reconstructing a Scene from Two Projections,” Nature Vol. 293, 10 Sep 1981, pp. 133-135. [20] Gelb, A., Kasper, J., Nash, R., Price, C., Sutherland, A., and TASC, Applied Optimal Estimation The M. I. T. Press, Cambridge, MA, 1974. [21] Grewal, M. and Andrews, A., Kalman Filtering: Theory and Practice Using MATLAB John Wiley and Sons, Inc., New York, 2001. [22] Ma, Y., Soatto, S., Kosecka, J., and Sastry, S., An Invitation to 3-D Vision Springer-Verlag, New York, 2004.

PAGE 153

141 [23] Webb, T., “Flight Control System for a Computer Controlled Aircraft with Limited Sensors,” Technical Note USAFA-TN-854, United States Air Force Academy, CO, 30 April 1985. [24] Roskam, J., Airplane Flight Dynamics and Automatic Flight Controls Roskam Aviation and Engineering Corporation, Lawrence, KS, 1979. [25] Brogan, W., Modern Control Theory Quantum Publishers, Inc., New York, 1974. [26] Wise, K., and Lavretsky, E., “Robust and Adaptive Control Theory,” AIAA short course, Providence, RI, 14-15 August 2004. [27] Hamdan, A. and Nayfeh, A., “Measures of Modal Controllability and Observability for First and Send Order Linear Systems,” Journal of Guidance, Control, and Dynamics Vol. 12, No. 3, May-June 1989, pp. 421-428. [28] Causey, R., Lind, R., Prazenica, R., “Effects of Camera Uncertainty on VisionBased Feedback,” AIAA2005-6097, AIAA Guidance, Navigation, and Control Conference San Francisco, CA, 15-18 August 2005.

PAGE 154

142 BIOGRAPHICAL SKETCH Thomas P. (Phil) Webb grew up in Fl orence, Mississippi and graduated from Florence High School in 1968. He graduated from the United States Air Force Academy in 1972, with a Bachelor of Science degree in aeronautical engin eering and served 20 years active duty with the United States Air Force. During that time he earned a Master of Science in aeronautics and astronautics from Purdue Univ ersity. Since retiring from the Air Force in 1992, Phil has been employe d by JE Sverdurp Technology at Eglin Air Force Base, Florida, providing engineeri ng support to air-to-air and air-to-ground weapons development programs. He enrolled as a part-time student at the University of Florida in 1999 through the University’s Rese arch and Engineering Education Facility adjacent to Eglin. In 2003 he was awarded a Master of Science in aerospace engineering. He hopes to receive the Doctor of Philosophy degree from the University of Florida in the spring of 2007. Phil is married to th e former Elizabeth Ballard of Lexington, Kentucky. They have two children, Jeffery and Kimberly.


Permanent Link: http://ufdc.ufl.edu/UFE0019262/00001

Material Information

Title: Vision-Based State Estimation for Uninhabited Aerial Vehicles Using the Coplanarity Constraint
Physical Description: Mixed Material
Language: English
Creator: Webb, Thomas Philip ( Dissertant )
Kurdila, Andrew ( Thesis advisor )
Lind, Rick ( Reviewer )
Dixon, Warren ( Reviewer )
Latchman, Haniph ( Reviewer )
Publisher: University of Florida
Place of Publication: Gainesville, Fla.
Publication Date: 2007
Copyright Date: 2007

Subjects

Subjects / Keywords: Aerospace Engineering thesis, Ph.D
Dissertations, Academic -- UF -- Mechanical and Aerospace Engineering
Genre: bibliography   ( marcgt )
theses   ( marcgt )
non-fiction   ( marcgt )

Notes

Abstract: We developed and evaluated a vision-based state estimation algorithm for uninhabited aerial vehicles (UAVs) using the implicit extended Kalman filter (IEKF) and the coplanarity constraint (also known as the epipolar or essential constraint). The coplanarity constraint, a well-known property in the structure from motion (SFM) field, has advantages for this application in that the feature point locations in three dimensional space do not have to be known and tracked and that feature points can be discarded and acquired as necessary. This reduces the computational load which is important for real time applications such as aircraft control. Advantages of the IEKF are that, in principle, the current estimate uses all previous information, not just the current observations, and that the estimate will propagate forward in an orderly fashion in the case of interrupted or reduced measurements. The dynamics of the aircraft are included in the process model which improves the observability of the states and resolves the SFM scale factor ambiguity. The algorithm was implemented in a numerical simulation and exhibited divergence problems in the presence of measurement noise. These effects were particularly evident in the velocity estimates. The problems were eliminated by zeroing out the small velocity dependent terms in the measurement matrix. The algorithm was exercised in a Monte Carlo fashion and found to be robust to errors in the process model and to measurement noise. Sensitivities to filter and focal plane implementation parameters including camera depression angle, field of view, measurement interval, and feature point location and number were also assessed. The modified estimator was then employed in a closed loop UAV simulation to provide feedback to a simple autopilot. The simulation demonstrated that the state estimates provided were sufficiently accurate to allow control of the UAV through successful waypoint navigation. This simulation used feature points generated at random locations in the field of view. A second closed loop simulation was successfully run using synthetic imagery from the University of Florida's vision laboratory and a Lucas-Kanade feature point tracking algorithm.
Subject: control, coplanarity, epipolar, estimation, iekf, sfm, state, uav, vision
General Note: Title from title page of source document.
General Note: Document formatted into pages; contains 154 pages.
General Note: Includes vita.
Thesis: Thesis (Ph.D.)--University of Florida, 2007.
Bibliography: Includes bibliographical references.
General Note: Text (Electronic thesis) in PDF format.

Record Information

Source Institution: University of Florida
Holding Location: University of Florida
Rights Management: All rights reserved by the source institution and holding location.
System ID: UFE0019262:00001

Permanent Link: http://ufdc.ufl.edu/UFE0019262/00001

Material Information

Title: Vision-Based State Estimation for Uninhabited Aerial Vehicles Using the Coplanarity Constraint
Physical Description: Mixed Material
Language: English
Creator: Webb, Thomas Philip ( Dissertant )
Kurdila, Andrew ( Thesis advisor )
Lind, Rick ( Reviewer )
Dixon, Warren ( Reviewer )
Latchman, Haniph ( Reviewer )
Publisher: University of Florida
Place of Publication: Gainesville, Fla.
Publication Date: 2007
Copyright Date: 2007

Subjects

Subjects / Keywords: Aerospace Engineering thesis, Ph.D
Dissertations, Academic -- UF -- Mechanical and Aerospace Engineering
Genre: bibliography   ( marcgt )
theses   ( marcgt )
non-fiction   ( marcgt )

Notes

Abstract: We developed and evaluated a vision-based state estimation algorithm for uninhabited aerial vehicles (UAVs) using the implicit extended Kalman filter (IEKF) and the coplanarity constraint (also known as the epipolar or essential constraint). The coplanarity constraint, a well-known property in the structure from motion (SFM) field, has advantages for this application in that the feature point locations in three dimensional space do not have to be known and tracked and that feature points can be discarded and acquired as necessary. This reduces the computational load which is important for real time applications such as aircraft control. Advantages of the IEKF are that, in principle, the current estimate uses all previous information, not just the current observations, and that the estimate will propagate forward in an orderly fashion in the case of interrupted or reduced measurements. The dynamics of the aircraft are included in the process model which improves the observability of the states and resolves the SFM scale factor ambiguity. The algorithm was implemented in a numerical simulation and exhibited divergence problems in the presence of measurement noise. These effects were particularly evident in the velocity estimates. The problems were eliminated by zeroing out the small velocity dependent terms in the measurement matrix. The algorithm was exercised in a Monte Carlo fashion and found to be robust to errors in the process model and to measurement noise. Sensitivities to filter and focal plane implementation parameters including camera depression angle, field of view, measurement interval, and feature point location and number were also assessed. The modified estimator was then employed in a closed loop UAV simulation to provide feedback to a simple autopilot. The simulation demonstrated that the state estimates provided were sufficiently accurate to allow control of the UAV through successful waypoint navigation. This simulation used feature points generated at random locations in the field of view. A second closed loop simulation was successfully run using synthetic imagery from the University of Florida's vision laboratory and a Lucas-Kanade feature point tracking algorithm.
Subject: control, coplanarity, epipolar, estimation, iekf, sfm, state, uav, vision
General Note: Title from title page of source document.
General Note: Document formatted into pages; contains 154 pages.
General Note: Includes vita.
Thesis: Thesis (Ph.D.)--University of Florida, 2007.
Bibliography: Includes bibliographical references.
General Note: Text (Electronic thesis) in PDF format.

Record Information

Source Institution: University of Florida
Holding Location: University of Florida
Rights Management: All rights reserved by the source institution and holding location.
System ID: UFE0019262:00001


This item has the following downloads:


Full Text











VISION-BASED STATE ESTIMATION FOR UNINHABITED AERIAL VEHICLES
USING THE COPLANARITY CONSTRAINT
















By

THOMAS PHILIP WEBB


A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL
OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT
OF THE REQUIREMENTS FOR THE DEGREE OF
DOCTOR OF PHILOSOPHY

UNIVERSITY OF FLORIDA


2007
































O 2007 Thomas Philip Webb
















ACKNOWLEDGMENTS

I wish to thank my supervisory committee chair (Dr. Andy Kurdila) for his

guidance, direction, and support. He is, without doubt, a worldwide leader in this field

and I have been privileged to benefit from his extensive experience in both research and

education. I am grateful for his time and effort despite his many other responsibilities. I

thank Dr. Rick Lind for his advice and valuable contributions to this research. I also wish

to thank Dr. Warren Dixon and Dr. Haniph Latchman for serving on my committee and

providing their valuable input.

I would like to express my appreciation to Dr. Chad Prazenica, a researcher and

visiting professor at the UF Research and Engineering Education Facility. He has

coauthored papers with me, run numerous simulations for me in the vision laboratory,

spent valuable time discussing ideas and techniques, and provided much encouragement.

I could not have completed this effort without his kind support.

I wish to thank my employer, JE Sverdrup Technology, TEAS Group (Eglin Air

Force Base, Florida) for providing my tuition and books during my studies through the

Edwin "Bud" George Fellowship.

I thank my loving wife, Libby, for the support and encouragement she has given

me during this long and seemingly endless endeavor. Finally, I thank God for the ability

and opportunity to make any modest contribution I may have accomplished toward

understanding and harnessing his creation.




















TABLE OF CONTENTS


page

ACKNOWLEDGMENT S .............. .................... iii


LIST OF TABLES ............_ ..... ..__ ..............vi...


LIST OF FIGURES .............. ....................vii


AB STRACT ................ .............. xi


CHAPTER


1 INTRODUCTION ................. ...............1.......... ......


1.1 Previous Work in this Field ................. ......... ...............2.....
1.2 Current Research Effort ................. ...............5............ ...
1 .2. 1 Obj ective ................. .............. ...............5......
1.2.2 Outline of Tasks Accomplished .............. ...............6.....
1.2.3 Contributions .............. ...............6.....


2 THEORY AND CONCEPT S .............. ...............8.....


2.1 Coplanarity Constraint............... ...............
2.2 Scale Factor Ambiguity .............. ...............10....
2.3 Implicit Extended Kalman Filter ................. ......... ......... ...........1
2.4 Development of Measurement Model .............. ...............13....
2.5 Estimation Al gorithm Summary ................. ...............23................

3 APPLICATION WITH STATISTICAL DYNAMIC MODEL .............. .... .........._..27


3.1 Simulation Description ................ ...............27........... ....
3.1.1 Statistical M odel .............. .. .... ......... ............. ..... .........2
3.1.2 Simulation of Focal Plane and Vision Processing Output. ................... .....28
3.2 Simulation Results ............... .......... .............2
3.2. 1 Results for No Measurement Noise ...._._._._ ..... ... .___ ........._.._.....30
3.2.2 Results in the Presence of Measurement Noise............_.._ .........._._.....31
3.3 Sensitivity of Algorithm to Measurement Noise ........._.._........ .... ...........3 1
3.4 Observations from Application with Statistical Model .............. ...................33












4 APPLICATION WITH A UAV DYNAMIC MODEL ................. ......................39


4.1 Dynamic Model Description............... ..............3
4.2 Observability Considerations............... .............4
4.3 Simulation Description ........._.__........__. ...............47....
4.4 Results With No Measurement Noise ....._.__._ ..... ... .__. .. ..._.__.......4
4.5 Results With Measurement Noise .............. ...............49....
4.6 Degree of Observability ................. ...............52...___ .....
4.6. 1. Mod al Ob servability ............... ...............52....
4.6.2 The Measurement Matrix .............. .... ........ ............5
4.6.3 Modification of the IEKF Measurement Matrix .............. ...................55
4.7 Results With the Modified IEKF With Measurement Noise .............. ..............56


5 ROBUSTNESS AND SENSITIVITY ASSESSMENT ............... ...................6


5.1 Robustness to Modeling Uncertainties .............. ...............68....
5.2 Sensivity to Design Parameters .............. ...............72....
5.2.1 Camera Depression Angle ................. ...............72................
5.2.2 Camera Field of View .............. ...............73....
5.2.3 Measurement Interval ................. ......... ...............74. ...
5.2.4 Number of Feature Points. ................ ....................................76
5.2.5 Feature Point Location .............. ...............77....
5.3 Effect of Measurement Noise .............. .. ... ....... ............7
5.4 Overall Comments on Robustness and Sensitivity ................. ......................80

6 IEKF CLOSED LOOP PERFORMANCE ........._.._.. ....._.. ........__. .......10


6.1 Simulation with Computer Generated Feature Points .............. ....................10
6.2 Simulation With Synthetic Imagery .............. ...............109....

7 CONCLUSION............... ...............12


7.1 Summary ........._..... ...._... ...............121....
7.2 Follow On Work ........._..... ...._... ...............122...


APPENDIX


A UAV TRUTH MODEL DESCRIPTION .....__. ............... ........._.._.......12


B AUGMENTATION TO LINEARIZED AIRCRAFT EQUATIONS OF
M OTION .............. ...............134....


LIST OF REFERENCES ............ ......__ ...............139...


BIOGRAPHICAL SKETCH ............ ..... .__ ...............142...



















LIST OF TABLES

Table pg


5-1 Straight and level traj ectory Monte Carlo sets. ............ ...... __ ........_.._.....84

5-2 Vertical S trajectory Monte Carlo sets. ............. ...............84.....

5-3 Horizontal S trajectory Monte Carlo sets. ............. ...............85.....

5-4 Camera angle Monte Carlo sets. ..........._._ ....._._ ...............87..

5-5 Camera field of view Monte Carlo sets. .............. ...............90....

5-6 Measurement interval Monte Carlo sets. .............. ...............93....

5-7 Number of feature points Monte Carlo sets ..........._._ ......_.._ ......._.._......9

5-8 Feature point location Monte Carlo sets. ..........._..__........ ........_._.........0

5-9 Measurement noise Monte Carlo sets. ......._..__ ........__ ....._._ ..........10

A-1 Aerodynamic and mass parameters ............. ...............131....


















LIST OF FIGURES


Figure pg

1-1 Conceptual model. ............. ...............7.....

2-1 The perspective proj section camera model ......___ ... .....___ ......_._ ........2

2-2 Geometry of the coplanarity constraint. ............. ...............25.....

2-3 Measurement model geometry. ............. ...............26.....

3-1 Traj ectory for statistical dynamic model numerical simulation. ............. ..... ........._..34

3-2 Focal plane measurement. ............. ...............34.....

3-3 Feature point movement on focal plant for initial run with no measurement noise...35

3-5 Estimation results with .03 pixel measurement noise............... ...............36.

3-6 Estimation results with .14 pixel measurement noise............... ...............36.

3-7 Estimation results with .47 pixel measurement noise added at 6 seconds. ................37

3-8 Focal plane for results with measurement noise added at 6 seconds..........................37

3-9 Estimation results with extended baseline, .47 pixel measurement noise. ...............38

4-1 Eight foot wingspan class aircraft simulated for this effort. .............. .............. .57

4-2 Straight and level trajectory simulation, no measurement noise. ............... ...............58

4-3 Errors from straight and level trajectory simulation, no measurement noise. ............58

4-4 Vertical S trajectory simulation, no measurement noise. ............. .....................5

4-5 Errors from vertical S trajectory simulation, no measurement noise. ........................59

4-6 Horizontal S trajectory simulation, no measurement noise. ................... ...............60

4-7 Errors from horizontal S trajectory simulation, no measurement noise. ....................60

4-8 Errors from straight and level trajectory, one pixel measurement noise. ...................61










4-9 Errors from vertical S traj ectory, one pixel measurement noise. ............. .............61

4-10 Errors from horizontal S trajectory, one pixel measurement noise. .........................62

4-11 Monte Carlo runs, straight and level trajectory with measurement noise. ...............62

4-12 Straight and level trajectory errors with measurement noise, increased
measurement interval. ............. ...............63.....

4-13 Modal observability for straight and level trajectory, no measurement noise..........63

4-14 Measurement matrix values for straight and level, no noise ................. ...............64

4-15 Measurement matrix values for straight and level, one pixel measurement noise...64

4-16 Errors for modified IEKF, straight and level trajectory with measurement noise. ..65

4-17 Errors for modified IEKF, vertical S traj ectory with measurement noise. ........._....65

4-18 Errors for modified IEKF, horizontal S trajectory with measurement noise............66

4-19 Modified IEKF Monte Carlo, straight and level with measurement noise............_...66

4-20 Modified IEKF Monte Carlo, vertical S with measurement noise. ..........................67

4-21 Modified IEKF Monte Carlo, horizontal S with measurement noise. ...................67

5-1 Six run Monte Carlo set errors, case 1, straight and level traj ectory ......................82

5-2 Case 1 straight and level traj ectory, ensemble mean error and bounds. .........._.......82

5-3 Case 1 vertical S trajectory, ensemble mean error and bounds ................ ...............83

5-4 Case 1 horizontal S traj ectory, ensemble mean error and bounds..............._._..........83

5-5 Straight and level traj ectory, case statistics. ....._._._ .... ..... ... ...._.........8

5-6 Vertical S trajectory, case statistics. ............. ...............86.....

5-7 Horizontal S trajectory, case statistics. .............. ...............86....

5-8 Focal plane history for straight and level traj ectory with camera looking down,
intermediate baseline position, and level. ............. ...............87.....

5-9 Straight and level traj ectory, camera angle statistics ........._.. ....... ._. ............88

5-10 Vertical S traj ectory, camera angle statistics. ....._._._ .... ... .__ ........_._......8

5-11 Horizontal S traj ectory, camera angle statistics ....._._._ .... ... .. ........._......89











5-12 Straight and level trajectory, camera FOV statistics. ............. .....................9

5-13 Vertical S trajectory, camera FOV statistics. ............. ...............91.....

5-14 Horizontal S trajectory, camera FOV statistics. ................... ...............9

5-15 History of statistics from .5 second measurement interval ensemble, straight and
level traj ectory ........._.. ..... ._ __ ...............92....

5-16 Straight and level trajectory, measurement interval statistics. ............. .................94

5-17 Vertical S trajectory, measurement interval statistics. ............. .....................9

5-18 Horizontal S trajectory, measurement interval statistics. ................... ...............9

5-19 Straight and level trajectory, number of feature points statistics. ........._..._...............97

5-20 Vertical S traj ectory, number of feature points statistics ........._.._..... ...._.._........97

5-21 Horizontal S trajectory, number of feature points statistics. ................. ...............98

5-22 Focal plane history for straight and level traj ectory with area constraints. .............99

5-23 Focal plane history for straight and level traj ectory with area constraints. ............100

5-24 Straight and level traj ectory, feature point location statistics ................. ...............102

5-25 Vertical S traj ectory, feature point location statistics ................. .....................102

5-26 Horizontal S traj ectory, feature point location statistics ................. ................ ..103

5-27 Straight and level trajectory, measurement noise statistics ................. ...............105

5-28 Vertical S trajectory, measurement noise statistics. ................... ...............10

5-29 Horizontal S trajectory, measurement noise statistics. ................... ...............10

6-1 Closed loop simulation. ................ ...............114......... .....

6-2 Autopilot for closed loop simulation, computer generated feature points. .............114

6-3 Estimated and actual states, computer generated feature points. .........._................115

6-4 Horizontal traj ectory, computer generated feature points. ........._._... ......._._.....115

6-5 Vertical traj ectory, computer generated feature points. ........._.... ........._._.....1 16

6-6 Focal plane image from synthetic imagery simulation............_.._._ .............. .....116

6-7 Autopilot for closed loop simulation, synthetic imagery. .........._.._._ ........_.._.....117











6-8 Estimated and actual states, synthetic feature points..........._.._.. .........._.........117

6-9 Horizontal traj ectory, synthetic feature points. ........._.._.. ....__. ................1 18

6-10 Vertical traj ectory, synthetic feature points ................. ................. ...._ 11

6-11 Estimated and actual states, synthetic feature points, new guidance.....................119

6-12 Horizontal traj ectory, synthetic feature points, new guidance. ............. ...... ..........1 19

6-13 Vertical trajectory, synthetic feature points, new guidance............... ...............12

A-1 UAV truth model. ............ .............133......
















Abstract of Dissertation Presented to the Graduate School
of the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Doctor of Philosophy

VISION-BASED STATE ESTIMATION FOR UNINHABITED AERIAL VEHICLES
USING THE COPLANARITY CONSTRAINT

By

Thomas Philip Webb

May 2007

Chair: Andrew J. Kurdila
Major: Aerospace Engineering

We developed and evaluated a vision-based state estimation algorithm for

uninhabited aerial vehicles (UAVs) using the implicit extended Kalman fi1ter (IEKF) and

the coplanarity constraint (also known as the epipolar or essential constraint). The

coplanarity constraint, a well-known property in the structure from motion (SFM) Hield,

has advantages for this application in that the feature point locations in three dimensional

space do not have to be known and tracked and that feature points can be discarded and

acquired as necessary. This reduces the computational load which is important for real

time applications such as aircraft control. Advantages of the IEKF are that, in principle,

the current estimate uses all previous information, not just the current observations, and

that the estimate will propagate forward in an orderly fashion in the case of interrupted or

reduced measurements. The dynamics of the aircraft are included in the process model

which improves the observability of the states and resolves the SFM scale factor

ambiguity. The algorithm was implemented in a numerical simulation and exhibited









divergence problems in the presence of measurement noise. These effects were

particularly evident in the velocity estimates. The problems were eliminated by zeroing

out the small velocity dependent terms in the measurement matrix. The algorithm was

exercised in a Monte Carlo fashion and found to be robust to errors in the process model

and to measurement noise. Sensitivities to filter and focal plane implementation

parameters including camera depression angle, field of view, measurement interval, and

feature point location and number were also assessed. The modified estimator was then

employed in a closed loop UAV simulation to provide feedback to a simple autopilot.

The simulation demonstrated that the state estimates provided were sufficiently accurate

to allow control of the UAV through successful waypoint navigation. This simulation

used feature points generated at random locations in the field of view. A second closed

loop simulation was successfully run using synthetic imagery from the University of

Florida' s vision laboratory and a Lucas-Kanade feature point tracking algorithm.















CHAPTER 1
INTTRODUCTION

Interest in autonomous flight of small uninhabited aircraft has been increasing over

the past few years. Also, the demand and expectation for smaller and smaller vehicles

has increased as the size and weight of sensors, actuators, microprocessors, and

propulsion systems has decreased [1, 2]. Many of these aircraft employ a video camera

as a sensor in accomplishing their mission (e.g., information gathering, surveillance,

route documentation) or for manual control for some phases of flight using a remote

human pilot. Dual use of video for state estimation in a flight control system as well as

for mission accomplishment is attractive in that payload space and weight can be

conserved allowing for a smaller aircraft for a given mission. This research explores one

approach for using visual information as the sole measurement for estimating an adequate

number of aircraft states with enough accuracy to drive an autopilot. A conceptual model

is shown in Figure 1-1.

Multiple use of components for different functions in air vehicle design is certainly

not a new idea. A remarkable example in the extreme is the Gossomer Condor in which

designer Paul MacCready combined the propulsion power source, sensors, flight control

computer, control actuators, and flight recorder into a single "off the shelf' package in

the person of Bryan Allen [3].









1.1 Previous Work in this Field

Vision-based state estimation for control of aircraft has been around as long as the

airplane since vision is the sense most used by a human pilot. Of course flying creatures

have been doing this for much longer.

The University of Florida has successfully flown micro air vehicles (MAVs) using

an autopilot based on a vision-based horizon tracking scheme [4]. The Robotics Institute,

Carnegie Mellon University has much experience with "structure from motion" (SFM)

research and has applied that to aerial vehicles in recent years [5]. Georgia Institute of

Technology has recently flown a glider that successfully homes to a target of known size,

shape, and color using vision-only navigation, guidance, and control [6].

The technique used in my research described is drawn from the body of SFM work.

The computer vision problem of SFM has been described as "estimating 3-D motion and

structure of a rigid obj ect from a sequence of monocular images" [8]. This is equivalent

to estimating the motion of a moving camera and the structure of a fixed environment

from a sequence of images. SFM methods can generally be categorized as causal or non-

causal. Causal methods are those that, at a given point in time, use only current and past

information. These methods can be recursive in nature. In non-causal schemes, batch

processing is normally used after the fact to include all information for an estimate at any

given time of interest. Any algorithm used for real-time control would obviously have to

be causal [9].

Soatto and Perona [8] explore and compare the performance of a number of

recursive methods implemented using a form of the extended Kalman filter (EKF). The

basic measurement used in all these methods is the movement of "feature points" from

frame to frame on the camera focal plane. A feature point is a fixed point in the









environment that is identified and tracked as the camera moves. The methods examined

use various constraints imposed on feature point image movement on the focal plane by

geometry. Also, some methods attempt to estimate the positions of the feature points in

the environment (the structure) at the same time as the camera motion variables are

estimated, while other methods are able to decouple these operations.

If feature point location (structure) is not important, the decoupled methods are

attractive for real-time implementation because they are less demanding computationally

since the distance to feature points is not required to be known initially and updated.

Also, new feature points can be added and old ones discarded at will. These schemes are

able to deduce camera angular rates and direction of velocity (or, for the discrete case,

incremental changes in angular orientation and direction of position change). The

magnitude of the translational motion cannot be determined directly without some

reference length. This is the well-known scale factor ambiguity that comes from the fact

that the camera model used cannot distinguish whether a feature point is close with slow

relative motion or further away with faster relative motion [5, 8, 10].

Azarbayej ani and Pentland [1 1] developed and demonstrated an EKF method of

estimating structure, camera motion, and camera focal length. It carries a state in the

filter for each feature point, corresponding roughly to scaled range, as well as the

incremental rotations and scaled translations. Chiuso et al. [9] improved upon this

general approach by handling the problem of losing and adding feature points (feature

point occlusion). The method allows for the use of an arbitrary dynamic model for the

motion variables, however the examples published included only statistical random walk

models.









Soatto formalized the implicit EKF (IEKF) to implement recursive decoupled

methods using two different implicit geometric constraints; the coplanarity constraint

(also known as the epipolar or essential constraint) [12] and the subspace constraint [13].

These methods are attractive for use in estimating motion variables in real time since, as

mentioned previously, filter states for the feature points do not have to be carried.

Motion parameters estimated are formulated as angular rates and direction of velocity.

Again, statistical models are implemented in the examples.

A causal, but non-EKF method has been developed by Kanade et al. [5]. It links

two-frame SFM solutions of the essential constraint computed by a conventional non-

recursive computer vision algorithm. Drawbacks to this method for the current

application appear to be that since there is no dynamic model, there is no mechanism for

incorporating available information about the dynamic characteristics of the plant. Also,

the current state estimate uses only information from the current and last focal plane

measurement while the EKF blends in information from all previous measurements.

Even so, this method was shown to have promising results using vision data obtained

during an actual model aircraft flight of short duration with good initialization.

Kehoe et al. [14, 15] developed a method based on application of nonlinear

optimization techniques applied to the optical flow of tracked feature points. This

method is also not filter based and does not use any model information. The technique

was demonstrated using simulation.

Gurfil and Rotstein [16] added an aircraft dynamic model to the subspace

constraint IEKF method mentioned earlier [13]. Their numerical simulation results

demonstrated estimation of aircraft angle of attack, sideslip, and angular rates for a single










traj ectory using camera focal plane measurements with a limited amount of measurement

noise. They did not attempt to use the state estimates in aircraft control.

1.2 Current Research Effort

1.2.1 Objective

The obj ective of this research effort is to develop and demonstrate a causal, vision-

based state estimation algorithm for autonomous uninhabited aerial vehicles (UAVs)

capable of providing enough states of sufficient accuracy to reliably control the vehicle in

an urban, dynamic environment. The approach used is based on the coplanarity

constraint IEKF method mentioned earlier [12]. This method is attractive as a departure

point for several reasons. It is decoupled from the structure which decreases

computational demands and allows feature points to be efficiently discarded and added as

necessary as the field of view changes. It is not dependent on a discernable horizon

which is likely to be missing in an urban environment. If feature point measurements are

reduced or even lost completely for a time, the Kalman Filter will still provide state

estimates that gracefully degrade which contributes to the robustness and integrity of the

control system. And finally, the IEKF lends itself to the incorporation of a specific plant

model which provides more information to the process allowing the estimation of

additional states.

It is important to note that we assumed that the feature point correspondence and

tracking problem is solved. That is, the location of feature points on the focal plane from

frame to frame is known within a certain level of accuracy. That is not to say that the

location of the feature points in space is known. This vision processing problem is

certainly not trivial and is the subj ect of much ongoing research. The Lucas-Kanade

algorithm [5] is one proven method for this task.










1.2.2 Outline of Tasks Accomplished

A simulation of a small UAV was developed and "truth" data sets for various

traj ectories were compiled. The IEKF was augmented with a simplified dynamic model

representing the aircraft and a rigidly attached focal plane. The filter was exercised on

the traj ectories using randomly chosen feature points moving on a simulated focal plane

in accordance with the particular traj ectory being run. The filter exhibited some

divergence problems in the velocity states when measurement noise was added. A

modification to the filter measurement model was made which seemed to alleviate the

problem. Filter parameters were tuned during this process and state estimation accuracy

was evaluated. Robustness of the filter to initialization errors, vehicle modeling errors,

and measurement noise was addressed using Monte Carlo simulation runs. Sensitivity of

its performance to camera orientation, field of view, measurement frequency, and feature

point distribution and density was investigated. An autopilot tailored to the estimated

states was designed and a closed loop simulation including the IEKF, aircraft simulation,

and focal plane simulation was developed to demonstrate the suitability of the filter for

use in autonomous waypoint navigation for the UAV. The filter was also exercised using

synthetic video produced by the University of Florida Research and Engineering

Education Facility MAV vision laboratory. Some of this research has been documented

previously in references [17] and [18].

1.2.3 Contributions

This research effort makes several contributions to the state of the art in vision-

based state estimation and control for autonomous UAVs. The IEKF based on the

coplanarity constraint was applied to an agile aerial vehicle. A specific dynamic model

was used in the IEKF which expanded its capabilities in two respects. First, the plant










dynamics allow the estimation of states in addition to angular rates and direction of

velocity. In this case, they are the Euler angles of pitch and roll. Second, the scale factor

ambiguity is resolved for the velocities. A final contribution is that this effort

demonstrates that an autonomous UAV can be productively controlled using information

derived solely from focal plane measurements in a no-horizon environment.


Figure 1-1. Conceptual model.















CHAPTER 2
THEORY AND CONCEPTS

2.1 Coplanarity Constraint

The fundamental geometric constraint we used as a measurement is the coplanarity

constraint, also called the epipolar constraint or the essential constraint. This

relationship, credited to H. C. Longuet-Higgins [19], uses the perspective proj section or

pinhole camera model (Figure 2-1).

The coplanarity constraint is formulated based on the principle that the position

vectors of a feature point relative to the camera, proj ected onto the focal plane at two

instants in time, must be coplanar with the translation vector of the origin of the camera

frame. This is shown in Figure 2-2, where X and X' denote the position vectors of feature

point P in the camera reference frame, x and x' represent the corresponding position

vectors of the image of P on the focal plane, and T indicates the translation vector of the

origin of the camera frame. The development of this form of the constraint equation is

found throughout the SFM literature [12] and is repeated here for completeness.

Since the scalar triple product of three coplanar vectors is zero, the coplanar

constraint for this feature point is expressed as

X*(T x RX') = 0 (2-1)

and, equivalently for the focal plane,

x*(T x Rx ') = (2-2)

since x is co-linear with X and x' is co-linear with X Here, the common coordinate

system is arbitrarily chosen to be that of the camera at time t. R is the orthonormal









rotation matrix that transforms camera coordinates at time t+ A t to camera coordinates at

time t.

To handle multiple feature points, it is convenient to make the following

definitions.

A :[UVfT

where pu and v are the coordinate components in the focal plane as shown in Figure 2-1

and fis the camera focal length. If the components are normalized to the focal length

then let

x :[pulv ]T and x' A[u' v'1]T

Also, a skew-symmetric matrix is formed from Tto compute the cross product,

O -t3 2,
[Tx]4 t3 0 -t
-t2 t, 0

and the matrix E is defined as E A [Tx]R (2-3)

E belongs to a special class of matrices called "essential matrices" described in the SFM

literature [10, 12]. Equation 2-2 can now be written as [Fu, v, 1] E [pu', v ', 1]T = 0 for the

ith feature point. Carrying out the multiplication and making the subsequent definitions

yields Cze = (2-4)

where e is the 9xl column vector composed of the stacked columns of E and




The C, rows are stacked to form the constraint for all feature points

Ce = 0 (2-6)









Note that C contains only the observations from the focal plane, each row for a different

feature point, and e contains only camera movement parameters.

2.2 Scale Factor Ambiguity

Scale factor ambiguity was mentioned in chapter 1. For the coplanarity constraint

this limitation can be observed by considering the translation vector Tin Figure 2-2. The

coplanarity constraint provides information only about the orientation of T, not its

magnitude, since the triple product will be zero for any magnitude of T. In applying the

constraint to solve for motion, an additional constraint is placed on T. In the literature the

usual additional constraint is to set the magnitude of Tto unity realizing that the direction

of translation is what is produced. A similar condition will be imposed for this work.

This ambiguity also extends to the lengths of the position vectors from the camera to the

feature point, X and X' since it is the components of x and x' measured relative to the

focal plane that are used. Scale ambiguity is the price paid for being able to decouple

structure from motion which is arguably a good tradeoff when one is only interested in

camera motion. Scale can be restored at any time if the magnitude of T is known or the

magnitude of Xor X' is known for any single feature point since the three form the sides

of a triangle whose angles are known.

2.3 Implicit Extended Kalman Filter

A brief review of the discrete extended Kalman filter (EKF) algorithm is presented

followed by an explanation of the modification to the measurement update that leads to

the implicit extended Kalman filter (IEKF).

The EKF algorithm is derived and can be found in any of a number of texts, for

example Gelb et al. or Grewal and Andrews [20, 21]. For the discrete case the results

are:










* Dynamic model:

xk =k-1kl k-1 Wk-1 k ~ N(0, Qk ) (2-7)

Where x is the state vector, u is an independent known control input vector, w is a white

random vector sequence with properties denoted by matrix N(0, Q) meaning normal

distribution, zero mean, and covariance matrix Q. The symbol represents a known

vector function and k is shorthand for kcdt.

* Measurement model:

Zk =hk Xk +k, k ~ N(0, Rk ) (2-8)

Note that this gives z, the measurement, explicitly.

* State propagation:

xk = fk-1 -1, u) (2-9)

Where "-" denotes a value prior to the measurement update and "+" is the value post

update. The "^`" indicates that the value is an estimate.

* Measurement prediction:

Zk = k k)(2-10)

* State covariance propagation:

P = Oklk-1P~ +Qk-1 (2-11)


where the state transition matrix: Ok- = i (2-12)
drx


* Gain computation:

kk =D H~ Hk H + Rk i (2-13)



where the measurement matrix: Hk =ld (2-14)
S~ x'X









* State update:

x,= x, + kk( Zk k)- (2-15)

* Covariance update:

Pk = [I-kkHk]Pk (2-16)

In the IEKF, the explicit measurement model is replaced with an implicit constraint

involving measurements. For this case the coplanarity constraint, Equation 2-6, is used.

This leads to the following modifications as given by Soatto [12].

The quantity Ce becomes the predicted "pseudo measurement", Sk. The

measurement, zk iS replaced by zero, the true value of the constraint. The state update,

Equation 2-15, now becomes:

Xk= k- kkC(U, 1 pl> 171, 117"">M~N> ,NMN, 1N)k e(Xk) (2-17)

where N is the number of feature points.

The measurement matrix, Hk is more specifically defined as:

8(Ce) del
Hk C (2-18)


Also, it is useful to express the measurement noise covariance term, Rk for the

EKF, in terms of the covariance of the measurement error of the positions of the feature

points on the focal plane, Rk (which is not the same as the covariance of the error in Ce).

To first order, this is:

R, = DkRkD, (2-19)

where :


Rk C cOV[ p V~ p V v 62 6v ...6pN VN p ]T ~ 4 Nx4 N









8 denotes the measurement error of the particular parameter

N is the number of feature points

h 8(Ce)
Dk (2-20)
a[ n, v, p* v'V IU2 V2 NU V~...~ N TR uyvy

For this IEKF the gain, Equation 2-13, now becomes:

kk, =H [HkIH' +Dk k)kT -1 (2-21)


2.4 Development of Measurement Model

In this section, the specifics of the measurement model for the coplanarity

constraint in the IEKF are developed in terms of the state variables. In reference [12],

Soatto described two approaches with regards to the state variables used in the IEKF.

The first assumed the usual quantities of linear and angular velocities which he referred

to as the "local coordinates estimator." The second used the components of the essential

matrix E as the state variables and was referred to as the "essential estimator." The

essential estimator results in a linear Kalman filter, however a nonlinear transformation is

required to obtain the desired state variables. The local coordinates estimator yielded

slightly better results according to Soatto [12] and is a more straight-forward

implementation. That is the approach used in this work and described here.

In section 2.1 it was shown that the camera frame to frame movement information

is contained in the essential matrix, E, which is comprised of the translation vector, T and

the rotation matrix, R. The obj ective then is to express these in terms of aircraft motion

variables. T and R are determined by the linear and angular velocities of the camera with

respect to the environment, denoted as vectors Vc and coc, respectively. These in turn are

related to the velocity vector of the aircraft center of mass, Vb, and the aircraft angular









velocity vector, wb The six components of Vb and wb COmprise the state vector for

purposes of this section. The geometry for this is shown in Figure 2-3. The right-handed

set of unit vectors {(i,), a } describes the camera Eixed reference frame and likewise the


right-handed set {(, b, kb } describes the aircraft body Eixed frame. In the development

that follows, R will be determined as a function of we and T will be determined as a

function of Vc and wc. Then we and Vc will be expressed in terms of wb and Vb.

It is convenient to make the following definitions for the velocity components:









and for the skew symmetric matrix:



[q x] 4 ,O -re 0O -pc4, (2-23)



Note: As stated previously, these velocities are with respect to the environment even

though they are coordinated in different frames.

The rotation matrix R can be computed using the well known exponential

expression (e.g., from chapter two of reference [22]):

R = elsor]" (2-24)

where At is the time increment between images.

This assumes that we is constant during At which is not too bad an assumption if

At is not too long. One popular method of evaluating this matrix exponential is









Rodrigues' formula (see again chapter two of reference [21]). For this work the more

general truncated Taylor series is used.

[cew([mcx]At)2 c ux~l3
R = e[~"~ = I +[mcux]At + + + (2-25)
2! 3!

In computing the translation vector T, recall that it is to be expressed in terms of the

camera frame of reference at the first of the two images. In general, the camera is

rotating as it is translating which means the reference frame for [uc,,'c, "c is rotating

as well. Defining z as the time from the first image of the pair during A t, an infinitesimal

T on this interval is:



dT = R(r) vwu dr= elsex, iv d

c c (2-26)

In carrying out the integration it is assumed that the values for [uc,,'c, "c as well as for

~coc are constant.


u + ) z v~u
=Ic [WXZIWV]r; &i ] ([me x]r)2


T elx3 d= (I+[mx + +-)d vc
o! o 2!

wqain w-5ad22 ieRadTntrso aearltv eoiis o
ch EFmaueetmdl hsene ob rte ntrso aicratrlt

velocitiesu (2-27)rskoldg fteorettonaddslaeeto tecm









reference frame with respect to the aircraft body reference frame. Let the orientation of

the camera frame with respect to the aircraft frame described by the usual ordered set of

Euler angles ryc,0,,( about the 3,2, 1 axes, respectively, and the corresponding rotation

matrix that transforms from the aircraft frame to the camera frame be denoted by Rbc.

Referring to Figure 2-3, these can be thought of as the camera gimbal angles. Providing

for a changing orientation, let the angular rate of the camera with respect to the aircraft be

defined in the aircraft frame aS Obc [Pbc > bc- > be I[b, jb kb]T This would represent the

camera slew rates. Let the displacement from the origin of the aircraft frame to that of

the camera frame be defined in the aircraft frame as dbe~ xc, bc.c, Zbc b > b > bT Again,

it is assumed that the values for rye, Sc, 4~,, mb, db are known at all times. The camera

relative velocities can now be written as the following:

1 0 0 cos 8, O sin Oc cos rye sin ry 0
Rbc = 0 cos ( sin ( 0 1 0 sin rye cos ry 0 (2-28)
0 sin (e cos (e sin On 0 cos On 0 0 1



cee RbcT Whb I ~ hbc h b bc I (2-29)
I c b Zbcb yb Zbc

where the dot denotes the time rate of change of the coordinate.



90, = RbcT r br I bc (2-30)
re r b I h Pebc

The expressions in 2-29 and 2-30 can be substituted into 2-25 and 2-27 to obtain R and T

in terms of aircraft motion variables which are the IEKF state variables.










At this point several simplifying assumptions will be made. The camera is

assumed to be mounted rigidly to the aircraft, i.e. the camera orientation and position

relative the aircraft frame is fixed so that y,4, 8,, db, are constant and ~cac is zero. Also,

the camera is assumed to be mounted at the aircraft center of mass so that dbe is zero.

Finally, the camera is mounted in the aircraft vertical plane at a fixed elevation

(depression) angle so that rye and (are zero. These assumptions simplify the

computations and reduce the potential for errors but do not detract from the overall

obj ectives and results of this effort. If at anytime it is desirable to remove any of these

assumptions, it would be a straight forward process to include the appropriate terms and

reformulate the implementation used here. With these assumptions and the appropriate

sub stituti ons :

0 -(pb Sin 8,+Oco 8b COS qb
[c), x] = (pb Sin 8 ,co 8b CO~) 0 -(pb COS@ 8,-b Sin 8c) (2-31)
-qb (Pb COS 8c b Sin 8c) 0


u =I Ubcs8 9 i (2-32)

we' ubSin8c+~11Sin8

From R and T, the essential matrix E can be calculated and the predicted pseudo

measurement from section 2.3 can be expressed in terms of the available state variable

estimates.




The measurement matrix, H, must also be expressed in terms of the aircraft state

variable estimates. As indicated in Equation 2-18 repeated below, determining H

requires taking the partial derivatives of E with respect to the aircraft state variables,









8(Ce) del
Hk C (2-18 repeated)


where xi [u v ,~, wb ,p, qb, r, ]T (2-34)

H is an Nx6 matrix with N corresponding to the number of feature points being tracked

during a given frame interval. Matrix C is the Nx9 matrix described earlier and de / Dx is

the 9x6 Jacobian matrix of partial derivatives of e with respect to x. Each of these six

columns is formed by stacking the columns of the partial derivative of the essential

matrix E with respect to a different aircraft state variable. This requires the partial of T

and R as indicated in Equation 2-35.

dE [ Tx ] dR\
R +[T x] (2-35)


These partial are approximated using only the first two terms of the series given in

Equations 2-25 and 2-27. The result is the following:

At cos 8,+At qe sin ,/ 2
dT
= A t Ie cos 8, / 2 At pe Sin 8, / 2 (2-36)
-At qe cos 8,/ 2 +Atsin B


dT
A B~c/B2c2t (2-37)


-At sin 8c+ABt qecos8, /2
dT
= -At 7e sin Be/ 2 -ABt pcos 8,/ 2 (2-38)
At qe sin 8,/ 2 +Atcos B

-At ve sin Be/ 2
dT
= -At2 O co~/ 2 +At2 usin / 2 (2-39)
At ve cos8, / 2










0TIBtW (2-40)
8qb
-At2U


-At2v co Cs/ 2
dT
= A t2W cSin e/ 2 +At2U cCOs e/2 (2-41)
br 2 c, Sin 8 /2


dR dR dR
= 0 (2-42)


0 -At sin 8, 0
dR
= A t sing 0, -At cos 8 (2-43)
O 0t tcos@ 0,


dR
=0 00 (2-44)
8qb
-At 0 0

0 -At cos( 0,
dR
= A t cos 8, 0 At sin 8 (2-45)
Br
b 0 -At sing 0,


8 [T x]
The terms in Equation 2-3 5 are formed in the usual way with the skew-symmetric


dT
form of the vectors ~. The velocity components in the expressions are intentionally


left in camera frame coordinates for compactness.

It is now necessary to impose the additional constraint on T discussed in section 2.2

due to the scale factor ambiguity. For this work the constraint will be that the magnitude

of the aircraft velocity, | Vb|, will remain constant during the state update. This means

that only two components of the velocity vector can vary independently. For this work vb









and wb are chosen to be the independent components and ub is chosen as the dependent

component. The reason for these choices will become apparent in a later chapter. This

additional constraint is then:

rvI 2 ~2 _l;)2 _)2)
ub 2 b b2(2-46)


aT
The partial affected by this are Equations 2-36 through 2-38. For this
cit ,vb> b

adjustment the partial derivative of ub with respect to vb and wb are required.



(2-47)



An assumption is made here that ub is positive. This should always be the case for

purposes of this effort using a fixed wing aircraft. If in a later application this is not the

case (e. g. for a helicopter or balloon) then the signs on the expressions in 2-47 will

reverse for a negative ub. Equation 2-37 now becomes:

dT dT dT c"it
-= + --


Iv bt II b
-At2 c / 2 At cos 8, + At2q cSin 8c / 2 (2-48)
At t2u co CO O / 2 At2 c Sin 8c / 2 vb( 2 2~ -
At2p c /2 -At2q co COS, / 2+ At sin B

Equation 2-38 becomes:









dT dT dT c'it
-=-+--
c"sl Es cit. 8 wb
-At sin 90+ABt2q cCOS8c / 2
= -At2r cSin c/ 2 -At2p cCOS8c / 2
At2q cSin 8c/ 2 +Atcos9 B


At cos8, +ABt2q cSin c/ 2
2r cCO 8 / 2 -At2P cSinc 8
-At2q cCOS8c / 2 +Atsin B


and Equation 2-36 becomes:

dTI
= 0 (2-50)
l~x= x

The remaining component of the measurement model is the expression for the NxN

pseudo measurement noise covariance matrix, R?, from Equations 2-19 and 2-20 repeated

below.


Rk = DkRkD,


(2-19 repeated)


h 8(Ce)
Dk T(2-20 repeated)
a[nU1v, puvp 2: N? ~U :.. N Nh N~'VL]

It is assumed that the value of the 4Nx4N covariance matrix, R, for the error of the

feature point measurements is known or can be approximated. The Nx4N matrix, D, can

be rewritten as:


8 (Cze)


0

c(8~p


0 ---z- 0
D= 8?(p27 2, p',V')


8 (Cze)



where each 1x4 component corresponding to a different feature point:


(2-51)


\blT 2 h2Y


(2-49)












= e

p' 0 0 u v,' 1 0 0 1 (2-52)



The assumption will be made that the measurement errors for each feature point location

have the same variance, 62 and are uncorrelated with each other which implies that:


R = = a2 4Nx4N (2-53)



R? can now be expressed as the diagonal matrix:

R? = DRDT = a2DDT

8 (Cze) 8 (Ce)

=1 2




(2-5 4)

At this point it might be useful to review the assumptions, approximations, and

simplifications that have been made in this section:

*The linear and angular velocities are constant during each measurement interval.

*The camera is mounted rigidly to the UAV at the center of mass.

*The camera frame of reference differs from that of the aircraft frame only by a
rotation of 8, about the number two axis, the camera elevation angle.

*In obtaining the measurement matrix, H, the partial derivatives of the essential
matrix, E, with respect to the state variables, x, are approximated by using only the
first two terms of the given series.










* The component of UAV velocity in the ib, direction, u1b, iS positive. (The UAV is
moving forward.)

* All of the focal plane measurement errors are characterized by the same variance
and they are all uncorrelated with each other.

It is felt that none of these seriously detract from the overall obj ectives and results of this

effort.

2.5 Estimation Algorithm Summary

This section provides an outline of how the concepts and expressions described in

this chapter are assembled to form the main loop of the recursive state estimator.

1. Propagate state estimate from last update using (2-9). The measurement model,
f(x,u), has yet to be defined.

2. Compute state transition matrix using (2-12).

3. Propagate state error covariance using (2-11).

4. Compute the essential matrix with (2-3) and (2-23 through 2-32).

5. Acquire the focal plane measurements from the vision processing module.

6. Form the predicted pseudo measurement using (2-5 and 2-6).

7. Compute the measurement matrix with (2-18) and (2-3 5 through 2-50).

8. Compute the measurement noise covariance with (2-52 through 2-54).

9. Compute the Kalman gain with (2-13).

10. Update the state using (2-15).

11. Update the state error covariance using (2-16).
















Feature
Point


Center of
Projection


Focal Plane


Figure 2-1. The perspective projection camera model.































t+A
Time t


Figure 2-2. Geometry of the coplanarity constraint.



















Camera Fixed
Frame (c)


ci~


Frame (b)


Figure 2-3. Measurement model geometry.















CHAPTER 3
APPLICATION WITH STATISTICAL DYNAMIC MODEL

This chapter describes a numerical simulation of the estimator using a simple

statistical dynamic model instead of the more complex UAV dynamics. The aim of this

simulation is to reproduce results similar to Soatto et al. [8, 12] and to gain basic insight

into the suitability and usefulness of the coplanarity constraint as an IEKF measurement.

For this simulation the camera simply moves around a circular path at constant speed

with the lens pointed toward the center, observing a cloud of feature points located inside

the circle. The camera rotates at constant rate so as to point in the same direction relative

to the traj ectory. The aim is slightly offset from the center in order to provide a velocity

along two of the camera axes. This arrangement is depicted in Figure 3-1. A description

of the simulation to include the dynamic model, the focal plane, the feature point

generation method, and the vision processing output is given. Results from the filter

simulation along the given traj ectory are examined.

3.1 Simulation Description

3.1.1 Statistical Model

The state vector is defined as in Equation 2-34 from the last chapter.


xk [, V, w,, p, q~ r]kT (3-1)

The subscripts on the components have been omitted. They will refer to body axes unless

stated otherwise.










The form of the dynamic model was given in the previous chapter in Equation 2-7

as xk k- k-1x,, 1) + wk-1. The statistical model used here and in Soatto et al. [12] is a

first order random walk.

xk =k-1 +k-1 k ~ N(0, Qk ) (3 -2)

The model assumes a random increment is added to each component of the state at each

step. This model is typically used when little is known about the dynamics of the

physical system. Referring to Equation 2-9, the state propagation is simply

xk k-1~ (3-3)

and the state error covariance propagation from Equation 2-11 is

Pk~ = Pk-1 +k-1 (3 -4)

The value of the process noise covariance, O, is used as a tuning factor in the Kalman

fi1ter implementation to compensate for system modeling uncertainties and nonlinearities

as well as disturbances.

3.1.2 Simulation of Focal Plane and Vision Processing Output

The measurement is the location of the proj section of each feature point on the focal

plane, x, = [pu,, v,, 1]T, as given in the last chapter and illustrated in Figure 3.2. The Eixed

position of each feature point in the environment is known as well as the position and

orientation of the camera along the actual or "truth" trajectory at any time. To simulate a

particular feature point measurement, the position vector, X, from the camera to the

feature point is obtained in earth Eixed coordinates, then transformed to body coordinates

using the appropriate rotation matrix transformations. An additional rotation

transformation is done to go from body axes to camera axes. In this simulation 8, the

camera elevation angle, is 90 degrees so that the camera is looking in the direction of the









velocity component u. This position vector in the camera frame is then normalized to the

third component, the ke component, which also normalizes it to the focal length,J; as

discussed in chapter 2. This produces the desired parameters CI and v for each feature

point. It is noted here that the estimator has no knowledge of the camera location and

orientation or the location of the feature points in space.

Measurement noise is simulated by adding a random white Gaussian component to

each CI and v from a random number generator. The focal plane is chosen to be 320 x 240

pixels (4:3 aspect ratio), a value that is typical throughout the literature for readily

available hardware. Pixel density is specified in order to relate measurement noise to a

physical characteristic. The standard deviation of the noise is defined for this work in

terms of pixel widths and is a key parameter that can be varied and is specified for any

simulation run.

3.2 Simulation Results

The IEKF and measurement simulation algorithms as described previously were

implemented in Matlab@. The truth trajectory for this simulation (computed apriori) was

a 10 second duration arc. The state vector for the trajectory was constant at x= [-12.48,

124.38, 0, 0, O -. 1]T with units of feet/second and radians/second. This combination of

velocity and angular rate results in the camera following a circular path of radius 1250

feet with the camera always pointed .1 radians to the left of the center of the circle, see

again Figure 3-1. (Since u was negative, the signs on equation 2-47 were reversed.) A

cloud of 20 randomly placed feature points was located at the center of the circle which

provided the measurements for the estimator. A Hield of view of 60 degrees horizontal

was chosen for the camera. The 4:3 aspect ratio resulted in a vertical Hield of view of










approximately 46.8 degrees. The measurement noise value input to the IEKF via the

variance matrix R in Equation 2.53 was the equivalent of one pixel standard deviation for

the 60 degree field of view or a = .0036. The update rate chosen for the fi1ter was 20

Hertz (hz). Based on preliminary runs using trial and error, a value of Q = diag[0 100

100 .1 .1 .1] was chosen for the process noise covariance and P = diag[0 100 100 .01 .01

.01] was chosen to initialize the state estimation error covariance.

3.2.1 Results for No Measurement Noise

An initial run was made with no measurement noise; that is, although the fi1ter ran

assuming noise equivalent to a = .0036, there was no noise actually being added to the

simulated focal plane measurements. Figure 3-3 depicts the location of the feature points

on the focal plane each time a measurement was taken during the 10 second run. This

shows how the feature points move on the focal plane.

The estimated states in the fi1ter were intentionally initialized with an error to allow

observation of the convergence of the fi1ter. The initial values were

x = [-12.48, 110, 5,.1,.1, 0] which equates to initial errors of 14.38 and 5 ft/sec for v and

w, respectively, and .1 rad/sec for p, q, and r. The results are shown in Figure 3-4.

Initializing the state u with no error in effect scales the other velocities. This is due

to several factors. Scale factor ambiguity, the choice of v and w as the independent

velocity variables, and the additional constraint that the magnitude of velocity is constant

over the update were discussed in the last chapter. In addition, with this simple model,

there is no interdependency among any of the state variables. The measurements shape

the direction of velocity through the coplanarity constraint while the u component

influences the magnitudes since it is not altered in either state propagation or the update.










Referring to Figure 3-4 it can be seen that the angular rate estimates converge

rapidly, within less that one second, to their true values. The velocity component

estimate w corrects almost as quickly to its true value of zero. The v component of

velocity is much more sluggish but appears to be slowly converging

3.2.2 Results in the Presence of Measurement Noise

For the next run everything remained the same except the equivalent of .03 pixel

width standard deviation of noise was added to the measurement. The results are shown

in Figure 3-5. The noise is evident in the plots of the state estimates however the values

do converge to the truth values as before.

Another run was made with the measurement noise increased to the equivalent of

.14 pixel standard deviation. Results are shown in Figure 3-6. Initial convergence for the

angular rates is still good however v appears to diverge from the beginning. After about

four seconds divergence can also be seen in r.

A final run was made with initially no measurement noise then at six seconds, noise

equivalent to .47 pixel standard deviation was abruptly added. The results for this are

shown in Figure 3-7. As expected from the first run, the filter starts out well but when

the noise is added at the six second point, the filter fails. A depiction of the focal plane is

shown in Figure 3-8 to indicate in a qualitative fashion the magnitude of the measurement

noise. The point at which the noise begins on the feature point traces can be observed if

closely compared to Figure 3-3. Intuitively, this amount of noise would not be

unreasonable, particularly on a 320x240 pixel focal plane.

3.3 Sensitivity of Algorithm to Measurement Noise

The failure of the filter with a reasonable amount of noise added to the focal plane

positions was unexpected as this had not been demonstrated in either of the sets of









simulation examples from Soatto et al. [8, 12]. However reference [7], the companion

paper to reference [8], briefly discusses that SFM becomes extremely sensitive to noise

with short baselines. Ma et al. [22] say that "one needs to exercise caution to make sure

that there is sufficient parallax for the algorithm to be well conditioned" in their chapter

Reconstruction from Two Calibrated Views which deals with using the coplanarity

constraint. Baseline and parallax refer to how much translation of the camera there is

between frames and how much feature point movement on the focal plane there is due to

this translation. However references [12] and [22] both indicate that when translation is

almost zero, it is the direction of velocity that becomes ill-conditioned, the angular rates

can still be computed. This seems to be apparent in Figure 3-6, at least during the initial

part of the run. Kehoe et al. had similar findings in their work [14, 15].

Increasing the baseline or parallax would obviously be desirable for state

estimation. Baseline can be extended in several ways. An obvious way is to increase the

time between measurements. The disadvantages to this are less measurement updates

and some degradation due to the assumption that linear and angular velocities are

constant over the update interval. Also, a longer interval between feature point

correlations on the focal plane from frame to frame (correspondence) makes it more

difficult for the feature point tracking algorithm [7]. Another avenue is to pick feature

points that move more on the focal plane in response to the anticipated direction of

translation. For example for motion in the direction that the camera is looking, feature

points on the periphery would have more movement than those in the center. A

disadvantage is that in this case, these feature points are in view for a much shorter period









of time. Orienting the focal plane parallel (or more parallel) to the direction of motion

where feasible should also improve the baseline.

To observe the effect of using a longer baseline, an additional run was made with

the update rate changed from 20 hz to 4 hz. The propagated state and error covariance

were still computed at 20 hz. Noise equivalent to .47 pixel standard deviation was

applied to the measurements from the beginning. The results are shown in Figure 3-9.

The filter no longer fails and appears to converge for all states except v, which may be

slowly converging. The r estimate has large variations but appears to be centered around

the correct value.

3.4 Observations from Application with Statistical Model

In this chapter implementation of the IKEF with the coplanarity constraint has been

demonstrated, basically paralleling the work of Soatto et al. [8, 12]. However the

extreme sensitivity of the algorithm to noise, particularly for the velocity estimates, was

not apparent in their examples. The simulations here have shown a lack of robustness in

the presence of noise that may affect the usefulness of this algorithm as applied to the

state estimation task in UAVs. The problem appears to be related to baseline length for

the measurements. Gurfil and Rotstein [16], referred to in chapter 1, mentioned similar

problems in the presence of noise in their work using the sub space constraint with aircraft

dynamics as the model. In the following chapters the method will be applied with a

specific model that includes the dynamics of the UAV.






















**


Figure 3-1. Trajectory for statistical dynamic model numerical simulation.










Xn

Feature
Point










Camera
Center of
Projection


Focal Plane


Figure 3-2. Focal plane measurement.




















0.4-













-0.2-


-0.1










-0.4-


-0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5







Figure 3-3. Feature point movement on focal plant for initial run with no measurement

noise.


0 05'
0 2 4 6 8 10




0 05

-0 1

0 15

0n -


Body Velocities


Body Angular Rates


01

0 05


1246


8 10


0


-r

-


2 1



Sec


2 4 6 8 10
Sec


Figure 3-4. Estimation results for initial run with no measurement noise.






















-13 -

0246810


125 ........ ....... ....... ..............

120






0246810


Body Angular Rates
O 15

0 1

0 05



-0 051- :
0246810

niE


SEstimated
..... Actual


0246810


iles

- Estimated


Body Velocit


-11


-12


0 005


R"~'~:~'~"~~'~~"'":~~"~~:~"""'


02 4


6 810


$2



-2 1
0246810
Sec


-0 15I


-0 2
02 4


Figure 3-5. Estimation results with .03 pixel measurement noise.



Body Velocities Body Angular Rates


0 1

0 05



-0 05
0246810

0 15

01

~005



-0 05 I
0246810


2 4


8 11


50


005r


10
0246810
Sec


02 4


Figure 3-6. Estimation results with .14 pixel measurement noise.


























150




1004 8


-.......1...1...~........~........ ...~...~



-I



-1
















--,



-OS5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 OS5


Body Velocities
-11 ,
SEstimated
-12~ ***** Actual


Body Angular Rates


10


O 2

01
O
0
-0 1

-0 2
0


-13


-14
0 2


2 4 6 8 10


0 1


Figure 3-7. Estimation results with .47 pixel measurement noise added at 6 seconds.


O.4


0.3




0.1




-0.1





-0.2


-0.3


nu




Figure 3-8. Focal plane for results with measurement noise added at 6 seconds.


-10~ 1- -0 2

-15 -0 3
0246810 0246810














Body Velocities


Body Angular Rates


0 1


- Estimated
***** Actual


-12


-0s 05 `
0246810


0246810


0 15

01- ~ ~



-0 005

0246810


'1101-


0246810

20







10
0246810
Sec


-02--- -



0246810
Sec


Figure 3-9. Estimation results with extended baseline, .47 pixel measurement noise.


----~- --~- --j---- ----















CHAPTER 4
APPLICATION WITH A UAV DYNAMIC MODEL

In this chapter a more complex dynamic model, that of a typical fixed-wing model

aircraft, is implemented in the estimator. It is shown that the additional information

provided by the dynamic model allows more states to be estimated than the five from

previous efforts that were demonstrated in the last chapter. Numerical results along

several trajectories are examined. A modification to the IEKF is implemented to

overcome deficiencies evident in the presence of measurement noise.

4.1 Dynamic Model Description

The UAV simulated is an eight foot wingspan class radio controlled model aircraft

pictured in Figure 4-1. The aircraft has four control inputs; ailerons (6a), rudder (6,),

elevator (6e), and throttle (JR). A high fidelity 12 state nonlinear model of the aircraft is

given in reference [23] which was derived from the general non-linear equations of

motion for a rigid body. Mass properties were measured on the actual model and

aerodynamic coefficients were measured in a wind tunnel. This high fidelity model was

coded and used as the truth model. A brief description of this model is given in appendix



For the filter model, it was desired to use something simpler than the truth model

and representative of what might be reasonably expected to be obtainable for a vehicle in

the field. The model chosen was the "perturbed state model" which is a linearization of

the truth model about a nominal steady state or "trimmed" flight condition. In this









technique the deviations of the original state variables and control inputs from the

trimmed conditions are the new perturbation variables and controls such that:

x = xss+ Ax
(4-1)


Here x is the familiar state vector, x,, is comprised of the constant steady state values, and

Ax is the perturbation state vector. The control inputs, u, are handled in a similar fashion.

This method is quite frequently used in aircraft control system development [24]. (It is

envisioned that, in practice, this model could also come from system identification

analysis of flight test data for the desired type UAV flown remotely by hand at the

trimmed condition since no high fidelity model is likely to exist.) Regardless of the type

and fidelity of the model used to propagate the state, recall from the previous chapters

that a linear representation is required to propagate the state error covariance.

The IEKF state vector was expanded to include two additional states; the aircraft

pitch attitude (0) and roll attitude (0). These are two of the three usual Euler angles that

describe the orientation of the aircraft body axes with respect to the local level inertial

frame with yr, 8, 0 being the ordered rotations about the 3,2,1 axes, respectively. This is

similar to the rotations described in Equation 2-28. The justification for adding these

states will be discussed later. The resulting IKEF state vector is now given as:

xi [u, v, w, p, q, r, 8, 95]" (4-2)

and the control vector is defined as:

uA [s,,, 4, ]' (4-3)



Su is used to denote both the scalar for the component of velocity along the body i axis and the vector of
controls to the UAV to conform to popular convention. Differentation can be made by context.









For this study, the case of no wind is assumed. The trimmed flight condition used

is straight and level at a velocity of 73.3 ft/s (50 mph) at an altitude of 7500 ft, standard

day. For this condition the steady state values computed from the truth model are

xss= [72.75, 0, 9.19, 0, 0, 0, .126, 0]' where units are ft/s, rad/s and rad as appropriate and

uss= [0, 0, -2.82, 1.1 1]' where units are degrees of deflection and horsepower. The

equations of motion from appendix A were linearized about this condition using the

methods given in Roskam [24]. This continuous representation was then converted to

discrete form by using well known techniques from state-space linear systems theory as

given in, for example, Brogan [25]. The interval chosen for the discrete form was .05

seconds as this is the minimum filter measurement interval expected to be used. (This

linearization and discretization process could have also been done using numerical

linearization routines from Matlab@ toolboxes.) The resulting linear discrete

perturbation model is given below:

Axk+1 = Ahxk + BLn (4-4)

where the perturbation state vector

Ax 4 [Au, Av, Aw, Ap, Aq, Ar, AO, Ag$]" (4-5)

the perturbation control vector

Au 4 [A6,, A(, A(, AS, ]T (4-6)

the system or state transition matrix










.988 0 .0309 0 -.371 0 -1.591 0
0 .954 0 .419 0 -3.48 0 1.58
-.0122 0 .791 0 2.71 0 -. 170 0
0 -.0213 0 .638 0 .0957 0 -.0187
A=.000644 0 -.000664 0 .829 0 -.000458 0 47
0 .00802 0 -.0119 0 .976 0 .00647
.0000166 0 -.0000179 0 .0456 0 1 0
0 -.000550 0 .0340 0 .00839 0 1

and the control matrix

0 0 .00289 .2782
.0999 .0236 0 0
0 0 -.0334 .0297
.412 .0197 0 0
B= 0 0 -.00926 .00008(48
.00217 -.00485 0 0
0 0 -.000239 .00000136
.0110 .000513 0 0


Those readers familiar with conventional aircraft dynamics will recognize that the

resulting equations decouple into two independent sets of dynamics known in the

literature as the "longitudinal" set and the "lateral-directional" set [25]. The linearized

equations of motion about this condition produce the conventional aircraft dynamic

modes. The longitudinal set is comprised of two oscillatory modes, the "phugoid" and

"short-period" governing motion in the vertical plane and about the pitch axis. The

lateral-directional set consists of an oscillatory mode, "Dutch-roll", and two periodic

modes, "roll" and "spiral" concerning motion in the horizontal plane and about the roll

and yaw axes. In the dynamics for this particular flight condition, the spiral mode has an

unstable pole at .076 (about 9 seconds time to double amplitude) indicating the aircraft









will tend to roll off into a tightening "spiral" if perturbed from trim with no correcting

control inputs.

For state propagation in the IEKF this linear representation, Equation 4-4, was

augmented as follows:

Au Ar v q~w
Av Ap~w Ar~u
Aw Aq~u p~v +g(cos 8f- 1)
Ap -Aq~r(Iz -I )/II
Ax =l = Ahxk + BLn + z t (4-9)
k+1 -~d~ xx zz yyZ
Ar -Apdq(Iw -Ixx)/IZZ
AO Aq(cosA8 -1i)- ArA



where Ii, II?, I zz = 1.7, 6.8, 9.3 slug-ft2 are the vehicle moments of inertia about the roll,

pitch, and yaw axes, respectively, g is the acceleration of gravity, and At is the integration

interval, .05 seconds in this case. These kinematic higher order non-linear terms increase

the accuracy of the IEKF model as it strays from its steady state condition and yet require

very little knowledge of the specific vehicle. The derivation is in Appendix B.

The following summarizes the steps in the IEKF used to propagate the aircraft

states and error covariance from k-1 to k:

* Obtain Axk-1 by subtracting xss from the previous updated estimate of xk-1 (Equation
4-1).

* Obtain An -.1by subtracting uss from the previous value of uk-1 (Equation 4-1).

* Compute the propagated value of Axk USing Equation 4-9.

* Obtain the propagated value of xk by adding Axk and x,,.

* Compute the propagated value of the error covariance using Equation 2-11 which
becomes P, = APk-1AT + Q where Q, the process noise matrix, is a constant for this
application.









It is assumed that the control input vector, u, which in general is not a constant, is known

to the filter with a high degree of accuracy. This is a reasonable assumption since these

commands are either sent to the aircraft or generated on board by an autopilot.

Therefore, control uncertainty does not enter into the error statistics computation. Also,

since the trimmed values of the states are known constants, it is only the uncertainty in

the perturbed states that affects the error covariance. Recall from chapter two that Q is

used to account for modeling errors and uncertainties as well as process noise and in

practice is a tuning factor in the filter.

4.2 Observability Considerations

In the last chapter it was demonstrated that the IEKF with the coplanarity constraint

was able to estimate, at least to some degree, five parameters related to camera motion,

angular rates and direction of velocity, using a very simple system model that provided

almost no information about the dynamics of the motion. The additional information

provided in the more complex UAV dynamic model allows the estimation of additional

states. Computing and examining the observability matrix from linear systems theory

(see reference [25]) gives insight into this.

For a linear system given by:

xk+1 = Axk + Buk
(4-10)
Zk+1 = k+1 k+1

where : is the measurement and H is the measurement matrix, the observability

matrix is given by:









H
HA
OAI HA2 (4-11)


HA"'

Where n is the number of states.

The system is fully observable if O has full rank, n.

For a cursory look at observability using the UAV dynamic model given in the last

section, assume that the five states v, w, p, q, r can be measured and that the measurement

equation is



01000000
00100000
zk+1 Hxk+1 = 0 0 0 1 0 0 011 0 (4-12)
00001000
00000100




Although the real, time varying measurement matrix, expressed in Equations 2-18, 2-5,

and 2-35 through2-50, is much more complex, this simplification is useful for a

preliminary assessment of observability based on the results of the last chapter. The

measurement matrix H from Equation 4-12 can be used with the UAV system matrix A

from Equation 4-7 to compute the observability matrix O using Equation 4-11. When

this is done, O has rank eight indicating that the system is completely observable. In fact,

if the measurement is reduced to just the angular velocities, i.e.













00010000
zk+1 Hxk+1 = 0 0 0 0 1 0 011 0 (4-13)
00000100





the resulting observability matrix has rank eight indicating that all eight states are still

observable.

A similar exercise can be done with a system matrix for the statistical model used

in the last chapter but augmented with the additional two Euler angle states. In this case

the system is described in Equation 4-13. (Note that some information has actually been

added by using the kinematic relationship between the angular rates and the Euler angles

for small angles over the .05 sec interval.)

100O O 0 0 00 u
0 10 0 0 000 v
00O1 0 0 000 Ow
0 0 0 1 0 0 0 0 p
xk~ = Ax = (4-14)
k+1 k 00 0 1 000 q


00 0 .050O100
0 0 0 .05 0 0 0 1_ q-

When the observability matrix is computed using the system matrix A above and the

measurement matrix H from Equation 4-12, the rank is five indicating incomplete

ob servability.

This section has shown that the information provided by the more complex

dynamic UAV model should result in increased observability of the aircraft states.









4.3 Simulation Description

The IEKF simulation described in the last chapter was modified for the UAV

dynamic model. The major modifications were to the system model in the fi1ter used to

propagate the state estimate and error covariance and the addition of the new states to the

filter, as discussed in the previous sections. The measurement matrix, Hk, WaS augmented

with zero columns to accommodate the new states. A provision was also made to add

new feature points along the way at random locations in the field of view to replace those

that dropped from view. The simulation runs were started with 15 to 20 feature points

with new ones added whenever the number dropped below ten. The field of view was

unchanged at 60 degrees horizontal and 47 degrees vertical, 320 x 240 pixels, and the

filter measurement noise covariance value for the Kalman gain computation was also left

unchanged at the equivalent of one pixel standard deviation, 0=.0036. The camera

elevation angle was set at 60 degrees (30 degrees down from the longitudinal axis of the

aircraft) so that the camera was looking "ahead and slightly down". The update rate for

the filter remained at 20 Hertz (hz). Based on preliminary runs using trial and error, a

value of Q = diag[0 0 0 .49 .0025 .49 0 0] was chosen for the process noise covariance

and P = diag[100 1 1 .01 .01 .01 .01 .01] was chosen to initialize the state estimation

error covariance.

Three 10 second truth traj ectories were used to assess filter performance. These

traj ectories were generated using the truth simulation (appendix A) driven by a simple

autopilot to achieve the desired flight paths. The data needed from these truth

simulations were the true values for the states to be estimated, the true aircraft position

and orientation values for feature point measurement generation, and the aircraft control

inputs. The first traj ectory was simply straight and level at the trim condition with the










trimmed controls. The second was a vertical S maneuver which started at the trimmed

flight condition with an initial pitch down command from the elevator (6e) then a pitch up

at 2.5 seconds followed by reversals at 2.5 second intervals. The third was a horizontal S

maneuver initiated from the trim flight condition which used elevator (6e), rudder (6r),

and aileron (6a) inputs to command horizontal tumns in alternate directions at 2.5 second

intervals. The throttle control (6T) remained constant at the trim setting for all

traj ectories. The truth values for each of these three traj ectories are included in the plots

for the next section.

4.4 Results With No Measurement Noise

Runs with the filter simulation were made using the three traj ectories with no noise

added to the focal plane measurements (although the filter still assumed that there was

noise equivalent to o--.0036). In each case the estimated states were intentionally

misinitialized at a value different from trim to aid in observing filter convergence. Filter

initialization values were ic = [67.75, .5, 9.69, 0, 0, 0, .126, 0] for all three cases which

equates to errors of 5, .5, and .5 ft/s for u, v, and w, respectively. These are within the

one sigma bounds for the initial state estimate error covariance, P, given earlier.

Results of the runs are shown in the figures at the end of the chapter. For each state

estimated, three values are shown; the actual or truth value, the estimated value from the

filter, and a propagated value. This propagated value is computed external to the filter,

open loop, using the same dynamic model that is in the filter, starting with the same

initial conditions and applying the same true control history. This is what the IEKF

would produce with no measurements. The purpose of this is to help assess how much

the measurements are contributing to filter performance.










Figure 4-2 shows the results for the straight and level trajectory. The estimated

states seem to be converging for all states and provide better response than the

propagated states in all cases except for perhaps w. The estimates for the angular

velocities appear to be particularly "tight". Also, the propagated value for the roll angle

phi is diverging as a result of the unstable spiral mode however the measurements keep

the estimated value in check.

Figure 4-3 shows the results for the straight and level traj ectory in a different way.

Here the just the errors (value truth) are plotted. Also included are the fi1ter estimates

for the one sigma error bounds (square root of the covariance) for each of the estimated

states. In this case these uncertainties are overly pessimistic because the expected

measurement noise is not present.

Figures 4-4 and 4-5 show the results for the vertical S traj ectory with no

measurement noise and Figures 4-6 and 4-7 show results for the horizontal S traj ectory.

The updated estimated states appear to generally be more accurate than the propagated

states although in some of the velocities for the horizontal S traj ectories the propagated

states are actually better for a portion of the trajectory. In all cases the angular rate

estimates appear to benefit significantly from the measurements and divergence in the

roll angle estimate is overcome.

4.5 Results With Measurement Noise

The three runs from the last section were repeated with Gaussian measurement

noise added to the feature point measurement locations on the focal plane using the same

procedure as with the statistical model simulations in chapter three. The equivalent of

one pixel standard deviation (o--.0036) was used for the noise which is the same value

used in the IEKF gain calculation. The estimated states were initialized with the same










errors as in the previous section. The three sets of error plots are shown in Figures 4-8

through 4-10.

The results are disappointing although not entirely unexpected given the results for

the statistical model with measurement noise shown in the last chapter. The estimates for

the velocities and Euler angles seem to develop extreme bias-like errors that are well

outside the expected bounds indicated by the predicted error covariances and

substantially larger than for the propagated states. The angular rate estimates appear to

be somewhat better. Although they are very noisy, the errors are approximately

contained within the predicted one sigma error bounds and the mean error is close to

zero.

In an attempt to ensure that the single set of filter initialization errors chosen was

not dominating the filter performance, a Monte Carlo ensemble of six runs was made for

the straight and level case. Random draws for initialization errors for each of the eight

states were made using a standard deviation corresponding to the initialization covariance

values in P. The results of this set of runs are shown in Figure 4-11 and indicate similar

results regardless of the filter initialization errors.

As discussed in the last chapter for the statistical model, these problems seem to be

a result of ill-conditioning due to short measurement baseline (lack of parallax). It was

demonstrated that lengthening the baseline by increasing the measurement interval would

mitigate the problem. To investigate whether that also applies with the UAV dynamic

model filter, the straight and level case was run with the measurement interval increased

from .05 sec to .2 sec. The results are shown in Figure 4-12.










With the longer baseline the state estimates do not diverge as they did with the

shorter baseline. However, the estimates are generally inferior to the propagated state

values and the usefulness of the estimates for vehicle control is doubtful. It is possible

that some returning of the fi1ter might improve the performance but a more robust remedy

for this problem is desired. To this end some possible weaknesses in the algorithm are

examined.

There are several factors in the implementation of the IEFK into the true EKF

framework that "stretch" the basic assumptions made in deriving the original Kalman

fi1ter. The EKF formulation assumes the measurement noise to be additive but this is not

really the case for the IEKF as indicated in the following equations from chapter 2.

Conventional EKF: zk hk() k k (2-8 repeated)

IEKF: Sk = C(UVU V7 Ik ek) (2-33 repeated)

In the IEKF the noisy measurements are mixed in a nonlinear fashion with the estimated

states to form the pseudo measurement. This could lead to higher order terms that are not

negligible or zero mean as assumed for the EKF. A second factor is that the noise in the

focal plane measurements is not truly "white" or uncorrelated over time as stipulated for

the EKF. The "current" feature point locations on the focal plane that are a part of the

current measurement, along with their errors, will also be a part of the next measurement

as the "last" location. Soatto et al. address this in reference [12] and conclude that for

their application, the small amount of correlation over just two steps has no discernable

effect. In most successful Kalman fi1ter applications, factors such as these are absorbed

into the process noise as part of the tuning process. For the IEKF implementation being

studied in this effort, the ill-conditioning experienced, particularly with the shorter









baselines, appears to accentuate these factors and leads to the type of errors observed

when a reasonable amount of measurement noise is present. In the following sections,

this ill-conditioning and its effects on filter performance are explored in more detail and a

robust solution is implemented.

4.6 Degree of Observability

In practice it has been noted that low observability of all or some states of a system

leads to high gains in a Kalman filter which is essentially ill-conditioning [26]. In section

4.2 it was shown that the eight states for the case at hand were observable in the absolute

sense but "how observable" or the degree of observability was not addressed.

4.6.1. Modal Observability

A measure for degree of modal observability is given in references [26] and [27]

which can be extended to the discrete case. For the linear system given earlier in this

chapter:

xk+1 = Axk + B21k
(4-10 repeated)
Zk+1 = k+1 k+1

If the system has distinct eigenvalues then it can be decomposed into its modal form with

a similarity transformation:

xk Mqk
qk+1 =M 'Al~qk +M 'B21k (4-1 5)
Zk+1 = Hk+1Mlqk+1

where MA [nz z,, nz ] is the matrix of the right eigenvectors of A and M-'AM~ is a

diagonal matrix with the eigenvalues on the diagonal. IfHk+1 is defined by rows as

Hk+1 [h,k+1, h,k+1,..--h;,k+1 T then a measure of the degree of observability is:









h *m
coso,,k+ B ,k+1 (4-16)
,,kl Ih ,k+1 Ill? i2 2


for the observability between measurement i and mode j. cos8o is a measure of the

orthogonality between h, and m,. A value approaching zero means little observability and

a value approaching one means high observability.

The value of this measure was computed for a run with the estimation filter on the

straight and level trajectory with no measurement noise. The results are shown in Figure

4-13 for a typical feature point. The larger values vary with different feature points but

the two lower values are very close to the same for all feature points observed. The

legend delineates the values with respect to the modes described earlier in the section on

the UAV model; short period, Phugoid, roll, Dutch roll, and spiral. For the oscillatory

modes, the value shown was computed for a single component in the complex conjugate

pair. The measure indicates low observability for both longitudinal modes, the short

period and Phugoid. This is an indicator that the filter can develop high gains and

therefore is susceptible to conditioning problems [26].

4.6.2 The Measurement Matrix

Examining the behavior of the values in the measurement matrix during a run

provides insight into the relative observability of the particular states and how that is

related to measurement noise. Recall from Equation 2.18 that the elements of the

measurement matrix, Hk, reflect how the pseudo measurements change with respect to

each state variable.

8(Ce) del
Hk C-- (2-18 repeated)










Figure 4. 14 depicts the actual values of the measurement matrix during an

estimation filter run for the straight and level traj ectory with no measurement noise.

Different feature points will have different values that change as the feature points move

on the focal plane. Depicted are the values for a single representative feature point that

form a single row of Hk. The legend delineates which state each value corresponds to.

The partial derivative terms with respect to u, 8, and 0 are always zero as explained in

chapter two and earlier in this chapter. In the lower plot the vertical scale has been

expanded. The units for the angular rates are "per rad/sec" and for the velocities are "per

ft/sec". Note that the values for the angular rates are much larger than for the velocities.

This indicates that the pseudo measurement, the coplanarity constraint, is much more a

function of angular rates than of velocity, at least for these circumstances. Also note that

the curves are smooth. It was demonstrated earlier that the filter performance was good

for this case.

Figure 4-15 shows the measurement matrix values obtained for the same feature

point when the equivalent of one pixel measurement noise is added. As demonstrated

earlier, the filter did not perform well. For this run the filter was modified so that the

state updates were not added to the state estimates. Also, the initialization errors were

removed. This resulted in the estimated states matching the true states, thus the

measurement matrix was calculated using error-free state estimates so any errors are due

solely to the measurement noise. Note that the curves now appear noisy, apparently due

to the measurement noise. The noise seems to have a minor effect on the relatively large

values for the angular rates which are still very close to those of Figure 4-14 percentage

wise. The values for the velocities, however, are extremely erratic and bear little










resemblance to those of Figure 4.14. (A smoothed mean value may approach the curve in

Figure 4. 14 but even that is doubtful.)

The poor performance of the filter in the presence of measurement noise seems to

be caused by two factors working together. First, the measurement matrix is a function

of the noisy measurements as well as the states. This is not the case in the standard EKF

as was pointed out in the last section. Second, the pseudo measurement has a relatively

weak dependence on the velocities (direction of velocity, actually). This allows the

measurement matrix and the computed gains related to the velocities to be corrupted by

the measurement noise to the extent that the resulting velocity estimates are worthless.

The errors also propagate through to the estimated Euler angles. The angular rate

estimates seem to be more robust (see Figure 4-1 1) due to a healthy dependence of the

pseudo measurement on angular rates.

Another way of looking at this is that the angular rate estimates are better

conditioned due to their greater observability making them more resistant to the

measurement errors. The velocity estimates are poorly conditioned due to weak

observability making them more vulnerable to the measurement errors. This ill

conditioning is worse with shorter baselines (due to slower UAV speed or a higher

measurement rate, for example).

4.6.3 Modification of the IEKF Measurement Matrix

Several factors suggest a possible solution to the problems experienced by the

IEKF in the presence of measurement noise.

*The noise appears to significantly corrupt the part of the measurement matrix
relating the pseudo measurement to the velocities (more properly, the direction of
velocity) .










* These components of the measurement matrix, the partial derivatives with respect
to v and w, are very small values.

* The UAV model is completely observable just from knowledge of the angular rates
(reference section 4.2).

* The updated angular rate states appear to still be fairly well behaved in the presence
of measurement noise.

The solution suggested is to neglect the components of the measurement matrix relating

to velocity altogether, that is, set these values to zero in Hk.

4.7 Results With the Modified IEKF With Measurement Noise

The modification to the IEKF described in the last section was made. The two

columns in Hk relating the pseudo measurement to v and w were set to zero. Runs on the

three traj ectories were repeated with the same conditions as in section 4.5. The results

are shown in Figures 4-16, 4-17, and 4-18. A comparison with Figures 4-8, 4-9,and 4-10

shows that estimated values from the modified IEKF are clearly better. The errors are

less and appear to be bounded for the most part by the filter predicted covariances. In

some cases the estimated states have slightly more error than the propagated states at

certain points in the traj ectory. The angular rates are especially noisy but the averages of

the estimated values appear to track better than the propagated values, particularly during

the maneuvering profiles. Further filtering of these state estimates might be beneficial

prior to use for autopilot feedback.

A series of six runs was made along each of the three traj ectories varying the initial

errors in a Monte Carlo fashion as was done previously for the straight and level case in

section 4.5. The results are shown in Figures 4.19, 4.20, and 4.21. These results also

show much better results after the modification to the IEKF. The angular rates are noisy

but all the states seem to converge. Some momentary deviations are noted during










maneuvers especially on the horizontal S which involves control inputs on all three

aircraft axes.

In this chapter, the IEKF using the coplanarity constraint has been applied to a

specific set of UAV dynamics. It was shown that the extra information provided by the

UAV model allowed the estimation of additional states over that of the simple statistical

model used in previous work in this area. Ill-conditioning problems in the velocity

updates were evident in the presence of measurement noise. These were overcome by

setting the small noisy velocity partial derivatives terms in the measurement matrix to

zero. Performance using this modified IEKF was demonstrated on three different

traj ectories.


Figure 4-1. Eight foot wingspan class aircraft simulated for this effort [23].
















Body Velocities
75 -





$765
0 2 46


Body Angular Rates
0 02



Actual
Propagated
-0 02
8 10 0 2 4 6 8 10


I_;o~U--~~Ltus~--tt~----~f~~Fm
3
~
~4~t~ttttCF~ e

II~ jli


0 2 4 6 8 10

10

~9 5- - -



0 2 4 6 8 10
Euler Angles


0 2 4 6 81

0 02





-0 02
0 2 4 6 81
Euler Angles


0 05 -0 01
0 2 4 6 80 0 2 4 68 0


Velocity Errors, Body Axes


Angular Rate Errors, Body Axes


0 -~~~---- Estl/maed0 -- -
-** "******* -- ---- Propagated -* s .
-10 2 4 t ******* Cov Es -02 2 4 6 8 10

4 0 1




-4 -0 1
0 2 4 8 0 0 2 4 6 8 1








O 2 4 6 8 1 0 2 4 6 8 1
Euler Angle Errors Euler Angle Errors


O1 --- ---r..,,l -I 0 -


~- 01 **--- --..* *-. """"i'" -0 1 ---*** 1-r
0 2 4 8 0 0 2 4 6 8 1


Figure 4-2. Straight and level trajectory simulation, no measurement noise.


Figure 4-3. Errors from straight and level trajectory simulation, no measurement noise.










59






Body Velocities Body Angular Rates



80- 002








,70 02



65 -0 02
0 2 4 6 8 10rpga 0 2 4 6 8 10

0 0
04 -



00 02
-0 0
0246810 0246810







10 02

*~~~~ L.....:*- -.............*----......... .. **..


6 0 02

0246u.U 0246810

001




002

0. 0



-0 21
0246810 0246810






Euler Angl ErrorsBd s Enuler RAnge Errors, oyA

0 1---*...J....... ..,_ 1-.....Ci

$O 0

-0-- 1rpgae -----+- --"" -0 1 --****** ...,.

0 2 4 6 8 1 0 2 4 6 8 10

Sec Sec




Figur 4-.Err rmvria rjcor iuain omaueetnie






















































































O 2 4 6 8 1


Body Velocities





60 I I I Propagated

0 26 00 Poaae





-10



0246810


Body Angular Rates

2t~ t~





024681C


2 4 6
Euler Angles


10 0 24 6


Euler Angles







0246810


02





-0 1
0246810


Figure 4-6. Horizontal S trajectory simulation, no measurement noise.


Velocity Errors, Body Axes


Angular Rate Errors, Body Axes


E r
~ O ~


3


0



0246810
Euler Angle Errors




-0 1


0246810
Sec


Euler Angle Errors







0246810
Sec


Figure 4-7. Errors from horizontal S trajectory simulation, no measurement noise.


O

















Velocity Errors, Body Axes


10



02

S1







20 -





02






022




O,


6 810


4








4


4 6
Euler Angle Errors


4 6
Euler Angle Errors


10


6 8 10


Figure 4-8. Errors from straight and level trajectory, one pixel measurement noise.




Velocity Errors, Body Axes Angular Rate Errors, Body Axes


810 0246810


0 05 - - -


8 10 0 2 4 6 8 10







-- os I I
8 10 0 2 4 6 8 10

01





1 -0 1
8 10 0 2 4 6 8 10
Suec nl Err


O4 t



2










04-2












-0 2


246I


-W --







Euler Angle Errors


0 2


Figure 4-9. Errors from vertical S trajectory, one pixel measurement noise.


Angular Rate Errors, Body Axes

02 ---L L L L




0246810








0246810


01 -




-0 1t
10 ` o










































































































EulerAngle Erors


Velocity Errors, Body Axes
10 Estimated
O ~- - -Propagated--
S-10 --Cov Est


0


4 6

Euler Angle Errors


10 0 2 4 6 8 10









10 0 2 4 6 8 10

Euler Angle Errors


10 0


2 4 6 8 10
See


See


Figure 4-10. Errors from horizontal S trajectory, one pixel measurement noise.


L-+--..ii .. '/11111. ... ..g
)111


"'~~""'~mrmr~mr~l Imlrl;l~:.~
'; ;"'*r'-~~lr~"""~n"""""'Li'L~-: '-i


Figure 4-11. Monte Carlo runs, straight and level trajectory with measurement noise.


Angular Rate Errors, Body Axes

O 2I





0246810


01 o



O






































































































0 20 40 60 80 100 120 140 160 180 200
Filter Step k




Figure 4-13. Modal observability for straight and level trajectory, no measurement noise.


Velocity Errors, Body Axes


Angular Rate Errors, Body Axes
02





-0 2
0246810


111~

O lo-rrrr 3 '" I
s
'"" "' "'~' "' ""'
In~~ e
""' ",,
,,


0246810









0246810

Euler Angle Errors


0246810








-02
0246810

Euler Angle Errors


0 1 C----- -- -...... -I 02 2 ~



S-0 1 ~---- r-*r "'' ----rr -r -- -- -02


0246810
Sec


0246810
Sec


Figure 4-12. Straight and level trajectory errors with measurement noise, increased

measurement interval.


Modal Observability for Feature Point 2

-SP
...Phu
- roll
----- DR
..... Spiral


0 6







































































-0 15
0 20 40 60 80 100 120 140 160 180 200



5 1
g 3
i!ier i
i 5 i i a r i
i 1 2 e e 'I i II
ii a i E I
:: jiie
1 ii
I i "
3 ii 11 I i
$ ii
I ii I ii
e :: ii ii n
$O B
5 i
I
c YR~tYII~ illil81111 U FI II \I~ II III


64












H Matnx Rowfor Feature Point 2


O 20 40 60 80 100 120 140 1( O 18 O0




x 10~


S20 40 60 80 100
Filter Step k


120 140 160 180 200


Figure 4-14. Measurement matrix values for straight and level, no noise.



H Matrix Row for Feature Point 2


0 20 40 60 80 100
Filter Step k


120 140 160 180 200


Figure 4-15. Measurement matrix values for straight and level, one pixel measurement
noise.


















Velocity Errors, Body Axes Angular Rate Errors, Body Axes



10



02



-2 . . - - -0 0
0246810 0246810




024681 0246810



0 15 -- --- -

0E 0

2 2-0 1.~!..-..-----*---- -----
--0
0246810 0246810













VelotyErrrsl BIodyAxs An ular Ratge ErrorsoyAe

0-- O
... ...4. -I Propagatedl...........




:* 0

-2 m-***' .m.... ************ ---- ** *- -. 0 0 -* "










-0 1 *---*-***"*'-0 1 *
0246810 0246810
Sec Sec






Figure 4-17. Errors for modified IEKF, vertichtald Se trajectory with measurement nie

















Angular Rate Errors, Body Axes
02





-0 2
0 2 4 6 8 1

01


0


-010 261




0246810

01 -






0246810





Sec


O 1


)24 6 810









)24 6 810
Euler Angle Errors


0246810

1






0246810
Euler Angle Errors


Velocity Errors, Body Axes
10 .. ,




.. ... --- Propagated
-10 ****CvEt




0246810













0246810


Sec


Figure 4-18. Errors for modified IEKF, horizontal S trajectory with measurement noise.


Velocity Errors, Body Axes







4 ******* Cv st 0


Angular Rate Errors, Body Axes







0 2 4 6 8 1


a


i~x?-01


0 2


10 0 2


Figure 4-19. Modified IEKF Monte Carlo, straight and level with measurement noise.

















Angular Rate Errors, Body Axes
-j02



rror -0 2
ov Est0 2 4 61 8 0


01


Velocity Errors, Body Axes


111_


'III E

0 2 4 6******


0246810





Euler Angle Errors


O



-010 261




0246810





Euler Angle Errors


-0 2

0246810 0246810


Figure 4-20. Modified IEKF Monte Carlo, vertical S with measurement noise.


Velocity Errors, Body Axes


Angular Rate Errors, Body Axes


1


-20 ---i--****Cv s

0246 024


-5 -0 1
0 2 4 6 8 10 0 2 4 6 8 10



2I 02




020


-0 2 -0 I

0 2 4 6 8 10 0 2 4 6 8 10


Figure 4-21. Modified IEKF Monte Carlo, horizontal S with measurement noise.















CHAPTER 5
ROBUSTNESS AND SENSITIVITY ASSESSMENT

For practical use, the modified IEKF developed in the last chapter must be tolerant

of inaccuracies in the system model that is used in the fi1ter. These inaccuracies are

unavoidable and come from simplifieations in the governing equations and from

uncertainties in parameters such as UAV aerodynamics, mass properties, and trim

conditions. Some of these parameters may vary from flight to flight for a given set of

hardware. In this chapter the robustness of the modified IEKF fi1ter developed in the last

chapter is assessed using a Monte Carlo analysis. It is also desirable to gain some insight

into how some other parameters that are controllable by the designer affect performance.

These are items such as camera depression angle, camera Hield of view, feature point

selection, etc. Filter performance sensitivity to some of these parameters is examined

using the same Monte Carlo framework. Finally, the effect of varying amounts of

measurement noise on fi1ter performance is investigated.

5.1 Robustness to Modeling Uncertainties

Robustness of the IEKF to uncertainties in the system model was assessed using

Monte Carlo runs on the same truth traj ectories used in the last chapter. An attempt was

made to choose the Monte Carlo variables and their bounds in a realistic fashion. (These

are constants in the system model but are changed for each individual run using draws

from a random number generator.) Five groups of parameters were chosen to vary.

*Steady-state or trimmed flight condition. The UAV might be trimmed at a
condition that is not exactly what is in the filter model. The values given in chapter
four were used as the mean with standard deviations of 5 ft/s for u,,, 1 ft/s for v,,










and w,,, and .02 radians for 8ss and #ss. The steady-state angular rates were not
varied as it was assumed the UAV would always be trimmed to fly in a straight
path.

* Steady-state or trimmed controls. The trimmed flight control settings might not be
precisely known or might be offset in the UAV controls. The values given in
chapter four were used as the mean with standard deviations of .1 deg for
deflections and .1 horsepower for throttle.

* Aerodynamic parameters. The aerodynamics of the system model are contained in
the A and B matrices given in Equations 4-7 and 4-8. Aerodynamic coefficients
are present in the terms in the upper left 6x6 partition of the A matrix and the upper
left 6x3 partition of the B matrix. (The other terms in these matrices have to do
with kinematics, gravity, and thrust.) This is shown in Roskam [24]. The
aerodynamic uncertainties were modeled by varying these parameters individually
by a five percent standard deviation around the nominal. This was done with the
continuous form of the matrices and then the effect over .05 seconds was added to
the discrete matrices in Equations 4-7 and 4-8. This was an approximation since all
the terms varied are not strictly "only aerodynamic: but their maj or effects are.

* Mass properties. There are likely to be uncertainties in the UAV mass and
moments of inertia. These uncertainties were modeled similar to aerodynamic
uncertainties by varying the A and B matrices. The first three rows in these
matrices come from "acceleration equals force divided by mass" equations so these
terms in the matrices were varied together corresponding to an approximate five
percent standard deviation variation in mass about the nominal value. Again, this
was done with the continuous form of the matrices and then the effect over .05
seconds was added to the values in Equations 4-7 and 4-8. The moment equations
were handled in a similar fashion. The fourth row was varied for a five percent
standard deviation in roll moment of inertia, the fifth row for pitch inertia, and the
sixth row for yaw inertia. The higher order terms in Equation 4-9 were not varied
from the nominal moment of inertial values.

* Longitudinal center of mass location. There is likely to be uncertainty in the center
of mass location for the UAV and it may also vary if components are shifted from
flight to flight. An allowance for an uncertainty of .1 foot standard deviation along
the longitudinal axis of the vehicle from the nominal location was provided. This
was done again using the A and B matrices. The pitch and yaw moment equations,
rows five and six, were changed together by adjusting each moment component by
an amount determined using the change in center of mass location and the
appropriate term from the force equations. For example the pitching moment due
to w term, A(5,3), was adjusted using the normal force due to w term, A(3,3),
through a lever arm of the change in center of mass and rescaled with mass divided
by pitch moment of inertia. Again, this was done using the continuous equations
first.










In all, 39 Monte Carlo parameters were chosen. This list is certainly not all

inclusive. For instance camera inaccuracies and atmospheric turbulence are not included.

However this assortment should allow enough perturbation from the baseline conditions

to assess robustness. To provide insight into the effects of the different types of

uncertainties, seven cases were run against the three trajectories for a total of 21 sets.

Each set consisted of 50 runs. The cases are described below.

* Case 1. None of the model parameters were varied. The initial state estimates
were varied and measurement noise was applied as in the Monte Carlo runs in
chapter 4.

* Case 2. Same as case 1 plus the steady-state flight condition Monte Carlo
parameters.

* Case 3. Same as case 1 plus the steady-state control Monte Carlo parameters.

* Case 4. Same as case 1 plus the aerodynamic Monte Carlo parameters.

* Case 5. Same as case 1 plus the mass properties Monte Carlo parameters.

* Case 6. Same as case 1 plus the longitudinal center of mass Monte Carlo
parameter.

* Case 7. Case 1 plus all 39 Monte Carlo parameters at once.

The modified IEKF was used with the same setup as described in section 4.3 with

regards to field of view, camera depression angle, process and measurement noise

parameters, focal plane, measurement interval, and feature points. Actual measurement

noise was one pixel standard deviation, o =.0036, for all runs.

A suitable metric was needed to characterize each Monte Carlo set. In Figure 5-1

the estimation errors for a set of six runs are shown for case 1 on the straight and level

trajectory to demonstrate the process. The error plots are informative for six runs,

however when 50 runs are made the overlaid plots would be useless.










For the 50 run sets, or ensembles, the mean error and its standard deviation about

the mean are computed at each .05 second interval. Plots of this are shown for case one

on all three trajectories in Figures 5-2, 5-3, and 5-4. Mean values close to zero and small

standard deviations are desirable. There is some trending in the mean during the

maneuvers on the vertical and horizontal trajectories but no divergence. The metric

chosen to characterize each set was the average of the error means and standard

deviations over the last Hyve seconds. This was to allow the fi1ter a chance to converge

from the erroneous initial conditions and also to average out the effects of the traj ectory

maneuvers (since the maneuvers were not random). Results for the 21 sets are tabulated

in Tables 5-1, 5-2, and 5-3. In order to make the results more readable, bar graphs of the

results are plotted in Figures 5-5, 5-6, and 5-7. Note that the absolute values of the mean

instead of the actual values from the table are plotted.

An additional finding is that none of the 1050 runs had divergences, that is, errors

increasing without bound. Overall, the results indicate that the modified fi1ter is robust to

model inaccuracies. The angular rates are very insensitive to model variations. This is

because of the high observability of the angular rates in the measurement matrix as

discussed in the last chapter. Deviations in the trim conditions, case two for the state

variables and case three for controls, seemed to have more effect than the errors in

aerodynamics, mass properties, and center of mass location, cases four, fiye, and six. The

states that seem the most vulnerable are u and 8. These are longitudinal states. Recall

from section 4.6.1 that these modes were predicted to have low observability. Notice

from the tabulated values of the means the correlation in signs. When u error is positive,

a error is negative. This is inherent in the dynamics of the model since the UAV tends to










slow down as pitch increases and speed up as pitch decreases. The results for case seven

indicate good robustness to all the error sources applied at once.

5.2 Sensivity to Design Parameters

Certain parameters in the implementation of the estimation scheme are chosen by

the designer. Sensitivity of the filter performance to some of these parameters was

assessed. The parameters examined were camera depression angle, camera field of view,

filter measurement interval, and number and location of feature points.

5.2.1 Camera Depression Angle

Recall that the camera angle is referenced to the UAV vertical body axis, i.e. Oc=0

when aligned with the body z axis (or approximately down when the UAV is at trim).

Three camera angles were chosen for evaluation, Oc=0 degrees, Oc=60 degrees (the

baseline configuration), and Oc=82.8 degrees which at trim conditions correspond to 82.8,

22.8, and 0 degrees down from the horizon, respectively. To demonstrate the effect this

has on feature point movement, three focal plane histories with the different camera

angles are shown in Figure 5-8 for the straight and level trajectory with no measurement

noise.

Fifty run Monte Carlo sets were performed for each of the three camera angles on

each of the three traj ectories for a total of nine sets. The only Monte Carlo variables used

were the initial state variables and the measurement noise which corresponds to case one

in the last section. The same metrics were computed as in the last section. The results

are presented in similar fashion in Table 5-4 and Figures 5-9, 5-10, and 5-11.

The angular rates show a consistent trend for all three traj ectories. The estimation

for yaw rate, r, is worse when the camera is looking down and best with the camera on

the horizon. Estimation for roll rate, p, is best when the camera is looking down. Both









roll and pitch rate estimation, p and q, are a little worse at the intermediate camera angle.

Side velocity, v, follows the same trend as r. The other variables do not seem to have a

consistent trend through out all three traj ectories. None of the state estimates diverged

during any of the 450 runs. Overall, the parameter of camera depression angle appears to

have a definite but not overpowering effect on fi1ter estimation performance for this

application.

5.2.2 Camera Field of View

The focal plane specifies remain the same at 320 x 240 pixels with aspect ratio 4:3.

Recall that the baseline Hield of view is 60 degrees horizontal by approximately 47

degrees vertical. The design parameter to be varied here is Hield of view while using the

same focal plane. This is equivalent to changing the focal length of the lens used in the

camera. When Hield of view is narrowed with the same focal plane, an increase in

resolution is gained which should translate into more accurate feature point coordinates.

For the for the focal plane simulation algorithm used in this effort, the measurements are

normalized to the focal length as explained in chapters two and three. The increased

resolution is realized by reducing the measurement noise standard deviation to keep it at

the equivalent of one pixel. The opposite is done when the field of view is widened.

For this assessment, three Hields of view were chosen; 30 degrees, 60 degrees (the

baseline), and 90 degrees horizontal. The measurement noise applied had standard

deviations of .001675, .0036, and .00625, respectively, to keep the noise at the equivalent

of one pixel. The filter assumed value for measurement noise remained unchanged. As

in the previous section, 50 run Monte Carlo sets were done for each field of view on each

of the three traj ectories for a total of nine sets. Monte Carlo variables remained the same










and Oc was set back to the baseline value of 60 degrees. The results are presented in

previous fashion in Table 5-5 and Figures 5-12, 5-13 and 5-14.

Consistent trends noted over the three traj ectories are that the pitch and yaw rate

errors appear smaller for the narrower fields of view but the roll angle errors are smaller

at the wider fields of view. Over all, the results seem to show surprisingly little variation

in performance as the field of view varies.

5.2.3 Measurement Interval

The effect of increasing the measurement interval was investigated. This was

explored in chapters three and four with the unmodified filter as a way of increasing

feature point parallax to help with ill-conditioning. Three measurement intervals were

assessed; the baseline value of .05 seconds, .25 seconds, and .5 seconds. The Monte

Carlo variables and filter configuration remained the same with the field of view at 60

degrees. Three sets of 50 Monte Carlo runs were done on each of the traj ectories for a

total of nine sets. To see the effect of increased sample interval, a time history of

statistics from the .5 second measurement interval set of runs on the straight and level

traj ectory is shown in Figure 5-15. The "saw tooth" effect on the one sigma bounds for

the angular rates is typical behavior as the uncertainty grows between updates. This

cyclic behavior propagates through to v and w. The overall results for the nine sets are

presented in previous fashion in Table 5-6 and Figures 5-16, 5-17, and 5-18.

The results for the straight and level and vertical S cases are unexpected at first

glance. The overall performance of the filter as indicated by the error standard deviations

seems to improve with less frequent updates for most states which is counterintuitive.

More measurements mean more information which should result in better estimates.

However, for the horizontal S case, the performance shows an overall deterioration with









increased update interval which would be expected. These results are not fully

understood but a plausible explanation has to do with filter tuning which, as explained in

chapter three, is the selection of the process noise covariance matrix, Q. Q is what drives

the estimated filter uncertainty, P, through the system model (Equation 2-11) to offset

modeling uncertainties and non-linearities as well as disturbances. The Kalman gain

matrix, k, is determined based on P and the measurement noise (Equations 2-13 and

2.21). It can be thought of as a weighting on the measurement residual based on the

estimated uncertainty in the current estimate as compared to the assumed noise in the

pseudo-measurement. A high uncertainty in the estimate and a relatively low

measurement noise leads to larger gains and updates while the converse leads to smaller

gains and updates. For k to be optimum, accurate values of process and measurement

noise must be used. Q and the feature point measurement noise, R, were constant for

these sets. It can be argued that the "real" uncertainty is highest for the horizontal S

profile since the UAV is maneuvering the most and therefore further from the trim

conditions on which the model is based. If the filter is tuned for the maneuvering case,

then it may be mistuned for the non-maneuvering case resulting in gains that are proper

for the maneuvering case but too high for the non-maneuvering case. This might explain

why the results make sense for the horizontal S case and not for the other two. Also note

that for the single state variable 0, the trends are correct for all three traj ectories (that is,

the estimate gets worse as measurement interval increases). Recall that this is the

variable that most reflects the one unstable mode in the process model. It requires

updates to prevent divergence and might not be as sensitive to the higher gains in the in

the non-maneuvering case.









5.2.4 Number of Feature Points

More feature points result in more measurements at each update which should, as a

general rule, increase the fi1ter performance. Non-fi1ter based methods employing the

coplanarity constraint require a minimum of eight points or Hyve points [5, 12, 19] to

obtain a solution. As mentioned in chapter one, an advantage of this fi1ter based method

is that it will provide estimates that gracefully degrade when feature point measurements

are reduced or even lost completely for a time. Soatto reported in his work that filter

performance tended to "saturate", that is reach a point of limited improvement, after

about 20 feature points [12].

The effect of varying the number of feature points used for the measurements was

investigated. Runs were made using fiye, ten, twenty, and forty feature points. The

Monte Carlo variables and fi1ter configuration remained the same. The 60 degree field of

view and .05 second measurement interval from the baseline configuration were used.

Each run was started with the desired number of feature points. As feature points

dropped from view, new ones were generated at random positions. At some points in the

runs, the number of feature points dipped below the desired value momentarily because

only one new feature point was allowed to be added at each measurement. This was true

more for the maneuvering profiles at the higher numbers of feature points which tended

to drop points more rapidly. Four sets of 50 Monte Carlo runs were done on each of the

trajectories for a total of 12 sets. The results are presented in previous fashion in Table

5-7 and Figures 5-19, 5-20, and 5-21.

The performance of the filter appears to improve with the increase in feature points,

at least for the lateral-directional states, v, p, r, and 0. The accuracy of the longitudinal










state estimates does not seem to be affected as much. The saturation in performance

mentioned earlier does not seem to occur, that is, performance improvement does not

seem to be leveling off with 40 feature points. None of the estimates diverged during any

of the 600 runs.

5.2.5 Feature Point Location

Insight into the observability of p, q, and r with respect to feature point location can

be obtained by examining the values in measurement matrix H for different feature point

locations. Figure 5-22 shows feature point traces on the focal plane for several feature

points for a filter run on the straight and level traj ectory with no measurement noise. The

triangle at the top of the focal plane depiction in Figure 5-22 is the point from which all

the feature points appear to emanate or be moving away from which is constant during

this run since the vehicle is in a trimmed, straight, steady state flight condition. This

point is known as the focus of expansion (FOE) in optic flow literature [14, 15]. The

FOE also depicts the direction of translational motion of the vehicle. The position of the

feature point with respect to the FOE is what seems to be relevant. Also shown in Figure

5-22 is a plot of the changing values in H that relate the pseudo-measurements from a

specific feature point to the state variables p, q, and r. (Recall that the components ofH

are the partial derivatives of the coplanarity constraint for each feature point with respect

to the state variables and that these are set to zero for all states except p, q, and r.) The

Hyve numbered segments on the plot correspond to the Hyve numbered feature point traces

on the focal plane depiction. A larger magnitude of h indicates a greater observability of

the particular state variable for that feature point location. Figure 5-22 seems to indicate

the following:










* q (pitch rate) observability increases as the magnitude of the horizontal distance, v,
from the FOE increases. Segments 2, 4, and 5 have smaller absolute values for h
and the corresponding feature points are closer horizontally to the FOE than for 1
and 3.

* r (yaw rate) observability increases as the magnitude of the vertical distance, pu,
from the FOE increases. Segments 3 and 5 have smaller absolute values for h and
the corresponding feature points are closer vertically to the FOE than for 1,2, and 4.

* p (roll rate) observability increases as the overall distance from the FOE increases.
Segments 4 and 5 have smaller absolute values for h and the corresponding feature
points are closer the the FOE than for 1, 2, and 3.

It should be noted that for the run shown, the standard set-up was used so the FOE was

not aligned exactly with the body axis system used to define the angular rates. However

it is only off in pitch by the angle of attack of the vehicle, approximately seven degrees,

which should not affect the validity of the above subj ective observations.

An attempt was made to assess how feature point location on the focal plane affects

filter performance. It seems to be a reasonable assumption that filter performance would

be related to feature point location in accordance with the preceding discussion on

observability. To investigate this, Monte Carlo runs were made with restrictions on the

focal plane as to location of feature points. Four configurations were chosen as

illustrated in Figure 5-23; the full 60 degree field of view, a vertical strip centered on the

FOE of width .2, a horizontal strip from top of the field of view at pu=.433 to pu=.233

(including the FOE at pu=.42), and a restricted box around the FOE with CI of .433 and .1

and v +/- .2. The Monte Carlo variables and filter configuration remained the same as in

the previous section and runs were made using all three trajectories. The runs were

started with approximately 10-15 feature points. As feature points dropped from view,

new ones were generated at random positions in the selected area of the focal plane to try

to maintain 10 points in view. At some points in the runs, the number dipped below 10










during the maneuvering profies, particularly with the restricted Hields of view. Four sets

of 50 Monte Carlo runs were done on each of the traj ectories for a total of 12 sets. The

results are presented in previous fashion in Table 5-8 and Figures 5-24, 5-25, and 5-26.

The results are mixed. For roll rate, p, performance is worse on all traj ectories for

feature points restricted to close to the FOE. Roll angle, 0, is also worse for this case.

This is what was predicted in the earlier discussion. For pitch rate, q, the poorest

performance was expected for the vertical strip case. This was true on the horizontal S

traj ectory but not on the other two traj ectories. For yaw rate, r, performance actually

seemed better on two of three traj ectories for the horizontal strip case which should have

been the worst. Filter performance for the other state variables did not seem to show any

discernable trends with regards to feature point location. One possible problem in this

assessment is that the FOE moves around on the focal plane for the maneuvering

traj ectories which could not be taken into account.

5.3 Effect of Measurement Noise

The effect of measurement noise, that is the error in the feature point locations on

the focal plane provided to the fi1ter, was investigated by varying the standard deviation

of the random noise that was added to the computed feature point locations in the Monte

Carlo simulations. For all the previous runs in this chapter, the standard deviation used

has been the equivalent of one pixel. For these runs, standard deviations equivalent to

.25, .5, 1, and 1.5 pixels were used. Otherwise the baseline parameters were used, i.e. the

full focal plane at 60 degrees field of view with a measurement interval of .05 seconds, Oc

of 60 degrees, and approximately 10 feature points. The value of measurement noise

used by the fi1ter for computing the Kalman gains was left at the equivalent of one pixel










for all runs. This simulates the case where the filter assumes a value for measurement

noise but does not really "know" what the noise is. Four sets of 50 Monte Carlo runs

were made for the four values of measurement noise on each of the three traj ectories for a

total of 12 sets. The results are presented in previous fashion in Table 5-9 and Figures

5-27, 5-28, and 5-29.

The results are generally as expected. The fi1ter performance gets progressively

worse as the noise level is increased. This is true for p, q, r, v, and 0 on all three

trajectories. The states u, w, and a do not seem to be as affected by the noise, perhaps

because these states are not as observable and the noise tends to be filtered out. This is

similar is to the results noted for the sensitivity to number of feature points. None of the

estimates diverged during any of the 600 runs.

5.4 Overall Comments on Robustness and Sensitivity

The fi1ter design seems to be robust and very tolerant of errors in the UAV process

model. Monte Carlo results indicated that the fi1ter was tolerant of modeling errors due

to misinitialization, measurement noise, incorrect trimmed condition and controls,

aerodynamic parameters, and mass properties. No filter divergences were encountered

during the 1050 runs. The filter seemed most sensitive to errors in the trimmed states and

controls.

The effects of variation of several parameters in filter and focal plane

implementation on filter performance were assessed. Parameters examined were camera

depression angle, field of view, measurement interval, and number and location of feature

points. The Monte Carlo results indicated less than expected sensitivity to these factors.

Number of feature points (the more, the better) seemed to have the most consistent










results. Also, u, w, and a seemed to be least affected during all these assessments.

Again, no filter divergences were noted. Increasing the measurement noise level had the

expected detrimental effect on filter performance.

Overall, the filter showed a great degree of robustness to errors and inaccuracies in

the system model and variations in filter implementation. However the effects of the

various changes in filter design parameters were difficult to differentiate, at least by the

Monte Carlo methods and metrics used in this chapter. This could be in part due to the

stable nature of the particular UAV model for which the filter was implemented. Another

factor could be that the large amount of noise that feeds through to the angular rate

estimates masks some of the trends that might be present.

















Velocity Errors, Body Axes


Angular Rate Errors, Body Axes
05



0

10 0 2 4 6 8 10


-02
0246810



0"




-0 50 2 4 6 8 1

Euler Angle Errors


I li l 1

0246810







-20 0

Euler Angle Errors


-0 2
10 0 2


8 10


Figure 5-1. Six run Monte Carlo set errors, case 1, straight and level trajectory.


Velocity Errors, Body Axes
20

10 .. -- -|- -- ******** One Sgma Bounds



-10
0246810





2-2~~~~~ I



0 2 4 6 10









0246810
Euler Angle Errors
01

0



-0 1 --- -
0246810
Sec


Angular Rate Errors, Body Axes


0 05





-0 05"" i-i;
0 2 4 6 8 1


02~


Euler Angle Errors


Figure 5-2. Case 1 straight and level trajectory, ensemble mean error and bounds.









83





Velocity Errors, Body Axes Angular Rate Errors, Body Axes


1 0 t -*. ,,,,,, e Sigma Bounds


"-10 -0 2aS
0246810 0246810

0 05



-2 -04-1- 05




-1 -0 05
0246810 0246810

01 **01


,0 0
.....---** ...-
-0 1 -- -0 1
0246810 0246810






V uelocity e ErrorsBd xs An ular Ratge Errors, oyAe
010 ..-- ..
00


0 2u 0 261




--0 0


0- 02

0246810 0246810





Sec Sec





Figure 5-4. Case 1 horizonal S trajectory, ensemble mean error and bounds.





Table 5-2. Vertical S trajectory Monte Carlo sets.
Average of mean error
Case u v w p q r 8
ft/s ft/s ft/s rad/s rad/s rad/s rad rad
1 1.421 0.0381 -0.14139 0.000192 0.000838 7.5:E-05 -0.01822 0.001268
2 2.5879 0.14814 -0.31278 0.000468 0.00135 0.000618 -0.02446 -0.0005
3 1.516 -0.09106 -0.15493 8.04E-05 0.000649 0.000357 -0.02015 -0.0023
4 1.1249 -0.01991 -0.13966 -0.00076 0.000421 -0.00057 -0.01526 -0.0006
5 1.105 -0.01391 -0.12363 3 25E-05 0.000372 0.000244 -0.01609 -0.00065
6 1.4917 -0.01935 -0.13931 -B.7:EE-05 0.001124 -0.00011 -0.023 -0.00053
7 3.1734 0.17221 -0.37537 0.000698 0.0006 0.000596 -0.0265 0.007965
Average of standard deviation of error
1 4.2084 0.45093 0.27757 0.11863 0.026223 0.11071 0.043465 0.010757
2 7.0845 1.1267 1.184 0.11797 0.02647 0.10963 0.054424 0.019862
3 4.8097 2.7291 0.32119 0.11952 0.026548 0.1116 0.051041 0.070892
4 3.9335 0.43276 0.2939 0.12023 0.025823 0.11124 0.042417 0.00977
5 4.4422 0143519 0128523 0.11898 01026631 0.11121 01045047 01010223
6 4.8925 0.44398 0.30731 0.12032 0.026206 0.11341 0.050393 0.009408
7 7.6246 2.8276 1.0144 C'.1212 C'.026668 111124 C'.058297 C'.065056


84





Table 5-1. Straight and level traj ectory Monte Carlo sets.


Average of mean error
w p q r
ft/s rad/s rad/s rad/s rad rad
0.018962 -0.00013 0.000682 -3.37E-05 -0.00011 -0.00063
0.025485 -0.00013 0.000238 -0.00016 0.018332 -0.00349
0.057816 -0.00033 0.000701 -0.00011 0.000472 -0.00755
C'.068606 -0.00029 C'.000453 -0.00034 C'.005701 -0.00044
0.046013 -0.00019 0.000598 -0.00017 0.002837 -0.00074
-0.00398 -0.00014 C'.000816 -0.00012 -0.00786 -0.00115
C'.1051 -6.10E-05 C'.001071 -0.00027 C'.005477 C'.004862

Average of standard deviation of error
C'.19498 1111372 C'.024872 C'.13119 C'.039212 C'.008291
0.9574 0.11387 0.024342 0.13156 0.045087 0.02205
C'.22039 111134 C'.024514 0.13065 C'.050022 C'.059248
0.23638 0.11404 0.024892 0.13389 0.045329 0.008675
0.22646 0.11294 0.024465 0.13041 0.046088 0.007784
Cs.2609 1111551 C'.024682 C'.13335 C'.051796 C'.008558
0.95319 0.1127 0.024138 0.13058 0.063676 0.066345


Case u v
ft/s ft/s
1 -0.04335 -0.0195
2 -1.6186 -0.01527
3 -0.80384 -0.29529
4. -0.95018 C'.031218
5 -0.58888 -0.03818
6 Cs.49949 -0.02877
7 -2.7942 C'.076492


1 3.7724 C'.47367
2 6.1804 1.1113
3 4.1802 2.2906
4 4.3554 0.5206
5 4.1598 0.47511
6 5.105 Cs.50105
7 8.265 2.7318












Table 5-3. Horizontal S trajectory Monte Carlo sets.


Velocity Error Statistics Angular Rate Error Statistics
10 0 2
m Mean (abs value)

21 .1 .1a IIIIII
1234567 1234567

3~ 002




1234567 1234567

1 02


~05O 01 I I I
1234567 1234567
Euler Angle Error Statistics Euler Angle Error Statistics
01 01


0 50 05
1234567 1234567
Case Case


Average of mean error
p q
rad/s rad/s


Case u

ft/s


r

rad/s


1 3.47 ().21437 -().282()1 -().()()() -().()(212 6.33E-)5
2 3.()(8 ().27835 -().4()86 -().()()35 -().()(239 -().()()38
3 2.8316 ).()26()12 -().26288 -().()(127 -().()(248 -().()()76
4 3.1572 ().18199 -().22469 -().()()82 -().()(234 -().()()73
5 2.4691 ().29688 -().24576 -().()()94 -().()(26 -().()()78
6 2.4493 ().2()664 -().24868 -().()()27 -().()(321 ().()()338
7 2.33()3 -().5()543 -().152()5 -().()(158 -().()(243 ().()()192
Average of standard deviation of error
1 5.2436 1.I673 ().34853 ().I3531 ().()26771 ().11883


-).()3259
-).()358
-).()2785
-).()2843
-).()2429
-).()2937
-).()2967


().()(711
().()(5282
-().()(181
().()(3626
().()(6344
().()(6169
-().()(253


().()5()474 ().()32863


7.3113
4.6546
5.1832
4.2636
6.()214
8.4666


1.6()49
2.3222
1.2399
1.2429
1.3711
2.4958


1.2454
().43665
().332)8
().31771
().37397
1.()198


().13187
().1328
().12947
().13265
().13341
().13()85


).()26863
).()26843
).()26154
).()26852
).()27145
).()266)9


().11615
().11618
().11283
().116)4
().11943
().11552


).()5478
).()49777
).()46634
).()4356
).()59269
).()58652


).()38959
).()66782
).()33341
).()34434
).()34()22
).()67813


Figure 5-5. Straight and level trajectory, case statistics.










































Angular Rate Error Statistics


86



Velocity Error Statistics Angular Rate Error Statistics
10 ~Mean (abs value) 2
O Std Dev

.1 .1.1.1 1 IIlli llI
12 34 5 67 12 34 56 7
3 003
2 0o 02


12 34 5 67 12 34 56 7
1 5 0 2
1 j

05 0 111
12 34 5 67 12 34 56 7
Euler Angle Error Statistics Euler Angle Error Statistics
01 01



12 34 5 67 12 34 56 7
Case Case


Figure 5-6. Vertical S trajectory, case statistics.


Velocity Error Statistics


Figure 5-7. Horizontal S trajectory, case statistics.


10 0 2
H Mean (abs value)
5 01~L

1234567 1234567
3 003



.. 1 . 0I.1 .1.1 .1. .
1234567 1234567
15 02
1

lul.. ..l .. .....1. II I
1234567 1234567
Euler Angle Error Statistics Euler Angle Error Statistics
0 1 0 1


00 I l lI II mi II / 1 1_ 1.1.
1234567 1234567
Case Case




































Table 5-4. Camera angle Monte Carlo sets.
Oc u v w p q r 8 9

deg ft/s ft/s ft/s rad/s rad/s rad/s rad rad
Average of mean error


Theta c = O deg


Theta c = 60 deg


Theta c = 82.8 deg


0.4 -
0.2
0-
-0.2
-0.4 A
-0.5 O


0.4

-0.2



0.5 -0.5


nu nu


O


0.5 -0.5 O
nu


Figure 5-8. Focal plane history for straight and level trajectory with camera looking
down, intermediate baseline position, and level.


Straight and Level Trajectory
-0.05409 0.007032 0.021851
-0.04335 -0.0195 0.018962
-0.30927 0.014068 0.025078
Vertical S Trajectory
1.4645 0.035481 -0.13992
1.421 0.0381 -0.14139
1.9163 -0.04465 -0.18002
Horizontal S Trajectory
3.5958 0.12057 -0.35776
3.47 0.21437 -0.28201


0
60
82.8


0
60
82.8


0
60
82.8


9.75E-06
-0.00013
-0.0002


0.000203
0.000192
8.13E-05


-0.00045
0.000682
0.000201


-0.00084
0.000838
-1.68E-05


-0.00019
-3.37E-05
1.81E-05


-0.0008
7.55E-05
0.000126


-0.00591
-0.00011
0.002525


-0.02906
-0.01822
-0.02353


-3.68E-05
-0.00063
0.000485


-1.87E-06
0.001268
-0.00089


-0.0005 -0.00871 -0.00041 -0.05065 0.003908
-0.00011 -0.00212 6.33E-05 -0.03259 0.00711


3.6006 0.24286


-0.29331 -0.00075 -0.00241 -0.00057 -0.03297 0.005555


Average of standard deviation of error
Straight and Level Traj ectory
0 4.0577 0.74843 0.087288 0.03742 0.017928 0.16092


0.034328 0.012611


60
82.8


0
60
82.8


0
60
82.8


3.7724 0.47367 0.19498
4.477 0.36341 0.23067
Vertical S Trajectory
4.1344 0.74597 0.21869
4.2084 0.45093 0.27757
4.8849 0.36355 0.32594
Horizontal S Trajectory
5.5306 1.3797 0.42478
5.2436 1.1673 0.34853
5.5214 1.1121 0.35725


0.11372 0.024872 0.13119 0.039212 0.008291
0.083895 0.023681 0.066755 0.048836 0.006345


0.035919
0.11863
0.087702


0.042608
0.13531
0.11398


0.018287
0.026223
0.024275


0.02244
0.026771
0.025551


0.15869
0.11071
0.062332


0.17705
0.11883
0.062243


0.043088
0.043465
0.05171


0.063435
0.050474
0.050303


0.013405
0.010757
0.007588


0.02951
0.032863
0.02991














02


01


003
9002
-001
0

02


01


0 04

~002

0


Angular Rate Error Statistics















-I -




Euler Angle Error Statistics





0 60 82 8
theta c (deg)


Figure 5-9.



6 -

P4





1 ,
0 5





04






0 06
0 04

0


Straight and level traj ectory,



Velocity Error Statistics


82 8


Figure 5-10. Vertical S trajectory, camera angle statistics.


Velocity Error Statistics

84




15
S1
'05




04

02


Euler Angle Error Statistics
0 06
~004


0 60 82 8
theta c (deg)


camera angle statistics.



Angular Rate Error Statistics
02






0 03
3002




02





Euler Angle Error Statistics
0 04

-002
H m


I I


.s .1 mi

Euler Angle Error Statistics




II m I
0 60 82 8
theta c (deg)


0 60
theta c (deg)