Embed Size (px)
DESCRIPTION
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. - PowerPoint PPT Presentation
Citation preview
Complete Pose Determination for Low Altitude Unmanned Aerial Vehicle Using Stereo Vision
Luke K. Wang, Shan-Chih Hsieh, Eden C.-W. Hsueh1
Fei-Bin Hsaio2, Kou-Yuan Huang3
National Kaohsiung Univ. of Applied Sciences, Kaohsiung
Taiwan, R.O.C.
1National Space Program Office, Hinchu, Taiwan, R.O.C
2National Cheng Kung University, Tainan, Taiwan, R.O.C.
3National Chiao-Tung University, Hsinchu, Taiwan, R.O.C
Introduction
Fundamental Concepts
Simulation Results
Conclusion
Outline
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
UKF
Estimated States
Feature Extraction
Initial State & Error Covariance
Measurement & Process Error
CAMER (Right)
CAMER (Left)
Outline
Introduction
Fundamental Concepts
Simulation Results
Conclusion
?What is needed
Fundamental Concepts
•Quaternion
•GPS Observation Equation
•Perspective Projection
•Coordinate Transformation
•Unscented Kalman Filter (UKF)
0 0 1 2 3 1 2 3
1 1 0 3 2 0 3 2
2 2 3 0 1 3 0 1
3 3 2 1 0 2 1 0
1
20 0
01 1 1
02 2 2
0
x y z
x z y x
y z x y
z y x z
q q q
q q q q q q q q
q q q q q q q q
q q q q q q q q
q q q q q q q q
x
y
z
In matrix form the derivative of a quaternion may be written:
Quaternion
The unit quaternion is defined by
0
1
2
3
cos2
sin2
sin2
sin2
qiq
jq
k
If angular velocity is constant, equation is a system of first order linear time invariant differential equation with a closed-form solution
2
4 4 1
4 4 1
2cos( ) sin( ) ( ) if 02 2
( ) if =0
( )
t t q t
q t
q t
I
I
1
2
0
0
0
0
x y z
x z y
y z x
z y x
where
q q
Fundamental Concepts
•Quaternion
•Perspective Projection
•Coordinate Transformation
•Unscented Kalman Filter (UKF)
C
C
C
C
Xu f
Z
Yv f
Z
3-D to 2-D Perspective Projection
: Image point associated with [ ]
: Focal length.
TC C Cu v X Y Z
f
Fundamental Concepts
•Quaternion
•GPS Observation Equation
•Perspective Projection
•Coordinate Transformation
•Unscented Kalman Filter (UKF)
ee bt br r R r
3 1 3 3
1 3 1 3
1 3
0
0 1 0 11 1
0 1 1
1
beb et
b bee e t
b ee
R rr r
R R r r
r
I
T
The Homogeneous Transformation
The Homogeneous Transformation
1 1
C
e b Cb C
C
XX
YYT T
ZZ
(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) bTC : Transformation between {b} and {c}
(6) eTb : Transformation between {e} and {b}
Note
Fundamental Concepts
•Quaternion
•GPS Observation Equation
•Perspective Projection
•Coordinate Transformation
•Unscented Kalman Filter (UKF)
UKF
The UT is a method for calculating the statistics of a random variable which undergoes a nonlinear transformation [Julier et al., 1995].
A L dimensional random vector having mean and covariance , and propagates through an arbitrary nonlinear function.
The unscented transform creates 2L+1 sigma vectors and weights W.
Unscented Transformation (UT)
2( )
0
2( )
0
( ) 0,..., 2
( )( )
i i
i Lm
i ii
i Lc T
i i ii
h i L
W
W
y
y
P y y
Nonlinear function h
0
( )0
( ) ( ) 20 0
( )( ) ( ) 0
2
( ) 1,...,
( ) 1,..., 2
( )
(1 )
1,..., 22
( 1)
( )
i i
i i L
m
c m
mm c
i i
i L
i L L
WL
W W
WW W i L
L
L
x
x
x
x P
x P
thx
: determines the spread of the sigma points around
: incorporate prior knowledge of the distribution of
( ) : row or column of the matrix square root of Pi i
x
x
x
P
1
1 1 1
State equation ( )
Measurement equation ( )k k k k
k k k
f G w
y h v
x x
x
The discrete time nonlinear transition equation
UT
[Haykin, 2001]
Unscented Kalman Filter (UKF)
UKF
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].
None of any linearization procedure is taken.
Drawback of UKF -- computational complexity, same order as the EKF.
UKF
Time update equations(Prediction):
, 1, 1
2( )
, 10
2( )
, 1 , 10
, 1 , 1
2( )
, 10
( ) 0,1,..., 2
( )( )
i ki k k
Lm
k i i k ki
Lc T
k kk i ki k k i k ki
i k k i k k
Lm
ik i k ki
i L
W
W
W
F
x
P x x Q
H
y
0 0 0 0 0 0 0E ( )( )TE
x x P x x x x
Measurement update equations (Correction):
~ ~
~ ~
~ ~ ~ ~
~ ~
2( )
, 1 , 10
2( )
, 1 , 10
1
( )( )
( )( )
( )
k k
k k
k k k k
k k
Lc T
i kk ki k k i k ky y i
Lc T
ki ki k k i k kx y i
kx y y y
k k k k k
Tk k k k
y y
W
W
K
K y
K K
P y y R
P x y
P P
x x y
P P P
UKF
Time update equations(Prediction):
, 1, 1
2( )
, 10
2( )
, 1 , 10
, 1 , 1
2( )
, 10
( ) 0,1,..., 2
( )( )
i ki k k
Lm
k i i k ki
Lc T
k kk i ki k k i k ki
i k k i k k
Lm
ik i k ki
i L
W
W
W
F
x
P x x Q
H
y
, 1
: process noise covariance matrix.
: the computed sigma point.
The prediction of the state variable (output)
at time instant based on the state variable (input)
at time instant 1 is denoted by
k
i k k
k
k
Q
subscript 1.k k
UKF
Measurement update equations (Correction):
~ ~
~ ~
~ ~ ~ ~
~ ~
2( )
, 1 , 10
2( )
, 1 , 10
1
( )( )
( )( )
( )
k k
k k
k k k k
k k
Lc T
i kk ki k k i k ky y i
Lc T
ki ki k k i k kx y i
kx y y y
k k k k k
Tk k k k
y y
W
W
K
K y
K K
P y y R
P x y
P P
x x y
P P P
~ ~
~ ~
: measurement noise covariance matrix.
: measurement correlation matrix.
: cross-correlation matrix.
: Kalman gain.
: updated state.
: update state covariance matrix.
: current measurement
k k
k k
k
y y
x y
k
k
k
k
K
y
R
P
P
x
P
.
State Assignment
Process (Dynamic) Model
Measurement (Sensor) Model
( )k k ky h v x
1 k k k k kwx A x G
T
k k k k kx q P v a
State Assignment
0 1 2 3
[ ]
T
k k k k k
Tk k k k k k k xk yk zk xk yk zkq q q q X Y Z v v v a a a
x q P v a
Process (Dynamic) Model
1k k k k kw x A x G
4 3 4 3 4 3
2
3 4 3 3 3
3 4 3 3 3
3 4 3 3 3
0 0 0
02
0 0
0 0 0
k
k
tt
t
I I IA
I I
I
Measurement (Sensor) Model
( )k k ky h v x,1 ,1 ,1 ,1 , , , ,[ v v ... v v ]T
k l l r r l i l i r i r iy u u u u
,1 ,1 ,1 ,1 , , , ,
,1 ,1 ,1 ,1 , , , ,
( ) [ ... ] cl cl cr cr cl i cl i cr i cr i Tk
cl cl cr cr cl i cl i cr i cr i
X Y X Y X Y X Yh f f f f f f f f
Z Z Z Z Z Z Z Zx
Measurement (Sensor) Model
( )k k ky h v x,1 ,1 ,1 ,1 , , , ,[ v v ... v v ]T
k l l r r l i l i r i r iy u u u u
,1 ,1 ,1 ,1 , , , ,
,1 ,1 ,1 ,1 , , , ,
( ) [ ... ] cl cl cr cr cl i cl i cr i cr i Tk
cl cl cr cr cl i cl i cr i cr i
X Y X Y X Y X Yh f f f f f f f f
Z Z Z Z Z Z Z Zx
,
,
, 1 3 1 3
,
,
, 1 3
0 1 0 1
1 1 1
0 1
1 1
cl i i i
cl cl b bcl i i icl b b b l e e b
b ecl i i i
cr i i
cr cr b bcr i icr b b b r e
b ecr i i
X X X
Y Y YR R O R R OT T
Z Z Z
X X
Y Y R R O RT T
Z Z
1 30 1
1
i
ie b
i
X
YR O
Z
Quaternion prediction block diagram
MU: Measurement Update
Standard UKF
4 4
4 4
2cos( ) sin( ) if 02 2
if =0
k
t t
I
I
Quaternion prediction block diagram
?
Modified UKF
MU: Measurement Update
When the instantaneous angular rate is assumed constant, the quaternion differential equation has a closed- form solution
4 41
0, 1 1, 1 2, 1 3, 1
1, 1 0, 1 3, 1 2, 1
2, 1 3, 1 0, 1 1, 1
3, 1 2, 1 1, 1 0, 1
2cos( ) sin( )
2 2
k k
k k k k k k k k
k k k k k k k k
k
k k k k k k k k
k k k k k k k k
q t t q
q q q q
q q q qq
q q q q
q q q q
I
0
0102
0
x y z
x z y
y z x
z y x
2 2 2x y z
1, 1 10, 11
0, 1
2, 1 10, 11
0, 1
3, 1 10, 11
0, 1
2cos ( )
sin(cos ( ))
2cos ( )
sin(cos ( ))
2cos ( )
sin(cos ( ))
k k
x k kk k
k k
y k kk k
k k
z k kk k
q t
q t
q t
0, 1
11, 1
1
2, 1
3, 1
cos( )
1sin( )
1sin( )
1sin(
2
2
2
2)
k kx
k k
k k
k ky
k k
z
t
t
t
t
q
qq q
q
q
10, 1
10, 1
cos ( )
2cos ( )
2 k k
k k
q
qt
t
Quaternion prediction block diagram
ok
Modified UKF
MU: Measurement Update
Outline
Introduction
Fundamental Concepts
Simulation Results
Conclusion
Case 1: Four image marks are distributed evenly around the optical axis.
Landmark 1
Landmark 4
Landmark 3
Landmark 2
Notice that a rotation of at sampling instant 32. 18
Notice that a rotation of at sampling instant 32. 18
Notice that a rotation of at sampling instant 32. 18
Notice that a rotation of at sampling instant 32. 18
Notice that a rotation of at sampling instant 32. 18
Notice that a rotation of at sampling instant 32. 18
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 1
Landmark 2
Landmark 3
Landmark 4
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 100th iteration.
150 m/s
20 m/s
0 m/s
200 m/s
20 m/s
50 m/s
0 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 maneuveringTarget tracking can be implemented very easily
Thank you！
