STATE ESTIMATION FOR ROBOTICS - University of Toronto

1 STATE ESTIMATION FOR. ROBOTICS . Timothy D. Barfoot c 2019. Copyright Cambridge University Press is the Official Publisher This Unofficial Version Compiled on October 1, 2019. Send errata to Revision History 13 May 2017 Version best matching published first edition 12 Aug 2017 Equation ( ): x changed to xx 12 Aug 2017 Page 111, bullet 4: y changed to yy 12 Aug 2017 Page 117, bullet (ii): changed 4(a) to 3. 12 Aug 2017 Equation ( ): P changed to P k 12 Aug 2017 Equation ( ): P k changed to P k 12 Aug 2017 Equation ( ): xop,k,0 changed to xop,k,i 12 Aug 2017 Equation ( ): removed negative sign 22 Nov 2017 Fixed typo in Jacobi identity (page 218). 10 Dec 2017 Equation ( ): 1yy yx changed to xy yy 1. vk i 10 Dec 2017 Inline above ( ): rvi i changed to ri T.

2 10 Dec 2017 Equation ( ): 0 changed to 0. 10 Dec 2017 Inline below ( ): e(xop ) = Lu(xop ) changed to e(xop ) = L 1 u(xop ). 10 Dec 2017 Angular acceleration: 21 changed to .. 21.. 10 Dec 2017 Equation ( ): corrected a double comma 10 Dec 2017 Equation ( ): nk,j changed to y k,j 5 Jan 2018 Page 227, definition of Ad(SE(3)): removed extra comma before final bracket 5 Jan 2018 Equation ( ): changed j? to ?j 5 Jan 2018 Equation ( ): added missing VT in an intermediate step 5 Jan 2018 Equation ( ): changed ? to . 5 Jan 2018 Equation ( ): changed subscript ` to j 5 Jan 2018 Equation ( ): changed G k` to G jk 16 Feb 2018 Pages 267-8: corrected J 1 ` and J 1. r to J` and Jr 25 Mar 2018 Page 108: corrected mean of output is' to be mean of input is'.

3 25 Mar 2018 Equation ( ): corrected rbab to be rb ab 13 Apr 2018 Equation ( ): corrected j 1 to j = 1. 22 Apr 2018 Equation ( ): added missing 1 in definition of . iii iv Revision History 12 May 2018 Equations ( ) and ( ): fixed two negative signs each to repair proof 12 May 2018 Identity page: fixed summation on series definitions of T, T , and their inverses to start at n = 0 not n = 1. 27 May 2018 Started an appendix with new material since first edi- tion 13 Jul 2018 Equation ( ): changed T21 = T1 T 1 2 to T21 =. T2 T 1. 1 just below this equation in the text 13 July 2018 Equation ( ): made second term negative 13 Aug 2018 Equation ( ): corrected Ju to Jv 13 Aug 2018 Exercise : adjusted wording to due to poor use of the term affine'.

4 22 Nov 2018 Equation ( ): corrected n0i to be n0k,i 26 Nov 2018 Exercise : corrected N to K in upper limit of mean summation 28 Nov 2018 Added Appendix on the posterior covariance re- cursion for the Cholesky and RTS smoothers. 31 Jan 2019 Page 29, cleaned up an inconsistency involving M. 24 Feb 2019 Equation ( ): corrected upper integration limit from tk to tk:k 1. 29 Apr 2019 Equations ( ), ( ), ( ), ( ): adjust- ments made to fix an inconsistent dimension problem 23 Jul 2019 Equation ( ): v1:k 1 corrected to v1:k 1 Oct 2019 Equation ( ) and ( ): v corrected to . Contents Acronyms and Abbreviations xi Notation xiii Foreword xv 1 Introduction 1. A Little History 1. Sensors, Measurements, and Problem Definition 3. How This Book Is Organized 4.

5 Relationship to Other Books 5. Part I ESTIMATION Machinery 7. 2 Primer on Probability Theory 9. Probability Density Functions 9. Definitions 9. Bayes' Rule and Inference 10. Moments 11. Sample Mean and Covariance 12. Statistically Independent, Uncorrelated 12. Normalized Product 13. Shannon and Mutual Information 14. Cram er-Rao Lower Bound and Fisher Information 14. Gaussian Probability Density Functions 15. Definitions 15. Isserlis' Theorem 16. Joint Gaussian PDFs, Their Factors, and Inference 18. Statistically Independent, Uncorrelated 20. Linear Change of Variables 20. Normalized Product of Gaussians 22. Sherman-Morrison-Woodbury Identity 23. Passing a Gaussian through a Nonlinearity 24. Shannon Information of a Gaussian 28.

