High-Accuracy Preintegration for Visual Inertial Navigation

Transcription

1 High-Accuracy Preintegration for 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 Last Updated - July, 06

2 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: where ω L t L t R exp ωt t I 3 3 sin ωt t ω + cos ωt t ω ω ω 0 ω z ω y ω z 0 ω x The right Jacobian of SO3, J r φ, is defined by see : ω y ω x 0 J r φ I 3 3 cos φ φ sin φ φ φ + φ 3 φ Given a small angle vector perturbation δφ, we can mae the following approximation for the rotation matrix : exp φ + δψ exp φ exp J r φδφ 3 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 3, 4, which parametrizes the rotation as follows: Introduction L t L t q ω ω sin ωt t. 4 cos ωt t Visual-inertial navigation systems VINS that fuse visual and inertial information to provide accurate localization, have become nearly ubiquitous in part because of their low cost and light weight e.g., see 5, 6, 7. IMUs provide local angular velocity and linear acceleration measurements, while cameras are a cheap yet informative means for sensing the the surrounding environment and thus is an ideal aiding source for inertial navigation. In particular, these benefits have made VINS popular in resource-constrained systems such as micro aerial vehicles MAVs 8. Traditionally, navigation solutions have been achieved via extended Kalman Filters EKFs, where incoming proprioceptive IMU and exteroceptive camera measurements are processed to propagate and update state estimates, respectively. These filtering methods do not update past state estimates that have been marginalized out, thus causing them to be highly susceptible to drift due to the compounding of errors. Graph-based optimization methods, by contrast, process all measurements taen over a trajectory simultaneously to estimate a smooth history of sensor states. These methods achieve higher accuracy due to the ability to relinearize nonlinear measurement functions and correct previous state estimates 9. Recently, graph-based formulations have been introduced that allow the incorporation of IMU measurements into preintegrated factors by performing integration of the system dynamics in a local frame of reference, 0,. However, these methods often simplify the required preintegrations by resorting to discrete solutions under the approximation of piece-wise constant accelerations. To improve this IMU preintegration, in this report, we instead model the IMU measurements as piece-wise constant and rigorously derive closedform solutions of the integration equations. Based on that, we offer analytical computations of the mean, covariance, and bias Jacobians of the preintegrated measurements. RPNG-06-00

