A Multi-State Constraint Kalman Filter

for Vision-aided Inertial Navigation

Anastasios I. Mourikis and Stergios I. Roumeliotis

Abstract— In this paper, we present an Extended Kalman geometric constraints involving all these poses. The primary
Filter (EKF)-based algorithm for real-time vision-aided inertial contribution of our work is a measurement model that
navigation. The primary contribution of this work is the expresses these constraints without including the 3D feature
derivation of a measurement model that is able to express
the geometric constraints that arise when a static feature is position in the filter state vector, resulting in computational
observed from multiple camera poses. This measurement model complexity only linear in the number of features. After a
does not require including the 3D feature position in the state brief discussion of related work in the next section, the de-
vector of the EKF and is optimal, up to linearization errors. tails of the proposed estimator are presented in Section III. In
The vision-aided inertial navigation algorithm we propose has Section IV we describe the results of a large-scale experiment
computational complexity only linear in the number of features,
and is capable of high-precision pose estimation in large-scale in an uncontrolled urban environment, which demonstrate
real-world environments. The performance of the algorithm that the proposed estimator enables accurate, real-time pose
is demonstrated in extensive experimental results, involving a estimation. Finally, in Section V the conclusions of this work
camera/IMU system localizing within an urban area. are drawn.
I. I NTRODUCTION II. R ELATED W ORK
In the past few years, the topic of vision-aided inertial One family of algorithms for fusing inertial measurements
navigation has received considerable attention in the re- with visual feature observations follows the Simultaneous
search community. Recent advances in the manufacturing of Localization and Mapping (SLAM) paradigm. In these meth-
MEMS-based inertial sensors have made it possible to build ods, the current IMU pose, as well as the 3D positions
small, inexpensive, and very accurate Inertial Measurement of all visual landmarks are jointly estimated [1]–[4]. These
Units (IMUs), suitable for pose estimation in small-scale approaches share the same basic principles with SLAM-
systems such as mobile robots and unmanned aerial vehicles. based methods for camera-only localization (e.g., [5], [6],
These systems often operate in urban environments where and references therein), with the difference that IMU mea-
GPS signals are unreliable (the “urban canyon”), as well as surements, instead of a statistical motion model, are used
indoors, in space, and in several other environments where for state propagation. The fundamental advantage of SLAM-
global position measurements are unavailable. The low cost, based algorithms is that they account for the correlations that
weight, and power consumption of cameras make them ideal exist between the pose of the camera and the 3D positions of
alternatives for aiding inertial navigation, in cases where GPS the observed features. On the other hand, the main limitation
measurements cannot be relied upon. of SLAM is its high computational complexity; properly
An important advantage of visual sensing is that images treating these correlations is computationally costly, and
are high-dimensional measurements, with rich information thus performing vision-based SLAM in environments with
content. Feature extraction methods can typically detect and thousands of features remains a challenging problem.
track hundreds of features in images, which, if properly Several algorithms exist that, contrary to SLAM, estimate
used, can result is excellent localization results. However, the pose of the camera only (i.e., do not jointly estimate
the high volume of data also poses a significant challenge the feature positions), with the aim of achieving real-time
for estimation algorithm design. When real-time localization operation. The most computationally efficient of these meth-
performance is required, one is faced with a fundamental ods utilize the feature measurements to derive constraints
trade-off between the computational complexity of an algo- between pairs of images. For example in [7], an image-
rithm and the resulting estimation accuracy. based motion estimation algorithm is applied to consecutive
In this paper we present an algorithm that is able to pairs of images, to obtain displacement estimates that are
optimally utilize the localization information provided by subsequently fused with inertial measurements. Similarly,
multiple measurements of visual features. Our approach is in [8], [9] constraints between current and previous image are
motivated by the observation that, when a static feature is defined using the epipolar geometry, and combined with IMU
viewed from several camera poses, it is possible to define measurements in an Extended Kalman Filter (EKF). In [10],
This work was supported by the University of Minnesota (DTC), the
[11] the epipolar geometry is employed in conjunction with
NASA Mars Technology Program (MTP-1263201), and the National Sci- a statistical motion model, while in [12] epipolar constraints
ence Foundation (EIA-0324864, IIS-0643680). The authors would like to are fused with the dynamical model of an airplane. The use of
thank Faraz Mirzaei for his invaluable help with the experiments. feature measurements for imposing constraints between pairs
The authors are with the Dept. of Computer Science & Engi-
neering, University of Minnesota, Minneapolis, MN 55455. Emails: of images is similar in philosophy to the method proposed in
{mourikis|stergios}@cs.umn.edu this paper. However, one fundamental difference is that our
algorithm can express constraints between multiple camera Algorithm 1 Multi-State Constraint Filter
poses, and can thus attain higher estimation accuracy, in Propagation: For each IMU measurement received,
cases where the same feature is visible in more than two propagate the filter state and covariance (cf. Section III-B).
images.
Pairwise constraints are also employed in algorithms that Image registration: Every time a new image is recorded,
maintain a state vector comprised of multiple camera poses. • augment the state and covariance matrix with a copy of
In [13], an augmented-state Kalman filter is implemented, the current camera pose estimate (cf. Section III-C).
in which a sliding window of robot poses is maintained in • image processing module begins operation.
the filter state. On the other hand, in [14], all camera poses
are simultaneously estimated. In both of these algorithms, Update: When the feature measurements of a given image
pairwise relative-pose measurements are derived from the
become available, perform an EKF update (cf. Sections III-D
images, and used for state updates. The drawback of this
and III-E).
approach is that when a feature is seen in multiple images,
the additional constraints between the multiple poses are
discarded, thus resulting in loss of information. Further- treatment of the effects of the earth’s rotation on the IMU
more, when the same image measurements are processed measurements (cf. Eqs. (7)-(8)), the global frame is chosen as
for computing several displacement estimates, these are not an Earth-Centered, Earth-Fixed (ECEF) frame in this paper.
statistically independent, as shown in [15]. An overview of the algorithm is given in Algorithm 1. The
One algorithm that, similarly to the method proposed IMU measurements are processed immediately as they be-
in this paper, directly uses the landmark measurements come available, for propagating the EKF state and covariance
for imposing constraints between multiple camera poses is (cf. Section III-B). On the other hand, each time an image
presented in [16]. This is a visual odometry algorithm that is recorded, the current camera pose estimate is appended
temporarily initializes landmarks, uses them for imposing to the state vector (cf. Section III-C). State augmentation
constraints on windows of consecutive camera poses, and is necessary for processing the feature measurements, since
then discards them. This method, however, does not in- during EKF updates the measurements of each tracked
corporate inertial measurements. Moreover, the correlations feature are employed for imposing constraints between all
between the landmark estimates and the camera trajectory camera poses from which the feature was seen. Therefore,
are not properly accounted for, and as a result, the algorithm at any time instant the EKF state vector comprises (i) the
does not provide any measure of the covariance of the state evolving IMU state, XIMU , and (ii) a history of up to Nmax
estimates. past poses of the camera. In the following, we describe the
A window of camera poses is also maintained in the various components of the algorithm in detail.
Variable State Dimension Filter (VSDF) [17]. The VSDF
is a hybrid batch/recursive method, that (i) uses delayed A. Structure of the EKF state vector
linearization to increase robustness against linearization in- The evolving IMU state is described by the vector:
accuracies, and (ii) exploits the sparsity of the information
T
matrix, that naturally arises when no dynamic motion model XIMU = IG q̄ T bg T G vI T ba T G pTI (1)
is used. However, in cases where a dynamic motion model
where IG q̄ is the unit quaternion [19] describing the rotation
is available (such as in vision-aided inertial navigation) the
from frame {G} to frame {I}, G pI and G vI are the IMU
computational complexity of the VSDF is at best quadratic
position and velocity with respect to {G}, and finally bg
in the number of features [18].
and ba are 3 × 1 vectors that describe the biases affecting
In contrast to the VSDF, the multi-state constraint filter the gyroscope and accelerometer measurements, respectively.
that we propose in this paper is able to exploit the benefits of The IMU biases are modeled as random walk processes,
delayed linearization while having complexity only linear in driven by the white Gaussian noise vectors nwg and nwa ,
the number of features. By directly expressing the geometric respectively. Following Eq. (1), the IMU error-state is defined
constraints between multiple camera poses it avoids the as:
computational burden and loss of information associated with  T
X IMU = δθ T b T Gv  T T Gp
b  T (2)
pairwise displacement estimation. Moreover, in contrast to I g I a I
SLAM-type approaches, it does not require the inclusion For the position, velocity, and biases, the standard additive
of the 3D feature positions in the filter state vector, but error definition is used (i.e., the error in the estimate x̂
still attains optimal pose estimation. As a result of these of a quantity x is defined as x  = x − x̂). However, for
properties, the described algorithm is very efficient, and as the quaternion a different error definition is employed. In
shown in Section IV, is capable of high-precision vision- particular, if q̄ˆ is the estimated value of the quaternion q̄,
aided inertial navigation in real time. then the orientation error is described by the error quaternion
III. E STIMATOR D ESCRIPTION δ q̄, which is defined by the relation q̄ = δ q̄ ⊗ q̄ˆ. In this
expression, the symbol ⊗ denotes quaternion multiplication.
The goal of the proposed EKF-based estimator is to track The error quaternion is
the 3D pose of the IMU-affixed frame {I} with respect to
1 T T
a global frame of reference {G}. In order to simplify the δ q̄  2 δθ 1 (3)