6 Mutual Information of a Joint Gaussian PDF 30. Cram er-Rao Lower Bound Applied to Gaussian PDFs 30. Gaussian Processes 32. Summary 33. Exercises 33. v vi Contents 3 Linear-Gaussian ESTIMATION 37. Batch Discrete-Time ESTIMATION 37. Problem Setup 37. Maximum A Posteriori 39. Bayesian Inference 44. Existence, Uniqueness, and Observability 46. MAP Covariance 50. Recursive Discrete-Time Smoothing 51. Exploiting Sparsity in the Batch Solution 52. Cholesky Smoother 53. Rauch-Tung-Striebel Smoother 55. Recursive Discrete-Time Filtering 58. Factoring the Batch Solution 59. Kalman Filter via MAP 63. Kalman Filter via Bayesian Inference 68. Kalman Filter via Gain Optimization 69. Kalman Filter Discussion 70. Error Dynamics 71. Existence, Uniqueness, and Observability 72.

7 Batch Continuous-Time ESTIMATION 74. Gaussian Process Regression 74. A Class of Exactly Sparse Gaussian Process Priors 77. Linear Time-Invariant Case 83. Relationship to Batch Discrete-Time ESTIMATION 87. Summary 88. Exercises 88. 4 Nonlinear Non-Gaussian ESTIMATION 91. Introduction 91. Full Bayesian ESTIMATION 92. Maximum a Posteriori ESTIMATION 94. Recursive Discrete-Time ESTIMATION 96. Problem Setup 96. Bayes Filter 97. Extended Kalman Filter 100. Generalized Gaussian Filter 103. Iterated Extended Kalman Filter 105. IEKF Is a MAP Estimator 106. Alternatives for Passing PDFs through Nonlinearities 107. Particle Filter 116. Sigmapoint Kalman Filter 118. Iterated Sigmapoint Kalman Filter 123. ISPKF Seeks the Posterior Mean 126.

8 Taxonomy of Filters 127. Batch Discrete-Time ESTIMATION 127. Maximum A Posteriori 128. Bayesian Inference 135. Maximum Likelihood 137. Discussion 142. Contents vii Batch Continuous-Time ESTIMATION 143. Motion Model 143. Observation Model 146. Bayesian Inference 146. Algorithm Summary 147. Summary 148. Exercises 149. 5 Biases, Correspondences, and Outliers 151. Handling Input/Measurement Biases 152. Bias Effects on the Kalman Filter 152. Unknown Input Bias 155. Unknown Measurement Bias 157. Data Association 159. External Data Association 160. Internal Data Association 160. Handling Outliers 161. RANSAC 162. M- ESTIMATION 163. Covariance ESTIMATION 166. Summary 168. Exercises 168. Part II Three-Dimensional Machinery 171.

9 6 Primer on Three-Dimensional Geometry 173. Vectors and Reference Frames 173. Reference Frames 174. Dot Product 174. Cross Product 175. Rotations 176. Rotation Matrices 176. Principal Rotations 177. Alternate Rotation Representations 178. Rotational Kinematics 184. Perturbing Rotations 188. Poses 192. Transformation Matrices 193. ROBOTICS Conventions 194. Frenet-Serret frame 196. Sensor Models 199. Perspective Camera 199. Stereo Camera 206. Range-Azimuth-Elevation 208. Inertial Measurement Unit 209. Summary 211. Exercises 212. viii Contents 7 Matrix Lie Groups 215. Geometry 215. Special Orthogonal and Special Euclidean Groups 215. Lie Algebras 217. Exponential Map 219. Adjoints 226. Baker-Campbell-Hausdorff 230. Distance, Volume, Integration 237.

10 Interpolation 240. Homogeneous Points 246. Calculus and Optimization 246. Identities 254. Kinematics 255. Rotations 255. Poses 258. Linearized Rotations 261. Linearized Poses 265. Probability and Statistics 266. Gaussian Random Variables and PDFs 267. Uncertainty on a Rotated Vector 271. Compounding Poses 273. Fusing Poses 280. Propagating Uncertainty through a Nonlinear Camera Model 285. Summary 292. Exercises 293. Part III Applications 295. 8 Pose ESTIMATION Problems 297. Point-Cloud Alignment 297. Problem Setup 298. Unit-Length Quaternion Solution 298. Rotation Matrix Solution 302. Transformation Matrix Solution 316. Point-Cloud Tracking 319. Problem Setup 319. Motion Priors 320. Measurement Model 321. EKF Solution 322.