IMU-RGBD Camera Extrinsic Calibration:Observability Analysis and Consistency Improvement

Chao X. Guo and Stergios I. Roumeliotis

Abstract— In this paper, we address the problem of ex-trinsically calibrating an inertial measurement unit (IMU)with respect to an RGBD sensor. In particular, we study theobservability of the nonlinear IMU-RGBD calibration systemand prove that the calibration parameters are observable givenobservations to a single point feature. Moreover, we show thatthe system has four unobservable directions corresponding tothe global translation and rotations about the gravity vector.Based on the results of the observability analysis, we design aconsistency-improved, observability constrained (OC) extendedKalman filter (EKF)-based estimator for calibrating the sensorpair while at the same time tracking its pose and creating a 3Dmap of the environment. Finally, we validate the key findingsof the observability analysis and assess the performance of theOC-EKF estimator both in simulation and experimentally.

I. INTRODUCTION AND RELATED WORK

In many robotic applications (e.g., search and rescue,environmental monitoring, planetary exploration, etc.), it isnecessary to precisely know the robot’s 3D position andorientation (pose). Over short periods of time, a robotcan track its pose by fusing rotational velocity and linearacceleration measurements provided by an IMU. However,the integration of the bias and noise contaminating the IMUsignals, makes the resulting estimates unreliable. For thisreason, most inertial navigation systems (INS) rely on GPSmeasurements for bounding the estimation errors. For robotsoperating in GPS-denied areas, alternative sensors, such ascameras, have been employed for aiding INS, in what isknown as vision-aided (V)-INS. Specifically, by processingobservations of point features detected in images, a cameracan provide periodic corrections to the inertial estimates(e.g., [1]). Although the small size, low weight and powerrequirements of cameras make them ideal for mobile robots,their inability to provide the depth to the scene poses certainchallenges. In particular, a feature needs to be reliablytracked across multiple images and the baseline betweenthem must be accurately estimated in order to determine thedepth to it. Moreover, imprecise depth estimation can causelarge linearization errors which can reduce the estimationaccuracy and even lead to divergence.

An alternative to V-INS would be to fuse IMU measure-ments with observations from an RGBD camera, such asthe Microsoft kinect [2], which directly measures the depthto the scene. To do so, however, one needs to precisely

The authors are with the Department of Computer Science & En-gineering, University of Minnesota, Minneapolis, MN 55455, USAchaguo|stergios@cs.umn.edu. This work was supported bythe University of Minnesota through the Digital Technology Center (DTC)and AFOSR MURI FA9550-10-1-0567.

determine the 6 degrees of freedom (dof) transformation be-tween the two sensors. Although numerous algorithms havebeen developed for IMU-noise characterization (e.g., [3]) andfor intrinsically calibrating cameras (e.g., [4]) and RGBDsensors (e.g., [5]), to the best of our knowledge, there existsno algorithm for IMU-RGBD extrinsic calibration. Instead,one could potentially use methods designed for IMU-cameracalibration (e.g., [6], [7], [8], [9], [10]). However, thesewould result in sub-optimal calibration accuracy since theywould neglect the depth information available in the RGBDdata. Furthermore, to date the conditions under which theIMU-RGBD calibration parameters are observable are notknown. Previous efforts to study the observability propertiesof the IMU-camera calibration system have either reliedon known calibration targets [8], or have assumed that thecamera’s roll and pitch angles are perfectly known and atleast 3 point features are detected by the camera [9].

In order to address these limitations, this paper makes thefollowing key contributions:

• We study the observability of the nonlinear systemdescribing the IMU-RGBD camera calibration and forthe first time prove that although the system is unobserv-able, the unknown 6-dof transformation lies within itsobservable subspace and can be determined even whenonly one (previously unknown) point feature is detected.In this analysis, we do not make any assumptions aboutthe prior location of the feature or the pose of the cam-era and do not rely on inferred camera measurements asis the case in [8], [9]. This is achieved by employing anew methodology for studying the observability of non-linear systems [11] which employs appropriate changeof variables for isolating the unobservable modes of thesystem.

• We analytically determine the vectors spanning theunobservable directions of the IMU-RGBD calibrationsystem and use them in order to improve the consis-tency of the EKF designed for estimating the unknowncalibration parameters.

• We validate the performance of the developed estimator,both in simulation and experimentally, when performingsimultaneous localization, mapping, and IMU-RGBDcalibration.

The rest of the paper is structured as follows. In Section II-A, we briefly describe the method of Hermann and Krener[12] for studying the observability of nonlinear systems anddiscuss the challenges for proving that a system is unobserv-able when following this method. In Section II-B, we present

the approach introduced in [11] that employs change ofvariables to appropriately factorize the observability matrixand determine its unobservable directions. In Section III,we apply this method to the specific problem of IMU-RGBD calibration and show that although the calibrationparameters are observable, the global position and rotationabout the gravity vector are not. In Section IV, we present theobservability-constrained (OC) EKF algorithm we developedfor determining the unknown transformation and brieflydiscuss how its consistency [13] is improved using the resultsof Section III. In Section V, we assess the performance ofthe OC-EKF algorithm both in simulation and experimentallyand verify its consistency. Finally, Section VI, presents theconclusions of this work and provides possible directions offuture research.

II. NONLINEAR SYSTEM OBSERVABILITY ANALYSIS

