16.323 Lecture 7: Numerical Solution in Matlab

16.323 Lecture 7 Numerical Solution in Matlab Comparison with b=0.1 8 Analytic Numerical 7 6 tf 5 4 3 2 1 −2 10 −1 10 0 10 α 1 10 2 10 Simple Problem • Performance � tf J= 16.323 7–1 (u − x)2dt t0 with dynamics ẋ = u and BC t0 = 0, x0 = 1, tf = 1. – So this is a fixed final time, free final state problem. • Form Hamiltonian H = (u − x)2 + pu • Necessary conditions become: ẋ = u ṗ = −2(u − x)(−1) 0 = 2(u − x) + p (7.25) (7.26) (7.27) with BC that p(tf ) = 0. • Rearrange to get ṗ = −p ⇒ p(t) = c1e−t But now impose BC to get p(t) = 0 (7.28) (7.29) (7.30) • This implies that u = x is the optimal solution, and the closed-loop dynamics are ẋ = x with solution x(t) = et. – Clearly this would be an unstable response on a longer timescale, but given the cost and the short time horizon, this control is the best you can do. Simple Zermelo’s Problem 16.323 7–2 • Consider ship that has to travel through a region of strong currents. The ship is assumed to have constant speed V but its heading θ can be varied. The current is assumed to be in the y direction with speed of w. • The motion of the boat is then given by the dynamics ẋ = V cos θ ẏ = V sin θ + w (7.31) (7.32) • The goal is to minimize time, the performance index is � tf J= 1dt = tf 0 – with BC x0 = y0 = 0, xf = 1, yf = 0 – Final time is unspecified. • Define costate p = [p1 p2]T , and in this case the Hamiltonian is H = 1 + p1(V cos θ) + p2(V sin θ + w) • Now use the necessary conditions to get (ṗ = −HxT ) ∂H =0 ∂x ∂H ṗ2 = − =0 ∂y ṗ1 = − → p 1 = c1 (7.33) → p 2 = c2 (7.34) 16.323 7–3 • Control input θ(t) is unconstrained, so have (Hu = 0) ∂H = −p1V sin θ + p2V cos θ = 0 ∂u which gives the control law p2 −p2 tan θ = = p1 −p1 (7.35) (7.36) – Since p1 and p2 are constants, then θ(t) is also a constant. • Optimal control is constant, so can integrate the state equations: x = V t cos θ y = V t(sin θ + w) (7.37) (7.38) – Now impose the BC to get x(tf ) = 1, y(tf ) = 0 to get tf = 1 V cos θ • Rearrange to get √ cos θ = sin θ = − w V V 2 − w2 V which gives that 1 V 2 − w2 – Does this make sense? tf = √ θ = − arcsin w V Numerical Solutions 16.323 7–4 • Most of the problems considered so far have been simple. Things get more complicated by the need to solve a two-point boundary value problem when the dynamics are nonlinear. • Numerous solution techniques exist, including shooting methods13 and collocation – Will discuss the details on these later, but for now, let us look at how to solve these use existing codes • Matlab code called BVP4C exists that is part of the standard package 14 – 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)) = 0 • The solution is an approximation S(t) which is a continuous function that is a cubic polynomial on sub-intervals [tn, tn+1] of a mesh a = t0 < t1 < . . . < tn−1 < tn = b – This approximation satisfies the boundary conditions, so that: g(S(a), S(b)) = 0 – And it satisfies the differential equations (collocates) at both ends and the mid-point of each subinterval: Ṡ(tn) = f (S(tn), tn) Ṡ((tn + tn+1)/2) = f (S((tn + tn+1)/2), (tn + tn+1)/2) Ṡ(tn+1) = f (S(tn+1), tn+1) 13 Online 14 Matlab reference help and BVP4C Tutorial 16.323 7–5 • 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 functions that evaluate the differential equation ẏ = f (y, t) and the residual of the boundary condition (e.g. y1(a) = 1, y2(a) = y1(b), and y3(b) = 0 ): function res = [ res = bvpbc(ya, yb) ya(1) − 1 ya(2) − yb(1) yb(3)]; • Redo example on page 4–15 using numerical techniques – Finite time LQR problem with tf = 10 Figure 7.1: Results suggest a good comparison with the dynamic LQR result 16.323 7–6 TPBVP for LQR 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 function m = TPBVPlqr(p1,p2,p3) global A B x0 Rxx Ruu Ptf t_f=10;x0=[1 1]’; Rxx=p1;Ruu=p2;Ptf=p3; solinit = bvpinit(linspace(0,t_f),@TPBVPlqrinit); sol = bvp4c(@TPBVPlqrode,@TPBVPlqrbc,solinit); time = sol.x; state = sol.y([1 2],:); adjoint = sol.y([3 4],:); control = -inv(Ruu)*B’*sol.y([3 4],:); m(1,:) = time;m([2 3],:) = state;m([4 5],:) = adjoint;m(6,:) = control; %-------------------------------------------------------------------------­ function dydt=TPBVPlqrode(t,y) global A B x0 Rxx Ruu Ptf dydt=[ A -B/Ruu*B’; -Rxx -A’]*y; %-------------------------------------------------------------------------­ function res=TPBVPlqrbc(ya,yb) global A B x0 Rxx Ruu Ptf res=[ya(1) - x0(1);ya(2)-x0(2);yb(3:4)-Ptf*yb(1:2)]; %-------------------------------------------------------------------------­ function v=TPBVPlqrinit(t) global A B x0 b alp v=[x0;1;0]; return 25 26 27 28 29 30 31 32 33 34 35 36 % 16.323 Spring 2007 % Jonathan How % redo LQR example on page 4-15 using numerical approaches clear all;close all; set(0, ’DefaultAxesFontSize’, 14, ’DefaultAxesFontWeight’,’demi’) set(0, ’DefaultTextFontSize’, 14, ’DefaultTextFontWeight’,’demi’) % global A B Ptf=[0 0;0 4];Rxx=[1 0;0 0];Ruu=1;A=[0 1;0 -1];B=[0 1]’; tf=10;dt=.01;time=[0:dt:tf]; m=TPBVPlqr(Rxx,Ruu,Ptf); % numerical result 37 38 39 40 41 42 43 44 45 46 % integrate the P backwards for LQR result P=zeros(2,2,length(time));K=zeros(1,2,length(time)); Pcurr=Ptf; for kk=0:length(time)-1 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 47 48 49 50 51 52 53 54 % simulate the state x1=zeros(2,1,length(time));xcurr1=[1 1]’; for kk=1:length(time)-1 x1(:,:,kk)=xcurr1; xdot1=(A-B*K(:,:,kk))*x1(:,:,kk); xcurr1=xcurr1+xdot1*dt; end 55 56 57 58 59 60 61 figure(3);clf plot(time,squeeze(x1(1,1,:)),time,squeeze(x1(2,1,:)),’--’,’LineWidth’,2), xlabel(’Time (sec)’);ylabel(’States’);title(’Dynamic Gains’) hold on;plot(m(1,:),m([2],:),’s’,m(1,:),m([3],:),’o’);hold off legend(’LQR x_1’,’LQR x_2’,’Num x_1’,’Num x_2’) print -dpng -r300 numreg2.png Conversion 16.323 7–7 • 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. 23, No. 2, 238-254. Apr., 1981. • Key step is to re-scale time so that τ = t/tf , then τ ∈ [0, 1]. – Implications of this scaling are that the derivatives must be changed since dτ = dt/tf d d = tf dτ dt • Final step is to introduce a dummy state r that corresponds to tf with the trivial dynamics ṙ = 0. – Now replace all instances of tf in the necessary/boundary conditions for state r. – Optimizer will then just pick an appropriate constant for r = tf 16.323 7–8 • Recall that our basic set of necessary conditions are, for t ∈ [t0, tf ] ẋ = a(x, u, t) ṗ = −HxT Hu = 0 • And we considered various boundary conditions x(t0) = x0, and: – If tf is free: ht + g + pT a = ht + H(tf ) = 0 – If xi(tf ) is fixed, then xi(tf ) = xif ∂h – If xi(tf ) is free, then pi(tf ) = (tf ) ∂xi • Then ẋ = a(x, u, t) ⇒ x� = tf a(x, u, τ ) and ṗ = −HxT ⇒ p� = −tf HxT Example: 7–1 16.323 7–9 • Revisit example on page 6–6 • Linear system with performance/time weighting and free end time – Necessary conditions are: ẋ = Ax + Bu ṗ = −AT p � � 0 = bu + 0 1 p with state conditions x1(0) x2(0) x1(tf ) x2(tf ) −0.5bu2(tf ) + αtf = = = = = 10 0 0 0 0 • Define the state of interest z = [xT pT r]T and note that dz dz = tf dτ dt ⎡ � � ⎤ A −B 0 1 /b 0 = z5 ⎣ 0 −AT 0 ⎦z 0 0 0 ⇒ z� = f (z) which is nonlinear with BC: z1(0) z2(0) z1(1) z2(1) = = = = 10 0 0 0 −0.5 2 z (1) + αz5(1) = 0 b 4 16.323 7–10 • Code given on following pages – Note – it is not particularly complicated – Solution time/iteration count is a strong function of the initial so­ lution – not a particularly good choice for p is used here • Analytic solution gave tf = (1800b/α)1/5 – Numerical result give close agreement in prediction of the final time Comparison with b=0.1 8 Analytic Numerical 7 6 tf 5 4 3 2 1 −2 10 −1 10 0 10 α 1 10 2 10 Figure 7.2: Comparison of the predicted completion times for the maneuver 16.323 7–11 3 2 u(t) 1 0 −1 −2 −3 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 Time −8 uAnalytic(t)−UNumerical 4 x 10 Analytic 3.5 3 2.5 2 1.5 1 0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 Time Figure 7.3: Control Inputs 10 1 Analytic Numerical 0 dX(t)/dt X(t) 8 6 4 2 0 Analytic Numerical −1 −2 −3 0 1 2 3 4 −4 5 0 1 2 Time −8 7 3 4 5 3 4 5 Time −7 x 10 1 6 x 10 0.8 4 Error Error 5 3 0.6 0.4 2 0.2 1 0 0 1 2 3 4 5 0 0 1 Time 2 Time Figure 7.4: State response 16.323 7–12 TPBVP 1 2 3 4 5 function m = TPBVP(p1,p2) % 16.323 Spring 2007 % Jonathan How % global A B x0 b alp 6 7 8 9 10 11 A=[0 1;0 0]; B=[0 1]’; x0=[10 0]’; b=p1; alp=p2; 12 13 14 solinit = bvpinit(linspace(0,1),@TPBVPinit); sol = bvp4c(@TPBVPode,@TPBVPbc,solinit); 15 16 17 18 19 20 21 22 23 time = sol.y(5)*sol.x; state = sol.y([1 2],:); adjoint = sol.y([3 4],:); control = -(1/b)*sol.y(4,:); m(1,:) = time; m([2 3],:) = state; m([4 5],:) = adjoint; m(6,:) = control; 24 25 26 27 28 %-------------------------------------------------------------------------­ function dydt=TPBVPode(t,y) global A B x0 b alp dydt=y(5)*[ A -B*[0 1]/b zeros(2,1); zeros(2,2) -A’ zeros(2,1);zeros(1,5)]*y; 29 30 31 32 33 %-------------------------------------------------------------------------­ function res=TPBVPbc(ya,yb) global A B x0 b alp res=[ya(1) - x0(1);ya(2)-x0(2);yb(1);yb(2);-0.5*yb(4)^2/b+ alp*yb(5)]; 34 35 36 37 38 %-------------------------------------------------------------------------­ function v=TPBVPinit(t) global A B x0 b alp v=[x0;1;0;1]; 39 40 41 return 16.323 7–13 TPBVP Main 1 2 3 4 5 6 7 8 9 10 11 12 % 16.323 Spring 2007 % Jonathan How % TPmain.m % b=0.1; %alp=[.05 .1 1 10 20]; alp=logspace(-2,2,10); t=[]; for alpha=alp m=TPBVP(b,alpha); t=[t;m(1,end)]; end 13 14 15 16 17 18 19 20 figure(1);clf semilogx(alp,(1800*b./alp).^0.2,’-’,’Linewidth’,2) hold on;semilogx(alp,t,’rs’);hold off xlabel(’\alpha’,’FontSize’,12);ylabel(’t_f’,’FontSize’,12) legend(’Analytic’,’Numerical’) title(’Comparison with b=0.1’) print -depsc -f1 TPBVP1.eps;jpdf(’TPBVP1’) 21 22 23 24 25 26 27 28 29 30 % code from opt1.m on the analytic solution b=0.1;alpha=0.1; m=TPBVP(b,alpha); tf=(1800*b/alpha)^0.2; c1=120*b/tf^3; c2=60*b/tf^2; u=(-c2+c1*m(1,:))/b; A=[0 1;0 0];B=[0 1]’;C=eye(2);D=zeros(2,1);G=ss(A,B,C,D);X0=[10 0]’; [y3,t3]=lsim(G,u,m(1,:),X0); 31 32 33 34 35 36 37 38 39 40 41 42 figure(2);clf subplot(211) plot(m(1,:),u,’g-’,’LineWidth’,2); xlabel(’Time’,’FontSize’,12);ylabel(’u(t)’,’FontSize’,12) hold on;plot(m(1,:),m(6,:),’--’);hold off subplot(212) plot(m(1,:),abs(u-m(6,:)),’-’) xlabel(’Time’,’FontSize’,12) ylabel(’u_{Analytic}(t)-U_{Numerical}’,’FontSize’,12) legend(’Analytic’,’Numerical’) print -depsc -f2 TPBVP2.eps;jpdf(’TPBVP2’) 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 figure(3);clf subplot(221) plot(m(1,:),y3(:,1),’c-’,’LineWidth’,2); xlabel(’Time’,’FontSize’,12);ylabel(’X(t)’,’FontSize’,12) hold on;plot(m(1,:),m([2],:),’k--’);hold off legend(’Analytic’,’Numerical’) subplot(222) plot(m(1,:),y3(:,2),’c-’,’LineWidth’,2); xlabel(’Time’,’FontSize’,12);ylabel(’dX(t)/dt’,’FontSize’,12) hold on;plot(m(1,:),m([3],:),’k--’);hold off legend(’Analytic’,’Numerical’) subplot(223) plot(m(1,:),abs(y3(:,1)-m(2,:)’),’k-’) xlabel(’Time’,’FontSize’,12);ylabel(’Error’,’FontSize’,12) subplot(224) plot(m(1,:),abs(y3(:,2)-m(3,:)’),’k-’) xlabel(’Time’,’FontSize’,12);ylabel(’Error’,’FontSize’,12) print -depsc -f3 TPBVP3.eps;jpdf(’TPBVP3’) Zermelo’s Problem 16.323 7–14 • Simplified dynamics of a UAV flying in a horizontal plane can be mod­ eled as: ẋ(t) = V cos θ(t) ẏ(t) = V sin θ(t) + w where θ(t) is the heading angle (control input) with respect to the x axis, V is the speed. • Objective: fly from point A to B in minimum time: � tf min J = (1)dt 0 where tf is free. – Initial conditions are: x(0) = x0 y(0) = y0 – Final conditions are: x(tf ) = x1 y(tf ) = y1 16.323 7–15 • Apply the standard necessary conditions with H = 1 + p1V (cos θ(t)) + p2(V sin θ(t) + w) ẋ = a(x, u, t) ṗ = −HxT Hu = 0 ẋ(t) = V cos θ(t) ẏ(t) = V sin θ(t) + w p˙1(t) = 0 p˙2(t) = 0 0 = −p1 sin θ(t) + p2 cos θ(t) – Then add extra state for the time. • Since tf is free, must add terminal condition that H(tf ) = 0, which gives a total of 5 conditions (2 initial, 3 terminal). Figure 7.5: Zermelo examples 16.323 7–16 TPBVPZermelo 1 2 function m = TPBVPzermelo(p1,p2) global x0 x1 V w 3 4 5 solinit = bvpinit(linspace(0,1),@TPBVPinit); sol = bvp6c(@TPBVPode,@TPBVPbc,solinit); 6 7 8 9 10 time = sol.y(5)*sol.x; state = sol.y([1 2],:); adjoint = sol.y([3 4],:); control = atan2(-sol.y([4],:),-sol.y([3],:)); 11 12 13 14 15 16 m(1,:) = time; m([2 3],:) = state; m([4 5],:) = adjoint; m(6,:) = control; return 17 18 19 20 %-------------------------------------------------------------------------­ function dydt=TPBVPode(t,y) global x0 x1 V w 21 22 23 24 25 % x y p1 p2 t % minimizing form sinth=-y(4)/sqrt(y(3)^2+y(4)^2); costh=-y(3)/sqrt(y(3)^2+y(4)^2); 26 27 28 29 30 31 32 33 34 dydt=y(5)*[V*costh ; V*sinth+w;0;0;0]; %-------------------------------------------------------------------------­ function res=TPBVPbc(ya,yb) global x0 x1 V w % x y p1 p2 t % minimizing form costhb=-yb(3)/sqrt(yb(3)^2+yb(4)^2); sinthb=-yb(4)/sqrt(yb(3)^2+yb(4)^2); 35 36 37 38 res=[ya(1) - x0(1);ya(2)-x0(2); yb(1) - x1(1);yb(2)-x1(2); 1+V*costhb*yb(3)+V*(sinthb+w)*yb(4)]; 39 40 41 42 43 44 45 %-------------------------------------------------------------------------­ function v=TPBVPinit(t) global x0 x1 V w %v=[x0;-1;-1;norm(x1-x0)/(V-w)]; v=[x0;1;1;norm(x1-x0)/(V-w)]; return 46 47 48 49 50 51 clear all global x0 x1 V w w=1/sqrt(2); x0=[-1 0]’;x1=[0 0]’;V = 1; mm=TPBVPzermelo; 52 53 54 55 56 57 58 59 60 61 figure(1);clf plot(mm(2,:),mm([3],:),’LineWidth’,2);axis(’square’);grid on axis([-2 5 -2 1.5 ]) xlabel(’x’,’FontSize’,12);ylabel(’y’,’FontSize’,12); hold on; plot(x0(1),x0(2),’rs’);plot(x1(1),x1(2),’bs’); text(x0(1),x0(2),’Start’,’FontSize’,12) text(x1(1),x1(2),’End’,’FontSize’,12) hold off 62 63 64 65 figure(2);clf plot(mm(1,:),180/pi*mm([6],:),’LineWidth’,2);grid on;axis(’square’) xlabel(’t’,’FontSize’,12);ylabel(’u’,’FontSize’,12); 66 67 print -dpng -r300 -f1 BVP_zermelo.png; 16.323 7–17 68 print -dpng -r300 -f2 BVP_zermelo2.png; 69 70 71 72 73 74 clear all global x0 x1 V w w=1/sqrt(2); x0=[0 1]’;x1=[0 0]’;V = 1; mm=TPBVPzermelo; 75 76 77 78 79 80 81 82 83 84 figure(1);clf plot(mm(2,:),mm([3],:),’LineWidth’,2);axis(’square’);grid on axis([-2 5 -2 1.5 ]) xlabel(’x’,’FontSize’,12);ylabel(’y’,’FontSize’,12); hold on; plot(x0(1),x0(2),’rs’);plot(x1(1),x1(2),’bs’); text(x0(1),x0(2),’Start’,’FontSize’,12) text(x1(1),x1(2),’End’,’FontSize’,12) hold off 85 86 87 88 figure(2);clf plot(mm(1,:),180/pi*mm([6],:),’LineWidth’,2);grid on;axis(’square’) xlabel(’t’,’FontSize’,12);ylabel(’u’,’FontSize’,12); 89 90 91 92 print -dpng -r300 -f1 BVP_zermelo3.png; print -dpng -r300 -f2 BVP_zermelo4.png; Orbit Raising Example 16.323 7–18 • Goal: (Bryson page 66) determine the maximum radius orbit transfer in a given time tf assuming a constant thrust rocket (thrust T ).15 – Must find the thrust direction angle φ(t) – Assume a circular orbit for the initial and final times • Nomenclature: – r – radial distance from attracting center, with gravitational con­ stant µ – v, u tangential, radial components of the velocity – m mass of s/c, and ṁ is the fuel consumption rate (constant) • Problem: find φ(t) to maximize r(tf ) subject to: ṙ = u v2 µ T sin φ u̇ = − 2+ r r m0 − |ṁ|t uv T cos φ v̇ = − + r m0 − |ṁ|t Dynamics : with initial conditions � r(0) = r0 u(0) = 0 v(0) = µ r0 and terminal conditions u(tf ) = 0 v(tf ) − � µ =0 r(tf ) • With pT = [p1 p2 p3] this gives the Hamiltonian (since g = 0) ⎡ ⎤ u 2 ⎢ µ T sin φ ⎥ H = pT ⎣ vr − r2 + m0−|ṁ|t ⎦ T cos φ − uv r + m0 −|ṁ|t 15 Thanks to Geoff Huntington 16.323 7–19 – Then Hu = 0 with u(t) = φ(t) gives � � � � T cos φ −T sin φ p2 + p3 =0 m0 − |ṁ|t m0 − |ṁ|t which gives that p2(t) p3(t) that can be solved for the control input given the costates. tan φ = • Note that this is a problem of the form on 6–6, with � � u(tf� ) m= =0 v(tf ) − r(tµ ) f which gives � w = −r + ν1u(tf ) + ν2 v(tf ) − � µ r(tf ) � • Since the first state r is not specified at the final time, must have that � ∂w ν2 µ p1(tf ) = (tf ) = −1 + ∂r 2 r(tf )3 – And note that ∂w (tf ) = ν2 ∂v which gives ν2 in terms of the costate. p3(tf ) = 16.323 7–20 Figure 7.6: Orbit raising examples 16.323 7–21 Figure 7.7: Orbit raising examples 16.323 7–22 Orbit Raising 1 2 3 4 5 6 7 %orbit_bvp_how created by Geoff Huntington 2/21/07 %Solves the Hamiltonian Boundary Value Problem for the orbit-raising optimal %control problem (p.66 Bryson & Ho). Computes the solution using BVP4C %Invokes subroutines orbit_ivp and orbit_bound clear all;%close all; set(0, ’DefaultAxesFontSize’, 14, ’DefaultAxesFontWeight’,’demi’) set(0, ’DefaultTextFontSize’, 14, ’DefaultTextFontWeight’,’demi’) 8 9 10 11 %Fixed final time %Tf = 3.3155; Tf = 4; four=0; % not four means use bvp6c 12 13 14 15 16 %Constants global mu m0 m1 T mu=1; m0=1; m1=-0.07485; T= 0.1405; %mu=1; m0=1; m1=-.2; T= 0.1405; 17 18 19 20 21 22 23 24 25 26 27 28 29 30 %Create initial Guess n=100; y = [ones(1,n); %r zeros(1,n); %vr ones(1,n); %vt -ones(1,n); %lambda_r -ones(1,n); %lambda_vr -ones(1,n)]; %lambda_vt x = linspace(0,Tf,n); %time solinit.x = x;solinit.y = y; %Set optimizer options tol = 1E-10; options = bvpset(’RelTol’,tol,’AbsTol’,[tol tol tol tol tol tol],’Nmax’, 2000); 31 32 33 34 35 36 37 38 39 %Solve if four sol = bvp4c(@orbit_ivp,@orbit_bound,solinit,options); Nstep=40; else sol = bvp6c(@orbit_ivp,@orbit_bound,solinit,options); Nstep=30; end 40 41 42 43 44 45 46 47 48 %Plot results figure(1);clf plot(sol.x,sol.y(1:3,:),’LineWidth’,2) legend(’r’,’v_r’,’v_t’,’Location’,’NorthWest’) grid on; axis([0 4 0 2]) title(’HBVP Solution’) xlabel(’Time’);ylabel(’States’) 49 50 51 52 53 54 55 56 figure(2);clf plot(sol.x,sol.y(4:6,:),’LineWidth’,2) legend(’p_1(t)’,’p_2(t)’,’p_3(t)’,’Location’,’NorthWest’) grid on; axis([0 4 -3 2]) title(’HBVP Solution’) xlabel(’Time’);ylabel(’Costates’) 57 58 59 60 61 62 63 64 65 ang2=atan2(sol.y([5],:),sol.y([6],:))+pi; figure(3);clf plot(sol.x,180/pi*ang2’,’LineWidth’,2) grid on; axis([0 4 0 360]) title(’HBVP Solution’) xlabel(’Time’);ylabel(’Control input angle \phi(t)’) norm([tan(ang2’)-(sol.y([5],:)./sol.y([6],:))’]) 66 67 68 69 print -f1 -dpng -r300 orbit1.png print -f2 -dpng -r300 orbit2.png print -f3 -dpng -r300 orbit3.png 70 71 % Code below adapted inpart from Bryson "Dynamic Optimization" 16.323 7–23 72 73 74 75 76 dt=diff(sol.x); dth=(sol.y(3,1:end-1)./sol.y(1,1:end-1)).*dt; % \dot \theta = v_t/r th=0+cumsum(dth’); pathloc=[sol.y(1,1:end-1)’.*cos(th) sol.y(1,1:end-1)’.*sin(th)]; 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 figure(4);clf plot(pathloc(:,1),pathloc(:,2),’k-’,’LineWidth’,2) hold on zz=exp(sqrt(-1)*[0:.01:pi]’); r0=sol.y(1,1);rf=sol.y(1,end); plot(r0*real(zz),r0*imag(zz),’r--’,’LineWidth’,2) plot(rf*real(zz),rf*imag(zz),’b--’,’LineWidth’,2) plot(r0,0,’ro’,’MarkerFace’,’r’) plot(rf*cos(th(end)),rf*sin(th(end)),’bo’,’MarkerFace’,’b’) fact=0.2;ep=ones(size(th,1),1)*pi/2+th-ang2(1:end-1)’; xt=pathloc(:,1)+fact*cos(ep); yt=pathloc(:,2)+fact*sin(ep); for i=1:Nstep:size(th,1), pltarrow([pathloc(i,1);xt(i)],[pathloc(i,2);yt(i)],.05,’m’,’-’); end; %axis([-1.6 1.6 -.1 1.8]); axis([-2 2 -.1 1.8]); axis(’equal’) hold off 96 97 1 2 print -f4 -dpng -r300 orbit4.png; function [dx] = orbit_ivp(t,x) global mu m0 m1 T 3 4 5 6 %State r = x(1);u = x(2);v = x(3); lamr = x(4);lamu = x(5);lamv = x(6); 7 8 9 10 %Substitution for control sinphi = -lamu./sqrt(lamu.^2+lamv.^2); cosphi = -lamv./sqrt(lamu.^2+lamv.^2); 11 12 13 14 15 %Dynamic Equations dr = u; du = v^2/r - mu/r^2 + T*sinphi/(m0 + m1*t); dv = -u*v/r + T*cosphi/(m0 + m1*t); 16 17 18 19 dlamr = -lamu*(-v^2/r^2 + 2*mu/r^3) - lamv*(u*v/r^2); dlamu = -lamr + lamv*v/r; dlamv = -lamu*2*v/r + lamv*u/r; 20 21 1 2 dx = [dr; du; dv; dlamr; dlamu; dlamv]; function [res] = orbit_bound(x,x2) global mu m0 m1 T 3 4 5 6 %Initial State r = x(1);u = x(2);v = x(3); lamr = x(4);lamu = x(5);lamv = x(6); 7 8 9 10 %Final State r2 = x2(1);u2 = x2(2);v2 = x2(3); lamr2 = x2(4);lamu2 = x2(5);lamv2 = x2(6); 11 12 13 14 15 16 17 18 %Boundary Constraints b1 = r - 1; b2 = u; b3 = v - sqrt(mu/r); b4 = u2; b5 = v2 - sqrt(mu/r2); b6 = lamr2 + 1 - lamv2*sqrt(mu)/2/r2^(3/2); 19 20 21 %Residual res = [b1;b2;b3;b4;b5;b6];