Lecture 10. Numerical Solution in Matlab

Transcription

1 6. Lecture Numerical Solution in Matlab

2 Spr 6 6. Numerical Solutions Most of the problems of interest so far have been complicated by the need to solve a two point boundary value problem. That plus the dynamics are nonlinear Numerous solution techniques exist, including shooting methods and collocation Will discuss the details on these later, but for now, let us look at how to solve these use existing codes Exists a Matlab code called BVP4C that is part of the standard package. Solves problems of a standard form : ẏ = f (y, t, p) a t b where y are the variables of interest, and p are extra variables in the problem that can also be optimized Where the system is subject to the boundary conditions: g(y(a), y(b)) = The solution is an approximation S(t) that is a continuous function that is a cubic polynomial on sub intervals [t n, t n+ ] of a mesh a = t < t <... < t n < t n = b This approximation satisfies the boundary conditions, so that: g(s(a), S(b)) = And it satisfies the differential equations (collocates) at both ends and the mid point of each subinterval: Ṡ(t n ) = f (S(t n ), t n ) Ṡ((t n + t n+ )/) = f (S((t n + t n+ )/), (t n + t n+ )/) Ṡ(t n+ ) = f (S(t n+ ), t n+ )

3 Spr 6 6. Now constrain continuity in the solution at the mesh points converts problem to a series of nonlinear algebraic equations in the unknowns Becomes a root finding problem that can be solved iteratively (Simpson s method). Inputs to BVP4C are funcitons that evaluate the differential equation ẏ = f (y, t) and the residual of the boundary condition (e.g. y (a) =, y (a) = y (b), and y (b) = ): function res = bvpbc(ya, yb) res = [ ya() ya() yb() yb()]; Redo example on page 4 5 using numerical techniques Finite time LQR problem with t f =.4 Dynamic Gains LQR x. LQR x Num x Num x.8.6 States (sec) Figure : Results suggest a good comparison with the dynamic LQR result

4 Spr 6 6. TPBVP for LQR function m = TPBVPlqr(p,p,p) global A B x Rxx Ruu Ptf t_f=;x=[ ] ; Rxx=p;Ruu=p;Ptf=p; solinit = bvpinit(linspace(,t_f),@tpbvplqrinit); sol = bvp4c(@tpbvplqrode,@tpbvplqrbc,solinit); time = sol.x; state = sol.y([ ],:); adjoint = sol.y([ 4],:); control = inv(ruu)*b *sol.y([ 4],:); m(,:) = time; m([ ],:) = state; m([4 5],:) = adjoint; m(6,:) = control; % function dydt=tpbvplqrode(t,y) global A B x Rxx Ruu Ptf dydt=[ A B/Ruu*B ; Rxx A ]*y; % function res=tpbvplqrbc(ya,yb) global A B x Rxx Ruu Ptf res=[ya() x();ya() x();yb(:4) Ptf*yb(:)]; % function v=tpbvplqrinit(t) global A B x b alp v=[x;;]; return % redo LQR example on page 4 5 using numerical approaches % global A B Ptf=[ ; 4];Rxx=[ ; ];Ruu=; A=[ ; ];B=[ ] ; tf=;dt=.;time=[:dt:tf]; m=tpbvplqr(rxx,ruu,ptf); % integrate the P backwards P=zeros(,,length(time));K=zeros(,,length(time)); Pcurr=Ptf; for kk=:length(time) P(:,:,length(time) kk)=pcurr; K(:,:,length(time) kk)=inv(ruu)*b *Pcurr; Pdot= Pcurr*A A *Pcurr Rxx+Pcurr*B*inv(Ruu)*B *Pcurr; Pcurr=Pcurr dt*pdot; end x=zeros(,,length(time)); xcurr=[ ] ; for kk=:length(time) x(:,:,kk)=xcurr; xdot=(a B*K(:,:,kk))*x(:,:,kk); xcurr=xcurr+xdot*dt; end figure();clf plot(time,squeeze(x(,,:)),time,squeeze(x(,,:)),, LineWidth,), xlabel( (sec) );ylabel( States );title( Dynamic Gains ) hold on;plot(m(,:),m([],:), s,m(,:),m([],:), o );hold off legend( LQR x_, LQR x_, Num x_, Num x_ ) print depsc reg_.eps;jpdf( reg_ )

5 Spr 6 Conversion 6. 4 BVP4C sounds good, but this standard form doesn t match many of the problems that we care about In particular, free end time problems are excluded, because the time period is defined to be fixed t [a, b] Can convert our problems of interest into this standard form though using some pretty handy tricks. U. Ascher and R. D. Russell, Reformulation of Boundary Value Problems into Standard Form, SIAM Review, Vol., No., Apr., 98. Key step is to re scale time so that τ = t/t f, then τ [, ]. Implications of this scaling are that the derivatives must be changed since dτ = dt/t f d d dτ = t f dt Final step is to introduce a dummy state r that corresponds to t f with the trivial dynamics ṙ =. Now replace all instances of t f in the necessary/boundary conditions for state r. The optimizer will then just pick an appropriate constant for r = t f

