Continuous Preintegration Theory for Graph-based Visual-Inertial Navigation

Transcription

1 Continuous Preintegration Theory for Graph-based Visual-Inertial Navigation Kevin Ecenhoff - ec@udel.edu Patric Geneva - pgeneva@udel.edu Guoquan Huang - ghuang@udel.edu Department of Mechanical Engineering University of Delaware, Delaware, USA Robot Perception and Navigation Group RPNG Tech Report - RPNG-08-CPI Last Updated - April 9, 08

2 Contents Preliminaries. System State Graph Optimization Continuous Preintegration 3 Standard IMU Processing 4 Model : Piecewise Constant Measurements 3 4. Measurement Mean State-Transition Matrix Discrete Covariance Propagation Acceleration Bias Jacobians Gyro Bias Jacobians Measurement Residual Measurement Jacobian Model : Piecewise Constant Local Acceleration 5. Measurement Mean Measurement Covariance Bias Jacobian Discussion Measurement Residual Measurement Jacobian Appendix A Useful Identities 7 References 8

3 Preliminaries. System State The IMU state of an aided inertial navigation system at time step is given by [: x = [ G q b ω G v b a G p where G q is the unit quaternion of JPL form parameterizing the rotation GR from the global frame {G} to the current local frame {} [, b ω and b a are the gyroscope and accelerometer biases, and G v and G p are the velocity and position of the IMU expressed in the global frame, respectively. The error state corresponding to Equation is given by: δx = [ δθg δb ω G δv δb a G δp The relationship between the vector quantities with true value v, mean value ˆv, and error state δv taes the form v = ˆv + δv. For quaternions in JPL convention, with true value q, mean value ˆ q, and error state δθ, we have: [ δθ q = ˆ q 3 where is the quaternion multiplication. We write these relationships compactly as x = ˆx δx and δx = x ˆx.. Graph Optimization Given a set of measurements with residuals e i and information matrices Λ i, we solve the following maximum a posteriori MAP problem to estimate our state [3: ˆx = argmin x e i x Λ i 4 i We solve this MAP problem through iterative linearization, where for each iteration we solve the following problem: δˆx = argmin δx e i ˆx + J i δx Λ i 5 i J i = e i ˆx δx 6 δx δx=0 It is clear that in order to use this formulation in estimation, we must define the appropriate measurement residuals e i, Jacobians J i, and information matrices Λ i. In this technical report we show how to optimally utilize IMU measurements in this formulation through the use of continuous preintegration. Continuous Preintegration An IMU attached to the robot collects inertial readings of the underlying state dynamics. In particular, the sensor receives angular velocity ω m and local linear acceleration a m measurements RPNG-08-CPI

4 which relate to the corresponding true values ω and a as follows: ω m = ω + b ω + n ω 7 a m = a + I GR G g + b a + n a 8 where G g = [ is the global gravity, noting that the gravity is slightly different in different parts of the globe, and I GR is the rotation from the global frame to the instantaneous local inertial frame. The measurements are corrupted both by the time-varying biases b ω and b a which must be co-estimated with the state, and the zero-mean white Gaussian noises n ω and n a. The standard dynamics of the IMU state is given by [4: I G q = Ωω m b ω n ω I G q 9 ḃ ω = n ωb 0 G v I = G I R a m b a n a G g ḃ a = n ab Gṗ I = G v I 3 where Ωω = [ ω ω ω Standard IMU Processing Given a series of IMU measurements, I, collected over a time interval [t, t +, the standard graph-based IMU processing considers the following propagation function: x + = g x, I, n 5 That is, the future state at time step + is a function of the current state at step, the IMU measurements I, and the corresponding measurement noise n. Conditioning on the current state, the expected value of the next state is found by evaluating the propagation function with zero noise: x + = g x, I, 0 6 which implies that we perform integration of the state dynamics in the absence of noise. The residual for use in batch optimization of this propagation now constrains the start and end states of the interval and is given by: c IMU x = x + x + Q = x + g x, I, 0 Q where Q is the linearized, discrete-time noise covariance computed from the IMU noise characterization and is a function of the state. This noise covariance matrix and the propagation function can be found by the integration of Equations 9-3 and their associated error state dynamics, to which we refer the reader to [,. RPNG-08-CPI 7 8

5 It is clear from Equation 6 that we need constantly re-evaluate the propagation function g and the residual covariance Q whenever the linearization point state estimate changes. The high frequency nature of the IMU sensors and the complexity of the propagation function and the noise covariance can mae direct incorporation of IMU data in real-time graph-based SLAM prohibitively expensive. 4 Model : Piecewise Constant Measurements IMU preintegration sees to directly reduce the computational complexity of incorporating inertial measurements by removing the need to re-integrate the propagation function and noise covariance. This is achieved by processing IMU measurements in a local frame of reference, yielding measurements that are, in contrast to Equation 6, independent of the state [5. Specifically, by denoting T = t + t, we have the following relationship between a series of IMU measurements, the start state, and the resulting end state [6: G p + = G p + G v T t+ G g T + G R t G v + = G v G g T + G R t+ s t u R a m b a n a duds 9 u R a m b a n a du 0 t + G R = + R GR b ω+ = b ω + b a+ = b a + t+ t n ωb du t+ t n ab du 3 From the above, we define the following preintegrated IMU measurements: α + = β + = t+ s t t+ t u R a m b a n a duds 4 t u R a m b a n a du 5 where along with these preintegrated inertial measurements the preintegrated relative-orientation measurement + q or + R can be obtained from the integration of the gyro measurements. To remove the dependencies of the above preintegrated measurements on the true biases, we linearize about the current bias estimates at time step t, b a and b ω. Defining b = b b, we have: GR G p + G p G v T + G g T α + b ω, b α b a + b b ω ω + α b b ω b a a 6 a GR G v + G v + G g T β + b ω, b β b a + b b ω ω + β b b ω b a a a 7 + G R GR R b + R b b ω ω R b ω ω 8 RPNG-08-CPI 3

6 Note that Equations 6 and 7 are simple Taylor series expansions for our α + and β + measurements, while Equation 8 models an additional rotation induced due to a change of the linearization point estimate of the gyro bias [6, 7. The preintegrated measurement s mean values, ᾰ +, β+, and + q, must be computed for use in graph optimization. It is important to note that current preintegration methods [5, 7, 8 are all based on discrete integration of the measurement dynamics through Euler or midpoint integration. In particular, the discrete approximation used by Forster et al. [7 in fact corresponds to a piecewise constant global acceleration model. By contrast, we here offer closed-form solutions for the measurement means under the assumptions of piecewise constant measurements and of piecewise constant local acceleration in Section Measurement Mean We now derive the closed-from solutions for + q, ᾰ +, β+. In particular, the quaternion + q can be found using the zeroth order quaternion integrator [. This can be compounded successively to get the final measurement mean + q. In a similar fashion, we can find, in closed form, ᾰ + and β+. We have derived the following continuous time linear system: [ ᾰu = βu [ [ᾰ [ 0 I u βu R a m b a 9 u The solution of this linear dynamical system is given by: [ᾰ [ᾰ t+ [ + 0 = Φt +, t β+ + Φt +, u β R a m b a du 30 t u where the state-transition matrix Φt +, t is given by the matrix exponential: Φt +, t = exp [ 0 I t = I [ 0 I t [ 0 0 t = 0 0 [ I I t 0 I 3 where t = t + t. Substituting the state-transition into Equation 30 yields: [ᾰ + β+ = = = [ I I t 0 I [ᾰ + β t β + [ᾰ β + [ᾰ + β t β + t+ t t+ t [ I It+ u 0 I [ t+ u Râ u u Râ [ 0 u R [ + t+ R t t + u + u Râ du R t + + u Râ du t + â du 3 du 33 where â = a m b a. Using ˆω = ω m b ω and δt = t + u, and the Rodrigues formula 35 we have: + u R = exp ˆω δt = I 34 sin ˆω δt cos ˆω δt ˆω + ˆω ˆω ˆω 35 RPNG-08-CPI 4

7 [ᾰ [ᾰ + = + [ + t β t R β+ + β = + [ᾰ + β t β 0 δti R t + 0 I [ + t R ˆω tcos ˆω t sin w t I + + R ti sin ˆω δt ˆω ˆω + sin ˆω δt ˆω ˆω + cos ˆω δt ˆω ˆω âdδt cos ˆω δt ˆω ˆω âdδt ˆω + ˆω t cos ˆω t ˆω tsin ˆω t+ ˆω ˆω 3 ˆω â 4 cos ˆω t ˆω t sin ˆω t ˆω + ˆω ˆω ˆω â State-Transition Matrix In order to use the derived continuous preintegration measurement means, we must have the covariances associated with their error quantities. To do this, we examine the time evolution of the corresponding error states: δ α u = δβ u 38 δ β u = R u I + u δθ a m b a n a R u a m b a 39 = R u b a n a + R u u δθ a m b a 40 u δθ = ˆω b u ω δθ b w n w 4 where we have used the standard error associated with JPL-convention quaternions, u q = δ q u q, and δ q [δθ/. This yields the following linearized system describing our error states: uδθ ˆω I u δθ I b ω δβ u = R â u 0 0 R b ω u 0 δβ u b a b + 0 I 0 0 n ω 0 0 R u 0 n ωb a I n a 4 δα 0 0 I 0 0 n u δα u ab ṙ = Fr + Gn As compared to the original preintegration paper [6, the above system incorporates bias errors that captures the drift in a given bias over an interval. Note that these bias error terms b ω and b a, describe the deviation of the bias over the interval due to the random-wal drift, rather than the error of the current bias estimate. The discrete state transition matrix can be found by solving the following differential equation: 4.3 Discrete Covariance Propagation 43 Φt u, t = Fu Φt u, t 44 Φt, t = I Using the expressions for the state-transition matrix, the covariance propagation for our preintegrated measurements taes the form: P = P + = Φt +, t P Φt +, t + Q 47 Q = t+ t Φt +, uguq c Gu Φt +, u du 48 RPNG-08-CPI 5

8 In practical applications, the state transition matrix Φt +, t and discrete measurement covariance Q can be numerically integrated. 4.4 Acceleration Bias Jacobians We have derived the closed-form measurement update for ᾰ and β see Equation 37. We need only compute the gradients of our closed-form update expressions with respect to changes in bias. In particular, since each update term is linear in the estimated acceleration, â = a m b a, we can find the bias Jacobians of α + and β + with respect to b a as follows: [ α b a β =: b a R + [ Hα + = H β + [ Hα + H β t H β t I ˆω tcos ˆω t sin ˆω t R ˆω + ˆω t cos ˆω t ˆω tsin ˆω t+ ˆω ˆω 3 ˆω 4 cos ˆω t ˆω t sin ω t ˆω + ˆω ˆω ˆω 3 ti and for small values of ˆω we have: [ [ Hα + Hα + H = β t H β + H β lim ˆω 0 [ + t R I t3 t4 3 ˆω + 8 ˆω R ti + t t3 ˆω + 6 ˆω Gyro Bias Jacobians We find the derivatives of α + and β + with respect to each entry of the gyro bias by taing the derivative with respect to each gyro bias entry. We see to find the following entries: J α = J β = [ α + α + α + b ω b ω b ω3 [ β + β + β + b ω b ω b ω3 For each of the above entries, from Equation 37, we have the following: α + = α b ωi b ωi + β t b ωi + b +R t I wi ˆω tcos ˆω t sin ˆω t ˆω 3 ˆω + ˆω t cos ˆω t ˆω tsin ˆω t + ˆω 4 ˆω 5 5 â 53 The first two terms are from the previous integration time step, as we build these Jacobians incrementally. Defining ê i as the unit vector in the i th direction, and ˆω i the corresponding entry in ˆω the third term can be found as the following: t +R I + f ˆω + f ˆω = + R t I + f ˆω + f ˆω b ωi b ωi + R f + ˆω f ê i + f ˆω f ê i ˆω + ˆω ê i b ωi b ωi 54 RPNG-08-CPI 6

9 where f and f are the corresponding coefficients in Equation 53, and their derivatives are computed as: For small ˆω, f = ˆω i ˆω t sin ˆω t 3sin ˆω t + 3 ˆω tcos ˆω t b ωi ˆω 5 55 f = ˆω i ˆω t 4cos ˆω t 4 ˆω tsin ˆω t + ˆω t cos ˆω t + 4 b ωi ˆω 6 56 lim ˆω 0 f t 5 = ˆω i b ωi 5 57 lim ˆω 0 f t 6 = ˆω i b ωi 7 58 Similarly, we have: β + b ωi = β b ωi + + R + + R ti + f3 ˆω + f 4 ˆω â b ωi f3 ˆω f 3 ê i + f 4 ˆω f 4 ê i ˆω + ˆω ê i b ωi b ωi â 59 where f 3 b ωi f 4 b ωi = ˆω icos ˆω t + ˆω tsin ˆω t ˆω 4 60 = ˆω i ˆω t + ˆω tcos ˆω t 3sin ˆω t ˆω 5 6 For small ˆω we have: lim ˆω 0 lim ˆω 0 f 3 t 4 = ˆω i b ωi f 4 t 5 = ˆω i b ωi We now show how to derive the derivative of the rotation matrix with respect to a change in bias, i.e., b ω = b ω b ω. To do so, we consider a set of measurements over the interval and for the first measurement interval, [t, t, with measurement ω m, we integrate over its sub-interval to get the following: R = R = exp ω m b ω b ω t 64 exp ω m b ω texp J r b ω t 65 = exp J r b ω texp ω m b ω t 66 = exp J r b ω t 67 I J r b ω t 68 RPNG-08-CPI 7

10 where J ri is the right Jacobian of SO3 evaluated at the i th measurement i.e., ω mi, see Equation 64. This decomposition can be done for every measurement interval: i+ i R I J ri+ b ω t i+ i R 69 We can now loo at what happens when we compound measurements. In particular, at the secondstep [t, t, we have the following: R = R R 70 I J r b ω t RI3 3 + J r b ω t 7 = I J r b ω ti RJr b ω t 7 I J r + RJr b ω t 73 Here we have used the property that R w R = Rw for a rotation matrix. process for the interval [t, t 3 yields: Repeating this 3 R = 3 R R 74 I J r3 b ω t 3 RI3 3 + J r + RJr b ω t 75 = I J r3 b ω I RJr + 3 RJr b ω t 3 76 I J r3 + 3 RJr + 3 RJr b ω t 3 77 We thus see the pattern developing and can write the updated rotation at any time step t u as: u R = exp J qub ω b ω u R 78 u u with J q u = r t 79 = Each of these values can be calculated incrementally by noting that: J q u + = u+ u = u+ u R u = u RJ r t + J ru+ t 80 RJ q u + J ru+ t 8 The derivative of every rotation with respect to the i th entry of the gyro bias, which appears in both Equation 54 and 59 can be approximated using: The total rotation after a bias update can be expressed as: u R I J q ub ω b ω u R 8 ur J q uê i u b R ωi 83 ur b ωi u R J q uê i 84 + R = exp J q + b ω b ω + R RPNG-08-CPI 8

11 4.6 Measurement Residual e IMU x = 87 vec + G q G q + q q b b ω+ b ω G R G v + G v + G g T J β bω b ω Hβ ba b a β+ b a+ b a G R G p + G p G v T + G g T J α bω b ω Hα ba b a ᾰ + where θ q b = θ sin θ cos θ 88 θ = J q bω b ω Measurement Jacobian Let us partition the preintegrated residual for the first model as: e IMU = [ e θ e b ω e v e b a e p 90 The measurement Jacobian with respect to one element of the state vector can be found by perturbing the measurement function by the corresponding element. For example, the relative-rotation measurement residual is perturbed by a change in gyro bias around the current estimate i.e., b ω b ω = ˆb ω + δb ω b ω : e θ = vec =: vec = vec + G ˆ q Gˆ q + q ˆ q r Lˆ q r [ Jqˆb ω +δb ω b ω [ Jqˆb ω +δb ω b ω [ Jqˆb ω +δb ω b ω [ J [ˆqr,4 I = vec 3 3 ˆq r ˆq qˆb ω +δb ω b ω r ˆq r ˆq r,4 = ˆq r,4 I 3 3 ˆq r J q ˆb ω + δb ω b ω + other terms So that our Jacobian with respect to a perturbance in bias is: e θ δb ω = ˆq r,4 I 3 3 ˆq r J q 9 RPNG-08-CPI 9

12 Similarly, the Jacobian with respect to + δθ G can be found as follows: [ + δθ G e θ = vec + G ˆ q Gˆ q + q ˆ q b [ + δθ G = vec ˆ q rb Yielding the Jacobian: [ + δθ G = vec Rˆq rb [ [ˆqrb,4 I = vec ˆq rb ˆq + δθ G rb ˆq rb ˆq rb,4 = ˆq rb,4 I ˆq rb + δθ G + other terms The Jacobian with respect to δθ G is given by: [ e θ = vec = vec = vec + G ˆ q Gˆ q [ ˆq n δθ G Lˆq n R q mb e θ + δθ G = ˆq rb,4 I ˆq rb 9 [ δθ G ˆq mb δθ G [ˆq = vec n,4 I 3 3 ˆq n ˆq n ˆq n ˆq n,4 [ qmb,4 I 3 3 q mb q mb Which gives the Jacobian: q mb q mb,4 [ δθ G + q ˆ q b = ˆq n,4 I 3 3 ˆq n q mb,4 I 3 3 q mb + ˆq n q mb δθ G + other terms e θ δθ G = ˆq n,4 I 3 3 ˆq n q mb,4 I 3 3 q mb + ˆq n q mb Note than in the preceding Jacobians, we have defined several intermediate quaternions, ˆ q r, ˆ q rb, ˆ q n, and ˆ q mb for ease of notation. Following the same methodology, we can find the Jacobians of the α measurement with respect to the position, velocity and bias. e bω = I δb ω 93 e bω = I δb ω+ 94 RPNG-08-CPI 0

13 e v = G ˆRGˆv + Gˆv + g t G δθ G 95 e v = J β δb ω 96 e v G = δv ˆR G 97 e v G = δv ˆR G + 98 e v = H β δb a 99 e ba = I δb a 00 e ba = I 0 δb a+ = G δθ + Gˆp Gˆv t + G g t 0 G = J α δb ω 03 G = δv ˆR t G 04 = H α δb a 05 G = δp ˆR G 06 G = δp ˆR G Model : Piecewise Constant Local Acceleration The previous preintegration Model assumes that noiseless IMU measurements can be approximated as remaining constant over a sampling interval, which, however, might not always be a good approximation. For example, in the case of an IMU rotating against the direction of gravity, the measurement will change over a sampling interval continuously due to the effect of gravity. In this section, we propose a new preintegration model that instead assumes piecewise constant true local acceleration during the sampling time interval, which may better approximate motion dynamics in practice. To this end, we first rewrite the state dynamics as: G p + = G p + G v T + G R t+ G v + = G v + G R t+ t s t u Ra duds 09 t u Ra du 0 RPNG-08-CPI

14 Note that we have moved the effect of gravity bac inside the integrals. We then define the following vectors: p = v = t+ s t t+ t u Ra duds t u Ra du which essentially are the true local position displacement and velocity change during [t, t +, and yields: ṗ = v 3 v = ura 4 In particular, between two IMU measurement times inside the preintegration interval, [t, t + [t, t +, we assume that the local acceleration will be constant: t u [t, t +, at u = at 5 Using this sampling model we can rewrite Equation 4 as: v = ur a m b a n a R GR G g 6 We now write the relationship of the states at the beginning and end of the interval as see Equations 09 and 0: GR G p + G p G v T = p 7 GR G v + G v = v 8 It is important to note that, since p and v are functions of both the biases and the initial orientation, we perform the following linearization with respect to these states: GR G p + G p G v T p b ω, b a, G q + p b ω b ω b ω + p b a b a b a + p θ G q θ 9 GR G v + G v v b ω, b a, G q + v b b b ω ω + v b b ω b a a + v a θ G q θ 0 where θ = vec G q G q is the rotation angle change associated with the change of the linearization point of quaternion G q. 5. Measurement Mean To compute the new preintegrated measurement mean values, we first determine the continuoustime dynamics of the expected preintegration vectors by taing expectations of Equations 3 and 6, given by: p = v v = R u a m b a R GR G g RPNG-08-CPI

15 As in the case of Model, we can formulate a linear system of the new preintegration measurement vectors and find the closed-from solutions. Specifically, we can integrate these differential equations and obtain the solution similar to Equation 37, while using the new definition: â = a m b a R G R G g, which serves as the estimate for the piecewise constant local acceleration over the sampling interval. 5. Measurement Covariance In order to use the derived continuous preintegration measurement means, we must have the covariances associated with their error quantities. We compute the derivative of our measurement error states with respect to error sources as follows: p = v v = ṽ ṽ = R u I + u δθ a m b a b a I δθ R I θ GR G g n a R u a m b a R GR G g = u R â u δθ u R b a u R ğ δθ u R R g θ u Rn a 6 where g = G R G g is the global gravity rotated into the local frame of the linearization point. In the above expressions, we have used three angle errors: i u δθ corresponds to the active local IMU orientation error, ii δθ corresponds to the cloned orientation error at the sampling time t, and iii θ is the global angle error of the starting orientation of the preintegration time interval. In addition, the bias errors b describe the deviation of the bias from the starting value over the interval due to bias drift. With this, we have the following time evolution of the full preintegrated measurement error state: u δ θ u δθ I b ω b ω 0 I 0 0 ṽ ṽ 0 0 R u 0 b a = F b a I p p δ θ δθ θ θ n ω n ωb n a n ab 7 ṙ = Fr + Gn 8 where ω I R â u 0 0 R u 0 R u ğ R u R g F = I The above continuous time measurement error evolution can be can be numerically integrated to get the appropriate state transition Φt +, t and additive measurement noise covariance Q. RPNG-08-CPI 3

16 Having defined the state transition matrix and additive measurement noise covariance, for each incoming IMU measurement we can propagate our total preintegration covariance P as follows: P = 0 30 P + = Φt +, t P Φt +, t + Q 3 P + = BP + B 3 where B is the cloning matrix that allows us to replace the previous static orientation error δθ to the new one + δθ when moving to the next preintegration measurement time interval i.e., from [t, t + to [t +, t +, and is given by: I I I B = I I 0 0 I I The resulting preintegrated measurement covariance is extracted from the top left 5 5 bloc of P + after the propagation in Equations 30-3 is over for the entire preintegration interval [t, t Bias Jacobian Discussion Model two preintegration depends on the initial orientation R for each IMU sampling interval [t, t + and thus, when integrating over one interval to the other, this initial orientation error changes from δθ to + δθ. This requires to extra care when computing the total Jacobian state transition matrix, Ψ +, over the entire preintegration interval [t, t + ; that is, we propagate this matrix as follows: Ψ = I 34 Ψ + = BΦt +, t Ψ 35 where B is defined the same as in Equation 33. Clearly, the total Jacobin matrix Ψ + will be the Jacobian of the resulting preintegrated measurement error with respect to the initial error. Therefore, we can simply extract the corresponding blocs to obtain the bias and initial orientation Jacobians. Denoting Ψ + i, j the 3 3 bloc of the Jacobian matrix starting at index i, j, we have: J q = Ψ + 0, 3 36 J a = Ψ +, 3 37 J b = Ψ + 6, 3 38 H a = Ψ +, 9 39 H b = Ψ + 6, 9 40 O a = Ψ +, 8 4 O b = Ψ + 6, 8 4 where we flip the sign of J q to match the same definition used in Model. RPNG-08-CPI 4

17 5.4 Measurement Residual vec + G q G q + q q b b ω+ b ω G R G v + G v Jβ bω b ω e IMU x = H β ba b a Oβ vec G q G q v b a+ b a G R G p + G p G v T J α bω b ω H α ba b a Oα vec G q G q p 43 where θ q b = θ sin θ cos θ 44 θ = J q bω b ω Measurement Jacobian Let us partition the preintegrated residual for the second model as: e IMU = [ e θ e b ω e v e b a e p 46 Instead of directly computing the derivatives, the measurement Jacobian with respect to one element of the state vector can be found by perturbing the measurement function by the corresponding element. The orientation and bias change measurement Jacobians remain unchanged under the new model. However, since the definition of e v and e p have changed, their Jacobians must be changed appropriately. The Jacobians are as follows: e bω = I δb ω 47 e bω = I δb ω+ 48 e v = δθ ˆR Gˆv G + Gˆv Oβ q 4 I + q G 49 e v = J β δb ω 50 e v G = δv ˆR G 5 RPNG-08-CPI 5

18 e v G = δv ˆR G + 5 e v = H β δb a 53 e ba = I δb a+ 54 e ba = I δb a 55 = δθ ˆR Gˆp G + Gˆp Gˆv T O α q 4 I + q G 56 = J α δb ω 57 G = δv ˆR T G 58 = H α δb a 59 G = δp ˆR G 60 G = δp ˆR G + 6 where [ q q 4 = Gˆ q G q RPNG-08-CPI 6

19 Appendix A: Useful Identities We provide some useful identities that are used in our derivations throughout the report. Given a constant angular velocity ω between times t and t, the rotation matrix between the two frames L t and L t is given by the matrix exponential: L t L t R = exp ωt t = I 3 3 sin ωt t ω ω + cos ωt t ω 6 ω where the sew-symmetric is defined as: 0 ω z ω y ω = ω z 0 ω x 63 ω y ω x 0 The right Jacobian of SO3, J r φ, is defined by see [9: J r φ = I 3 3 cos φ φ sin φ φ φ + φ 3 φ 64 Given a small angle vector perturbation δφ, we can mae the following approximation for the rotation matrix [7 : exp φ + δψ exp φ exp J r φδφ 65 This allows us to map a perturbation of the Lie algebra so3 to a perturbation on the group of SO3. The JPL natural order quaternion is used throughout the paper [, 0, which parametrizes the rotation 6 as follows: ω L t ω L t q = sin ωt t. 66 cos ωt t RPNG-08-CPI 7

20 References [ A. I. Mouriis and S. I. Roumeliotis. A multi-state constraint Kalman filter for vision-aided inertial navigation. In: Proceedings of the IEEE International Conference on Robotics and Automation. Rome, Italy, 007, pp [ Niolas Trawny and Stergios I. Roumeliotis. Indirect Kalman Filter for 3D Attitude Estimation. Tech. rep. University of Minnesota, Dept. of Comp. Sci. & Eng., Mar [3 R. Kummerle et al. go: A general framewor for graph optimization. In: Proc. of the IEEE International Conference on Robotics and Automation. Shanghai, China, 0, pp [4 Averil B. Chatfield. Fundamentals of High Accuracy Inertial Navigation. Reston, VA: American Institute of Aeronautics and Astronautics, Inc., 997. [5 Todd Lupton and Salah Suarieh. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions. In: IEEE Transactions on Robotics 8. 0, pp [6 Kevin Ecenhoff, Patric Geneva, and Guoquan Huang. High-Accuracy Preintegration for Visual-Inertial Navigation. In: in Proc. of International Worshop on the Algorithmic Foundations of Robotics. 06. [7 Christian Forster et al. IMU preintegration on manifold for efficient visual-inertial maximuma-posteriori estimation. In: Robotics: Science and Systems XI. 05. [8 Yonggen Ling, Tianbo Liu, and Shaojie Shen. Aggressive quadrotor flight using dense visualinertial fusion. In: Robotics and Automation ICRA, 06 IEEE International Conference on. IEEE. 06, pp [9 Gregory S Chirijian. Stochastic Models, Information Theory, and Lie Groups, Volume : Analytic Methods and Modern Applications. Vol.. Springer Science & Business Media, 0. [0 WG Brecenridge. Quaternions proposed standard conventions. In: Jet Propulsion Laboratory, Pasadena, CA, Interoffice Memorandum IOM 999, pp RPNG-08-CPI 8