T
Intuitively, the quaternion δ q̄ describes the (small) rotation where nIMU = nTg nTwg nTa nTwa is the system
that causes the true and estimated attitude to coincide. Since noise. The covariance matrix of nIMU , QIMU , depends on
attitude corresponds to 3 degrees of freedom, using δθ to the IMU noise characteristics and is computed off-line during
describe the attitude errors is a minimal representation. sensor calibration. Finally, the matrices F and G that appear
Assuming that N camera poses are included in the EKF in Eq. (10) are given by:
state vector at time-step k, this vector has the following form:  
−ω̂ × −I3 03×3 03×3 03×3
 T  03×3
T CN ˆT  03×3 03×3 03×3 03×3  
X̂k = X̂TIMU C 1ˆ
G q̄ p̂C1 . . . G q̄
G T
p̂CN
G T (4)
k 
F = −Cq̂ â × 03×3 −2ω G × −Cq̂ −ω G ×2 
T T

 03×3 03×3 03×3 03×3 03×3 
where C iˆ
G q̄ and
G
p̂Ci , i = 1 . . . N are the estimates of the
camera attitude and position, respectively. The EKF error- 03×3 03×3 I3 03×3 03×3
state vector is defined accordingly: where I3 is the 3 × 3 identity matrix, and
 T  