In this section, we provide a brief overview of the methodin [12] for studying the observability of nonlinear systemsand then introduce a new methodology for determining itsunobservable directions.

A. Observability Analysis with Lie Derivatives

Consider a nonlinear, continuous-time system:x = f0(x) +

∑li=1 fi(x)ui

y = h(x)(1)

where u =[u1 . . . ul

]Tis its control input, x =[

x1 . . . xm]T

is the system’s state vector, y is the systemoutput, and fi, i = 0, . . . , l are the process functions. Thezeroth-order Lie derivative of a measurement function h isdefined as the function itself [12]:

L0h = h(x) (2)

while the span of the ith order Lie derivative is defined as:

∇Lih =[∂Lih∂x1

∂Lih∂x2

. . . ∂Lih∂xm

](3)

For any ith order Lie derivative, Lih, the i+ 1th order Liederivative Li+1

fjh with respect to a process function fj can

be computed as:

Li+1fj

h = ∇Lih · fj (4)

The observability matrix O of system (1) is defined as amatrix with block rows the span of the Lie derivatives of(1), i.e.,

O =

∇L0h∇L1

fih

∇L2fifj

h

∇L3fifjfk

h...

(5)

where i, j, k = 1, . . . , l. Based on [12], to prove that asystem is observable, it suffices to show that any submatrixof O comprising a subset of its rows is of full columnrank. In contrast, to prove that a system is unobservableand find its unobservable directions, we need to: (i) showthat the infinitely many block rows of O can be written as

a linear combination of a subset of its block rows, whichform a submatrix O′. (ii) find the nullspace of O′ in orderto determine the system’s unobservable directions. Althoughaccomplishing (ii) is fairly straightforward, achieving (i)is extremely challenging especially for high-dimensionalsystems, such as the one describing the IMU-RGBD cameracalibration.

To address this issue, in the following section, we presenta new methodology that relies on change of variables forproving that a system is unobservable and finding its unob-servable directions.

B. Observability Analysis with Basis Functions

We start by proving the following:Theorem 1: Assume that there exists a nonlinear trans-

formation β(x) =[β1(x)T . . . βt(x)T

]T(i.e., a set of

basis functions) of the variable x in (1), such that:(A1) β1(x) = h(x);(A2) ∂β

∂x · fi, i = 0, . . . , l is a function of β;(A3) The system:

β = g0(β) +∑li=1 gi(β)ui

y = h = β1(6)

where gi(β) = ∂β∂x fi(x), i = 0, . . . , l, is observable.

Then:(i) The observability matrix of (1) can be factorized as:

O = Ξ · B, where Ξ is the observability matrix of system(6) and B , ∂β

∂x

(ii) null(O) = null(B)

Proof: (i) Based on the chain rule, the span of any Liederivative ∇Lih can be written as:

∇Lih =∂Lih

∂x=∂Lih

∂β

∂β

∂x(7)

Thus the observability matrix O of (1) can be factorized as:

O =

∇L0h∇L1

fih

∇L2fifj

h

∇L3fifjfk

h...

=

∂L0h∂β

∂L1fih

∂β∂L2

fifjh

∂β∂L3

fifj fkh

∂β...

∂β

∂x= Ξ ·B (8)

Next we prove that Ξ is the observability matrix of thesystem (6) by induction.

To distinguish the Lie derivatives of system (1), let Idenote the Lie derivatives of system (6). Then, the span ofits zeroth order Lie derivative is:

∇I0h =∂h

∂β=∂L0h

∂β(9)

which corresponds to the first block row of Ξ.Assume that the span of the ith order Lie derivative of (6)

along any direction can be written as ∇Iih = ∂Lih∂β , which

corresponds to a block row of Ξ.Then the span of the i+ 1th order Lie derivative ∇Ii+1

gjh

with respect to the process function gj can be computed as:

∇Ii+1gj

h =∂Ii+1

gjh

∂β=∂(∇Iih · gj)

∂β=∂(∂L

ih∂β ·

∂β∂x fj(x))

∂β

=∂(∂L

ih∂x · fj(x))

∂β=∂Li+1

fjh

∂β(10)

which is also a block row of Ξ. Therefore, we conclude Ξ isa matrix whose rows are the span of all the Lie derivativesof system (6), thus the observability matrix of system (6).

Proof: (ii) From O = ΞB, we have null(O) = null(B)+null(Ξ) ∩ range(B). Moreover, from condition A3 system(6) is observable, and Ξ is of full column rank. Thereforenull(O) = null(B).

With the basis functions, the unobservable directions canbe determined with much less effort. To find a system’sunobservable directions, we first need to define the basisfunctions that satisfy conditions (A1) and (A2), and provethat the infinite-dimensional matrix Ξ has full column rank,which is condition (A3). Once all the conditions are satisfied,the unobservable directions of (1) correspond to the nullspaceof matrix B, which has finite dimension and thus it is easyto analyze.

In the following sections, we will leverage Theorem 1to prove that the IMU-RGBD camera calibration system isunobservable and find its unobservable directions. To do this,in Section III-A we first show the formulation of the IMU-RGBD camera calibration nonlinear system. In Section III-B, we find the set of basis functions that satisfy conditions(A1) and (A2) of Theorem 1, and construct the basis functionsystem as in (6) for this particular problem. In Section III-C, we prove that the observability matrix Ξ for the basisfunction system is of full column rank, which is condition(A3) in Theorem 1. Lastly, we determine the unobservabledirections of the IMU-RGBD camera calibration system byfinding the nullspace of matrix B.

