Investigation of Methods for Target State Estimation Using Vision Sensors

Transcription

1 AIAA Guidance, Navigation, and Control Conference and Exhibit 5-8 August 25, San Francisco, California AIAA Investigation of Methods for Target State Estimation Using Vision Sensors Gregory F. Ivey and Eric N. Johnson Georgia Institute of Technology, Atlanta, GA, 3332 Identifying object location and orientation are important tasks in target tracking and obstacle avoidance. This paper proposes an algorithm for identifying target location, size, and orientation without any prior knowledge based on machine vision. The algorithm uses target measurements taken from a video camera to update estimates of the target. Two forms of the Kalman filter were compared as methods for estimating the targets states. The first, the extended Kalman filter (EKF), uses a Taylor series expansion about the current state to estimate the nonlinear measurement update. The second, the square-root unscented Kalman filter (SRUKF), a form of the unscented Kalman filter (UKF), uses perturbed sigma points to estimate the mean and covariance of the measurement update. The algorithm was tested using periodic vehicle motion and random target location, orientation, and area. Both Kalman filter methods converged on the targets 3-D position, orientation, and area but required an irregular trajectory to accurately estimate the orientation and area. The estimators were simulated using identical parameters to compare them based on equivalent conditions. This produced similar results from the EKF and SRUKF and therefore neither showed a significant improvement over the other. Nomenclature B Body fixed reference frame C Camera fixed reference frame I Inertial reference frame x process state z measurement state C vector from inertial frame to camera, ft D vector from camera to center of target, [d x, d y, d z ] T, ft n target unit normal vector, [n x, n y, n z ] T, ft T vector from inertial frame to center of target, [X, Y, Z] T, ft A Area of target, ft 2 f focal length of the camera P A area of target in image plane, px 2 P Y horizontal position of target center in image plane, px P Z vertical position of target center in image plane, px P n pseudo-measurement of normal vector s magnitude px pixels Subscripts i k x y i th target k th iteration x-coordinate y-coordinate Graduate Research Assistant, Student Member AIAA, gregory ivey@ae.gatech.edu. Lockheed Martin Assistant Professor of Avionics Integration, Member AIAA, Eric.Johnson@aerospace.gatech.edu. of 2 Copyright 25 by the authors. Published by the American American Institute of Aeronautics Institute of and Aeronautics Astronautics, and Inc., Astronautics with permission.

2 z Symbols ν ζ z-coordinate zero mean measurement noise vector zero mean process noise vector I. Introduction Target tracking and obstacle avoidance have been prime tasks for autonomous unmanned aerial vehicles (UAV s). The mission capabilities of autonomous UAV s depend on how well they maintain surveillance on a desired target while avoiding surrounding objects. Vehicle attitude and location can be determined using an inertial measurement unit (IMU) and global positioning system (GPS) respectively, but determining the location and orientation of the vehicle in relation to its environment must be accomplished with other sensors. Machine vision has been used as a viable method to sense the environment around the UAV. 3 Monocular machine vision is the base of the algorithm presented in this paper and will be used for detecting the environment for either obstacle avoidance or target tracking. Exact knowledge of the surrounding objects location, size, and orientation allows the UAV to not only determine its position but also any new object s position and size relative to the known surroundings. An algorithm can be proposed that would constantly update a dynamic database of object information as the UAV moves through an environment. This algorithm is founded on the ability of the image processor and target estimator to create such a database. New advances in machine vision and image processing allow framegrabbers to capture still images from the camera and send them to the image processor, which then can determine the targets in each image. 4 The area and location of the center of each target in the image plane can be calculated and a database of all the targets information compiled. Figure displays the process of grabbing an image frame from the video camera, sending it to the image processor, and creating a database of target measurements. With a database of the targets sizes and locations in the 2-D image plane, an a priori estimate of the orientation, size, and location of the targets relative to the camera can be made. Kalman filtering can be used with an initial estimate of the Camera Framegrabber Image at time k Image Processor Identified Targets 2... i... N Database Figure. Framegrabber and image processing sequence distance from the camera to converge on the actual size, location, and orientation of each target. Since the relationship between the 2-D and the 3-D image coordinates is nonlinear, this paper investigates the extend Kalman filter (EKF) and the square-root unscented Kalman filter (SRUKF), a form of the unscented Kalman filter (UKF), as methods for estimation. The next section describes the system and measurement models used with the filtering approaches. Section III and IV detail the nuances of the EKF and SRUKF estimators, respectively. The design of the algorithm is outlined in section V with explanations on the solution of the correspondence problem. Simulations and the results are discussed in section VI. P Y P Z P A II. System Model The process model for the system is based on seven states for the i th target: target location (X i,y i,z i ), target orientation (n xi,n yi,n zi ), and target area (A i ). The orientation of the target is based on a unit vector normal to the plane of the target. Since the target is in view of the camera, the normal vector is assumed 2 of 2