k = X
X T δθ TC1 G p TC1 . . . δθ TCN G p  TCN (5) −I3 03×3 03×3 03×3
IMUk 03×3
 I3 03×3 03×3  
B. Propagation G= 0
 3×3 0 3×3 −C T
q̂ 03×3 

The filter propagation equations are derived by discretiza- 03×3 03×3 03×3 I3 
tion of the continuous-time IMU system model, as described 03×3 03×3 03×3 03×3
in the following:
2) Discrete-time implementation: The IMU samples the
1) Continuous-time system modeling: The time evolution
signals ω m and am with a period T , and these measurements
of the IMU state is described by [20]:
 I are used for state propagation in the EKF. Every time a new
1
G q̄(t) = 2 Ω ω(t) G q̄(t), ḃg (t) = nwg (t)
I ˙ IMU measurement is received, the IMU state estimate is
(6) propagated using 5th order Runge-Kutta numerical integra-
G
v̇I (t) = G a(t), b˙a (t) = nwa (t), G p ˙ I (t) = G vI (t) tion of Eqs. (9). Moreover, the EKF covariance matrix has to
be propagated. For this purpose, we introduce the following
In these expressions G a is the body acceleration in the global
partitioning for the covariance:
frame, ω = [ωx ωy ωz ]T is the rotational velocity expressed 
in the IMU frame, and PIIk|k PICk|k
  Pk|k = (11)
 PTICk|k PCCk|k
0 −ωz ωy
−ω × ω
Ω(ω) = , ω × =  ωz 0 −ωx  where PIIk|k is the 15×15 covariance matrix of the evolving
−ω T 0
−ωy ωx 0 IMU state, PCCk|k is the 6N × 6N covariance matrix of the
The gyroscope and accelerometer measurements, ω m and camera pose estimates, and PICk|k is the correlation between
am respectively, are given by [20]: the errors in the IMU state and the camera pose estimates.
With this notation, the covariance matrix of the propagated
ω m = ω + C(IG q̄)ω G + bg + ng (7) state is given by:
am = C(IG q̄)(G a − g + 2ω G × vI + ω G ×
G G 2 G
pI ) 
PII k+1|k Φ(tk + T, tk )PIC k|k
+ ba + na Pk+1|k =
(8) PIC k|k Φ(tk + T, tk )
T T
PCC k|k
where C(·) denotes a rotational matrix, and ng and na are where PII k+1|k is computed by numerical integration of the
zero-mean, white Gaussian noise processes modeling the Lyapunov equation:
measurement noise. It is important to note that the IMU
measurements incorporate the effects of the planet’s rotation, ṖII = FPII + PII FT + GQIMU GT (12)
ω G . Moreover, the accelerometer measurements include the Numerical integration is carried out for the time interval
gravitational acceleration, G g, expressed in the local frame. (tk , tk +T ), with initial condition PII k|k . The state transition
Applying the expectation operator in the state propagation matrix Φ(tk + T, tk ) is similarly computed by numerical
equations (Eq. (6)) we obtain the equations for propagating integration of the differential equation
the estimates of the evolving IMU state:
Φ̇(tk + τ, tk ) = FΦ(tk + τ, tk ), τ ∈ [0, T ] (13)
˙ 1 ˙
G q̄ = 2 Ω(ω̂)G q̄ , b̂g = 03×1 ,
I ˆ I ˆ
with initial condition Φ(tk , tk ) = I15 .
G˙ 2 G
v̂I = CTq̂ â − 2ω G × v̂I − ω G ×
G
p̂I + g
G (9)
C. State Augmentation
˙
b̂a = 03×1 , G
p̂˙ I = G v̂I Upon recording a new image, the camera pose estimate is
computed from the IMU pose estimate as:
where for brevity we have denoted Cq̂ = C(IG q̄ˆ), â = am −
b̂a and ω̂ = ω m − b̂g − Cq̂ ω G . The linearized continuous- Cˆ C
G q̄ = I q̄ ⊗ IG q̄ˆ, and G
p̂C = G p̂I + CTq̂ I pC (14)
time model for the IMU error-state is:
where C
I q̄ is the quaternion expressing the rotation between
˙ IMU = FX
X  IMU + GnIMU (10) the IMU and camera frames, and I pC is the position of
the origin of the camera frame with respect to {I}, both of compute the measurement residual:
which are known. This camera pose estimate is appended (j) (j) (j)
to the state vector, and the covariance matrix of the EKF is ri = zi − ẑi (20)
augmented accordingly: where
Ci 
  T C X̂j