III. OBSERVABILITY ANALYSIS OF THE IMU-RGBDCAMERA CALIBRATION SYSTEM

In this section, we present the observability analysis forthe IMU-RGBD camera calibration problem using basisfunctions. Since the Lie derivatives for all the landmarks haveidentical form, for simplicity we keep only one landmarkin the state vector. The extension to multiple landmarks isdescribed in [14].

A. System Model

In the IMU-RGBD camera calibration problem, the statevector we estimate is:

x =[IsTG

GvTIGpTI

GpTf bTa bTgCsTI

IpTC]T

where IsG is the Cayley-Gibbs-Rodriguez parameterization[15] representing the orientation of the global frame G inthe IMU’s frame of reference I, GvI and GpI representthe velocity and position of I in G, ba and bg are thebiases in the gyroscope and accelerometer measurements,CsI represents the orientation of I in the RGBD camera’s

frame of reference C, IpC is the position of C in I,and Gpf represents the estimated landmark in G.

1) System propagation model: The system model describ-ing the time evolution of the states is:

I sG(t) =1

2D (Iω(t)− bg(t))

GvI(t) = Ga(t) = Gg + C(IsG(t))T (Ia(t)− ba(t))GpI(t) = GvI(t)

Gpf(t) = 03×1

ba(t) = 03×1 bg(t) = 03×1C sI(t) = 04×1

IpC(t) = 03×1 (11)

where C(s) represents the corresponding rotation matrix ofs, Iω(t) =

[ω1 ω2 ω3

]Tand Ia(t) =

[a1 a2 a3

]Tare

the IMU rotational velocity and linear acceleration expressedin I, bg(t) and ba(t) are the gyroscope and accelerometerbiases, 1

2D , ∂s∂θ , D = (I + bs×c+ ssT ), and:

Ω(Iω(t)) =

[−bIω×c Iω−IωT 0

], Iω(t) =

[0 −ω3 ω2

ω3 0 −ω1

−ω2 ω1 0

]where θ = αk represents a rotation by an angle α aroundthe axis k.

2) System measurement model: The RGBD camera candirectly measure the 3D position of the landmark Cpf in itsown frame of reference as:

z = Cpf = C(CsI)C(IsG)(Gpf − GpI)−C(CsI)IpC

(12)

B. Determining the System’s Basis Functions

In this section, we define the basis functions for the IMU-RGBD camera calibration system that satisfy conditions(A1), (A2) in Theorem 1, and present system (6) with basisfunctions of this particular problem. In the next section, wewill show this system is observable.

For simplicity, we retain only a few of the subscripts andsuperscripts in the state elements and write the system statevector as:

x =[sT vT pT pTf bTa bTg sTC pTC

]TThen the IMU-RGBD camera calibration system can berewritten as:

svppf

ba

bg

sC

pC

=

− 12Dbg

g −CTba

v00000

︸ ︷︷ ︸

f0

+

12D0000000

︸ ︷︷ ︸

f1

ω +

0CT

000000

︸ ︷︷ ︸

f2

a

z = CCC (pf − p)−CCpC (13)

where C , C(s), CC , C(sC). Note that f0 is a 24 × 1vector, while f1 and f2 are both 24× 3 matrices which is a

compact way for representing three process functions:

f1ω = f11 ∗ ω1 + f12 ∗ ω2 + f13 ∗ ω3; (14)f2a = f21 ∗ a1 + f22 ∗ a2 + f23 ∗ a3;

To define the basis functions for this system, we follow theconditions of Theorem 1: (i) Select β1 as the measurementfunction z. (ii) Select the remaining basis functions so that∂β∂x · fi are functions of β for all the process functions. 1

The first basis is defined as the measurement function:

β1 , z = CCC(pf − p)−CCpC (15)

where β1 is a 3 × 1 vector representing in a compact form3 basis functions. The span of β1 with respect to x is:∂β1

∂x=[∂β1

∂θ∂θ∂s

∂β1

∂v∂β1

∂p∂β1

∂pf

∂β1

∂ba

∂β1

∂bg

∂β1

∂sC

∂β1

∂pC

]=[CCbC(pf−p)c ∂θ

∂s 0 −CCC CCC 0 0 bβ1c∂θC∂sC

−CC

]After the span of the first basis function β1 is obtained, weproject it onto all the process functions for finding otherbasis functions that satisfy condition (A2) of Theorem 1.Every time a term in the product can not be expressed bythe previously defined basis functions, we incorporate it asa new basis function:

∂β1

∂x· f0 = −CCbC(pf − p)cbg −CCCv

= −bβ1 + CCpCcCCbg −CCCv

, −bβ1 + β2cβ3 − β4

∂β1

∂x· f1i = CCbC(pf − p)cei

= bCCC(pf − p)cCCei

, bβ1 + β2cβ5i

∂β1

∂x· f2i = 0

where i = 1, 2, 3, e1 =[1 0 0

]T, e2 =

[0 1 0

]T,

e3 =[0 0 1

]T, ∂θ∂s

12D = ∂θ