6 Spr Recall that our basic set of necessary conditions are, for t [t, t f ] ẋ = a(x, u, t) ṗ = H T x H u = And we considered various boundary conditions x(t ) = x, and: If t f is free: h t + g + p T a = h t + H(t f ) = If x i (t f ) is fixed, then x i (t f ) = x if h If x i (t f ) is free, then p i (t f ) = (t f ) x i Then and ẋ = a(x, u, t) x = t f a(x, u, τ ) ṗ = H T x p = t f H T x

7 Spr 6 Example: 6. 6 Revisit example on page 6 4 (results/code have been updated) Linear system with performance/time weighting and free end time Necessary conditions are: with state conditions ẋ = Ax + Bu ṗ = A T p [ ] = bu + p x () = x () = x (t f ) = x (t f ) =.5bu (t f ) + αt f = Define the state of interest z = [x T p T r] T and note that with BC: dz dz = t f dτ dt [ ] A B /b = z 5 A T z z = f (z) which is nonlinear z () = z () = z () = z () =.5 z4 () + αz 5 () = b

8 Spr Code given on following pages Note it is not particularly complicated Solution time/iteration count is a strong function of the initial solution not a particularly good choice for p is used here Analytic solution gave t f = (8b/α) /5 Numerical result give close agreement in prediction of the final time 8 7 Comparison with b=. Analytic Numerical 6 5 t f 4 α Figure : Comparison of the predicted completion times for the maneuver

9 Spr u(t) u Analytic (t) U Numerical 4 x Analytic Figure : Control Inputs 8 Analytic Numerical Analytic Numerical X(t) 6 4 dx(t)/dt x 8 x 7 Error Error Figure 4: State response

10 Spr TPBVP function m = TPBVP(p,p) global A B x b alp 4 A=[ ; ]; 5 B=[ ] ; 6 x=[ ] ; 7 b=p; 8 alp=p; 9 solinit = bvpinit(linspace(,),@tpbvpinit); sol = bvp4c(@tpbvpode,@tpbvpbc,solinit); 4 time = sol.y(5)*sol.x; 5 state = sol.y([ ],:); 6 adjoint = sol.y([ 4],:); 7 control = (/b)*sol.y(4,:); 8 m(,:) = time; 9 m([ ],:) = state; m([4 5],:) = adjoint; m(6,:) = control; % 4 function dydt=tpbvpode(t,y) 5 global A B x b alp 6 7 dydt=y(5)*[ A B*[ ]/b zeros(,); zeros(,) A zeros(,);zeros(,5)]*y; 8 9 % function res=tpbvpbc(ya,yb) global A B x b alp res=[ya() x();ya() x();yb();yb();.5*yb(4)^/b+ alp*yb(5)]; 4 5 % 6 function v=tpbvpinit(t) 7 global A B x b alp 8 v=[x;;;]; 9 4 return 4

11 Spr 6 6. TPBVP Main %main program % b=.; %alp=[.5. ]; alp=logspace(,,); t=[]; for alpha=alp m=tpbvp(b,alpha); t=[t;m(,end)]; end figure();clf semilogx(alp,(8*b./alp).^.,, Linewidth,) hold on;semilogx(alp,t, rs );hold off xlabel( \alpha, FontSize,);ylabel( t_f, FontSize,) legend( Analytic, Numerical ) title( Comparison with b=. ) print depsc f TPBVP.eps;jpdf( TPBVP ) % code from opt.m on the analytic solution b=.;alpha=.; m=tpbvp(b,alpha); tf=(8*b/alpha)^.; c=*b/tf^; c=6*b/tf^; u=( c+c*m(,:))/b; A=[ ; ];B=[ ] ;C=eye();D=zeros(,);G=ss(A,B,C,D);X=[ ] ; [y,t]=lsim(g,u,m(,:),x); figure();clf subplot() plot(m(,:),u, g, LineWidth,); xlabel(, FontSize,);ylabel( u(t), FontSize,) hold on;plot(m(,:),m(6,:), );hold off subplot() plot(m(,:),abs(u m(6,:)), ) xlabel(, FontSize,) ylabel( u_{analytic}(t) U_{Numerical}, FontSize,) legend( Analytic, Numerical ) print depsc f TPBVP.eps;jpdf( TPBVP ) figure();clf subplot() plot(m(,:),y(:,),, LineWidth,); xlabel(, FontSize,);ylabel( X(t), FontSize,) hold on;plot(m(,:),m([],:), k );hold off legend( Analytic, Numerical ) subplot() plot(m(,:),y(:,),, LineWidth,); xlabel(, FontSize,);ylabel( dx(t)/dt, FontSize,) hold on;plot(m(,:),m([],:), k );hold off legend( Analytic, Numerical ) subplot() plot(m(,:),abs(y(:,) m(,:) ), ) xlabel(, FontSize,);ylabel( Error, FontSize,) subplot(4) plot(m(,:),abs(y(:,) m(,:) ), ) xlabel(, FontSize,);ylabel( Error, FontSize,) print depsc f TPBVP.eps;jpdf( TPBVP )