3 to be pointing towards the camera. The complete process and measurement states are listed as Xi Yi PYi Zi PZi xi = and zi =, nxi PAi ny i P~ni n zi Ai () where PAi is the area and (PY i,pz i ) is the coordinate of the center of each target in the image plane in pixels. The image processor calculates these values for each object and compiles a database. The fourth measurement, P~ni, is a construct of the normal vector s magnitude. This pseudo-measurement is computed during estimation and it ensures that the magnitude remains constant. During calculation of these states, the position vectors of the vehicle and camera are assumed to be constant for each iteration of the algorithm, that way the vehicle state dynamics can be modeled separately from the dynamics of the targets states. For simplicity, the targets being tracked are assumed to be stationary. Since the states being estimated, target position, orientation, and size, are constant in the inertial reference frame, the process model updating these states is an autonomous one of the form (2) xk = xk. The measurements, however, are not related to the states by such a simple linear equation. Equations (3) (7) describe the nonlinear relationship between the measurement values and the state variables. ~i D PYi PZ i PAi P~ni ~ = T~i C f dy = dxi i f = dz dxi i 2 f = Ai dxi = (3) (4) (5) ~i ~ni D ~ ik k~ni k kd (6) 2 nxi + n2yi + n2zi 2 A negative sign is placed in front of ~ni in Eq. (6) to obtain the acute angle between the target normal vector (~ni ) and the vector from the camera to the center of the target ~ i ), since these vectors point in opposite directions. The (D curvature of the camera lens is assumed to be negligible so targets are projected onto a flat image plane. Figure 2 shows an example of the camera in view of two targets and the vectors used in defining the measurement equations. III.! (7) B C C I Extended Kalman Filter The extended Kalman filter is a linearization approach based on a Taylor series expansion about the current state.5 The expansion requires determining the derivatives of the nonlinear equations and formulating Jacobian matrices from the derivatives. The process state equations at any instance, given in Eq. (2), are linear, whereas the measurements are governed by nonlinear equations of the form zk = h(xk, vk ), given in Eqs. (3) - (7). n D D2 Target T T2 n2 Target 2 Figure 2. Diagram showing position vectors of the targets (green), position vector of the camera (blue), and vectors from the camera to the center of the targets (red). I, B, and C are the inertial, body, and camera reference frames, respectively. 3 of 2

4 A. Process Update Since the process update is autonomous, no derivatives need be calculated for the process model. Equations (8) and (9) describe the process model. B. Measurement Update ˆx k = ˆx k (8) P k = P k + Q k (9) The measurement update requires the determination of the Jacobian, H k = h (Ref. 6), () x k xk =x k in every iteration. The measurement update can be derived as follows: 5 K k = P k H T k (H k P k H T k + R k ) () ˆx k = ˆx k + K k(z k h(ˆx k )) (2) P k = (I K k H k )P k (3) The measurement and process noise covariances, R k and Q k respectively, are assumed to be constant over all the iterations and are given an initial value at the beginning of the algorithm. C. Variable Process and Measurement Noise Covariances The measurement and process noise covariances may also be dynamic to help improve the convergence properties of the EKF. In Ref. 7, Boutayeb and Aubry developed an extended Kalman observer which alters R k and Q k based upon Lyapunov theory. Their paper may be used as a method for improving the convergence of the target tracking EKF algorithm. They utilize the measurement residual e k = z k h(ˆx k ) to adjust the process noise covariance. They propose choices of and Q k = γe T ke k I 7 + δi 7 (4) R k+ = λh k+ P k+ H T k+ + δi 3 (5) where γ, δ, and λ are constants chosen such that γ, δ >, and λ >. The equations for the measurement and process noise covariances are derived by determining bounds on linear matrix inequalities which satisfy a decreasing Lyapunov function. Proof of these choices for R and Q can be found in Ref. 7. IV. Unscented Kalman Filter The UKF was found by Julier, Uhlmann, and Durrant-Whyte in Ref. 8 to be easier to implement than the EKF because it does not require calculation of Jacobian matrices. It was also found by Wan and van der Merwe in Ref. 9 to have better accuracy than the EKF and equal complexity. These results prompted investigation into its use for the target tracking algorithm. The unscented Kalman filter is based on the unscented transform (UT) and does not require linearization to handle nonlinear equations. The UT approximates the state estimate and covariance by creating weighted points and applying them to the nonlinear function to create the measurement estimate and covariance. The sigma points are chosen such that the their sample mean and covariance are equal to the state estimate and covariance. 8 A more computationally efficient form of the UKF is the Square-Root UKF (SRUKF). The SRUKF is fundamentally the same as the UKF but it uses a Cholesky factorization of the error covariance matrix to eliminate some of the computations. In Ref., van der Merwe and Wan found the SRUKF guarantees a non-negative definite state covariance matrix and requires less numerical computations. The SRUKF was implemented as a method of estimation to compare with the EKF. 4 of 2

5 A. Sigma Point Weights An outline of the SRUKF algorithm is described in Eqs. (6) - (32). These equations are based on the SRUKF algorithm presented in Ref.. An augmented state vector and covariance matrix are used in the SRUKF algorithm where the initial augmented state vector is given by Eq. (6) and the corresponding augmented covariance matrix is given by Eq. (7). The process noise mean vector (ζ) and the measurement noise mean vector (ν) are concatenated to the state vector. The lower-triangular Cholesky factors of the covariance matrices P, Q, and R are grouped into an augmented matrix, S a, where P = S x Sx T, Q = S ζ Sζ T, and R = S ν Sν T. ˆx a = [ˆx ζ ν] T (6) S x S a = S ζ (7) S ν Wan and van der Merwe propose in Ref. 9 the following equations for choosing the sigma point weights. The constant L is the dimension of the augmented state vector and λ is found using λ = α 2 (L + κ) L, where α is chosen to be small and positive and κ is usually chosen equal to. The optimal value of 2 is usually assigned to β. W (m) = λ/(l + λ) W (c) = λ/(l + λ) + ( α 2 + β) W (m) (8) j = /(2(L + λ)) j =,..., 2L W (c) j = /(2(L + λ)) j =,..., 2L B. Process Update The process update equations for the SRUKF are given in Eqs. (9) - (23). The 2L + sigma points are computed in Eq. (9), where η = L + λ. The augmented sigma point matrix is formed by the concatenation of the state sigma point matrix (X x ), the process noise sigma point matrix (X ζ ), and the measurement noise sigma point matrix (X ν ), such that X a = [ (X x ) T (X ζ ) T (X ν ) T] T. Xk a = [ˆx a k ˆx a k + ηsk a ˆx a k ηsk a ] X x (9) k = X x k + X ζ k (2) ˆx k = 2L j= {[ Sx k = qr W (m) j Xj,k x (2) W (c) S x k = cholupdate (X x ) ]} :2L,k ˆx k (22) } {S xk, (X,k x ˆx (c) k ), W (23) The functions qr{ } and cholupdate{ } are linear algebra operators that correspond to a QR decomposition and a rank Cholesky update, respectively. qr{a} returns the lower triangular part of R from the economy QR decomposition, A = QR. cholupdate{s, U, ±ω} returns the rank update or downdate (depending on the sign of ω) of the lower triangular Cholesky factor S by ωu. If U is a matrix, then N consecutive rank updates or downdates are performed by the columns of U, where N is the number of columns of U. C. Measurement Update The measurement update equations are given by Eqs. (24) - (32). The nonlinear measurement equations, h( ), are given in Eqs. (3) - (7). The linear algebra operator, /, is a pseudoinverse operation that uses pivoting. Z k = h ) (Xk x + Xk ν (24) 5 of 2

6 ẑ k = 2L j= W (m) j Z j,k (25) {[ ( ) ]} S zk = qr W (c) Z :2L,k ẑ k (26) { ( ) } S zk = cholupdate S zk, Z,k ẑ k, W (c) (27) P xk z k = 2L j= ) ( ) W (c) j (Xj,k x ˆx k Z j,k T ẑ k (28) K k = ( P xk y k /S T z k ) /Szk (29) ˆx k = ˆx k + K k(z k ẑ k ) (3) U = K k S zk (3) S xk = cholupdate { S x k, U, } (32) V. Algorithm Design The target tracking algorithm begins by taking the database of target information from the image processor and then using the data to create an initial estimate of the 3-D position of each target. The initial estimates are based on fixed values for X, n x, n y, and n z assigned to every new target. The other three state variables are calculated using these values and the measurement data from the database with Eqs. (3) - (6). This is a crude method and a thorough investigation into determining the optimal initial estimates will need to be done. The initial estimates are then sent to the estimator which will use either the standard EKF or the SRUKF to update the states. The next iteration of the algorithm receives updated measurements from the image processor. These measurements are correlated to the correct target estimates with the use of the statistical z-test. 2, 3 If the measurements correspond to an existing estimate, then the measurements and a priori state estimates are sent to the estimator to be updated. If a set of measurements does not match any of the estimates, then it is assumed that they are for a new target. A state estimate is added for the new target and computed with Eqs. (3) - (6) based on the fixed values for X, n x, n y, n z, and the measurements. A block diagram outlining the algorithm is shown in figure 3. The statistical z-test was implemented to check the correlation between the target s measurements and the state estimates stored in the database. The test can be defined with residuals as where the linear form of the residual is defined as 2... i... N Database at k Initialize First Image Xi P Y P Z P A Z i k _ Xi k = Xi k- _ Z k Yes _ Xi k i = h( ) _ Z i k Correspondence z-test Is target New? Z i k No _ Xi k Estimator Xi k Figure 3. Block diagram of the target tracking algorithm. Z = e T (E[e e T ]) e (33) e = z H ˆx. (34) Knowledge of the estimation and measurement errors given respectively in Eqs. (35) and (36), can be used to determine the known covariances R and P : x = ˆx x (35) z = z Hx, (36) R = E[ z z T ] (37) 6 of 2

7 P = E[ x x T ]. (38) The error equations, (35) and (36), can be substituted in Eq. (34) and the expected value, E[e e T ], transformed using (38) and (37) into E[e e T ] = R + HP H T. (39) Equation (39) is substituted into (33) to obtain the Z value Z = e T (R + HP H T ) e. (4) This equation is easily implemented in the EKF estimator and an equivalent form for the SRUKF estimator can be developed likewise. The Kalman gain is defined for the EKF in Eq. () as K = P H(HP H T + R) and can be compared to the Kalman gain defined for the UKF in Ref. 9 as K = P xz Pz. Based on this comparison, the Z value for the UKF can be assumed to be Z = e T (P z ) e. (4) The lower triangular Cholesky factorization, P z = S z S T z, and the pseudo-inverse operator, /, can be used with Eq. (4) to efficiently compute the Z value for the SRUKF. The SRUKF Z value equation is given by Z = e T [(I/S T z )/S z ] e. (42) The magnitude of Z depends on the covariance matrices and will be small when P and R are large. Therefore, even if the residual is large, great uncertainties in the estimates and measurements will cause the value of Z to be small. The z-test correlates the measurements and estimates by comparing the magnitude of Z to a critical value. If Z is larger than the critical value then they do not correspond. VI. Simulations and Results The simulations were performed with the expectation that later flight tests on the Georgia Tech GTMax research UAV 4 would be performed. Some base assumptions were made to simplify the simulations. The GTMax has a Hz processing speed, so the algorithm was simulated at Hz to replicate this. The camera was assumed to be rigidly attached to the nose of the vehicle and the camera, body, and inertial frames all aligned. The vehicle was simulated to perform periodic motions with little body roll, which was found to incur faster estimator convergence. No rolling, yawing, or pitching was assumed so the reference frames all remained aligned and any transformation to account for these rotations could be neglected. The vehicle started at coordinate (,, -3) ft in the inertial reference frame and traveled on a circular path that spanned all three directions. The trajectory had an amplitude of 2 ft in the x, y, and z directions and a periodic speed of.5π rad/s. This would result in the vehicle moving on the circular path at 27 ft/s. The vehicle started on one circular path for 2 seconds then changed direction and continued on a similar path for 7 seconds. The trajectory of the 9 second flight is shown in Figure 4. Zero mean noise with a standard deviation of. ft was added to the x, y, and z positions of the vehicle to simulate position disturbances. A set of targets with random locations, orientations, and areas was created and their image plane measurements simulated. A zero mean, Gaussian measurement noise vector was applied to the measurement calculations. The noise for P Y, P Z, and P A had standard deviations of 3 px, 3 px, and. px 2, respectively. For parity, the same noise vector was used in each estimator. The image plane was assumed to have a width of 74 px and a height of 48 px. The image plane was normalized by half the width so the corners of the image plane would have the following coordinates: ( , ), ( , ), ( , ), and ( 352 ) 352, 24. (43) 352 A 7 degree field of view was assumed for the camera and the image plane focal length was calculated using f = tan(35 ) (Ref. 4). (44) The algorithm initialized each target estimate with X =, n x = 3, n y =, and n z =. These states along with the initial measurements (P Y, P Z, P A ) were used to calculate the other states (Y, Z, A) 7 of 2

8 using Eqs. (3) - (6). The EKF and the SRUKF process noise and measurement noise covariance matrices, Q and R, were set to the values shown in Eqs. (45) and (46), respectively. The same initial estimate error covariance matrix, P, shown in Eq. (47), was also used for both the SRUKF and EKF. The filters were tuned with identical parameters for an equal comparison. Q = diag { } (45) { ( ) 2 ( ) 2 ( ) } R = diag (.) 2 (46) P = diag { } (47) The results for three random targets are shown in the Appendix in Figures 5-4. The actual values for the states of each target are shown Table. Figures 5-7 show the EKF and SRUKF estimates of the states compared to the actual values for each target. The error of the SRUKF estimate and EKF estimate for each state are shown in Figures 8-4. Plus and minus two standard deviation bounds of the state error are also included for comparison. The EKF and SRUKF filters converged on the X, Y, and Z positions of the targets with similar performances. Figures 8 - show the estimator errors compared to two standard deviations of the error. The SRUKF error for X, Y, and Z remained within two standard deviations. The EKF error for X, Y, and Z deviated from the two standard deviation bounds but always returned and was identical to the SRUKF estimate by the end of the simulation. Estimating the attitude and area required the vehicle view the targets from different locations and the change in circular trajectories at 2 seconds helped the filters converge on the correct values. This is apparent in Figure 6, where the estimates of n x, n y, n z, and A changed dramatically after 2 seconds when the vehicle changed trajectory. The EKF error for n x, n y, and n z in Figures, 2, and 3 remained within the two standard deviation bounds after the trajectory changed at 2 seconds. The SRUKF error for n y and n z had similar behavior, but the error for the n x state of Target 3 in Figure had divergent behavior and exceeded the two standard deviation bound at about 4 seconds. The SRUKF error of the area for Target and 2 in Figure 4 stayed within the two standard deviation bounds but diverged for Target 3. The EKF error of A for Target 2 and 3 converged into the two standard deviation bounds but the error for Target did not make it under the bounds by the end of the simulation. Table. Actual state values for three random targets given in the inertial reference frame. Target X (ft) Y (ft) Z (ft) n x (ft) n y (ft) n z (ft) A (ft 2 ) After the values are assigned, the normal vector is normalized so that it is a unit normal vector. VII. Conclusions The target tracking algorithm can quickly and accurately estimate the position of a target. It, however, has difficulty converging on the orientation and area and requires the vehicle obtain different views of each target and navigate an irregular trajectory. Further research needs to be done on the vehicle motion and how this affects the convergence of the algorithm. This could be used to command the vehicle to perform specific motions to help the algorithm estimate the correct values. A thorough study of the optimal initial state estimates also needs to be done. The SRUKF and EKF estimators performed similarly when given identical parameters. Both converged on the states but each had a problem estimating the attitude and area of the targets. The SRUKF estimator error tended to diverge for the states A and n x, whereas the EKF took longer than the SRUKF to converge on the area state for Target. It is possible to independently tune each filter for better estimator performances, but the identical tuning parameters chosen caused similar performances and the results show no significant improvement of one estimation method over the other. 8 of 2

9 Appendix (a) (b) Figure 4. Plots of the camera trajectory. (a) The X,Y, and Z camera position versus time. (b) The 3-D view of the camera trajectory (red) and the target locations (blue s). The start position is the green square and the end position is the magenta square. The vehicle travels on circular paths and changes its trajectory at 2 seconds, resulting in the two circles. 9 of 2

10 X Position, ft Y Position, ft Z Position, ft n x Component, ft n y Component, ft n z Component, ft 2 Target Area, ft Time, s Figure 5. Plots showing the states of target. The Actual state value is the solid red line, the EKF estimate is the dashed blue line, and the SRUKF estimate is the solid green line. of 2

11 X Position, ft Z Position, ft n x Component, ft n y Component, ft n z Component, ft Y Position, ft 9 Target Area, ft Time, s Figure 6. Plots showing the states of target 2. The Actual state value is the solid red line, the EKF estimate is the dashed blue line, and the SRUKF estimate is the solid green line. of 2

12 X Position, ft Y Position, ft Z Position, ft n x Component, ft n y Component, ft n z Component, ft 3 2 Target Area, ft Time, s Figure 7. Plots showing the states of target 3. The Actual state value is the solid red line, the EKF estimate is the dashed blue line, and the SRUKF estimate is the solid green line. 2 of 2

13 2 Target 2 X Position Error and Two Standard Deviations, ft 2 Target Target 3 2 Time, s Figure 8. Plots showing, for the target X position, EKF estimate error (dashed blue line), SRUKF estimate error (solid green line), plus and minus two standard deviations of the EKF error (dashed magenta line), and plus and minus two standard deviations of the SRUKF error (solid cyan line) for Targets, 2, and 3. 3 of 2

14 Target.5.5 Y Position Error and Two Standard Deviations, ft.5.5 Target 2 Target Time, s Figure 9. Plots showing, for the target Y position, EKF estimate error (dashed blue line), SRUKF estimate error (solid green line), plus and minus two standard deviations of the EKF error (dashed magenta line), and plus and minus two standard deviations of the SRUKF error (solid cyan line) for Targets, 2, and 3. 4 of 2

15 Target.5.5 Z Position Error and Two Standard Deviations, ft.5.5 Target 2 Target Time, s Figure. Plots showing, for the target Z position, EKF estimate error (dashed blue line), SRUKF estimate error (solid green line), plus and minus two standard deviations of the EKF error (dashed magenta line), and plus and minus two standard deviations of the SRUKF error (solid cyan line) for Targets, 2, and 3. 5 of 2

16 .2 Target.. Normal Vector X Component Error and Two Standard Deviations, ft Target Target Time, s Figure. Plots showing, for the target normal vector X component (n x), EKF estimate error (dashed blue line), SRUKF estimate error (solid green line), plus and minus two standard deviations of the EKF error (dashed magenta line), and plus and minus two standard deviations of the SRUKF error (solid cyan line) for Targets, 2, and 3. 6 of 2

17 .4 Target.2.2 Normal Vector Y Component Error and Two Standard Deviations, ft Target Target Time, s Figure 2. Plots showing, for the target normal vector Y component (n y), EKF estimate error (dashed blue line), SRUKF estimate error (solid green line), plus and minus two standard deviations of the EKF error (dashed magenta line), and plus and minus two standard deviations of the SRUKF error (solid cyan line) for Targets, 2, and 3. 7 of 2

18 .4 Target.2.2 Normal Vector Z Component Error and Two Standard Deviations, ft Target Target Time, s Figure 3. Plots showing, for the target normal vector Z component (n z), EKF estimate error (dashed blue line), SRUKF estimate error (solid green line), plus and minus two standard deviations of the EKF error (dashed magenta line), and plus and minus two standard deviations of the SRUKF error (solid cyan line) for Targets, 2, and 3. 8 of 2

19 2 Target 2 Area Error and Two Standard Deviations, ft 2 Target Target 3 2 Time, s Figure 4. Plots showing, for the target area, EKF estimate error (dashed blue line), SRUKF estimate error (solid green line), plus and minus two standard deviations of the EKF error (dashed magenta line), and plus and minus two standard deviations of the SRUKF error (solid cyan line) for Targets, 2, and 3. 9 of 2

20 References Saripalli, S., Montgomery, J. F., and Sukhatme, G. S., Vision-based Autonomous Landing of an Unmanned Aerial Vehicle, Proceedings of the 22 IEEE International Conference on Robotics & Automation, Washington, DC, May 22, pp Langelaan, J. and Rock, S., Navigation of Small UAVs Operating in Forests, AIAA Guidance, Navigation, and Control Conference and Exhibit, Providence, Rhode Island, August Sinopoli, B., Micheli, M., Donato, G., and Koo, T. J., Vision Based Navigation for an Unmanned Aerial Vehicle, Proceedings of the 2 IEEE International Conference on Robotics & Automation, Seoul, Korea, May 2, pp De Wagter, C., Proctor, A. A., and Johnson, E. N., Vision-Only Aircraft Flight Control, Digital Avionics Systems Conference, 23. DASC 3. The 22nd, Vol. 2, October 23, pp. 8.B Welch, G. and Bishop, G., An Introduction to the Kalman Filter, Tr 95-4, Computer Science, University of North Carolina at Chapel Hill, Chapel Hill, NC, Watanabe, Y., Johnson, E. N., and Calise, A. J., Optimal 3-D Guidance from a 2-D Vision Sensor, AIAA Guidance, Navigation, and Control Conference and Exhibit, Providence, Rhode Island, August Boutayeb, M. and Aubry, D., A Strong Tracking Extended Kalman Observer for Nonlinear Discrete-Time Systems, IEEE Transactions on Automatic Control, Vol. 44, No. 8, 999, pp Julier, S., Uhlmann, J., and Durrant-Whyte, H. F., A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators, IEEE Transactions on Automatic Control, Vol. 45, No. 3, March 2, pp Wan, E. A. and van der Merwe, R., The Unscented Kalman Filter for Nonlinear Estimation, Adaptive Systems for Signal Processing, Communications,and Control Symposium 2. AS-SPCC. The IEEE 2, October 2, pp van der Merwe, R. and Wan, E., The Square-Root Unscented Kalman Filters for State and Parameter-Estimation, 2 IEEE International Conference on Acoustics, Speech, and Signal Processing, 2.Proceedings.(ICASSP )., Salt Lake City, Utah, May 2. van der Merwe, R., Wan, E., and Julier, S., Sigma-Point Kalman Filters for Nonlinear Estimation and Sensor-Fusion - Applications to Integrated Navigation, AIAA Guidance, Navigation, and Control Conference and Exhibit, Providence, Rhode Island, August Hayter, A. J., Probability and Statistics For Engineers and Scientists, Duxbury, Thomas Learning, Inc., 2nd ed., Hines, W. W., Montgomery, D. C., Goldsman, D. M., and Borror, C. M., Probability and Statistics in Engineering, John Wiley and Sons, Inc., 4th ed., Johnson, E. N. and Schrage, D. P., The Georgia Tech Unmanned Aerial Research Vehicle: GTMax, AIAA Guidance, Navigation, and Control Conference and Exhibit, Austin, Texas, August of 2