3 3 Analytical IMU Preintegration An IMU typically measures the local angular velocity ω and linear acceleration a of its body, which are assumed to be corrupted by the Gaussian white noise n w and n a and the random-wal biases b w and b a 4: ω m ω + b w + n w, a m a + g + b a + n a, ḃ w n wg, ḃ a n wa 5 where g is the gravity vector in the local frame whose global counterpart is constant e.g., G g T. The navigation state at time-step is given by: x L G qt b T w G v T b T a G p T T 6 where L G q is the natural order quaternion i.e., with JPL convention 3 that describes the rotation from frame {G} to frame {L }, and G v and G p are the velocity and position of the -th local frame in the global e.g., starting frame, respectively. The corresponding error state and operation used in batch optimization can be written as note that hereafter the transpose has been omitted for brevity: x L δθ Gṽ G p G bw ba 7 L δ q ˆL ˆL G ˆ q ˆb w + b w x ˆx x Gˆv + G ṽ 8 ˆb a + b Gˆp a + G p where the operator denotes quaternion multiplication, and L δ q is the error quaternion whose vector ˆL portion is half the error angle, L δθ G vec L δ q : L δq. We here use the following unit quaternion ˆL ˆL notation q : q T T q 4. The total full and error states are then comprised of all these vectors. To locally combine all the IMU measurements from time-step to + without accessing the state estimates in particular, the orientation, we can perform the following factorization of the current rotation matrix and integration of the measurements 3: G p + G p + G v t t+ G g t + G R R a m b a n a dds t t } {{} α + s : G p + G v t G g t + G R α + 9 t+ G v + G v G g t + G R R a m b a n a d t } {{ } β + : G v G g t + G R β G R R GR where t t + t. It becomes clear that the above integrals have been collected into the preintegrated measurements, α + and β +, which are expressed in the -th local frame. Rearrangement of these RPNG-06-00

4 equations yields: GR G p + G p G v t + G g t α + a m, ω m, n a, n w, b a, b w GR G v + G v + G g t β + a m, ω m, n a, n w, b a, b w 3 + G R GR + R ω m, n w, b w, t, t + 4 For the remainder of this paper the biases noted will refer to the those of state, and are approximated as constant over the preintegration interval. It is important to note that in the above equations, the preintegrated measurements are explicitly expressed as the functions of the IMU measurements, noise, and true biases to show their dependency on these variables, as we will see later that it is this dependency that maes the exact preintegration impossible. To address this issue, we employ the following first-order Taylor series expansion with respect to the biases: GR G p + G p G v t + G g t 5 α + a m, ω m, n a, n w, b a, b α w + b a + α b w b a ba b w bw GR G v + G v + G g t 6 β + a m, ω m, n a, n w, b a, b β w + b a + β b w b a ba b w bw + G R GR R b w + R ω m, n w, b w where the preintegration functions have been linearized about the current bias estimates, b w and b a, and b w : b w b w and b a : b a b a are the difference between the true biases and their linearization points. Note that in the case of the relative rotations, a change in bias is modeled as inducing a further rotation on our preintegrated relative rotation. The corresponding residuals of these preintegrated measurements for use in graph optimization are given by: δ α + GR G p + G p G v t + G g t α b a α b w ˆα + 8 b a b w δ β + GR G v + G v + G g t β b a β b w ˆβ+ 9 b a b w + δθ vec + G q G q + q q b w 0 where ˆα + and ˆβ+ are the current estimated preintegrated measurements. We have defined + q as the relative quaternion found through preintegration of the IMU measurements to distinguish from a quaternion achieved by multiplication of the current state quaternion estimates. Based on the above definitions of α + and β +, we have: Lastly, the rotation quaternion dynamics is given by: 7 α β β R a m b a n a where Ωω ω ω ω. 0 q Ωω m b w n w q 3 RPNG

5 4 Compute Preintegration Mean and Covariance via Linear Systems We now derive the closed-from solutions for ˆα, ˆβ and ˆ q see, and 3. In particular, the quaternion + q can be found using the zeroth order quaternion integrator 4. In a similar fashion, we can find, in closed form, ˆα + and ˆβ+. We begin by stacing and, and taing the expectation to find the following linear system: ˆ α 0 I ˆα 0 ˆ β ˆβ ˆR a m b a 4 The solution of this linear dynamical system is given by: ˆα + ˆα t+ 0 Φt +, t ˆβ+ + Φt +, u ˆβ ˆR u a m b a du 5 t u where the state-transition matrix Φt +, t is given by the matrix exponential: Φt +, t exp 0 I t I I t t 0 0 I I t 0 I 6 where t t + t. Substituting the state-transition into 5 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 7 du 8 where â u a m b a. Using ˆω u ω m b w, δt t + u and the matrix exponential of a sew symmetric matrix, as well as the Rodrigues SO3 rotation formula, we analytically compute the preintegration measurement evolution from time t to t + as follows the detailed derivations can be found in our supplementary material: ˆα + ˆα + + ˆβ t R t ˆβ+ + ˆβ ˆα + ˆβ t ˆβ 0 δti ˆR t + 0 I sin ˆω δt ˆω ˆω + sin ˆω δt ˆω ˆω + cos ˆω δt cos ˆω δt w 9 ˆω ˆω âdδt ˆω 30 âdδt + + t ˆR ˆω tcos ˆω t sin w t I + + ˆR ti ˆω + ˆω t cos ˆω t ˆω tsin ˆω t+ ˆω ˆω 3 ˆω â 4 cos ˆω t ˆω t sin ˆω t ˆω + ˆω ˆω ˆω â 3 3 When ˆω is very small, these equations are unstable. We therefore examine the limits as ˆω tends to zero: ˆα + ˆα lim + ˆβ t + t ˆR ˆω 0 ˆβ+ + I t3 t4 3 ˆω + 8 ˆω â ˆβ ˆR ti + t t3 ˆω ˆω â RPNG

6 5 State-Transition Matrix In order to use these measurements, we must have the covariances associated with their error quantities. To do this, we examine the time evolution of the corresponding error states: δ α δβ 33 δ β ˆR I + δθ a m b a n a ˆR a m ˆb a 34 ˆR b a n a + ˆR δθ a m ˆb a 35 δθ ˆω ˆb w δθ b w n w 36 where we have used the standard error associated with JPL-convention quaternions, q δ q ˆ q, and δθ δ q. This yields the following linearized system describing our error states: δ α I δ β ˆR δ θ a m b a ω m b w ṙ Fr + Gn δα δβ δθ ˆR na n w I 3 3 We ignore the error terms associated with bias, as the biases are being evaluated at their current estimates, essentially linearizing the measurements and their corresponding errors about those points. Under the approximation that F 38 does not change over a measurement time interval t, t +, the discrete-time state transition matrix, can be written as: Φt +, t expf t I F t + F t where t t + t. In order to find the analytical expression, we need to loo at the powers of F I F E, with E ˆR a m ˆb a and d ω m ˆb w d Based on this, we have: I I E F E E E d d d d E I E d F E d E E d d d d E d I E d F E d E E d d d d 4 F 5 By close inspection, it is not difficult to see that the discrete-time state transition matrix taes the form 4: Φ Φ Φ 3 Φt +, t Φ Φ Φ 33 RPNG

7 Based on 40-43, we find each entry of the state transition matrix as follows: Φ Φ I Φ I 3 3 t 46 Φ 3 E t + 3! E d t3 + 4! E d t Φ Φ 3 Φ Φ 3 E t + E d t + 3! E d t 3 + 4! E d 3 t Φ 33 I d t + d t + 50 We now first turn our attention to Φ 3. Using d 3 d d, we can write it as: Φ 3 E t + 3! E d t3 + 4! E d t 4 5! d E d t 5 6! d E d t 6 + 7! d 4 E d t E t + E 3! t3 5! d t d + E 4! t4 6! d t d E t + E d t sin d t d + E d 3 d 4 cos d t + d t d 5 Similarly, we repeat this process for Φ 3 : Φ 3 E t + E d t + 3! E d t 3 4! d E d t E t + E t 4! d t d + E 3! t3 5! d t d E t + E cos d t d + E d d 3 d t sin d t d 5 Lastly, Φ 33 is simply computed based on the Rodriques formula : Φ 33 exp d t I sin d t d + cos d t d d d 53 When ω, similarly when d, becomes small the above equations will become numerically unstable due to d, and thus ω, appearing in the denominator. We therefore loo to tae the limit as d approaches zero. lim Φ 3 E d 0 I 3 3 t + t3 t4 d d 54 lim Φ 3 E ti t t3 d + d 0 6 d 55 lim Φ 33 I t d + t d 0 d 56 RPNG

8 6 Discrete Covariance Propagation Using the expressions for the state-transition matrix, the covariance propagation for our preintegrated measurements taes the form: P t P t+ Φt +, t P t Φt +, t + Q d 58 Q d t+ t Φt +, uguq c G uφ t +, udu 59 t + Φ Φ Φ Φ Φ 3 ˆR u σ a I u ˆR t Φ 3 3 σ Φ Φ I wi Φ du I Φ 3 Φ 3 Φ 33 t+ t t+ t Φ Φ Φ Φ Φ Φ σ ai Φ Φ σwi Φ du 3 3 Φ 3 Φ 3 Φ 33 σaφ Φ + σ wφ 3 Φ 3 σaφ Φ + σ wφ 3 Φ 3 σwφ 3 Φ 33 σaφ Φ + σ wφ 3 Φ 3 σaφ Φ + σ wφ 3 Φ 3 σwφ 3 Φ 33 du 60 σwφ 3 Φ 33 σwφ 3 Φ 33 σwφ 33 Φ 33 Defining δt t + u, each of these integration entries can be written as t + t ft + udu t 0 fδtdδt. Using the expressions for the state-transition matrix, the discrete time noise covariance can be found: Q σai t 3 t σ we 0 I d 7 0 d t 40sin d t + 0 d t 3 3 d t d tcos d t d E Q σa t t I σwe 4 8 I Q 3 σwe I t3 + 3sin d t d t d tcos d t d 5 d + 8cos d t 4 d t + d t d tsin d t 8 8 d 6 d cos d t + d tsin d t d 4 d + 6 d t sin d t + d t3 + 6 d tcos d t 6 d 5 d t Q σa ti σwe 3 3 I 6sin d t 6 d t + d t d 5 d E Q 3 σ we t Q 33 tσ wi 3 3 Q Q Q 3 Q 3 Q 3 Q 3 I sin d t d t d 3 d 4sin d t d t d 4 d When ω, similarly when d, becomes small the above equations will become numerically unstable due to E 57 RPNG

9 d, and thus ω, appearing in the denominator. We therefore loo to tae the limit as d approaches zero. 7 Bias Jacobians lim Q σai t3 d σ we t5 0 I + t7 504 d E 6 lim Q σ t a d 0 I + σ we t4 8 I t5 60 d + t6 44 d E 6 lim Q 3 σwei d 0 6 t3 t4 t5 d + 40 d 63 lim Q σa ti + σwe t3 d 0 3 I + t5 60 d E 64 lim Q 3 σwe I t d 0 t3 t4 d d 65 lim Q 33 tσwi 66 d 0 Changes in biases are modeled as adding corrections to our preintegration measurements through the use of bias Jacobians see 5 and 6, it is critical to compute these Jacobians accurately. In particular, as seen from 3 that 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 Hα + H β + Hα + H β t H β + R t I w tcos w t sin w t ˆω + w t cos w t w tsin w t+ ˆω w 3 w 4 + R ti 3 3 cos ω t ˆω + w t sin ω t ˆω w w lim w 0 Hα + H β + Hα + H β t H β + R t + I t3 3 R ti t t4 ˆω + 8 ˆω t3 ˆω + 6 ˆω 69 Similarly, the Jacobians with respect to the gyro bias α b w : J α, b w : J β can be found by taing the derivative with respect to each of gyro bias entries. That is, we see the entries of J α α + α + α + b w b w b w3 and J β β + β + β + b w b w b w3. For each entry we derive the following: α + α + β t + b +R t I + wi β ˆω tcos ˆω t sin ˆω t ˆω 3 ˆω + ˆω t cos ˆω t ˆω tsin ˆω t + ˆω 4 ˆω â 70 RPNG

10 The first two terms can be found with the previous derivatives. Defining ê i as the unit vector in the ith direction, the third term can be found as: t +R I + f ˆω + f ˆω + R t I + f ˆω + f ˆω + f +R ˆω f ê i + f ˆω f ê i ˆω + ˆω ê i 7 where f and f are the corresponding coefficients in 70, and their derivatives are computed as: For small ˆω, f ω ê i ˆω t sin ˆω t 3sin ˆω t + 3 ˆω tcos ˆω t ˆω 5 7 f ω ê i ˆω t 4cos ˆω t 4 ˆω tsin ˆω t + ˆω t cos ˆω t + 4 ˆω 6 73 Similarly, we have see 3: where β + For small ˆω, lim w 0 lim w 0 f t 5 ˆω i 5 f t 6 ˆω i 7 β + + R ti + f3 ˆω + f 4 ˆω + ˆR f3 + ˆω f 3 ê i + f 4 ˆω f 4 ê i ˆω + ˆω ê i â 76 f 3 f ω ê i cos ˆω t + ˆω tsin ˆω t ˆω 4 77 ω ê i ˆω t + ˆω tcos ˆω t 3sin ˆω t ˆω 5 78 lim w 0 lim w 0 f 3 t 4 ˆω i f 4 t 5 ˆω i 60 We now show how to derive the derivative of the rotation matrix with respect to a change in bias, i.e., b w b w b w : R R exp ω m b w b w t 8 exp ω m b w texp J r b w t exp J r b w texp ω m b w t 83 exp J r b w t 84 I J r b w t 85 RPNG

11 where J ri is the right Jacobian of SO3, see equation. This decomposition can be done for every measurement interval: i+ i R I J ri b w t i+ i ˆR 86 We can now loo at what happens when we compound measurements. In particular, at the second-step, we have the following: R R R 87 I J r b w t ˆRI3 3 + J r b w t 88 I J r b w ti ˆRJr b w t 89 I J r + ˆRJr b w t 90 Here we have used the property that R w R Rw for a rotation matrix. Repeating this process for time-step 3 yields: 3 R 3 R R I J r3 b w t 3 ˆRI3 3 + J r + ˆRJr b w t 9 I J r3 b w I ˆRJr + 3 ˆRJr b w t 3 93 I J r3 + 3 ˆRJr + 3 ˆRJr b w t 3 94 We thus see the pattern developing and can write the updated rotation at any time step u as: u 9 R exp J q ub w b w u R 95 u u with J q u r t 96 Each of these values can be calculated incrementally by noting that: J q u + u+ u ˆR u u ˆRJ r t + J ru+ t u+ ˆRJ q u + J ru+ t 97 The derivative of every rotation with respect to the ith entry of the gyro bias, which appears in both 7 and 76 can be approximated using: The total rotation after a bias update can be expressed as: u u R I J q ub w b w u R 98 ur J q uê i u b R wi 99 ur ur J q uê i 00 + R exp J q + b w b w + R 0 0 RPNG

12 Note that this is essentially the same update as seen in, although we parametrize opposite rotations. We use the symbol to denote an estimate after update and before update. The angle measurement residual can be written as: + δθ vec + G q G q + q quatexp J q + b w b w, 03 + vec G q G q + q J qb w b w, 04 where quat denotes the transformation of a rotation matrix to the corresponding quaternion. In the above expression, we have also used the common assumption that b w b w is small. Note that we only use this approximation for the computation of Jacobians, while 05 is used for the evaluation of actual residuals. 8 Preintegration Measurement Jacobians Our total measurement residuals can be written as: GR G p + G p G v t + G g t J α b w b w H α b a b a ˆα + r GR G v + G v + G g t J β b w b w H β b a b a ˆβ+ vec + G q G q + q quatexp J q b w b 05 w In order to use the preintegrated measurement residuals in graph-based optimization, the corresponding Jacobians with respect to the optimization variables are necessary. To this end, we first rewrite the relativerotation measurement residual as: + δθ vec + G q G q + q q b 06 where q b is the quaternion induced by a change in gyro bias and + q is the quaternion of relative rotation obtained by integrating the IMU measurements. 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. For example, the relative-rotation measurement residual is perturbed by a change in gyro bias around the current estimate i.e., b w b w ˆb w + b w b w : + δθ vec : vec + G ˆ q + G ˆ q + q ˆ q r J qˆb w+ b w b w J qˆb w+ b w b w J qˆb w+ b w b w vec Lˆ q r ˆqr,4 I vec 3 3 ˆq r ˆq J qˆb w+ b w b w r ˆq r ˆq r, ˆq r,4 I 3 3 ˆq r J q ˆb w + b w b w + other terms So that our Jacobian with respect to a perturbance in bias is: + δθ b w ˆq r,4 I 3 3 ˆq r J q RPNG-06-00

13 Similarly, the Jacobian with respect to + δθ G can be found as follows: + δθ vec + δθ G + G ˆ q Gˆ q + q ˆ q b vec + δθ G ˆ q rb vec Rˆq rb + δθ G ˆqrb,4 I vec ˆq rb ˆq rb + δθ G Yielding the Jacobian: The Jacobian with respect to δθ G is given by: + δθ vec vec vec + G ˆ q Gˆ q ˆq n ˆq rb ˆq rb, ˆq rb,4 I ˆq rb + δθ G + other terms 7 δθ G Lˆq n R q mb + δθ + δθ G ˆq rb,4 I ˆq rb 8 δθ G ˆq mb δθ G + q ˆ q b ˆqn,4 I vec 3 3 ˆq n ˆq n qmb,4 I 3 3 q mb q mb δθ G ˆq n ˆq n,4 q mb q mb,4 9 0 ˆq n,4 I ˆq n q mb,4 I 3 3 q mb + ˆq n q mb δθ G + other terms 3 Which gives the Jacobian: + δθ δθ G ˆq n,4 I 3 3 ˆq n q mb,4 I 3 3 q mb + ˆq n q mb 4 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. α + GR G p + G p G v t + G g t J α b w b w H α b a b a I 3 3 δθ G G ˆRGˆp + + G p + Gˆp G p Gˆv t G ṽ t + G g t J α ˆbw + b w b w H α ˆba + b a b a 5 RPNG-06-00

14 Then the following Jacobians immediately becomes available: ˆR G α + δθ G Gˆp + Gˆp Gˆv t + G g t 6 α + G p ˆR G 7 α + G p ˆR G + 8 α + G v ˆR t G 9 α + J α b w 30 α + H α b a 3 Similarly, we can write our β measurement as: β + GR G v + G v + G g t J β b w b w I 3 3 δθ G G ˆR Gˆv + + G ṽ + Gˆv G ṽ + G g t which leads to the following Jacobians: J β ˆb w + b w b w H β ˆb a + b a b a 3 β + G ˆRGˆv + Gˆv + g t G δθ G 33 β + G v ˆR G 34 β + G v ˆR G + 35 β + J β b w 36 β + H β b w 37 References Gregory S Chirijian. Stochastic Models, Information Theory, and Lie Groups, Volume : Analytic Methods and Modern Applications. Vol.. Springer Science & Business Media, 0. Christian Forster et al. IMU preintegration on manifold for efficient visual-inertial maximum-aposteriori estimation. In: Robotics: Science and Systems XI. EPFL-CONF WG Brecenridge. Quaternions proposed standard conventions. In: Jet Propulsion Laboratory, Pasadena, CA, Interoffice Memorandum IOM 999, 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 RPNG

15 5 J.A. Hesch et al. Camera-IMU-based localization: Observability analysis and consistency improvement. In: International Journal of Robotics Research 33 04, pp M. Li and A. Mouriis. High-Precision, Consistent EKF-based Visual-Inertial Odometry. In: International Journal of Robotics Research , pp G. Huang, M. Kaess, and J. Leonard. Towards Consistent Visual-Inertial Navigation. In: Proc. of the IEEE International Conference on Robotics and Automation. Hong Kong, China, 04, pp Vijay Kumar and Nathan Michael. Opportunities and challenges with autonomous micro aerial vehicles. In: International Journal of Robotics Research 3. Sept. 0, pp Fran Dellaert and M. Kaess. Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. In: International Journal of Robotics Research , pp T. Lupton and S. Suarieh. Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions. In: IEEE Transactions on Robotics 8. Feb. 0, pp Yonggen Ling, Tianbo Liu, and Shaojie Shen. Aggressive quadrotor flight using dense visual-inertial fusion. In: 06 IEEE International Conference on Robotics and Automation ICRA. 06, pp DOI: 0.09/ICRA R. Kümmerle et al. go: A General Framewor for Graph Optimization. In: Proc. of the IEEE International Conference on Robotics and Automation. Shanghai, China, 0, pp T. Lupton and S. Suarieh. Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions. In: IEEE Transactions on Robotics 8. 0, pp ISSN: DOI: 0.09/TRO Peter S. Maybec. Stochastic Models, Estimation, and Control. Vol.. London: Academic Press, 979. RPNG