I I (j) 1 i
X̂j
Pk|k ← 6N +15 Pk|k 6N +15 (15) ẑi = ,  Ci Ŷj  = C(C
G q̄ )( p̂fj − p̂Ci )
iˆ G G
J J Ci Ẑ
j
Ci
Ŷj Ci
Ẑj
where the Jacobian J is derived from Eqs. (14) as:
   Linearizing about the estimates for the camera pose and
C C I q̄ 03×9 03×3 03×6N for the feature position, the residual of Eq. (20) can be
J= (16)
CTq̂ I pC × 03×9 I3 03×6N approximated as:
(j) (j)  (j) (j)
D. Measurement Model r H X i + H Gp f + n
Xi (21)
fi j i
(j) (j)
We now present the measurement model employed for up- In the preceding expression HXi and Hfi are the Jacobians
dating the state estimates, which is the primary contribution (j)
of the measurement zi with respect to the state and the
of this paper. Since the EKF is used for state estimation, feature position, respectively, and G p fj is the error in the
for constructing a measurement model it suffices to define position estimate of fj . The exact values of the Jacobians in

a residual, r, that depends linearly on the state errors, X, this expression are provided in [21]. By stacking the residuals
according to the general form: of all Mj measurements of this feature, we obtain:
 + noise
r = HX (17) (j) 
r(j)  H X
(j)
+ H Gp  f + n(j) (22)
X f j

In this expression H is the measurement Jacobian matrix, and (j) (j)

where r(j) , HX , Hf , and n(j) are block vectors or matrices
the noise term must be zero-mean, white, and uncorrelated (j) (j) (j) (j)
to the state error, for the EKF framework to be applied. with elements ri , HXi , Hfi , and ni , for i ∈ Sj . Since
To derive our measurement model, we are motivated by the the feature observations in different images are independent,
fact that viewing a static feature from multiple camera poses the covariance matrix of n(j) is R(j) = σim 2
I2Mj .
results in constraints involving all these poses. In our work, Note that since the state estimate, X, is used to compute
the camera observations are grouped per tracked feature, the feature position estimate (cf. Appendix), the error G p fj
rather than per camera pose where the measurements were  Thus, the residual
in Eq. (22) is correlated with the errors X.
recorded (the latter is the case, for example, in methods that
r(j) is not in the form of Eq. (17), and cannot be directly
compute pairwise constraints between poses [7], [13], [14]).
applied for measurement updates in the EKF. To overcome
All the measurements of the same 3D point are used to define (j)
this problem, we define a residual ro , by projecting r(j) on
a constraint equation (cf. Eq. (24)), relating all the camera (j)
poses at which the measurements occurred. This is achieved the left nullspace of the matrix Hf . Specifically, if we let
without including the feature position in the filter state vector. A denote the unitary matrix whose columns form the basis
We present the measurement model by considering the of the left nullspace of Hf , we obtain:
(j) 
case of a single feature, fj , that has been observed from a r(j)
o = A (z
T (j)
− ẑ(j) )  AT HX X + AT n(j) (23)
set of Mj camera poses (C G q̄, pCi ), i ∈ Sj . Each of the
i G
 (j) + n(j)
= H(j) X (24)
Mj observations of the feature is described by the model: o o
C (j)
(j) 1 i
Xj (j) Since the 2Mj × 3 matrix Hf has full column rank, its
zi = Ci + ni , i ∈ Sj (18) (j)
Zj Ci Yj left nullspace is of dimension 2Mj − 3. Therefore, ro is
(j) a (2Mj − 3) × 1 vector. This residual is independent of
where ni is the 2 × 1 image noise vector, with covariance the errors in the feature coordinates, and thus EKF updates
(j) 2
matrix Ri = σim I2 . The feature position expressed in the can be performed based on it. Eq. (24) defines a linearized
camera frame, pfj , is given by:
Ci
constraint between all the camera poses from which the
C  feature fj was observed. This expresses all the available
i
Xj (j)
pfj =  Ci Yj  = C(C information that the measurements zi provide for the Mj
G q̄)( pfj − pCi )
Ci i G G
(19)
Ci
Zj states, and thus the resulting EKF update is optimal, except
for the inaccuracies caused by linearization.
where G pfj is the 3D feature position in the global frame.
Since this is unknown, in the first step of our algorithm It should be mentioned that in order to compute the
(j) (j)
we employ least-squares minimization to obtain an estimate, residual ro and the measurement matrix Ho , the unitary
G
p̂fj , of the feature position. This is achieved using the matrix A does not need to be explicitly evaluated. Instead,
(j) (j)
measurements zi , i ∈ Sj , and the filter estimates of the projection of the vector r and the matrix HX on the
(j)
the camera poses at the corresponding time instants (cf. nullspace of Hf can be computed very efficiently using
Appendix). Givens rotations [22], in O(Mj2 ) operations. Additionally,
Once the estimate of the feature position is obtained, we since the matrix A is unitary, the covariance matrix of the
 (j) (j)
