1、Batch Estimation, Solving Sparse Linear Systems in Information and Square-Root Form,June 12, 2017 Benjamin Skikos,Outline,Information & Square Root Filters Square Root SAM Batch Approach Variable ordering and structure of SLAM Incremental Approach 1 Bayes Tree Incremental Approach 2,Information Form
2、,Extended Information Filter,EKF represents posterior as mean and covarianceEIF represents posterior as information matrix and information vector,Information Filter Motion Update,From the EKF,Information Filter Measurement Updates,Square Root Filter,Historically motivated by limited computer precisi
3、on Factorize either covariance or information and rederive propagation and update equations Condition number is halved= ,Smoothing,In this context smoothing will be the “full SLAM problem”; estimate the robots trajectory and surroundings given all available measurements. For factor graphs, that mean
4、s optimize over all the unknown states Recall, factors encode the joint probability over all unknowns,Factor Graph Optimization,Recall, to solve a factor graph it is converted to LLS. From here on out it is all about crunching matricesA is the stack of all factor Jacobians (Measurement Jacobian) B i
5、s the stack of all measurement/process model error,Information Form - SAM,The solution is found by solving the normal equationsATA is the information matrix or Hessian Efficiently solved by factorization Batch problem now solved,Ex: Cholesky Factorization,In this variant, R is an upper triangular ma
6、trix The sparseness of R and I affects how long the factorization takes Worst case fully dense: n3/3 The sparseness of R changes with variable ordering in the information matrix,Matrix Structure,A corresponds to the factor graph I corresponds to the adjacency matrix of the Markov Random Field. Each
7、square root factor is associated with a triangulated (or chordal) graph whose elimination corresponds with the Bayes Net,Markov Random Field,Undirected graph with Markov properties: Pairwise: Any two non-adjacent variables are conditionally independent given all other variables Local Markov: A varia
8、ble is conditionally independent of all other variables given its neighbors Global Markov: Any two subsets of variables are conditionally independent given a separating subsethttps:/en.wikipedia.org/wiki/Markov_random_field,Factor Graph to Markov Random Field,Factors are abstracted out MRF edges rep
9、resent dependencies between random variables Like factor graphs, encode joint probability,Factor Graph To a Bayes Net,MRF to Bayes Net,Additional Conditionals,Variable Ordering,MRF,ColAMD Elimination Ordering,Landmarks, Then Poses,Finding the optimal ordering is NP-Complete Fewer edges means faster
10、back-substitution,Online SAM,Most new measurements only directly affect a small subset of the state vector Need a way to add state elements incrementally without redoing work,Incremental Approach - ISAM,Consider the QR factorization of A substituted for A,Givens Rotations,Jacobian Update, = Incremen
11、tally updating R is just more Givens rotations,Uncertainty and Data Association,Uncertainty of the state is required to perform certain common tasksIn order to match measurements to landmarks, maximum likelihood can be used: This requires computing the Mahalanobis distance between measured position
12、and each landmark Need covariance on state estimate,Marginal Covariance,The covariance of a subset of state variables may be all that is required,Marginal Recovery,Assuming the marginal of interest includes the rightmost variables,Last Column of Y,Diagonal Entries,Can recover full covariance matrixB
13、ack-substitutions,Exact Vs Approximate,Cliques,The cliques of a graph are subsets of fully connected vertices,Clique Tree,The square root factor is associated with a chordal graph,Bayes Net Again,Recall the Bayes Net,Bayes Tree,A factorization of the Bayes Net Encodes factored probability density Cl
14、iques discovered via Maximum Cardinality Search,X2, X3,L1,X1 : X2,L2 : X3,Incremental Approach - ISAM2,Bayes Tree representation can be updated incrementally,Variable Ordering,During the increment, the elimination of the intermediate factor graph can be reordered,Non-Linear Factors and Partial Updat
15、es,When incorporating non-linear factors, a Taylor expansion is typically used The process of updating Jacobians at new linearization points costs time Only update Jacobians if needed Similarly, defer updating states that dont change much,Complexity,Worst case is O(n3) for general matrix factorizati
16、on Planar mapping with restricted sensor range is O(n1.5) Incremental methods can often do better most of the time,Questions?,But What About Hard Deadlines?,Worst-case runtime is grows as the number of variables increase If constant time is required, need another solution,References,Course Reference
17、s: K. Wu, A. Ahmed, G. A. Georgiou, and S. I. Roumeliotis, “A square root inverse filter for efficient vision-aided inertial navigation on mobile devices.,” in Robotics: Science and Systems, 2015. M. Kaess, A. Ranganathan, and F. Dellaert, “isam: Incremental smoothing and mapping,” IEEE Transactions
18、 on Robotics, vol. 24, no. 6, pp. 13651378, 2008. M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping using the bayes tree,” The International Journal of Robotics Research, vol. 31, no. 2, pp. 216235, 2012. Additional References: J.
19、Lambers, “The QR Factorization” Lecture Notes. Retrieved from http:/www.math.usm.edu/lambers/mat610/sum10/lecture9.pdf S. Thrun, Y. Liu, D. Koller, A. Ng, Z. Ghahramani, H. Durrant-Whyte. “SEIF”. Retrieved from http:/robots.stanford.edu/papers/thrun.seif.pdf F. Dellaert, M, Kaess. “Square Root SAM:
20、Simultaneous Localization and Mapping via Square Root Information Smoothing” in The International Journal of Robotics Research, vol. 25, no. 12, pp. 1181-1203, 2006 M. Salzmann, “Some Aspects of Kalman Filtering” University of New Brunswick, August 1988All pictures taken from these sources and wikipedia,
