Trajectory GraphBased Planning Under Uncertainty

1
Trajectory Graph-Based Planning Under Uncertainty

• Pose path-planning as trajectory graph search

UWB
UWB
start
UWB
GOAL

• Given Trajectory Graph Map
• Goal Find path that results in Minimum
  Uncertainty at goal

2
How to predict uncertainty?

• Use Kalman Filter beliefs
• Propagate covariance with KF updates

• Assume
• mean discrete points along trajectory
• measurements KF predicted measurements

3
(Poor) Example
4
Multi-Step Update as One-Step

• For efficient graph search, want efficient
  update (One-Step)

5
Solution Decomposition
6
Solution Decomposition
7
Algebraic Solution
Matrix Inversion Lemma --gt Duality KF/IF
Adding Information
Losing Uncertainty
Losing Information
Adding Uncertainty
Both updates in Fractional Form --gt Matrix
Continued Fractions
8
Star Product