noise vector no is given by: where ro and no are vectors with block elements ro and
(j)
no , j = 1 . . . L, respectively, and HX is a matrix with
E{n(j)
o no
(j)T 2
} = σim 2
AT A = σim I2Mj −3 (j)
block rows HX , j = 1 . . . L.
The residual defined in Eq. (23) is not the only possible Since the feature measurements are statistically indepen-
(j)
expression of the geometric constraints that are induced by dent, the noise vectors no are uncorrelated. Therefore,
observing a static feature in Mj images. An alternative the covariance matrix of the noise vector no is equal to
2 L
approach would be, for example, to employ the epipolar Ro = σim Id , where d = j=1 (2Mj − 3) is the dimension
constraints that are defined for each of the Mj (Mj − 1)/2 of the residual ro . One issue that arises in practice is that d
pairs of images. However, the resulting Mj (Mj − 1)/2 can be a quite large number. For example, if 10 features are
equations would still correspond to only 2Mj − 3 indepen- seen in 10 camera poses each, the dimension of the residual
dent constraints, since each measurement is used multiple is 170. In order to reduce the computational complexity of
times, rendering the equations statistically correlated. Our the EKF update, we employ the QR decomposition of the
experiments have shown that employing linearization of the matrix HX [9]. Specifically, we denote this decomposition
epipolar constraints results in a significantly more complex as 
implementation, and yields inferior results compared to the
 TH
HX = Q1 Q2
approach described above. 0

E. EKF Updates where Q1 and Q2 are unitary matrices whose columns

form bases for the range and nullspace of HX , respectively,
In the preceding section, we presented a measurement
and TH is an upper triangular matrix. With this definition,
model that expresses the geometric constraints imposed by
Eq. (25) yields:
observing a static feature from multiple camera poses. We 
now present in detail the update phase of the EKF, in which
 TH
ro = Q 1 Q2
 + no ⇒
X (26)
the constraints from observing multiple features are used. 0
 T   T
EKF updates are triggered by one of the following two Q1 ro TH  Q1 no
events: = X+ (27)
QT2 ro 0 QT2 no
• When a feature that has been tracked in a number of
images is no longer detected, then all the measurements From the last equation it becomes clear that by projecting the
of this feature are processed using the method presented residual ro on the basis vectors of the range of HX , we retain
in Section III-D. This case occurs most often, as features all the useful information in the measurements. The residual
move outside the camera’s field of view. QT2 ro is only noise, and can be completely discarded. For
• Every time a new image is recorded, a copy of the
this reason, instead of the residual shown in Eq. (25), we
current camera pose estimate is included in the state employ the following residual for the EKF update:
vector (cf. Section III-C). If the maximum allowable  + nn
rn = QT1 ro = TH X (28)
number of camera poses, Nmax , has been reached,
at least one of the old ones must be removed. Prior In this expression nn = QT1 no is a noise vector whose
2
to discarding states, all the feature observations that covariance matrix is equal to Rn = QT1 Ro Q1 = σim Ir ,
occurred at the corresponding time instants are used, with r being the number of columns in Q1 . The EKF update
in order to utilize their localization information. In our proceeds by computing the Kalman gain:
algorithm, we choose Nmax /3 poses that are evenly  −1
K = PTTH TH PTTH + Rn (29)
spaced in time, starting from the second-oldest pose.
These are discarded after carrying out an EKF update while the correction to the state is given by the vector
using the constraints of features that are common to
∆X = Krn (30)
these poses. We have opted to always keep the oldest
pose in the state vector, because the geometric con- Finally, the state covariance matrix is updated according to:
straints that involve poses further back in time typically T
correspond to larger baseline, and hence carry more Pk+1|k+1 = (Iξ − KTH ) Pk+1|k (Iξ − KTH ) + KRn KT
valuable positioning information. This approach was (31)
shown to perform very well in practice. where ξ = 6N +15 is the dimension of the covariance matrix.
We hereafter discuss the update process in detail. Consider It is interesting to examine the computational complex-
that at a given time step the constraints of L features, selected ity of the operations needed during the EKF update. The
by the above two criteria, must be processed. Following the residual rn , as well as the matrix TH , can be computed
procedure described in the preceding section, we compute a using Givens rotations in O(r2 d) operations, without the
(j)
residual vector ro , j = 1 . . . L, as well as a corresponding need to explicitly form Q1 . On the other hand, Eq. (31)
(j)
measurement matrix Ho , j = 1 . . . L for each of these involves multiplication of square matrices of dimension ξ,
features (cf. Eq. (23)). By stacking all residuals in a single an O(ξ 3 ) operation. Therefore, the cost of the EKF update
vector, we obtain: is max(O(r2 d), O(ξ 3 )). If, on the other hand, the residual
vector ro was employed, without projecting it on the range of
 + no
