Efficient Belief Space Planning in High-Dimensional State Spaces By
Total Page:16
File Type:pdf, Size:1020Kb
Efficient Belief Space Planning in High-dimensional State Spaces by Exploiting Sparsity and Calculation Re-use Dmitry Kopitkov Efficient Belief Space Planning in High-dimensional State Spaces by Exploiting Sparsity and Calculation Re-use Research Thesis Submitted in partial fulfillment of the requirements for the degree of Master of Science in Technion Autonomous Systems Program (TASP) Dmitry Kopitkov Submitted to the Senate of the Technion — Israel Institute of Technology Adar 5777 Haifa March 2017 This research was carried out under the supervision of Assistant Prof. Vadim Indelman, in the Faculty of Aerospace. Acknowledgements I would like to hugely thank my advisor Prof. Vadim Indelman for all his guidance, help, support and patience through the years of my Master’s studies. My research skills got elevated significantly all thanks to his excellent schooling and high-standard demands. I would also like to thank the people from Technion Autonomous Systems Program, espe- cially so Sigalit Preger, for helping me getting over the bureaucracy of the university and for being supportive through sometimes very uneasy moments during the last years. Finally, I thank all my friends for having my back and being there for me this entire time. Your help allowed me to keep standing on my legs and to continue and accomplish this research. The generous financial help of the Technion is gratefully acknowledged. Contents List of Figures List of Tables Abstract 1 Abbreviations and Notations 3 1 Introduction 5 1.1 Related Work . .6 1.2 Contributions . .7 1.3 Organization . .8 2 Notations and Problem Formulation 11 3 Approach 17 3.1 BSP as Factor Graph . 17 3.2 BSP via Matrix Determinant Lemma . 21 3.2.1 Unfocused Case . 21 3.2.2 Focused Case . 23 3.3 Augmented BSP via AMDL . 25 3.3.1 Augmented Matrix Determinant Lemma (AMDL) . 25 3.3.2 Unfocused Augmented BSP through IG . 27 3.3.3 Focused Augmented BSP . 29 3.4 Re-use Calculations Technique . 33 3.5 Connection to Mutual Information Approach and Theoretical Meaning of IG . 35 3.6 Mutual Information - Fast Calculation via Information Matrix . 38 4 Application to Different Problem Domains 39 4.1 Sensor Deployment . 39 4.2 Augmented BSP in Unknown Environments . 43 4.3 Graph Reduction . 49 5 Alternative Approaches 51 6 Results 53 6.1 Sensor Deployment (focused and unfocused )................ 53 6.2 Measurement Selection in SLAM . 57 6.3 Autonomous Navigation in Unknown Environment . 58 7 Conclusions and Future Work 65 7.1 Future Work . 65 8 Appendix - Related Publications 67 9 Appendix - Proof of Lemmas 69 9.1 Proof of Lemma 3.3.1 . 69 9.2 Proof of Lemma 3.3.2 . 70 9.3 Proof of Lemma 3.3.3 . 71 9.4 Proof of Lemma 3.3.4 . 72 9.5 Proof of Lemma 3.3.5 . 73 9.6 Proof of Lemma 3.3.6 . 74 F 9.7 SLAM Solution - focus on last pose Xk+L ≡ xk+L ................ 75 F 9.8 SLAM Solution - focus on mapped landmarks Xk+L ≡ Lk ............ 77 9.9 Proof of Lemmas 3.6.1 and 3.6.2 . 80 Hebrew Abstract i List of Figures 2.1 Illustrution of Λk+L’s construction for a given candidate action in Augmented Aug 0 BSP case. First, Λk+L is created by adding n zero rows and columns. Then, the Aug T new information of belief is added through Λk+L = Λk+L + A A......... 13 3.1 Illustration of belief propagation in factor graph representation - not-augmented case. Two actions ai and a j are considered, introducing into graph two new factor sets F (ai) and F (a j) respectively (colored in green). 18 3.2 Illustration of belief propagation in factor graph representation - augmented case. Two actions ai and a j are considered, introducing their own factor graphs G(ai) and G(a j) (colored in pink) that are connected to prior Gk through factor conn conn sets F (ai) and F (a j) (colored in green) respectively. 19 3.3 Concept illustration of A’s structure. Each column represents some variable from state vector. Each row represents some factor from Eq. (2.6). Here, A represents set of factors F = f f1(xi−1; xi); f2(xi; l j)g where factor f1 of motion model that involves two poses xi and xi−1 will have non-zero values only at columns of xi and xi−1. Factor f2 of observation model that involves together variables xi and l j will have non-zero values only at columns of xi and l j..... 21 3.4 Partitions of Jacobians and state vector Xk+L in Augmented BSP case, unfocused scenario. Note: the shown variable ordering is only for illustration, while the developed approach supports any arbitrary variable ordering. Also note that all white blocks consist of only zeros. Top: Jacobian A of factor set F (a) = fF conn(a); F new(a)g. Bottom: Jacobians B and D of factor sets F conn(a) and F new(a) respectively. 25 3.5 Partitions of Jacobians and state vector Xk+L in Augmented BSP case, Focused F (Xk+L ⊆ Xnew) scenario. Note: the shown variable ordering is only for illus- tration, while the developed approach supports any arbitrary variable ordering. Also note that all white blocks consist of only zeros. Top: Jacobian A of factor set F (a) = fF conn(a); F new(a)g. Bottom: Jacobians B and D of factor sets F conn(a) and F new(a) respectively. 30 3.6 Partitions of Jacobians and state vector Xk+L in Augmented BSP case, Focused F (Xk+L ⊆ Xold) scenario. Note: the shown variable ordering is only for illustration, while the developed approach supports any arbitrary variable ordering. Also note that all white blocks consist of only zeros. Top: Jacobian A of factor set F (a) = fF conn(a); F new(a)g. Bottom: Jacobians B and D of factor sets F conn(a) and F new(a) respectively. 32 4.1 Illustration of belief propagation in factor graph representation - Sensor Deploy- ment scenario. The space is discretized through grid of locations L fl1;:::; l9g. Factor f0 within prior factor graph Gk represents our prior belief about state vec- tor, P0(X). Factors f1 − f3 represent measurements taken from sensors deployed 3 6 7 8 at locations l1, l2 and l4. Two actions ai = fl ; l g and a j = fl ; l g are consid- ered, introducing into graph two new factor sets F (ai) and F (a j) respectively (colored in green). In this example value of c0 is 2. 40 4.2 Illustration of belief propagation in factor graph representation - SLAM scenario. Nodes xi represent robot poses, while nodes li - landmarks. Factor f1 is prior on robot’s initial position x1; factors between robot poses represent motion model (Eq. (4.8)); factors between pose and landmark represent observation model (Eq. (4.9)). Two actions ai and a j are considered, performing loop-closure to re-observe landmark l1 and l2 respectively. Both actions introduce their own factor graphs G(ai) and G(a j) (colored in pink) that are connected to prior Gk conn conn through factor sets F (ai) and F (a j) (colored in green) respectively. 43 6.1 Unfocused sensor deployment scenario. Running time for calculating impact of a single action as a function of state dimension n (a) and as a function of Jacobian A’s height m (b). In (a), m = 2, while in (b) n = 625. rAMDL Unfocused Objective represents only calculation time of candidates’ impacts (IG objective for all actions), without one-time calculation of prior covariance; Covariance Inverse represents the time it took to calculate covariance matrix Σk −1 from dense information matrix Λk, Σk = Λk . (c) Running time for sequential decision making, i.e. evaluating impact of all candidate actions, each repre- senting candidate locations of 2 sensors. (d) prior and final uncertainty of the field, with red dots marking selected locations. (e) number of action candidates per decision. (f) running time for sequential decision making, with number of candidates limited to 100. 55 6.2 Focused sensor deployment scenario, (a) overall time it took to make deci- sion with different approaches; rAMDL Focused Objective represents only calculation of candidates’ impacts (IG objective for all actions) while rAMDL Focused - both one-time calculation of prior covariance Σk and candidates’ eval- uation. (b) Final uncertainty of the field, with red dots marking selected locations. (c) Focused set of variables (green circles) and locations selected by algorithm (red dots). (d) Overall system entropy (above) and entropy of focused set (bottom) after each decision, with blue line representing unfocused algorithm, and red line - focused algorithm. Note - all unfocused methods make exactly the same decisions, with difference only in their runtime complexity. Same is also true for all focused methods. 56 6.3 Measurement selection scenario, (a) simulated trajectory of robot; black dots are the landmarks, blue marks and surrounding ellipses are the estimated trajectory along with the uncertainty covariance for each time instant, red mark is robot’s initial pose; (b) number of measurement candidates per decision; (c) state’s dimension n per decision; (d) overall time it took to evaluate impacts of pose’s all measurements, with different approaches; rAMDL Unfocused Objective represents only calculation of candidates’ impacts (IG objective for all actions) while rAMDL Unfocused - both one-time calculation of marginal covariance M;XAll Σk and candidates’ evaluation. 57 6.4 Focused BSP scenario with focused robot’s last pose. (a) Dimensions of the BSP problem (state dimension, average number of new factor terms, average number of new variables, average number of old involved variables) at each time; (b) Number of action candidates at each time; (c) Final robot trajectory.