where
Ф
k,k-1
is the transition matrix,
X
k
–
and
P
k
–
are the priori
state vector and covariance respectively at epoch
k
,
G
is the
shaping matrix,
Q
is the driven white noise,
H
is the design
matrix. The state vector includes the errors in position, veloc-
ity, attitude and inertial sensor biases, namely [
δ
r
l
δ
V
l
ε
l
δ
f
b
δω
b
]
T
. Given the dynamic matrix shown in Equation 3, the
transition matrix can be obtained as:
Ф
k,k-1
=
I + F
l
dt
(8)
As mentioned before, the difference between the measured
pixel coordinates of features and the predicted pixel coordi-
nates of features by re-projection is the measurement update
in the
EKF
, given as:
Z
k
= [
u v
]
T
measured
– [
u v
]
T
predicted
.
(9)
The predicted pixel coordinates of features can be formed
using Equations 4 and 6. The left camera frame at the begin-
ning is selected as the world frame to uniformly denote the
coordinates obtained. Equation 4 can be rewritten as:
X R X R R R R feature T
c
w
c w
b
c
l
b
l
l
w
l
w
=
=
−
0
0
(
)
(10)
where
feature
w
is the feature coordinates expressed in the
world frame,
T
is the perspective center position in the world
frame obtained by the mechanization of
IMU
,
R
j
i
represents ro-
tation matrix from frame
i
to frame
j
,
c
is left camera frame,
b
is
IMU
body frame,
l
is the local-level frame,
l
0
is the local-lev-
el at the first epoch,
w
is the world frame namely the left cam-
era frame at the first epoch. The rotation matrix
R
c
b
is available
by the pre-measured lever arm;
R
l
b
is the output of
IMU
mecha-
nization;
R
w
l
0
is known by initialization. The rotation matrix
R
l
l
0
can be neglected if the translation from the beginning is not
too far. Otherwise, the rotation can be achieved as:
R
l
l
0
=
R
e
l
0
R
l
e
(11)
where
e
represents the
ECEF
(Earth-Centered, Earth-Fixed)
frame. The rotation from
LLF
to
ECEF
can be shown as:
R
l
e
=
−
−
−
sin sin cos cos cos
cos
sin sin cos sin
cos
sin
λ
φ λ
φ λ
λ
φ λ
φ λ
φ
φ
0
(12)
The feature coordinates in the world frame in Equation 10
are obtained by triangulation in the previous camera frame,
and then transformed to the world frame, given as:
feature R R R R feature T
w
l
w
l
l
b
l
c
b
p
l p
w
k
k
=
+
−
−
0
1
0
1
0
(13)
where
feature
p
is the triangulated feature coordinates in the
previous camera frame,
l
k-1
represents the local-level frame at
the previous epoch,
T
w
l
0
p
is the perspective center translation
from the beginning to the previous epoch expressed in the
world frame. Both
T
w
l
0
p
and
R
b
l
k–1
are available from the estima-
tion in the previous epoch. The translation
T
from the begin-
ning to the current in Equation 10 can be obtained as:
T R R R r r
b
c
l
b
e
l
e e
=
−
(
)
0
0
0
(14)
where
T
is the translation expressed in the world frame,
r
e
is the current
IMU
position obtained by the mechanization
using Equation 1 expressed in
ECEF
while
r
e
0
is the initial
IMU
position expressed in
ECEF
. The rotation matrices
R
b
l
0
and
R
e
l
0
are available by initialization,
R
c
b
is known by the pre-mea-
sured lever-arm. With known latitude, longitude, and height,
the
ECEF
coordinates can be obtained as:
x
y
z
R h
R h
R e h
n
n
n
=
+
+
− +
(
)cos cos
(
)cos sin
(
)
si
φ λ
φ λ
1
2
n
φ
.
(15)
So far, the only unknown in Equation 7 is the design matrix
H.
According to Equations 6) and 10, it can be seen that the
measurement is related to the position and attitude, given as:
H H
H
H
z
X
X
r
H
z
X
X
r
r
c
c
l
c
c
=
[
]
=
∂
∂
∂
∂
=
∂
∂
∂
∂
×
×
×
0
0 0
2 3
2 3 2 3
ε
ε
ε
(16)
where
H
r
and
H
ε
represent the design matrix elements related
to position and attitude errors. The partial derivatives with
respect to
X
c
can be obtained easily by Equation 6, given as:
∂
∂
= =
−
−
z
X
C
f
z
u
z
x
z
f
z
v
z
y
z
c
x
y
0
0
0
2
0
2
(17)
where
x
,
y
,
z
,
f
x
,
f
y
,
u
0
, and
v
0
are the same in Equation 3).
From Equations 10, 13, and 14, it can be seen that the pre-
dicted feature coordinates in camera frame
X
c
are related to
the position and attitude errors. The rotation matrix
R
c
b
,
R
l
l
0
,
R
w
l
0
in Equation 10 can be treated as constant. The differential
equation of Equation 10) can be given as:
δ
ψ
δ
X R R R R X R R R R T
c
b
c
l
b
l
l
w
l
w
b
c
l
b
l
l
w
l
= −
−
0
0
0
0
(18)
where
ψ
represents the skew-symmetric matrix of attitude er-
rors namely pitch, roll and yaw errors (
δ
p,
δ
r,
δ
y
), given as:
ψ
δ δ
δ
δ
δ δ
=
−
−
−
0
0
0
y r
y
p
r p
(19)
δ
T
in Equation 18 is the translation error in the world
frame, which can be expressed as:
M r
δ
δ
δ
T R R R r R R R
b
c
l
b
e
l
e
b
c
l
b
e
l
l
=
=
0
0
0
0
(20)
where
δ
r
e
is the position error in
ECEF
,
M
denotes the transfor-
mation from
δ
r
e
to
δ
r
l
, which can be derived from Equation
15, namely the relationship between Cartesian coordinates in
ECEF
and latitude, longitude, and height, given as:
1
0
M
R h
R h
R h
R
n
n
n
n
=
− +
− +
− +
(
)sin cos
(
)cos sin cos cos
(
)sin sin (
φ λ
φ λ
φ λ
φ λ
+
− +
h
R e h
n
)cos cos cos sin
(
)
cos
sin
φ λ
φ λ
φ
φ
2
(21)
18
January 2018
PHOTOGRAMMETRIC ENGINEERING & REMOTE SENSING