∂s∂s∂θ = I3, β2 = CCpC ,

β3 = CCbg , β4 = CCCv and β5i = CCei, i = 1, 2, 3.

To make the newly defined basis functions satisfy con-dition (A2), we also project their spans on the processfunctions. The spans of the basis functions β2 and β3 are:

∂β2

∂x=[0 0 0 0 0 0 bCCpCc ∂θC

∂sCCC

]∂β3

∂x=[0 0 0 0 0 CC bCCbgc ∂θC

∂sC0]

Multiplying either one with any process function fi resultsin zero, thus no new basis functions need to be defined. Thespan of β4, as well as its projections on the process functionsare:

∂β4

∂x=[CCbCvc ∂θ

∂sCCC 0 0 0 0 bCCCvc ∂θC

∂sC0]

1Note that although the definition of the basis functions is not unique,any basis functions that satisfy the conditions of Theorem 1 span the samespace.

∂β4

∂x· f0 = −bCCCvcCCbg + CCCg −CCba

, −bβ4cβ3 + β6 − β7

∂β4

∂x· f1i = bCCCvcCCei , bβ4cβ5i

∂β4

∂x· f2i = CCei , β5i

where β6 = CCCg and β7 = CCba.Projecting the span of β5i or β7 onto any process function

results in zero. The span of β6, as well as its projections onthe process functions are:

∂β6

∂x=[CCbCgc∂θ∂s 0 0 0 0 0 bCCCgc∂θC

∂sC0]

∂β6

∂x· f0 = −CCbCgcbg = −bβ6cβ3

∂β6

∂x· f1i = CCbCgcei = bβ6cβ5i

∂β6

∂x· f2i = 0

which can all be expressed by the previously defined basisfunctions.

Thus, we have defined all the required bases for the IMU-RGBD camera calibration problem:

β1 = CCC(pf − p)−CCpC β2 = CCpC

β3 = CCbg β4 = CCCv

β5i = CCei β6 = CCCg

β7 = CCba

which correspond to the landmark position, IMU position,gyroscope bias, IMU velocity, IMU orientation, gravity vec-tor, and accelerometer bias in the RGBD camera’s frame ofreference. Based on Theorem 1, the resulting system in thebasis functions is:

β1

β2

β3

β4

β51

β52

β53

β6

β7

=

−bβ1+β2cβ3−β4

00

−bβ4cβ3+β6−β709×1

−bβ6cβ30

︸ ︷︷ ︸

g0

+

bβ1+β2cβ5

00

bβ4cβ509×3

bβ6cβ50

︸ ︷︷ ︸

g1

ω +

000β5

09×300

︸ ︷︷ ︸

g2

a

z = β1 (16)

where β5 ,[βT51 βT52 βT53

]T. In the next section, we

will show that system (16) is observable by proving itsobservability matrix Ξ is of full column rank. Therefore,the basis functions β1 to β7 correspond to the system’sobservable modes.

C. Determining the System’s Observability MatrixIn this section, we determine the observability matrix O

of system (13) by computing the observability matrix Ξ ofsystem (16) and the derivatives of the basis functions Bseparately. First, we prove the matrix Ξ is of full columnrank. Then, we find the nullspace of matrix B, whichaccording to Theorem 1 correspond to the unobservabledirections of system (13).

Lemma 2: System (16) is observable.Proof: See Appendix I.Since system (16) is observable, we can find the unob-

servable directions of system (13) from the nullspace ofmatrix B.

Theorem 3: The IMU-RGBD camera extrinsic calibrationsystem (13) is unobservable, and its unobservable sub-spaceis spanned by 4 directions corresponding to the IMU-RGBDcamera global position and its rotation around the gravityvector in the global frame.

Proof: System (16) satisfies the conditions of Theorem1. Therefore, null(O) = null(B), which spans the unob-servable subspace of the original system (13). Stacking thederivatives of the basis functions with respect to the variablex, the matrix B is defined as:

CCbC(pf−p)c ∂θ∂s 0 −CCC CCC 0 0 (bCCC(pf−p)c−bCCpCc)

∂θC∂sC

−CC

0 0 0 0 0 0 bCCpCc∂θC∂sC

CC

0 0 0 0 0 CC bCCbgc∂θC∂sC

0

CCbCvc ∂θ∂s CCC 0 0 0 0 bCCCvc ∂θC

∂sC0

0 0 0 0 0 0 bCCe1c∂θC∂sC

0

0 0 0 0 0 0 bCCe2c∂θC∂sC

0

0 0 0 0 0 0 bCCe3c∂θC∂sC

0

CCbCgc ∂θ∂s 0 0 0 0 0 bCCCgc ∂θC

∂sC0

0 0 0 0 −CC 0 −bCCbac∂θC∂sC

0

(17)

We proceed by simplifying B using Gaussian elimination.Note that ∂θC

∂sCis a full rank matrix (its inverse is ∂sC

∂θC). Also

as proved in Appendix I, the matrix in the seventh columnbetween the fifth and seventh rows has full column rank.Therefore, all the other terms in the seventh column can beeliminated.

Let A =[AT

1 AT2 AT

3 AT4 AT

5 AT6 AT

7 AT8

]Tbe the right nullspace of the matrix B = ∂β

∂x . Then, weemploy the relation BA = 0 to determine the elements ofA. Specifically, from the second, third, fifth-seventh, ninthrows of the product BA, we have:

CCA8 = CCA6 = A7 = CCA5 = 0

⇒A5 = A6 = A7 = A8 = 0 (18)

From the eighth row of the product, we have the relationCCbCgc∂θ∂s A1 = 0, thus A1 = ∂s

∂θCg or A1 = 0.If A1 = ∂s

∂θCg, from the fourth row of BA we haveA2 = −bvcg, and from the first row we have A3 = −bpcgand A4 = −bpfcg.

If A1 = 0, from the fourth row of BA we have A2 = 0,and from the first row A3 = A4 = I.

In summary, the system’s unobservable directions arespanned by the matrix:

N ,

∂s∂θ

Cg 0−bvcg 0−bpcg I−bpfcg I

0 00 00 00 0

= [Ng Np] (19)

where the first row is the perturbing terms in s, while the

corresponding term for θ is Cg. Therefore, the unobservabledirections are the global position of the IMU-RGBD cameraand the point landmark, Np, and their rotation about thegravity vector, Ng .

IV. CALIBRATION ALGORITHM DESCRIPTION

We implement the IMU-RGBD camera extrinsic calibra-tion system (13) with the OC-EKF proposed in [13]. Specifi-cally, in [13] it was shown that due to the linearization errors,the filter gains spurious information along the unobservabledirections, which results in smaller uncertainty (that causesthe filter to be inconsistent) and larger estimation errors.To address this problem, the authors proposed to modifythe Jacobian matrices in both the OC-EKF’s propagationand update steps, so as to avoid changing the unobservabledirections due to the linearization errors.

We will briefly describe the methodology of the OC-EKFin the following; while the detailed derivations can be foundin [13]. In the OC-EKF, the spurious information is removedby enforcing the following observability constraints:

Nk+1 = ΦkNk (20)

HkNk = 0, ∀k > 0 (21)

where Nk and Nk+1 are the unobservable directions pre-sented in (19) at time step k and k + 1, Φk is the Jacobianmatrix of the system propagation equation (11) at time stepk and Hk is the Jacobian matrix of the system measurementequation (12) at time step k.

Due to the linearization error, the Jacobian matrices Φ′kand H′k evaluated using the current state estimates in generaldo not satisfy the observability constraints (20) and (21).However, we can update them using the following process:First, we propagate the state estimate to determine both theNk and Nk+1 in closed form. Then, we find a matrix Φk

that satisfies (20) and has the minimum distance to Φ′k inthe Frobenius norm sense, and use it for the covariancepropagation. Similarly, in the update step, we use the currentstate estimates to compute H′k and then find the closestmatrix Hk in the Frobenius norm sense satisfying (21).

V. SIMULATION AND EXPERIMENTAL RESULTS

We have employed the OC-EKF in both our simulationsand experiments, to: (i) Demonstrate the accuracy of our pro-posed IMU-RGBD camera extrinsic calibration algorithm.(ii) Show that by removing spurious information, the OC-EKF outperforms the standard EKF. (iii) Verify the system’sunobservable directions by demonstrating the improvementof the OC-EKF designed using these unobservable directions.

A. Simulation Results

In our simulation, the RGBD camera observes 20 land-marks, which are randomly generated in a plane. The IMUtravels in front of this plane with constant local rotationalvelocity

[2.8648 0 2.8648

]Tdeg/sec and linear accel-

eration[0 0.1 0

]Tm/sec2. The initial IMU velocity is

0 20 40 60 80 100−0.03

−0.02

−0.01

0

0.01

0.02

0.03

Time (s)

Err

or

about G

ravity V

ecto

r (d

eg)

Orientation Error and 3 Sigma Bounds

EKF

OC−EKF

(a)

0 20 40 60 80 1000

0.005

0.01

0.015

0.02

0.025

Time (s)

Tra

nsla

tion R

MS

E (

m)

EKF

OC−EKF

0 20 40 60 80 1000

0.01

0.02

0.03

0.04

0.05

Time (s)

Rota

tion R

MS

E (

deg)

EKF

OC−EKF

(b)

0 20 40 60 80 1000

0.2

0.4

0.6

0.8

Time (s)

Positio

n R

MS

E (

m)

EKF

OC−EKF

0 20 40 60 80 1000

0.2

0.4

0.6

0.8

1

Time (s)

Orienta

tion R

MS

E (

deg)

EKF

OC−EKF

(c)Fig. 1. The simulation results for OC-EKF and EKF: (a) Error and 3σ bounds for the rotation about the gravity vector in a single run. (b) RMSE for thetranslation and rotation between the two sensors over 20 Monte Carlo trials. (c) RMSE for the robot position and orientation over 20 Monte Carlo trials.

−1

0

1

2

3

40

1

2

3

4

5

−101

y (m)

3D POSITION

x (m)

z (

m)

(a)

0 10 20 30 40 500.4

0.5

0.6

EKF

OC−EKF

0 10 20 30 40 50−0.6

−0.5

−0.4

Rota

tion in Q

uate

rnio

n

0 10 20 30 40 50−0.6

−0.5

−0.4

0 10 20 30 40 500.4

0.5

0.6

Time (s)

(b)

0 10 20 30 40 50−0.1

0

0.1

EKF

OC−EKF

0 10 20 30 40 50−0.1