ro = HX X (25) HX , the computational cost of computing the Kalman gain
would have been O(d3 ). Since typically d  ξ, r, we see produces pose and velocity estimates that are consistent,
that the use of the residual rn results in substantial savings and can operate reliably over long trajectories, with varying
in computation. motion profiles and density of visual features. Unfortunately,
simulation data cannot be included in this paper, due to
F. Discussion space limitations. Instead, we here present the results of
We now study some of the properties of the described the algorithm in an outdoor experiment, which demonstrates
algorithm. As shown in the previous section, the filter’s that the method is capable of long-term operation in a real-
computational complexity is linear in the number of ob- world setting. Additional datasets of real-world experiments,
served features, and at most cubic in the number of states as well as simulation results of the use of our algorithm, can
that are included in the state vector. Thus, the number of be found in [21].
poses that are included in the state is the most significant The experimental setup consisted of a camera/IMU sys-
factor in determining the computational cost of the algorithm. tem, placed on a car that was moving on the streets of
Since this number is a selectable parameter, it can be tuned a typical residential area in Minneapolis, MN. The system
according to the available computing resources, and the comprised a Pointgrey FireFly camera, registering images of
accuracy requirements of a given application. If required, resolution 640 × 480 pixels at 3Hz, and an Inertial Science
the length of the filter state can be also adaptively controlled ISIS IMU, providing inertial measurements at a rate of
during filter operation, to adjust to the varying availability 100Hz. During the experiment all data were stored on a
of resources. computer and processing was done off-line. Some example
One source of difficulty in recursive state estimation with images from the recorded sequence are shown in Fig. 1,
camera observations is the nonlinear nature of the measure- while a video of all 1598 images, which were recorded in
ment model. Vision-based motion estimation is very sensitive about 9 minutes of driving, can be found online at [26].
to noise, and, especially when the observed features are at For the results shown here, feature extraction and matching
large distances, false local minima can cause convergence was performed using the SIFT algorithm [27]. During this
to inconsistent solutions [23]. The problems introduced by run, a maximum of 30 camera poses was maintained in
nonlinearity have been addressed in the literature using the filter state vector. Since features were rarely tracked
techniques such as Sigma-point Kalman filtering [24], par- for more than 30 images, this number was sufficient for
ticle filtering [4], and the inverse depth representation for utilizing most of the available constraints between states,
features [25]. Two characteristics of the described algorithm while attaining real-time performance. Even though images
increase its robustness to linearization inaccuracies: (i) the in- were only recorded at 3Hz due to limited hard disk space on
verse feature depth parametrization used in the measurement the test system, the estimation algorithm is able to process
model (cf. Appendix) and (ii) the delayed linearization of the dataset at 14Hz, on a single core of an Intel T7200
measurements [17]. By the algorithm’s construction, multiple processor (2GHz clock rate). During the experiment, a total
observations of each feature are collected, prior to using them of 142903 features were successfully tracked and used for
for EKF updates, resulting in more accurate evaluation of the EKF updates, along a 3.2km-long trajectory. A GPS sensor
measurement Jacobians. was not available during the experiment, and therefore no
One interesting observation is that in typical image se- ground-truth trajectory data exists. However, the quality of
quences, most features can only be reliably tracked over a the position estimates can be evaluated using a map of the
small number of frames (“opportunistic” features), and only area.
few can be tracked for long periods of time, or when re- In Fig. 2, the estimated trajectory is plotted on a map of the
visiting places (persistent features). This is due to the limited neighborhood where the experiment took place. We observe
field of view of cameras, as well as occlusions, image noise, that this trajectory follows the street layout quite accurately
and viewpoint changes, that result in failures of the feature and, additionally, the position errors that can be inferred from
tracking algorithms. As previously discussed, if all the poses this plot agree with the 3σ bounds shown in Fig 3(a). The
in which a feature has been seen are included in the state final position estimate, expressed with respect to the starting
vector, then the proposed measurement model is optimal, pose, is X̂final = [−7.92 13.14 − 0.78]T m. From the
except for linearization inaccuracies. Therefore, for realistic initial and final parking spot of the vehicle it is known that
image sequences, the proposed algorithm is able to optimally the true final position expressed with respect to the initial
use the localization information of the opportunistic features. pose is approximately Xfinal = [0 7 0]T m. Thus, the
Moreover, we note that the state vector Xk is not required final position error is approximately 10m in a trajectory of
to contain only the IMU and camera poses. If desired, the 3.2km, i.e., an error of 0.31% of the travelled distance. This
persistent features can be included in the filter state, and is remarkable, given that the algorithm does not utilize loop
used for SLAM. This would further improve the attainable closing, and uses no prior information (for example, non-
localization accuracy, within areas with lengthy loops. holonomic constraints or a street map) about the car motion.
Moreover, it is worth pointing out that the camera motion
IV. E XPERIMENTAL RESULTS is almost parallel to the optical axis, a condition which
The algorithm described in the preceding sections has is particularly adverse for image-based motion estimation
been tested extensively both in simulation and with real data. algorithms [23]. In Figs. 3(b) and 3(c), the 3σ bounds for
Our simulation experiments have verified that the algorithm the errors in the IMU attitude and velocity along the three
Fig. 1. Some images from the dataset used for the experiment. The entire Fig. 2. The estimated trajectory overlaid on a map of the area where the
video sequence can be found at [26]. experiment took place. The initial position of the car is denoted by a red
square, and the scale of the map is shown on the top left corner.
axes are shown. From these, we observe that the algorithm
and laser scanner data).
obtains accuracy (3σ) better than 1o for attitude, and better
than 0.35m/sec for velocity in this particular experiment. A PPENDIX
The results shown here demonstrate that the proposed al- To compute an estimate of the position of a tracked feature
gorithm is capable of operating in a real-world environment, fj we employ intersection [28]. To avoid local minima, and
and producing very accurate pose estimates in real-time. We for better numerical stability, during this process we use an
should point out that in the dataset presented here several inverse-depth parametrization of the feature position [25]. In
moving objects appear, such as cars, pedestrians, and trees particular, if {Cn } is the camera frame in which the feature
whose leaves move in the wind. The algorithm is able to was observed for the first time, then the feature coordinates
discard the outliers which arise from visual features detected with respect to the camera at the i-th time instant are:
on these objects, using a simple Mahalanobis distance test.
Robust outlier rejection is facilitated by the fact that multiple
Ci
pfj = C(C i
Cn q̄)
Cn
pfj + Ci pCn , i ∈ Sj (32)
observations of each feature are available, and thus visual
In this expression C(C i
Cn q̄) and
Ci
pCn are the rotation and
features that do not correspond to static objects become
translation between the camera frames at time instants n and
easier to detect. As a final remark, we note that the described
i, respectively. Eq. (32) can be rewritten as:
method can be used either as a stand-alone pose estimation
  Cn X  
