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
67
Embed
Complete Pose Determination for Low Altitude Unmanned Aerial Vehicle Using Stereo Vision
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
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.
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
qq
q t
qq
q t
qq
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