−0.05

0

Tra

nsla

tion (

m)

0 10 20 30 40 50−0.1

0

0.1

Time (s)

(c)Fig. 2. Experimental results: (a) The platform trajectory and estimated landmarks. (b) The rotation between IMU and kinect in quaternion representation.(c) The translation between the IMU and kinect.

set to[0.02 0 0.01

]Tm/sec. The rotation matrix and

translation of the IMU in the RGBD camera frame is set to[−1 0 0; 0 0 1; 0 1 0

]and

[.1 .1 .1

]Tm.

The initial standard deviation of the translation and rotationis set to 0.02 m and 2 degrees.

First, we examine the performance of the OC-EKF andEKF in a single run. The estimation error and 3σ bounds ofthe IMU’s orientation around the gravity vector are shown inFig. 1(a). As evident, the EKF gains spurious information,thus the covariance decreases along with time, while thecovariance of the OC-EKF does not. In addition, the EKFbecomes inconsistent gradually, while the OC-EKF remainsconsistent.

In Fig. 1(b) and Fig. 1(c), we present the results of 20Monte Carlo simulations. The estimation RMSE (root meansquare errors) for the IMU position, orientation and thetransformation between the two sensors are all smaller for theOC-EKF as compared to the EKF, which verifies the validityof our observability analysis based on which the OC-EKFwas designed.

B. Experimental Results

We compare the performance of the OC-EKF with theEKF in practice when performing simultaneous localization,mapping and extrinsic IMU-RGBD camera calibration. Inour experimental setup, we utilize a platform comprised ofan InterSense NavChip IMU and a kinect, which has bothan RGB camera and an infrared (IR) depth-finding camera.The intrinsic parameters of the kinect RGB camera andIR camera, as well as the transformation between the two

cameras, are calibrated offline with the algorithm describedin [5].

We train a vocabulary tree [16] with the SIFT featuresextracted in an indoor environment. During the experiments,the SIFT features of each image are passed through into thevocabulary tree’s leaves, and the leave indexes for each imageare recorded. Once a new image is captured, we pass all itsSIFT features through the vocabulary tree to its leaves, andfind the images with the most similar SIFT features. Thenthe identities of the SIFT features in the current image aredetermined by matching them to SIFT features in similarimages and employing a geometry consistency test. Since thetransformation between the IR camera and the RGB camerais known, the corresponding 3D points captured by the IRcamera can also be associated.

In our experiment, the IMU-kinect platform travels in anindoor office along a closed-loop trajectory, which means atthe end the platform comes back to its starting location. Thisinformation is used to access the estimated robot positionaccuracy. The experimental results are depicted in Fig. 2.The black line is the platform’s trajectory and the red starsdenote the estimated landmarks. The whole trajectory is 7.04m, and the final position error is 7.5 cm for the OC-EKF and8.1 cm for the EKF. We also plot the estimated translationand rotation between the IMU and kinect in Fig. 2. Asevident, the estimated transformation when using the OC-EKF is more stable than that from the EKF.

VI. CONCLUSION

In this paper, we present an observability analysis forthe IMU-RGBD camera calibration problem in an unknown

environment using Lie derivatives. Due to the requirementof finding the infinite dimensional observability matrix’snullspace, it is pretty challenge to determine a system’sunobservable directions. To address this problem, we de-compose the observability matrix into the product of twomatrices, Ξ and B, using a nonlinear transformation (basisfunction) of the original variables. Then we find the system’sunobservable directions from the nullspace of the matrix B,which has finite dimension. Moreover, we construct an ob-servable system using the basis functions whose observabilitymatrix is the first matrix Ξ, and show that the basis functionsare the IMU-RGBD camera calibration system’s observablemodes. We present extensive simulation and experimentalresults to show that the observability-constrained (OC)-EKFdesigned to adhere to the system’s unobservable directionssignificantly outperforms the EKF.

In our future work, we plan to perform observabilityanalysis of the IMU-camera extrinsic calibration system em-ploying the same methodology and improve its consistency.

APPENDIX I

In this section, we will compute the observability matrixof system (16), and show it is of full column rank; thussystem (16) is observable and the basis functions β are theobservable modes of the original system (13).

The zeroth order Lie derivative and its span are definedas:

L0h = β1

∇L0h =[∂L0h∂β1

∂L0h∂β2

∂L0h∂β3

∂L0h∂β4

∂L0h∂β5i

∂L0h∂β6

∂L0h∂β7

]= [I 0 0 0 03×9 0 0]

which is then used to compute the first-order Lie derivativesand their spans:

L1g0h = ∇L0h · g0 = [I 0 0 0 03×9 0 0] · g0

= −bβ1 + β2cβ3 − β4

L1g1ih = ∇L0h · g1i = [I 0 0 0 03×9 0 0] · g1i

= bβ1 + β2cβ5i

L1g2ih = ∇L0h · g2i = 0

∇L1g0h = [bβ3c bβ3c −bβ1 + β2c −I 03×9 0 0]

∇L1g11h = [−bβ51c −bβ51c 0 0 bβ1+β2c 03×6 0 0 ]

∇L1g12h = [−bβ52c −bβ52c 0 0 0 bβ1+β2c 0 0 0 ]

∇L1g13h = [−bβ53c −bβ53c 0 0 03×6 bβ1+β2c 0 0 ]