algorithm, or combined with additional sensing modalities Cn
j

to provide increased accuracy. For example, if a GPS sensor   Cn Z j

 1 
Ci
pfj = Cn
Zj C(C
Cn q̄)  C
i Yj  +
Cn Z
Ci
pCn  (33)
was available during this experiment, its measurements could n Zj
j
be used to compensate for position drift. 1
   
αj
V. C ONCLUSIONS = Cn Zj C(C i
Cn q̄)  βj  + ρj Ci
pCn  (34)
In this paper we have presented an EKF-based estima- 1
 
tion algorithm for real-time vision-aided inertial navigation. hi1 (αj , βj , ρj )
The main contribution of this work is the derivation of a = Cn Zj hi2 (αj , βj , ρj ) (35)
measurement model that is able to express the geometric hi3 (αj , βj , ρj )
constraints that arise when a static feature is observed from
multiple camera poses. This measurement model does not In the last expression hi1 , hi2 and hi3 are scalar functions
require including the 3D feature positions in the state vector of the quantities αj , βj , ρj , which are defined as:
of the EKF, and is optimal, up to the errors introduced Cn
Xj C1
Yj 1
by linearization. The resulting EKF-based pose estimation αj = Cn Z
, βj = Cn Z
, ρj = Cn Z
, (36)
j j j
algorithm has computational complexity linear in the number
of features, and is capable of very accurate pose estimation in Substituting from Eq. (35) into Eq. (18) we can express the
large-scale real environments. In this paper the presentation measurement equations as functions of αj , βj and ρj only:

