11/12/2007 11/12/2007 CSE-571 Probabilistic Robotics 2 Outputs are noisy function of inputs: Non-parametric regression model Distribution over functions y f ( x ) i i Fully specified by training data and kernel Function values are jointly Gaussian: function 1 2 2 Output variables are jointly Gaussian co v f ( x ) , f ( x ) k ( x , x ) e x p | x x | p q p q f p q 2 2 l Covariance given by distance of inputs in Considering noise: kernel space 2 c o v y , y k ( x , x ) p q p q n p q 2 p ( Y | X ) ( , 0 K ( X X , ) I ) n 11/12/2007 CSE-571 Probabilistic Robotics 3 11/12/2007 CSE-571 Probabilistic Robotics 4 1
11/12/2007 Training data: Maximize data log likelihood: D {( x , y ), ( x , y ), , ( x , y )} ( X y , ) log p ( y X | ) 1 1 2 2 n n 1 1 n 1 T 2 2 y K ( X X , ) I y log K ( X X , ) I log 2 Prediction given training samples: n n 2 2 2 * * 2 p y ( | x , , y X ) N , Compute derivatives wrt. params 2 2 , , l n f 1 * 2 Optimize using conjugate gradient K ( X , X ) K ( X X , ) I y n 1 2 * * * 2 * K ( x , x ) K ( x , X ) K ( X X , ) I K ( X x , ) n 11/12/2007 CSE-571 Probabilistic Robotics 5 11/12/2007 CSE-571 Probabilistic Robotics 6 [Ferris-Haehnel-Fox: RSS-06] Mean Variance 11/12/2007 CSE-571 Probabilistic Robotics 7 11/12/2007 CSE-571 Probabilistic Robotics 8 2
11/12/2007 [Ferris-Fox-Lawrence: IJCAI-07] GP-LVM: GP with latent / unobserved variables (locations) Can incorporate motion constraints 11/12/2007 CSE-571 Probabilistic Robotics 9 11/12/2007 CSE-571 Probabilistic Robotics 10 Controller development benefits from accurate model Two approaches to system modeling Parametric / physics-based models Non-parametric / data-driven models Combining these two approaches yields superior model 11/12/2007 CSE-571 Probabilistic Robotics 11 11/12/2007 CSE-571 Probabilistic Robotics 12 3
11/12/2007 e R v p ODE can be used to generate a step ahead b H ( ) d prediction function f s 1 dt v M ( Forces * Mv ) 1 J ( Torques * J ) s ( k 1 ) s ( k ) f ( s ( k ), u ( k )) Problems 12-D state=[pos,rot,transvel,rotvel] Describes evolution of state as ODE Limited accuracy Forces / torques considered: buoyancy, gravity, drag, thrust Noise not explicit in model 16 parameters are learned by optimization on ground truth motion capture data 11/12/2007 CSE-571 Probabilistic Robotics 13 11/12/2007 CSE-571 Probabilistic Robotics 14 Grimes et al 2006 Kocijan et al. 2005 State transition learned directly Target output takes parametric model into f account gt gt T ( k ) s ( k 1 ) s ( k ) gt gt gt T ( k ) s ( k 1 ) s ( k ) f ( s ( k ), u ( k )) Training data for GP: gt GP Enhanced-GP model equation D [ s ( k ), u ( k )], T ( k ) GP prediction s ( k 1 ) s ( k ) f ( s ( k ), u ( k )) g ([ s ( k ), u ( k )]) EGP s ( k 1 ) s ( k ) g ([ s ( k ), u ( k )]) Better accuracy Problems: Less training data necessary Generalizes poorly Noise incorporated into system Full coverage of state space difficult 11/12/2007 CSE-571 Probabilistic Robotics 15 11/12/2007 CSE-571 Probabilistic Robotics 16 4
11/12/2007 State: xyz pitch yaw Observation: xc yc width height theta Models: Parametric using computer graphics/vision, GP, EGP Mocap Mocap system system cameras 11/12/2007 CSE-571 Probabilistic Robotics 17 11/12/2007 CSE-571 Probabilistic Robotics 18 Propagation method pos(mm) rot(deg) vel(mm/s) rotvel(deg/s) Param 3.3 0.5 14.6 1.5 Modeling Major method pos(pix) axis(pix) Minor axis(pix) Theta(deg) GPonly 1.8 0.2 9.8 1.1 Param 7.1 2.9 5.7 9.2 EGP 1.6 0.2 9.6 1.3 Gponly 4.7 3.2 1.9 9.1 • Single step prediction error EGP 3.9 2.4 1.9 9.4 • ¼ sec timesteps • Avg over ~1000 test points 11/12/2007 CSE-571 Probabilistic Robotics 19 11/12/2007 CSE-571 Probabilistic Robotics 20 5
11/12/2007 u(k-1) u(k) u(k+1) R(k) u(k-1) u(k) u(k+1) Process s(k-1) s(k) s(k+1) model s(k-1) s(k) s(k+1) g s ( t ), u ( t ) s ( t 1 ) Observation z(k-1) z(k) z(k+1) h s ( t ) z ( t ) model z(k-1) z(k) z(k+1) Q(k) Sequential state estimation Linear dynamical system p ( s | s , u ) Prediction step: Extended-KF / Unscented-KF: Locally linearized t 1 t t state propagation and observation models Correction step: p ( z | s ) t t 11/12/2007 CSE-571 Probabilistic Robotics 21 11/12/2007 CSE-571 Probabilistic Robotics 22 GP GP u(k-1) R(k) Q(k) Q(k) R(k) i i for i 0 ... 2 n : g ( u , ) GP 2 n GP ' i i Process Process Observation Observation m s(k-1) s(k) z(k) model i 0 model model model 2 n ' i i ' i ' T ( )( ) c i 0 Determine sigma points based on covariance Use GP process and observation models Propagate using nonlinear function g Replace static noise parameters with Use propagated sigma points to recreate mean and uncertainty from GP covariance 11/12/2007 CSE-571 Probabilistic Robotics 23 11/12/2007 CSE-571 Probabilistic Robotics 24 6
11/12/2007 GP-UKF( ): u z , , , -Determine sigma points i g i - X GP X 1 , u k k k 1 -Recover new mean and sigma g - R GP 1 , u k k k 1 -Determine sigma points ˆ ˆ i h i Z GP X - k k -Recover new mean and sigma h ˆ - Q GP k k -Perform correction 11/12/2007 CSE-571 Probabilistic Robotics 25 11/12/2007 CSE-571 Probabilistic Robotics 26 Prediction error Propagation Tracking method pos(mm) rot(deg) vel(mm/s) rotvel(deg/s) algorithm pos(mm) rot(deg) vel(mm/s) rotvel(deg/s) MLL Param 3.3 0.5 14.6 1.5 UKF 141 9.6 141.5 8.1 2.1 GP-UKF GPonly 1.8 0.2 9.8 1.1 (GPonly) 107.9 10.2 71.7 5.9 5.1 EGP 1.6 0.2 9.6 1.3 GP-UKF (EGP) 86 6.1 57.1 5.7 12.9 Observation error • Average tracking error Modeling Major Minor method pos(pix) axis(pix) axis(pix) Theta(deg) • Trajectory ~12 min long Param 7.1 2.9 5.7 9.2 • 0.5 sec timesteps GPonly 4.7 3.2 1.9 9.1 EGP 3.9 2.4 1.9 9.4 11/12/2007 CSE-571 Probabilistic Robotics 27 11/12/2007 CSE-571 Probabilistic Robotics 28 7
11/12/2007 Full process model tracking GPs provide flexible modeling framework Take data noise and uncertainty due to data sparsity into account No right turn process model tracking Combination with parametric models increases accuracy and reduces need for training data Seamless integration into Bayes filters • Training data for right turns removed Compexity is problem: Training: Prediction • Uncertainty increases appropriately 3 2 O ( n ) O n ( ) 11/12/2007 CSE-571 Probabilistic Robotics 29 11/12/2007 CSE-571 Probabilistic Robotics 30 Complexity can be reduced by removing / ignoring data points (sparse GP) Input dependent signal noise (heteroscedastic GP) Input dependent kernel parameters Can be used for dimensionality reduction (e.g. GP-LVM) Uncertainty provides means for active exploration and optimal sensor placement 11/12/2007 CSE-571 Probabilistic Robotics 31 8
Recommend
More recommend
