Complete Pose Determination for Low Altitude Unmanned Aerial

Complete Pose Determination for Low Altitude Unmanned Aerial Vehicle Using Stereo Vision Luke K. Wang, Shan-Chih Hsieh, Eden C. -W. Hsueh 1 Fei-Bin Hsaio 2, Kou-Yuan Huang 3 National Kaohsiung Univ. of Applied Sciences, Kaohsiung Taiwan, R. O. C. 1 National Space Program Office, Hinchu, Taiwan, R. O. C 2 National Cheng Kung University, Tainan, Taiwan, R. O. C. 3 National Chiao-Tung University, Hsinchu, Taiwan, R. O. C

Outline §Introduction §Fundamental Concepts §Simulation Results §Conclusion

Outline §Introduction §Fundamental Concepts §Simulation Results §Conclusion

Introduction §Pose Estimation §Visual Motion Estimation §Kalman Filtering Technique §Unscented Kalman Filter vs. Extended Kalman Filter

The schematics illustration of image-based navigation system

IMAGE CAMER (Right) CAMER (Left) Feature Extraction Measurement & Process Error UKF Estimated States Initial State & Error Covariance

Outline §Introduction What is needed §Fundamental Concepts §Simulation Results §Conclusion ?

Fundamental Concepts • Quaternion • GPS Observation Equation • Perspective Projection • Coordinate Transformation • Unscented Kalman Filter (UKF)

Quaternion The unit quaternion is defined by In matrix form the derivative of a quaternion may be written:

If angular velocity is constant, equation is a system of first order linear time invariant differential equation with a closed-form solution where

Fundamental Concepts • Quaternion • Perspective Projection • Coordinate Transformation • Unscented Kalman Filter (UKF)

3 -D to 2 -D Perspective Projection

Fundamental Concepts • Quaternion • GPS Observation Equation • Perspective Projection • Coordinate Transformation • Unscented Kalman Filter (UKF)

The Homogeneous Transformation

The Homogeneous Transformation Note (1)Earth-Centered-Earth-Fixed (ECEF), i. e. , {e} (2)Camera coordinate , i. e. , {c} (3)Body frame , i. e. , {b} (4) [XC YC ZC]T : The target location expressed in {C} (5) b. TC : Transformation between {b} and {c} (6) e. Tb : Transformation between {e} and {b}

Fundamental Concepts • Quaternion • GPS Observation Equation • Perspective Projection • Coordinate Transformation • Unscented Kalman Filter (UKF)

UKF Unscented Transformation (UT) l. The UT is a method for calculating the statistics of a random variable which undergoes a nonlinear transformation [Julier et al. , 1995]. l. A L dimensional random vector having mean and covariance , and propagates through an arbitrary nonlinear function. l. The unscented transform creates 2 L+1 sigma vectors and weights W.

The discrete time nonlinear transition equation Nonlinear function

UT

UKF Unscented Kalman Filter (UKF) l. The UKF is an extension of UT to the Kalman Filter frame, and it uses the UT to implement the transformations for both TU and MU [Julier et al. , 1995]. l. None of any linearization procedure is taken. l. Drawback of UKF -- computational complexity, same order as the EKF.

UKF Time update equations (Prediction): Measurement update equations (Correction):

UKF Time update equations (Prediction):

UKF Measurement update equations (Correction):

State Assignment Process (Dynamic) Model Measurement (Sensor) Model

State Assignment Process (Dynamic) Model

Measurement (Sensor) Model

Measurement (Sensor) Model

Standard UKF Quaternion prediction block diagram MU: Measurement Update

Modified UKF ? Quaternion prediction block diagram MU: Measurement Update

When the instantaneous angular rate is assumed constant, the quaternion differential equation has a closedform solution

Modified UKF ok Quaternion prediction block diagram MU: Measurement Update

Outline §Introduction §Fundamental Concepts §Simulation Results §Conclusion

Case 1: Four image marks are distributed evenly around the optical axis. Landmark 3 Landmark 4 Landmark 2 Landmark 1

Notice that a rotation of at sampling instant 32.

Notice that a rotation of at sampling instant 32.

Notice that a rotation of at sampling instant 32.

Notice that a rotation of at sampling instant 32.

Notice that a rotation of at sampling instant 32.

Notice that a rotation of at sampling instant 32.

Case 2: Four image marks are initially distributed around the optical axis, but after 100 iterations, an image mark among them is gradually traveling away from the optical axis. Landmark 3 Landmark 4 Landmark 2 Landmark 1

Case 2: Four image marks are initially distributed around the optical axis, but after 100 iterations, an image mark among them is gradually traveling far away from the optical axis.

Case 3: UAV is moving. 282. 843 m/s

Case 3: UAV is moving. 282. 843 m/s At the beginning of the simulation, cluster-1 serves as landmarks.

Case 3: UAV is moving. 282. 843 m/s Because the flight vehicle is gradually departing far away from the cluster-1, it will cause landmarks to displace out of the FOV, and even cause UKF to diverge;

Case 3: UAV is moving. 282. 843 m/s so cluster-2 takes over after the 100 th iteration.

200 m/s 150 m/s 20 m/s

20 m/s 50 m/s

Outline §Introduction §Fundamental Concepts §Simulation Results §Conclusion

Conclusion A compact, unified formulation is made The use of UKF -- faster convergence rate, less dependent upon I. C. , no linearization is ever needed Successful identification of larger angle maneuvering Target tracking can be implemented very easily

Thank you！