has only focused on fusing inertial measurements with vi- (j) 1 hi1 (αj , βj , ρj ) (j)
sual measurements from a monocular camera. However, the zi = + ni (37)
hi3 (αj , βj , ρj ) hi2 (αj , βj , ρj )
approach is general and can be adapted to different sensing
(j)
modalities both for the proprioceptive, as well as for the Given the measurements zi , i ∈ Sj , and the estimates
exteroceptive measurements (e.g., for fusing wheel odometry for the camera poses in the state vector, we can obtain
 Position 3 σ (m)
Attitude 3 σ (degrees) Velocity 3 σ (m/sec)
15
0.1 0.4
10
x

0.05 0.2

x
5

0 0 0
0 100 200 300 400 500 600 0 100 200 300 400 500 600 0 100 200 300 400 500 600
20 0.2 0.4

10
y

0.1 0.2

y
0 0 0
0 100 200 300 400 500 600 0 100 200 300 400 500 600 0 100 200 300 400 500 600
2 1.5 0.02

1
1
z

0.01

z
0.5

0 0 0
0 100 200 300 400 500 600 0 100 200 300 400 500 600 0 100 200 300 400 500 600
Time (sec) Time (sec) Time (sec)

(a) (b) (c)

Fig. 3. The 3σ bounds for the errors in the position, attitude, and velocity. The plotted values are 3-times the square roots of the corresponding diagonal
elements of the state covariance matrix. Note that the EKF state is expressed in ECEF frame, but for plotting we have transformed all quantities in the
initial IMU frame, whose x axis is pointing approximately south, and its y axis east.

estimates for α̂j , β̂j , and ρ̂j , using Gauss-Newton least- [12] R. J. Prazenica, A. Watkins, and A. J. Kurdila, “Vision-based kalman
squares minimization. Then, the global feature position is filtering for aircraft state estimation and structure from motion,” in
Proceedings of the AIAA Guidance, Navigation, and Control Confer-
computed by: ence, no. AIAA 2005-6003, San Fransisco, CA, Aug. 15-18 2005.
  [13] R. Garcia, J. Puig, P. Ridao, and X. Cufi, “Augmented state Kalman
α̂j filtering for AUV navigation,” in IEEE International Conference on
1 nˆ 
G
p̂fj = CT (C G q̄ ) β̂j  + G p̂Cn (38) Robotics and Automation, Washington D.C., 2002, pp. 4010–4015.
ρ̂j [14] R. Eustice, H. Singh, J. Leonard, M. Walter, and R. Ballard, “Visu-
1 ally navigating the RMS Titanic with SLAM information filters,” in
We note that during the least-squares minimization process Proceedings of Robotics: Science and Systems, Cambridge, MA, June
2005.
the camera pose estimates are treated as known constants, [15] A. I. Mourikis and S. I. Roumeliotis, “On the treatment of relative-pose
and their covariance matrix is ignored. As a result, the min- measurements for mobile robot localization,” in Proceedings of the
IEEE International Conference on Robotics and Automation, Orlando,
imization can be carried out very efficiently, at the expense FL, May 15-19 2006, pp. 2277 – 2284.
of the optimality of the feature position estimates. Recall, [16] D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry for ground
however, that up to a first-order approximation, the errors in vehicle applications,” Journal of Field Robotics, vol. 23, no. 1, pp.
3–20, January 2006.
these estimates do not affect the measurement residual (cf. [17] P. McLauchlan, “The variable state dimension filter,” Centre for
Eq. (23)). Thus, no significant degradation of performance Vision, Speech and Signal Processing, University of Surrey, UK, Tech.
is inflicted. Rep., 1999.
[18] M. C. Deans, “Maximally informative statistics for localization and
R EFERENCES mapping,” in IEEE International Conference on Robotics and Automa-
tion, Washington D.C., May 2002, pp. 1824–1829.
[1] J. W. Langelaan, “State estimation for autonomous flight in cluttered [19] W. G. Breckenridge, “Quaternions proposed standard conventions,”
environments,” Ph.D. dissertation, Stanford University, Department of JPL, Tech. Rep. INTEROFFICE MEMORANDUM IOM 343-79-
Aeronautics and Astronautics, 2006. 1199, 1999.
[2] D. Strelow, “Motion estimation from image and inertial measure- [20] A. B. Chatfield, Fundamentals of High Accuracy Inertial Navigation.
ments,” Ph.D. dissertation, Carnegie Mellon University, 2004. Reston, VA: AIAA, 1997.
[3] L. L. Ong, M. Ridley, J. H. Kim, E. Nettleton, and S. Sukkarieh, [21] A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman
“Six DoF decentralised SLAM,” in Australasian Conf. on Robotics filter for vision-aided inertial navigation,” Dept. of Computer Sci-
and Automation, Brisbane, Australia, December 2003, pp. 10–16. ence and Engineering, University of Minnesota, Tech. Rep., 2006,
[4] E. Eade and T. Drummond, “Scalable monocular SLAM,” in IEEE www.cs.umn.edu/∼mourikis/tech reports/TR MSCKF.pdf.
Computer Society Conference on Computer Vision and Pattern Recog- [22] G. Golub and C. van Loan, Matrix computations. The Johns Hopkins
nition, June 17-26 2006, pp. 469 – 476. University Press, London, 1996.
[5] A. Chiuso, P. Favaro, H. Jin, and S. Soatto, “Structure from motion [23] J. Oliensis, “A new structure-from-motion ambiguity,” IEEE Transac-
causally integrated over time,” IEEE Transactions on Pattern Analysis tions on Pattern Analysis and Machine Intelligence, vol. 22, no. 7, pp.
and Machine Intelligence, vol. 24, no. 4, pp. 523–535, April 2002. 685–700, July 2000.
[6] A. J. Davison and D. W. Murray, “Simultaneous localisation and map- [24] A. Huster, “Relative position sensing by fusing monocular vision
building using active vision,” IEEE Transactions on Pattern Analysis and inertial rate sensors,” Ph.D. dissertation, Department of Electrical
and Machine Intelligence, vol. 24, no. 7, pp. 865 – 880, July 2002. Engineering, Stanford University, 2003.
[7] S. Roumeliotis, A. Johnson, and J. Montgomery, “Augmenting inertial [25] A. D. J. Montiel, J. Civera, “Unified inverse depth parametrization for
navigation with image-based motion estimation,” in IEEE Interna- monocular slam,” in Proceedings of Robotics: Science and Systems,
tional Conference on Robotics and Automation, Washington D.C., Philadelphia, PA, June 2006.
2002, pp. 4326–33. [26] http://www.cs.umn.edu/∼mourikis/icra07video.htm.
[8] D. D. Diel, “Stochastic constraints for vision-aided inertial navigation,” [27] D. G. Lowe, “Distinctive image features from scale-ivnariant key-
Master’s thesis, MIT, January 2005. points,” International Journal of Computer Vision, vol. 60, no. 2, pp.
[9] D. S. Bayard and P. B.Brugarolas, “An estimation algorithm for vision- 91–100, 2004.
based exploration of small bodies in space,” in American Control [28] B. Triggs, P. McLauchlan, R. Hartley, and Fitzgibbon, “Bundle ad-
Conference, June 8-10 2005, pp. 4589 – 4595. justment – a modern synthesis,” in Vision Algorithms: Theory and
[10] S. Soatto, R. Frezza, and P. Perona, “Motion estimation via dynamic Practice. Springer Verlag, 2000, pp. 298–375.
vision,” IEEE Transactions on Automatic Control, vol. 41, no. 3, pp.
393–413, March 1996.
[11] S. Soatto and P. Perona, “Recursive 3-d visual motion estimation
using subspace constraints,” IEEE Transactions on Automatic Control,
vol. 22, no. 3, pp. 235–259, 1997.