∇L1g2ih = 0

In the observability analysis, the objective is to prove thatmatrix Ξ is of full column rank. To do so, we only need toconsider a subset of the high-order Lie derivatives; the onesthat we will use for determining the rank of Ξ, Specifically,the selected second-order Lie derivatives and their gradientsare:

L2g0g2ih = −β5i

L2g0g0h = −bβ3cbβ1 + β2cβ3 − 2bβ3cβ4 − β6 + β7

∇L2g0g21h = [0 0 0 0 −I 03×6 0 0]

∇L2g0g22h = [0 0 0 0 0 −I 0 0 0]

∇L2g0g23h = [0 0 0 0 03×6 −I 0 0]

∇L2g0g0h =

[bβ3c2 bβ3c2 Π3 −2bβ3c 0 −I I

]where Π3 is the term that will be removed later on throughGaussian elimination, thus we do not show its explicitexpression.

The selected third-order Lie derivatives and their gradientsare:

L3g0g0g1ih = bβ3c2bβ1 + β2cβ5i − 2bβ3cbβ4cβ5i − bβ6cβ5i

L3g0g0g2ih = −2bβ3cβ5i

∇L3f0f0f1i

h

= [−bβ3c2bβ5ic −bβ3c2bβ5ic Π′′3i 2bβ3cbβ5ic Π5i bβ5ic 0 ]

∇L3g0g0g21h = [ 0 0 2bβ51c 0 −2bβ3c 03×6 0 0 ]

∇L3g0g0g22h = [ 0 0 2bβ52c 0 0 −2bβ3c 0 0 0 ]

∇L3g0g0g23h = [ 0 0 2bβ53c 0 03×6 −2bβ3c 0 0 ]

where Π′′3i and Π5i also do not affect the observabilitymatrix’s rank, thus we do not show their specific expressions.

By stacking together (as block rows) the span of theselected Lie derivatives, matrix Ξ is constructed as:

I 0 0 0 0 0 0 0 0bβ3c bβ3c −bβ1 + β2c −I 0 0 0 0 0−bβ51c −bβ51c 0 0 bβ1 + β2c 0 0 0 0−bβ52c −bβ52c 0 0 0 bβ1 + β2c 0 0 0−bβ53c −bβ53c 0 0 0 0 bβ1 + β2c 0 0

0 0 0 0 −I 0 0 0 00 0 0 0 0 −I 0 0 00 0 0 0 0 0 −I 0 0bβ3c2 bβ3c2 Π3 −2bβ3c 0 0 0 −I I

−bβ3c2bβ51c −bβ3c2bβ51c Π′′31 2bβ3cbβ51c Π′51 0 0 bβ51c 0−bβ3c2bβ52c −bβ3c2bβ52c Π′′32 2bβ3cbβ52c 0 Π′52 0 bβ52c 0−bβ3c2bβ53c −bβ3c2bβ53c Π′′33 2bβ3cbβ53c 0 0 Π′53 bβ53c 0

0 0 2bβ51c 0 −2bβ3c 0 0 0 00 0 2bβ52c 0 0 −2bβ3c 0 0 00 0 2bβ53c 0 0 0 −2bβ3c 0 0

(22)

Since the rank of a matrix does not change after Gaussianelimination, we employ this method to first simplify Ξ andthen show that the resulting matrix is of column rank.

We start by providing that K ,[bβ51cT bβ52cT bβ53cT

]Tis of full column rank.

To do so, we define a matrix M as:

M , CC

[[0 0 00 0 10 0 0

]CT

C

[0 0 00 0 01 0 0

]CT

C

[0 1 00 0 00 0 0

]CT

C

](23)

Recall that β5i = CCei. Multiplying M from the left by K:

MK = M

CCbe1cCCbe2cCCbe3c

CTC = CCI3C

TC = I3 (24)

which means the rank of MK is 3, thus K has full columnrank.

We now perform block-row Gaussian elimination on ma-trix Ξ in (22): (i) Using the identity matrix in the (1,1)block element of Ξ, all other terms in its first column canbe eliminated. Using the identity matrix in the fifth, sixth

and seventh columns, all other terms in these columns canbe eliminated. The resulting matrix is Ξ′:

I 0 0 0 0 0 0 0 00 bβ3c −bβ1 + β2c −I 0 0 0 0 00 −bβ51c 0 0 0 0 0 0 00 −bβ52c 0 0 0 0 0 0 00 −bβ53c 0 0 0 0 0 0 00 0 0 0 −I 0 0 0 00 0 0 0 0 −I 0 0 00 0 0 0 0 0 −I 0 00 bβ3c2 Π3 −2bβ3c 0 0 0 −I I0 −bβ3c2bβ51c Π′′31 2bβ3cbβ51c 0 0 0 bβ51c 00c −bβ3c2bβ52c Π′′32 2bβ3cbβ52c 0 0 0 bβ52c 00c −bβ3c2bβ53c Π′′33 2bβ3cbβ53c 0 0 0 bβ53c 00 0 2bβ51c 0 0 0 0 0 00 0 2bβ52c 0 0 0 0 0 00 0 2bβ53c 0 0 0 0 0 0

(25)

