where Φ
I
is the IMU error-state transition matrix, and w
d
ℓ
is a noise vector, with covariance
matrix Q
d
ℓ
. The filter’s covariance matrix is also propagated according to
P
ℓ+1|ℓ
=
Φ
I
ℓ
P
II
ℓ|ℓ
Φ
T
I
ℓ
+ Q
d
ℓ
Φ
I
ℓ
P
IF
ℓ|ℓ
P
T
IF
ℓ|ℓ
Φ
T
I
ℓ
P
F F
ℓ|ℓ
where P
II
ℓ|ℓ
is the covariance matrix of the IMU state, P
F F
ℓ|ℓ
is the covariance matrix of the
features, and P
IF
ℓ|ℓ
the cross-covariance between them.
Assuming a calibrated perspective camera, the observation of feature i at time step ℓ is described
by the equation
3
:
z
iℓ
= h(x
I
ℓ
, f
i
) + n
iℓ
=
C
ℓ
x
f
i
C
ℓ
z
f
i
C
ℓ
y
f
i
C
ℓ
z
f
i
+ n
iℓ
(7)
where n
iℓ
is the measurement noise vector, modelled as zero-mean Gaussian with covariance
matrix σ
2
I
2
, and the vector
C
ℓ
p
f
i
= [
C
ℓ
x
f
i
C
ℓ
y
f
i
C
ℓ
z
f
i
]
T
is the position of the feature with
respect to the camera at time step ℓ:
C
ℓ
p
f
i
=
C
I
R
I
ℓ
G
R
G
p
f
i
−
G
p
I
ℓ
+
C
p
I
(8)
with {
C
I
R,
C
p
I
} being the rotation and translation between the camera and the IMU. In EKF-
SLAM, the feature observations are used directly for updating the state estimates. For this process,
we employ the residual between the actual and expected feature measurement, and its linearized
approximation:
r
iℓ
= z
iℓ
− h(
ˆ
x
I
ℓ|ℓ−1
,
ˆ
f
i
ℓ|ℓ−1
)
≃ H
iℓ
ˆ
x
ℓ|ℓ−1
˜
x
ℓ|ℓ−1
+ n
iℓ
(9)
where H
iℓ
ˆ
x
ℓ|ℓ−1
is the Jacobian matrix of h with respect to the filter state, evaluated at the
state estimate
ˆ
x
ℓ|ℓ−1
. This is a sparse matrix, containing nonzero blocks only at the locations
corresponding to the IMU state (position and orientation) and the i-th feature:
H
iℓ
ˆ
x
ℓ|ℓ−1
=
H
I
iℓ
ˆ
x
ℓ|ℓ−1
0 · · · H
f
iℓ
ˆ
x
ℓ|ℓ−1
· · · 0
(10)
Once r
iℓ
and H
iℓ
have been computed, a Mahalanobis gating test is performed, and if successful
the standard EKF update equations are employed (Maybeck, 1982). Depending on the particular
feature parameterization used, the exact form of the above Jacobians, as well as the “bookkeeping”
required in the filter, will be slightly different.
In VIO, we must ensure that the computational cost of the algorithm remains bounded. To
achieve this, features are removed from the state vector immediately once they leave the field of
view of the camera. This of course means that the filter cannot process feature re-observations that
occur when an area is re-visited. However, such “loop-closure” events do not need to be handled
by a VIO algorithm: if desired, they can be handled by a separate algorithm running in parallel,
as done for example in (Mourikis and Roumeliotis, 2008). In the “prototypical” VIO scenario,
where the camera keeps moving in new areas all the time, the EKF-SLAM algorithm described
above will use all the available feature information.
3
We note that throughout this paper, we focus on the monocular-camera case, which is the more challenging one.
However, our theoretical analysis and the MSCKF 2.0 algorithm are equally applicable to the case where a stereo pair
is used for visual sensing.
剩余32页未读，继续阅读
