Trajectory Smoothing as a Linear Optimal Control Problem Biswadip Dey & P. S. Krishnaprasad Allerton Conference - 212 Monticello, IL, USA. October 4, 212
Background and Motivation 1 Background and Motivation 2 Problem Generative Model Inverse Problem Relationship between Linear and Non-linear Generative Models 3 Optimal Control Based Approach for Trajectory Reconstruction Path Independence Lemma Existence of Optimal Initial Condition Optimal Reconstruction as a Linear Smoother Co-State Based Approach 4 Cross-validation Approach to Inverse Problem 5 Numerical Results
Background and Motivation Background and Motivation To explore underlying strategies and motion (pursuit, collective motion etc.) governing control laws, by extracting parameters of motion (namely curvature, speed, lateral acceleration etc.) from sampled observations of trajectories. To extract control inputs from sampled data.
Problem 1 Background and Motivation 2 Problem Generative Model Inverse Problem Relationship between Linear and Non-linear Generative Models 3 Optimal Control Based Approach for Trajectory Reconstruction Path Independence Lemma Existence of Optimal Initial Condition Optimal Reconstruction as a Linear Smoother Co-State Based Approach 4 Cross-validation Approach to Inverse Problem 5 Numerical Results
Problem Generative Model Generative Models for a Curve inr 3 (Non-linear and Linear) Natural Frenet Frame ṙ = νt T = ν(k 1 M 1 +k 2 M 2 ) Ṁ 1 = νk 1 T Ṁ 2 = νk 2 T Trajectory T r M2 M1 (1) The natural curvatures are the steering inputs and the speed is a time function dictated by propulsive/lift/drag mechanisms. Linear Generative Model ṙ = v v = a ȧ = u (2) Jerk, i.e. the third-derivative of position, is viewed as the control. LTI representation ẋ = Ax+Bu r = Cx with, x = [ r T v T a T ] T ; A = I I ;B = I C = [ I ] ; (3) Controllable and Observable
Problem Inverse Problem Regularized Inverse Problem T M2 : Data Points r M1 Trajectory Given a time series of observed positions, generate a smooth trajectory to fit the data points. The inverse problem is ill-posed. Highly sensitive to noise. Non-unique. A regularization parameter is introduced to control the amount of smoothing. Ordinary cross validation is a standard approach to choose an optimal value for the regularization parameter.
Problem Inverse Problem Extracting Curvature (Inverse Problem) Non-linear Optimization Minimize Observations r i Regularized Inversion States, Controls r, [T,M 1,M 2],k 1,k 2,ν ( N ) T r(t i ) r i 2 +λ ( k 1 2 + k 2 2 + ν 2 )dt i= subject to Dynamics in (1), Initial Condition, and Input (4) Linear-Quadratic Control Minimize Observations r i ( N i= Regularized Inversion States, Controls r,v,a,u ) T r(t i ) r i 2 +λ u T udt subject to Dynamics in (3), Initial Condition, and Input (5)
Problem Relationship between Linear and Non-linear Generative Models Relationship between Two Approaches for Modelling a Curve Natural-Frenet Frame Linear Model (Triple Integrator) v = νt a = νt +ν 2 k 1 M 1 +ν 2 k 2 M 2 u = ( ν ν 3 (k1 2 +k2 2 ))T +(3ν νk 1 +ν 2 k1 )M 1 +(3ν νk 2 +ν 2 k2 )M 2 Linear Model (Triple Integrator) Natural-Frenet Frame ν = v T = v v T = 1 ν (a (a T)T) κ = T ν τ = v (a u) v a 2 k 1,k 2,M 1,M 2 can be computed by assuming suitable intial conditions. ( t ) k 1 (t) = κcos θ + τ(σ)dσ ( k 2 (t) = κsin θ + t ) τ(σ)dσ t M 1 (t) = M 1 () ν(σ)k 1 (σ)t(σ)dσ t M 2 (t) = M 2 () ν(σ)k 2 (σ)t(σ)dσ
Optimal Control Based Approach for Trajectory Reconstruction 1 Background and Motivation 2 Problem Generative Model Inverse Problem Relationship between Linear and Non-linear Generative Models 3 Optimal Control Based Approach for Trajectory Reconstruction Path Independence Lemma Existence of Optimal Initial Condition Optimal Reconstruction as a Linear Smoother Co-State Based Approach 4 Cross-validation Approach to Inverse Problem 5 Numerical Results
Optimal Control Based Approach for Trajectory Reconstruction Path Independence Lemma Application of Path Independence Lemma Optimal Control Problem: Minimize x(t ),u J(x(t ),u) = subject to x(t ) R n, u U, N i= Dynamics in (3) T r(t i ) r i 2 +λ u T udt (6) Path Independence: Along trajectories of (3) = x T (t i )K(t + i )x(t i) x T (t i+1 )K(t i+1 )x(t i+1) t [ ] T [ i+1 x K +A + T ][ K +KA KB x u B T K u t + i = x T (t i )η(t + i ) xt (t i+1 )η(t i+1 )+ t t + i i+1 [ x u ] dt ] T [ η +A T η B T η ] dt for all i {,1,,N 1}
Optimal Control Based Approach for Trajectory Reconstruction Path Independence Lemma Application of Path Independence Lemma Assumptions on the the dynamics and boundary values of K and η: K = A T K KA+KBB T K, K(t + N ) =, K(t + i ) K(t i ) = 1 λ CT C. (7) With the assumptions (7) and (8), we obtain η = ( A T KBB T) η, η(t + N ) =, η(t + i ) η(t i ) = 2 λ CT r i. ] N J(x(t ),u) = λ [x T (t )K(t )x(t )+x T (t )η(t ) + ri T r i 1 T 4 λ B T η(t) 2 dt i= T ( +λ u(t) +B T K(t)x(t)+ 1 ) 2 η(t) 2 dt. (9) Optimal control input: u opt (t) = B T ( K(t)x(t)+ 1 2 η(t) ) (1) Optimal initial condition: ] [K(t ) x opt (t )+ 1 2 η(t ) =. (11) (8)
Optimal Control Based Approach for Trajectory Reconstruction Existence of Optimal Initial Condition Existence of Solution for (11) - Sketch of Proof Proposition 1 The solution of the Riccati equation (7) assumes the form K(t i ) = 1 N Φ Σ (t i,t k )C T CΦ T Σ λ (t i,t k ) k=i for any i {,1,,N} where Σ(t) = (A 1 2 BBT K(t)) T and Φ Σ is the transition matrix of Σ. Holds true for i = N. Apply mathematical induction. Proposition 2 ( Σ T,C) forms an observable pair for the problem of our interest (3). Apply Silverman-Meadows rank condition.
Optimal Control Based Approach for Trajectory Reconstruction Existence of Optimal Initial Condition Existence of Solution for (11) - Sketch of Proof Theorem 1 The equation [K(t ) ] x opt (t )+ 1 2 η(t ) =. is uniquely solvable for almost any time index set {t i } N i=. Observe K(t ) can be represented as K(t ) = 1 λ CT C, with C CΦ Σ T(t 1,t ) C =.. CΦ Σ T(t N,t ) Consider the system ξ = Σ T ξ; γ = Cξ. The outputs, corresponding to two different initial conditions, do not match identically over any interval. ξ a ξ b Cξ a Cξ b (almost surely) Otherwise, consider an arbitrary close perturbation of the original time index set {t i } N i=, to obtain full rank for C.
Optimal Control Based Approach for Trajectory Reconstruction Optimal Reconstruction as a Linear Smoother Linearity in the Reconstructed Trajectory Closed loop dynamics: ẋ(t) = Σ T x(t) 1 2 BBT η(t) with Σ = [ A BB T K(t) ] T. x opt(t ) and η( ) are linear in observed data {r i } N i=. where r(t k ) = 1 N [ CF λ λ (k,i)c T] r i (12) i= F λ (k,i) = Φ T Σ(t [ ] 1Φ Σ(t,t k ) K(t ),t i ) + min{i,k} j=1 ( tj t j 1 Φ T Σ(σ,t k )BB T Φ Σ(σ,t i )dσ ) Can be be viewed as a global alternative to Savitzky-Golay smoothing filters. Can be used as a building block to obtain a fixed lag smoothing algorithm.
Optimal Control Based Approach for Trajectory Reconstruction Co-State Based Approach An Alternative Co-State Based Approach Co-state variables: p(t) K(t)x(t)+ 1 2 η(t) An optimal trajectory between two observation times can be viewed as the base integral curve of the following Hamiltonian dynamics [ ] [ ][ ] d x(t) A BB T x(t) = dt p(t) A T p(t) Jump condition for the co-state variables: p(t + i ) p(t i ) = 1 λ CT (r i r(t i )) Terminal condition for the co-state variables: p(t + N ) = p(t ) =
Optimal Control Based Approach for Trajectory Reconstruction Co-State Based Approach An Alternative Co-State Based Approach Forward-propagation of x(t i ) and p(t + i ): [ ] [ x(ti+1 ) e A i e A iw i p(t + i+1 ) = ] 1 λ CT Ce A i [e AT i + 1λ CT Ce A iw i [ ] + 1 r i+1 λ CT where W i is defined as ] [ x(ti ) p(t + i ) ] W i = i e Aσ BB T e ATσ dσ ( i = t i+1 t i ) Optimal initial condition is obtained by solving ( N 1 ) [ ] I N N 1 [ I] Λ i 1 x(t i= λ CT C ) = [ I] Λ j Γr i (13) i= j=i where, [ Λ i = e A i 1 λ CT Ce A i e A iw i ] [e AT i + 1 λ CT Ce A iw i ] [ ;Γ = 1 λ CT ]
Cross-validation Approach to Inverse Problem 1 Background and Motivation 2 Problem Generative Model Inverse Problem Relationship between Linear and Non-linear Generative Models 3 Optimal Control Based Approach for Trajectory Reconstruction Path Independence Lemma Existence of Optimal Initial Condition Optimal Reconstruction as a Linear Smoother Co-State Based Approach 4 Cross-validation Approach to Inverse Problem 5 Numerical Results
Cross-validation Approach to Inverse Problem Cross-validation Approach to Determination of Penalty Parameter We use leaving-out-one version of the Ordinary Cross Validation (OCV) technique. Let, {x [λ,k] opt,u[λ,k] } be a minimizer of: N T r(t i ) r i 2 +λ u T udt i= i k Let the reconstructed trajectory be r [λ,k] ( ). Then the OCV cost is defined as: V (λ) = 1 N +1 N r [λ,k] (t k ) r k 2 k= Hence, OCV estimate for λ is defined as: λ = argmin V (λ) λ R +
Numerical Results 1 Background and Motivation 2 Problem Generative Model Inverse Problem Relationship between Linear and Non-linear Generative Models 3 Optimal Control Based Approach for Trajectory Reconstruction Path Independence Lemma Existence of Optimal Initial Condition Optimal Reconstruction as a Linear Smoother Co-State Based Approach 4 Cross-validation Approach to Inverse Problem 5 Numerical Results
Numerical Results Numerical Result - Spherical Curve Avg. Fit Error/Radius: 13.686 1 3.
Numerical Results Numerical Result - Circular Helix Avg. Fit Error/Radius: 12.346 1 3.
References References E. Justh and P. S. Krishnaprasad, Optimal Natural Frames, Comm. Inf. Syst., 11(1):17-34, 211. T. Flash and N. Hogan, The coordination of arm movements: An experimentally confirmed mathematical model, The Journal of Neuroscience, 5(7):1688-173, 1985. R. L. Bishop, There is more than one way to frame a curve, The American Mathematical Monthly, 82(3):246-251, 1975. L. M. Silverman and H. E. Meadows, Controllability and observability in time-variable linear systems, SIAM J. on Control and Optimization, 5(1):64-73, 1967. P.V. Reddy, Steering laws for pursuit, M. S. Thesis, University of Maryland, College Park, 27. B. Dey and P. S. Krishnaprasad, Trajectory smoothing as a linear optimal control problem (extended version), http://www.isr.umd.edu/labs/isl/smoothing.
???