(ii) Between the third and fifth rows of the second columnthe only left terms are K. By multiplying with M to theserows, we get the identity matrix. Then, all the other termsin the second column can be eliminated. Similarly, all therows in the third column except the last three rows can beeliminated. The resulting matrix is Ξ′′:

I 0 0 0 0 0 0 0 00 0 0 −I 0 0 0 0 00 I 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 −I 0 0 0 00 0 0 0 0 −I 0 0 00 0 0 0 0 0 −I 0 00 0 0 −2bβ3c 0 0 0 −I I0 0 0 2bβ3cbβ51c 0 0 0 bβ51c 00 0 0 2bβ3cbβ52c 0 0 0 bβ52c 00 0 0 2bβ3cbβ53c 0 0 0 bβ53c 00 0 I 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0

(26)

(iii) The only left term in the second row now is a identitymatrix in the fourth column, thus all the other terms in thiscolumn can be eliminated:

I 0 0 0 0 0 0 0 00 0 0 −I 0 0 0 0 00 I 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 −I 0 0 0 00 0 0 0 0 −I 0 0 00 0 0 0 0 0 −I 0 00 0 0 0 0 0 0 −I I0 0 0 0 0 0 0 bβ51c 00 0 0 0 0 0 0 bβ52c 00 0 0 0 0 0 0 bβ53c 00 0 I 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0

(27)

(iv) With the matrix K in the eighth column, the iden-tity matrix in the above row can be eliminated. After the

Gaussian elimination, the matrix Ξ becomes:

I 0 0 0 0 0 0 0 00 0 0 −I 0 0 0 0 00 I 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 −I 0 0 0 00 0 0 0 0 −I 0 0 00 0 0 0 0 0 −I 0 00 0 0 0 0 0 0 0 I0 0 0 0 0 0 0 I 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 I 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0

(28)

which is obviously a full column rank matrix, thus the system(16) is observable and our defined basis functions are thesystem’s observable modes.

REFERENCES

[1] A. I. Mourikis, N. Trawny, S. I. Roumeliotis, A. E. Johson, A. Ansar,and L. Matthies, “Vision-aided inertial navigation for spacecraft entry,descent, and landing,” IEEETrans. on Robotics, vol. 25, no. 2, pp.264–280, Apr. 2009.

[2] “http://www.microsoft.com/en-us/kinectforwindows/,” Online.[3] R. O. Allen and D. H. Change, “Performance testing of the systron

donner quartz gyro (qrs11-100-420);sns 3332, 3347 and 3544,” JPL,Tech. Rep., 1993.

[4] J. Y. Bouguet, “Camera calibration toolbox for matlab,” Online, 2006,available: ”http://www.vision.caltech.edu/bouguetj/calibdoc/”.

[5] D. Herrera, J. Kannala, and J. Heikkila, “Joint depth and color cameracalibration with distortion correction,” IEEE Trans. on Pattern Analysisand Machine Intelligence, vol. 34, no. 10, pp. 2058–2064, Oct. 2012.

[6] P. Lang and A. Pinz, “Calibration of hybrid vision/inertial trackingsystems,” in InerVis, Barcelona, Spain, Apr. 2005.

[7] J. Lobo and J. Dias, “Relative pose calibration between visual andinertial sensors,” in InerVis, Barcelona, Spain, Apr. 2005.

[8] F. M. Mirzaei and S. I. Roumeliotis, “A kalman filter-based algorithmfor imu-camera calibration: Observability analysis and performanceevaluation,” IEEETrans. on Robotics, vol. 24, no. 5, pp. 1143–1156,Oct. 2008.

[9] J. Kelly and G. S. Sukhatme, “Visual-inertial sensor fusion: Local-ization, mapping and sensor-to-sensor self-calibration,” InternationalJournal of Robotics Research, vol. 30, no. 1, pp. 56–79, Jan. 2011.

[10] S. Weiss, M. W. Achtelik, S. Lynen, M. Chli, and R. Siegwart, “Real-time onboard visual-inertial state estimation and self-calibration ofmavs in unknown environments,” in Proc. of the IEEE InternationalConference on Robotics and Automation, Saint Paul, Minnesota, May14-18 2012, pp. 957–964.

[11] J. A. Hesch, “Toward consistent vision-aided inertial navigation,”Ph.D. dissertation, University of Minnesota, 2012.

[12] R. Hermann and A. J. Krener, “Nonlinear controllability and observ-ability,” IEEE Trans. on Automatic Control, vol. 22, no. 5, pp. 728–740, Oct. 1977.

[13] D. G. Kottas, J. A. Hesch, S. L. Bowman, and S. I. Roumeliotis, “Onthe consistency of vision-aided inertial navigation,” in InternationalSymposium on Experimental Robotics, Quebec City, Canada, June2012.

[14] C. X. Guo and S. I. Roumeliotis, “Imu-rgbd camera extrinsic cal-ibration in unknown environments: Observability analysis,” Uni-versity of Minnesota, Tech. Rep., 2012, available: ”http://www-users.cs.umn.edu/ chaguo/”.

[15] M. D. Shuster, “A survey of attitude representations,” AstronauticalSciences, vol. 41, no. 4, pp. 439–517, 1993.

[16] D. Nister and H. Stewenius, “Scalable recognition with a vocabularytree,” in Proc. of the IEEE Conference on Computer Vision and PatternRecognition, New York, NY, June 2006, pp. 2161–2168.