Chance Constrained Covariance Control for Linear Stochastic Systems With Output Feedback
CChance Constrained Covariance Control for LinearStochastic Systems With Output Feedback
Jack Ridderhof Kazuhide Okamoto Panagiotis Tsiotras
Abstract — We consider the problem of steering, via out-put feedback, the state distribution of a discrete-time, linearstochastic system from an initial Gaussian distribution to aterminal Gaussian distribution with prescribed mean and max-imum covariance, subject to probabilistic path constraints onthe state. The filtered state is obtained via a Kalman filter, andthe problem is formulated as a deterministic convex program interms of the distribution of the filtered state. We observe that,in the presence of constraints on the state covariance, and incontrast to classical Linear Quadratic Gaussian (LQG) control,the optimal feedback control depends on both the process noiseand the observation model. The effectiveness of the proposedapproach is verified using a numerical example.
I. INTRODUCTIONThe problem of covariance steering is a stochastic optimalcontrol problem aiming to design a controller that steers thestate covariance of a stochastic system to a target terminalvalue, while minimizing the expectation of a quadratic func-tion of the state and the input. In this work, we focus ona discrete-time linear time-varying stochastic system withpartially observable state, a given Gaussian initial statedistribution, and an independent and identically distributed(i.i.d.) standard Gaussian additive diffusion to the dynamicsand the measurement.The infinite horizon covariance control problem for lineartime invariant systems has been researched since the late80’s. In [1], [2] the authors investigated the state-feedbackgains that assign a state covariance value to the system,i.e., the system state covariance converges asymptoticallyto the assigned value. The finite horizon case has onlyrecently gained attention [3], [4], [5], [6], [7], [8]. Chanceconstraints, which are probabilistic constraints that imposea maximum probability of constraint violation, were firstintroduced to the covariance control problem in [9]. Thelatter work draws connections between covariance controland a large class of stochastic control problems for whichchance constraints are utilized in order to guarantee per-formance under uncertainty [10], [11], such as stochasticmodel predictive control (SMPC) [12], [13] and vehicle pathplanning in belief space [14], [15].
J. Ridderhof is a PhD student with the School of Aerospace Engineering,Georgia Institute of Technology, Atlanta, GA, 30332-0150, USA. Email:[email protected]. Okamoto is a PhD student with the School of Aerospace Engineering,Georgia Institute of Technology, Atlanta, GA, 30332-0150 USA. Email:[email protected]. Tsiotras is the Andrew and Lewis Chair Professor with the D. Guggen-heim School of Aerospace Engineering, and the Institute for Robotics andIntelligent Machines, Georgia Institute of Technology, Atlanta, GA 30332-0150, USA. Email: [email protected]
The majority of covariance control research only considerscontrolling the state covariance. However, as discussed in [9],when state chance constraints exist, the mean and the statecovariance are coupled. The authors in [9] introduced the firstcovariance steering controller that simultaneously deals withthe mean and the covariance dynamics such that the resultingtrajectories satisfy the state chance constraints. The approachwas further modified to be computationally more efficientin [15], which was eventually extended to deal with inputhard constraints [16] and nonlinear dynamics [17]. Further-more, covariance control theory was applied to autonomousvehicle control in [18] and spacecraft control in [19], [20],[21]. Finally, in [22], the authors applied covariance controltheory to SMPC for linear time-invariant systems underunbounded additive disturbance.The above-mentioned research on covariance control as-sumes full state feedback. This paper, in contrast, is con-cerned with covariance steering for the case when the stateis only indirectly accessible via noisy measurements.The problem of output-feedback covariance steering hasbeen visited in [23], [24], where the problem had no con-straints other than a terminal boundary constraint. Thus, theseworks only dealt with the control of the state covariance anddid not consider mean dynamics. The proposed approachdeals with state chance constraints and simultaneously steersthe mean and the covariance of the system state, and thus,can be applied to more realistic scenarios.A similar problem setup as the one addressed in thispaper has also been visited from the SMPC community [25],where an output feedback controller was designed to dealwith chance constraints. Although the approach in [25]successfully computes control commands that satisfy all theconstraints, the control policy suffers from conservativenessdue to the convex relaxation of the covariance dynamics.The approach in our work extends the covariance steeringcontroller in [15], which allows a direct assessment of thecovariance at each time step and eliminates the need toconduct conservative convex relaxations of the covariancedynamics.The main contribution of this work is the development ofa novel covariance steering control policy for linear systemswith Gaussian process and measurement noise. The proposedapproach is a nontrivial extension of the full-state feedbackcovariance control policy proposed in [15], which allowsone to directly assess the value of the covariance at eachtime step, while converting the original stochastic controlproblem to a deterministic convex programming problem.We observe that, as a direct consequence of the constraints a r X i v : . [ m a t h . O C ] S e p n the state covariance, and in contrast to the classical LQGsolution [26], the optimal feedback control depends on boththe process noise and the observation model. Notation
For a sequence x = ( x , . . . , x N ) = ( x k ) Nk =1 , we usethe shorthand ( x k ) to refer to the sequence and write x k to refer to an element of that sequence. We write σ ( z ) todenote the σ -algebra generated by the random variable z .For a symmetric matrix A , we write A > ≥ if A ispositive (semi-)definite.II. PROBLEM STATEMENTConsider the stochastic discrete-time linear system givenby x k +1 = A k x k + B k u k + G k w k , (1)for k = 0 , , . . . , N − , where x k ∈ R n x and u k ∈ R n u are the state and control, and A k ∈ R n x × n x , B k ∈ R n x × n u ,and G k ∈ R n x × n x are system matrices. Increments of thedisturbance process w k ∈ R n x are i.i.d. standard Gaussianrandom vectors. The state is measured through the observa-tion process y k = C k x k + D k v k , (2)where y k ∈ R n y is the measurement and v k ∈ R n y ismeasurement noise, and C k ∈ R n y × n x and D k ∈ R n y × n y are given. Increments of the measurement noise v k are i.i.d.standard Gaussian random vectors. Also, and in order tosimplify the filtering equations, we assume that the matrix D k is invertible. The case when D k is rank-deficient can betreated using well-known approaches [27]. Before the firstmeasurement is taken, we assume that we will be providedwith a state estimate ˆ x - with estimation error ˜ x - = x − ˆ x - .We assume that ˆ x - and ˜ x - are independent random vectorswith known distributions given as ˆ x - ∼ N (¯ x , ˆ P - ) , ˜ x - ∼ N (0 , ˜ P - ) , (3)where the positive semi-definite matrices ˜ P - , ˆ P - and thevector ¯ x are all fixed and known. That is, we do not assumeto know the initial state estimate when designing the controllaw, but we know its distribution. This allows for the controllaw to be designed before all measurements are collected.For example, in the case when we will be provided with theexact value of the state, then ˜ x - = 0 , ˆ x - = x , and ˜ P - = 0 .On the other hand, if we will not be provided with any newinformation about the state before step k = 0 , then ˆ x - = ¯ x and ˆ P - = 0 . Finally, we assume that ˆ x - , ˜ x - , ( w k ) , and ( v k ) are independent.Define the filtration ( F k ) Nk = − by F − = σ (ˆ x - ) and F k = σ (ˆ x - , y i : 0 ≤ i ≤ k ) for ≤ k ≤ N . This filtrationrepresents the information that can be used to estimate thestate and determine the control action, in the sense thatthe estimated state and the control at step k are both F k -measurable random vectors. The initial σ -algebra F − isdefined for logical consistency, since the initial state estimateis known before any measurements are taken. Let ¯ x k = E ( x k ) be the mean state, and define the esti-mated (filtered) state as ˆ x k = E ( x k | F k ) and the estimationerror as ˜ x k = x k − ˆ x k . The estimated state has mean E (ˆ x k ) = E ( E ( x k | F k )) = E ( x k ) = ¯ x k , (4)and hence the estimation error has zero mean, that is, E (˜ x k ) = 0 for all ≤ k ≤ N . Define the state, estimatedstate, and estimation error covariances as P k = E [( x k − ¯ x k )( x k − ¯ x k ) T ] , (5) ˆ P k = E [(ˆ x k − ¯ x k )(ˆ x k − ¯ x k ) T ] , (6) ˜ P k = E (˜ x k ˜ x T k ) = E [(ˆ x k − x k )(ˆ x k − x k ) T ] . (7)The estimated state is uncorrelated with the estimation error,since E (ˆ x k ˜ x T k ) = E [ˆ x k ( x k − ˆ x k ) T ]= E [ E [ˆ x k ( x k − ˆ x k ) T | F k ]]= E [ˆ x k E [ x T k − ˆ x T k | F k ]]= E [ˆ x k ( E [ x T k | F k ] − ˆ x T k )] = 0 , (8)and from this expression it can be shown that the statecovariance satisfies P k = ˆ P k + ˜ P k . Define the prior estimatedstate and prior estimation error as ˆ x k - = E ( x k | F k − ) and ˜ x k - = x k − ˆ x k - , respectively, with corresponding covariances ˆ P k - and ˜ P k - as above. It follows that the initial state isdistributed as x ∼ N (¯ x , P ) , (9)where P = ˆ P - + ˜ P - . We require that P ( x k / ∈ χ ) ≤ p fail , k = 0 , , . . . , N, (10)where < p fail < . is fixed, and where χ = N χ (cid:92) j =1 { x : α T j x ≤ β j } ⊂ R n x , (11)where α j ∈ R n x and β j ∈ R . Here the compliment of χ denotes a forbidden region in the state space, and thus wewe constrain the probability that the state is not in χ to beno more than p fail [10], [14]. Constraints of the form (10)are often referred to as chance constraints , and, likewise,optimization subject these constraints is referred to as chanceconstrained optimization [13].Finally, we assume for the remainder of this paper thatthe control input u k at each step is an affine function ofthe measurement data. We say that a control sequence ( u k ) is admissible if it satisfies this property at every step. Thisassumption is made to ensure that if x k is Gaussian, thenthe state x k +1 will also be Gaussian. It follows that, sincethe state is initially Gaussian distributed, the state will beGaussian distributed over the entire problem horizon, evenin the presence of the chance constraints.This paper is concerned with the following stochasticoptimal control problem. Problem 1:
Find the admissible control sequence u =( u k ) N − k =0 such that the chance constraints (10) are satisfied;the state at step N is distributed according to N (¯ x f , P N ) or P N = ˆ P N + ˜ P N ≤ P f , where ¯ x f and P f are given; andminimizes the cost functional J ( u ) = E (cid:32) N − (cid:88) k =0 x T k Q k x k + u T k R k u k (cid:33) , (12)for a given sequences of matrices ( Q k ≥ and ( R k > . Remark 1:
For simplicity, we do not consider chanceconstraints on the control. However, the method developedin this work may be easily extended to include chanceconstraints on the control. See, for instance, [20], [22].
A. Separation of the Observation and Control Problems
Since the system is linear and the state is Gaussian dis-tributed, the estimated state may be obtained by the Kalmanfilter. That is, the filtered state satisfies [28] ˆ x k = ˆ x k - + L k ( y k − C k ˆ x k - ) , (13) ˆ x k - = A k − ˆ x k − + B k − u k − , (14)where L k = ˜ P k - C T k ( C k ˜ P k - C T k + D k D T k ) − (15)is the Kalman gain, and the error covariances are given by ˜ P k = ( I − L k C k ) ˜ P k - ( I − L k C k ) T + L k D k D T k L T k , (16) ˜ P k - = A k − ˜ P k − A T k − + G k − G T k − . (17)We see that the estimation error covariance ˜ P k does notdepend on the control. Using properties of conditional ex-pectation, it is easy to show that E ( x T k Q k x k ) = tr ˜ P k Q k + E (ˆ x T k Q k ˆ x k ) , (18)and therefore the objective may be rewritten as J ( u ) = N − (cid:88) k =0 tr ˜ P k Q k + ˆ J ( u ) , (19)where ˆ J ( u ) = E (cid:32) N − (cid:88) k =0 ˆ x T k Q k ˆ x k + u T k R k u k (cid:33) . (20)Since the estimation error covariance ˜ P k is determined bythe Kalman filter and not by the control, optimizing overthe objective ˆ J ( u ) is equivalent to optimizing over J ( u ) .Furthermore, we can determine the distribution of the stateas a function of the mean and covariance of the estimated state process, that is, x k ∼ N (¯ x k , P k ) ⇐⇒ ˆ x k ∼ N (¯ x k , P k − ˜ P k ) . (21)It follows that, in order for the final state covariance to satisfy < P N ≤ P f , the maximum final covariance P f mustsatisfy P f > ˜ P N . Define now the innovation process (˜ y k - ) by ˜ y k - = y k − E ( y k | F k − ) , (22)for k = 0 , , . . . , N . Since E ( y k | F k − ) = E ( C k x k + D k v k | F k − ) = C k ˆ x k - , (23) we obtain, by substituting the observation model (2) in (22),that ˜ y k - = y k − C k ˆ x k - = C k ˜ x k - + D k v k . (24)The state error ˜ x k - depends linearly on ˜ x - , ( w i ) k − i =1 , and ( v i ) k − i =1 , which are each independent of v k . It follows that ˜ x k - and v k are independent, and therefore we can computethe covariance of the innovation process as P ˜ y k - = E (˜ y k - ˜ y T k - ) = C k ˜ P k - C T k + D k D T k . (25)Thus, the distribution of the innovation process is determinedby the estimation error covariance ˜ P k - , and therefore may becomputed prior to solving for the control inputs. We rewritethe estimated state process as ˆ x k +1 = A k ˆ x k + B k u k + L k +1 ˜ y ( k +1) - , (26)where ˆ x = ˆ x - + L ˜ y - . We have thus replaced the stateprocess (1) with noise term G k w k with a correspondingfiltered state process with noise L k +1 ˜ y ( k +1) - . The stochasticoptimal control problem may now be posed entirely in termsof the filtered state process (26). B. Block-Matrix Formulation
The filtered state process (26) may be written in matrixnotation as ˆ x ˆ x ˆ x ... = IA A A ... ˆ x - + B A B B . . . u u ... + L A L L A A L A L L . . . ˜ y - ˜ y - ˜ y - ... . (27)Let ˆ X and ˜ Y be column vectors constructed by stacking ˆ x k and ˜ y k - for k = 0 , , . . . , N , and, similarly, let U bethe column vector constructed by stacking u k for k =0 , , . . . , N − . Formally, we have that the column vector ˆ X is isomorphic to the sequence (ˆ x k ) , which we denoteby (ˆ x k ) ∼ = ˆ X (similarly, ( u k ) ∼ = U , (˜ y k - ) ∼ = ˜ Y ). Forappropriately constructed block matrices A , B , and L as in(27), the filtered state process can be written as the linearmatrix equation ˆ X = A ˆ x - + BU + L ˜ Y . (28)See [15], [9] for details on this construction. We may thenrewrite the cost function (20) in matrix form as ˆ J ( U ) = E ( ˆ X T Q ˆ X + U T RU ) , (29)where Q = blkdiag( Q , . . . , Q N − , ≥ and R =blkdiag( R , . . . , R N − ) > . Let E k be a matrix defined sothat E k ˆ X = ˆ x k , and denote the mean state by ¯ X = E ( ˆ X ) ∼ =(¯ x k ) . We can then write the terminal state distributionconstraints as E N ¯ X = ¯ x f , (30a) N E [( ˆ X − ¯ X )( ˆ X − ¯ X ) T ] E T N ≤ P f − ˜ P N . (30b)Finally, in terms of the column vector ˜ X ∼ = (˜ x k ) , the chanceconstraints (10) may be written as P ( E k ( ˆ X + ˜ X ) / ∈ χ ) ≤ p fail , k = 0 , , . . . , N. (31)The distribution of ˆ X + ˜ X is determined, per (21), by thefiltered process (28) and the sequence ( ˜ P k ) , and thereforethe probability in (31) depends solely, for fixed problemparameters, on the control sequence U . In summary, we havereformulated the original stochastic optimal control problem(1) in terms of the inaccessible state into the followingproblem in terms of the accessible filtered state. Problem 2:
Find the admissible control sequence U ∗ ∼ =( u ∗ k ) that minimizes the objective (29) subject to the terminalconstraints (30), for P f > ˜ P N , and chance constraints (31).III. CONTROL OF THE FILTERED STATEWe consider filtered state history feedback of the form u k = k (cid:88) i =0 K k,i (ˆ x i − ¯ x i ) + m k , (32)for k = 0 , , . . . , N − , where K k,i ∈ R n u × n x are feedbackgains and m k ∈ R n u are feedforward controls. Problem 2may thus be solved by identifying the sequences ( K k,i ) and ( m k ) . However, before attempting to solve Problem 2, weconsider the following conservative relaxation of the chanceconstraints (10) given by the condition N χ (cid:88) j =1 p j ≤ p fail , P ( α T j x k > β j ) ≤ p j , j = 1 , . . . , N χ . (33)This constraint represents a decomposition of (10) intoindependent half-plane constraints, which we expect to bea conservative approximation due to the subadditivity ofprobabilities. Indeed, it has been shown in [10] that if (33)holds, then the chance constraint (10) is satisfied. Theorem 3.1:
Given p j as in the chance constraint relax-ation (33), Problem 2 is convex. Proof:
Let M ∼ = ( m k ) be column a vector defined as U , and let K = K , · · · K , K , . . . · · · ... ... . . . K N − , K N − , · · · K N − ,N − . (34)We may then write the control process as the matrix equation U = K ( ˆ X − ¯ X ) + M. (35)The filtered state process (28) is thus given by ˆ X = A ˆ x - + BK ( ˆ X − ¯ X ) + BM + L ˜ Y . (36)Since ˜ Y has zero mean, it follows that ¯ X = E ( ˆ X ) = A ¯ x + BM, (37) and thus the terminal constraint E ( x N ) = ¯ x f is written as E N ¯ X = E N ( A ¯ x + BM ) = ¯ x f , (38)which is affine in M , and hence convex. Since U − E ( U ) = K ( ˆ X − ¯ X ) , we have ˆ X − ¯ X = A (ˆ x - − ¯ x ) + BK ( ˆ X − ¯ X ) + L ˜ Y , (39)which, after solving for ˆ X − ¯ X , we rewrite as ˆ X − ¯ X = ( I − BK ) − [ A (ˆ x - − ¯ x ) + L ˜ Y ] . (40)Since K is block lower-triangular and B is strictly blocklower-triangular, the matrix I − BK is invertible. Follow-ing [29], we define the new decision variable F as F = K ( I − BK ) − ∈ R Nn u × ( N +1) n x . (41)It follows that F is block lower-triangular and satisfies I + BF = ( I − BK ) − . (42)Furthermore, K is a function of F given by K = F ( I + BF ) − , (43)and therefore we may optimize over F in place of K [29].Substituting (42) into (40), we obtain ˆ X − ¯ X = ( I + BF )[ A (ˆ x - − ¯ x ) + L ˜ Y ] . (44)By assumption, ˆ x - is independent from both ˜ x - and v ,and therefore by (24) we have that ˆ x - is independent from ˜ Y . It follows that the bracketed term in the right-hand sideof (44) has covariance S = Cov[ A (ˆ x - − ¯ x ) + L ˜ Y ] = A ˆ P - A T + LP ˜ Y L T , (45)where, since steps of (˜ y k - ) are independent [30], the covari-ance of ˜ Y is the block-diagonal matrix P ˜ Y = E ( ˜ Y ˜ Y T ) = blkdiag( P ˜ y - , . . . , P ˜ y N - ) , (46)where P ˜ y k - as in (25). The covariance of the filtered processis thus ˆ P = E [( ˆ X − ¯ X )( ˆ X − ¯ X ) T ] = ( I + BF ) S ( I + BF ) T , (47)and the covariance of the control is given by P U = E [( U − E ( U ))( U − E ( U )) T ] = F SF T . (48)We can then rewrite the objective (29) as ˆ J ( F, M ) = ( A ¯ x + BM ) T Q ( A ¯ x + BM ) + M T RM + tr { [( I + BF ) T Q ( I + BF ) + F T RF ] S } , (49)which is convex in F and M , because Q ≥ and R > .The terminal covariance constraint (30b) may be written as E N ( I + BF ) S ( I + BF ) T E T N ≤ P f − ˜ P N , (50)or, equivalently [9], (cid:107) S / ( I + BF ) T E T N ( P f − ˜ P N ) − / (cid:107) − ≤ , (51)where S / denotes a matrix satisfying S = ( S / ) T S / .The matrix ( P f − ˜ P N ) − / exists since P f > ˜ P N . Next,e consider the conservative chance constraints (33). Theprobabilistic half-plane constraint in (33) has been shown in[15] to be equivalent to the constraint cdfn − (1 − p j ) (cid:107) P / k α j (cid:107) + α T j ¯ x k − β j ≤ , (52)where cdfn − is the inverse of the cumulative normal distri-bution function and P k is the state covariance at time step k , which we can write as P k = E k ˆ P E T k + ˜ P k . (53)In addition, P / k satisfies ( P / k ) T P / k = P k and is obtainedby P / k = (cid:20) S / ( I + BF ) T E T k ˜ P / k (cid:21) . (54)Notice that, because each p j < . , it follows that cdfn − (1 − p j ) > . Finally, substituting into the chanceconstraint, we obtain the second order cone constraint cdfn − (1 − p j ) (cid:13)(cid:13)(cid:13)(cid:13)(cid:20) S / ( I + BF ) T E T k ˜ P / k (cid:21) α j (cid:13)(cid:13)(cid:13)(cid:13) + α T j E k ( A ¯ x + BM ) − β j ≤ , j = 1 , . . . , N χ . (55)Given some constants p j , it follows that the problem of min-imizing the objective (49) with respect to the optimizationparameters F and M , subject to the constraints (38), (51),(55) is convex.We remark that the convex constraints (51) and (55)depend on the estimation error covariance ˜ P k , which, inturn, depend on the measurement model (2) and the filterdesign. Therefore, in contrast to separation-based control[26], the optimal control that solves Problem 2 cannot befound independently of the optimal filter, provided that theconstraints (51) and (55) are active. This result is intuitive: Ifthe state estimate is highly uncertain, then additional controleffort may be required to steer further away from an obstacle.Furthermore, the certainty equivalence property [28] does nothold when the constraints (51) and (55) are active, since theintensity of the process noise is represented in the matrix S .The controller (32) uses feedback of both the current andthe past values of the filtered state process at each step.It follows that the computational complexity of the convexformulation of Problem 2 as given in Theorem 3.1 scales with O ( N n x n u ) . For problems with a large time horizon, onemay restrict the matrix F to be block diagonal; the resultingcomputational complexity scales by O ( N n x n u ) [9]. Moregenerally, the matrix F can be set to be block banded,which allows the designer to trade controller performancewith computational complexity [29].IV. NUMERICAL EXAMPLEConsider for ∆ t = 0 . the following double integratorsystem with the horizon N = 20 given by, for all k , A k = t
00 1 0 ∆ t , B k = ∆ t / t / t
00 ∆ t , (56) Fig. 1. (Top:) 3 σ covariance ellipses drawn at each step; (Bottom:) detailof 3 σ covariance ellipses for steps k = 11 and k = N = 20 are shown.The arrows indicate the direction of motion and the compliment of χ ismarked by diagonal lines. The variables x (1) k and x (2) k denote the first twocoordinates of the state. and G k = 0 . × I , C k = (cid:2) × I (cid:3) , D k =diag(0 . , . , . . and G k = 0 . × I . The initial statedistribution is described by ˜ P - = diag(2 , , . , . × − , ˆ P - = diag(8 , , . , . × − , ¯ x = [1 . , . , , T ,and the target state distribution is constrained to have mean ¯ x f = [6 . , . , , T and maximum covariance P f =diag(6 , , . , . × − . Finally, the region χ is definedfor N χ = 2 half plane constraints as in (11) given by α = [1 , , , T , α = [1 , , , T , with β = 27 . , β = 9 , p = p = 5 × − , and p fail = p + p . We solvedthe convex optimization problem using YALMIP [31] withMOSEK [32]. The resulting trajectory of the distributionof the position coordinates are shown in Figure 1. In thisexample it is clear that the resulting control depends onthe observation model. Since the first position coordinate isnot directly measured, there is a larger uncertainty in theestimated value of the first position coordinate compared tothe second position coordinate. The controller compensatesaccordingly by using sufficient control effort along the firstposition coordinate so that the chance constraints are sat-sfied. We can see this by observing the shape of the σ ellipse of the filtered state covariance ˆ P k in the bottom plotof Figure 1. V. CONCLUSIONSIn this paper, we have developed a covariance steeringcontrol policy for discrete-time linear stochastic systems withGaussian process and measurement noise. The filtered statewas obtained by a Kalman filter, and then, in terms of thefiltered state, the covariance steering problem was posed as adeterministic convex optimization problem. It was observedthat, due to the covariance-based constraints on the statedistribution, the resulting optimal control depends on both theprocess noise and measurement model. Finally, the developedtheory was demonstrated using a numerical example.In future work, the proposed approach can be extended tovehicle path planning problems under Gaussian disturbanceand measurement uncertainty [33], [34]. In particular, theproposed approach can be extended to handle non-convexfeasible regions as in [15], where the authors convertedthe original stochastic vehicle path planning problem to adeterministic mixed integer convex programming problem.ACKNOWLEDGMENTThe work of the first author was supported by a NASASpace Technology Research Fellowship. The work of thesecond and third authors was supported by NSF awardCPS-1544814, by ARL under DCIST CRA W911NF-17-2-0181, and by ONR award N00014-18-1-2828. The authorswould also like to thank Dipankar Maity for many helpfuldiscussions and suggestions.R EFERENCES[1] A. F. Hotz and R. E. Skelton, “A covariance control theory,” in
IEEEConference on Decision and Control , vol. 24, (Fort Lauderdale, FL),pp. 552–557, Dec. 11 – 13, 1985.[2] A. Hotz and R. E. Skelton, “Covariance control theory,”
InternationalJournal of Control , vol. 46, no. 1, pp. 13–32, 1987.[3] Y. Chen, T. T. Georgiou, and M. Pavon, “Optimal steering of a linearstochastic system to a final probability distribution, Part I,”
IEEETransactions on Automatic Control , vol. 61, pp. 1158–1169, May2016.[4] E. Bakolas, “Optimal covariance control for discrete-time stochasticlinear systems subject to constraints,” in
IEEE Conference on Decisionand Control , (Las Vegas, NV), pp. 1153–1158, Dec. 12 – 14, 2016.[5] E. Bakolas, “Finite-horizon separation-based covariance control fordiscrete-time stochastic linear systems,” in
IEEE Conference on Deci-sion and Control , (Miami Beach, FL), pp. 3299–3304, Dec. 17 – 19,2018.[6] A. Beghi, “On the relative entropy of discrete-time Markov processeswith given end-point densities,”
IEEE Transactions on InformationTheory , vol. 42, no. 5, pp. 1529–1535, 1996.[7] M. Goldshtein and P. Tsiotras, “Finite-horizon covariance control oflinear time-varying systems,” in
IEEE 56th Annual Conference onDecision and Control , (Melbourne, Australia), pp. 3606–3611, Dec.12 – 15, 2017.[8] A. Halder and E. D. Wendel, “Finite horizon linear quadratic Gaussiandensity regulator with Wasserstein terminal cost,” in
American ControlConference , (Boston, MA), pp. 7249–7254, July 6 – 8, 2016.[9] K. Okamoto and P. Tsiotras, “Optimal stochastic vehicle path planningusing covariance steering,”
IEEE Robotics and Automation Letters ,vol. 4, no. 3, pp. 2276–2281, 2019.[10] L. Blackmore and M. Ono, “Convex chance constrained predictivecontrol without sampling,” in
AIAA Guidance, Navigation, and ControlConference , (Chicago, IL), Aug. 10 – 13, 2009. [11] A. Geletu, M. Kl¨oppel, H. Zhang, and P. Li, “Advances and applica-tions of chance-constrained approaches to systems optimisation underuncertainty,”
International Journal of Systems Science , vol. 44, no. 7,pp. 1209–1232, 2013.[12] M. Farina, L. Giulioni, and R. Scattolini, “Stochastic linear model pre-dictive control with chance constraints–a review,”
Journal of ProcessControl , vol. 44, pp. 53–67, 2016.[13] A. Mesbah, “Stochastic model predictive control: An overview andperspectives for future research,”
IEEE Control Systems , vol. 36, no. 6,pp. 30–44, 2016.[14] L. Blackmore, M. Ono, and B. C. Williams, “Chance-constrainedoptimal path planning with obstacles,”
IEEE Transactions on Robotics ,vol. 27, no. 6, pp. 1080–1094, 2011.[15] K. Okamoto, M. Goldshtein, and P. Tsiotras, “Optimal covariancecontrol for stochastic systems under chance constraints,”
IEEE ControlSystems Letters , vol. 2, pp. 266–271, April 2018.[16] K. Okamoto and P. Tsiotras, “Input hard constrained optimal covari-ance steering,” in
IEEE Conference on Decision and Control , (Nice,France), Dec 11 – 13, 2019.[17] J. Ridderhof, K. Okamoto, and P. Tsiotras, “Nonlinear uncertaintycontrol with iterative covariance steering,” in
IEEE Conference onDecision and Control , (Nice, France), Dec 11 – 13, 2019.[18] N. Chohan, M. A. Nazari, H. Wymeersch, and T. Charalambous,“Robust trajectory planning of autonomous vehicles at intersectionswith communication impairments,” in
Allerton Conference on Com-munication, Control, and Computing , (Monticello, IL), Sept 24 – 27,2019.[19] J. Ridderhof and P. Tsiotras, “Uncertainty quantification and controlduring Mars powered descent and landing using covariance steering,”in
AIAA Guidance, Navigation, and Control Conference , (Kissimmee,FL), Jan. 8 – 12, 2018.[20] J. Ridderhof and P. Tsiotras, “Minimum-fuel powered descent in thepresence of random disturbances,” in
AIAA Guidance, Navigation, andControl Conference , (San Diego, CA), Jan. 7 – 11, 2019.[21] J. Ridderhof, J. Pilipovsky, and P. Tsiotras, “Chance-constrained co-variance control for low-thrust minimum-fuel trajectory optimization,”in , (Lake Tahoe,CA), Aug. 9 – 13 2020.[22] K. Okamoto and P. Tsiotras, “Stochastic model predictive control forconstrained linear systems using optimal covariance steering,” arXivpreprint arXiv:1905.13296 , 2019.[23] Y. Chen, T. Georgiou, and M. Pavon, “Steering state statistics withoutput feedback,” in
IEEE Conference on Decision and Control ,(Osaka, Japan), pp. 6502–6507, Dec. 15 – 18, 2015.[24] E. Bakolas, “Covariance control for discrete-time stochastic linearsystems with incomplete state information,” in
American ControlConference , (Seattle, WA), pp. 432–437, May 24 – 26, 2017.[25] M. Farina, L. Giulioni, L. Magni, and R. Scattolini, “An approachto output-feedback MPC of stochastic linear discrete-time systems,”
Automatica , vol. 55, pp. 140–149, 2015.[26] E. Tse, “On the optimal control of stochastic linear systems,”
IEEETransactions on Automatic Control , vol. 16, no. 6, pp. 776–785, 1971.[27] F. W. Fairman and L. Luk, “On reducing the order of Kalman filters fordiscrete-time stochastic systems having singular measurement noise,”
IEEE Transactions on Automatic Control , vol. 30, no. 11, pp. 1150–1152, 1985.[28] A. E. Bryson and Y.-C. Ho,
Applied Optimal Control: Optimization,Estimation and Control . Ginn and Company, 1969.[29] J. Skaf and S. P. Boyd, “Design of affine controllers via convexoptimization,”
IEEE Transactions on Automatic Control , vol. 55,no. 11, pp. 2476–2487, 2010.[30] K. J. ˚Astr¨om,
Introduction to Stochastic Control Theory , vol. 70 of
Mathematics in Science and Engineering . Academic Press, 1970.[31] J. Lofberg, “YALMIP: A toolbox for modeling and optimizationin MATLAB,” in
IEEE International Conference on Robotics andAutomation , pp. 284–289, IEEE, 2004.[32] Mosek ApS, “The MOSEK optimization toolbox for MATLAB man-ual.” Version 9.1, 2019.[33] S. Patil, J. Van Den Berg, and R. Alterovitz, “Estimating probability ofcollision for safe motion planning under Gaussian motion and sensinguncertainty,” in
International Conference on Robotics and Automation ,(St. Paul, MN), pp. 3238–3244, May 14–18, 2012.[34] A.-A. Agha-Mohammadi, S. Chakravorty, and N. M. Amato, “FIRM:Sampling-based feedback motion-planning under motion uncertaintyand imperfect measurements,”