function part2

    values=[10 100 1000 100000]; % different values to try for Q
                                 % where Q=[value]*eye(2)

    for k=values   % outer loop run for each value in above vector

        dt=0.001;    % want this small so that the Euler methods later are more accurate
        t=[0:dt:6];  % set vector of times where we want solution values

        % un-comment the plot command to plot the trajectory
        for i=1:length(t)
            r_vals(i)=r(t(i));
        end
        %plot(t,r_vals)

        % problem setup - system matrices and initial conditions
        A=[0,1;-3/2,-1/20];
        B=[0;2];
        C=[1,0;0,0];
        Q=k*eye(2)
        R=0.1;
        Pi_f=eye(2);
        x0=[0;0];


        % solve the differential riccati equation, backwards
        init=C'*Pi_f*C;
        guess=[init(1,1);init(1,2);init(2,2)];   % pulls off the initial guess components
        [t,pi]=ode15s(@(t,p) p_solve(t,p,Q,R),t,guess);



        % create the various values of PI (forward-izing the solutions to the
        %           backwards riccati done earlier)
        for i=1:length(t)
            PI{length(t)-i+1}=[pi(i,1),pi(i,2);pi(i,2),pi(i,3)];
        end


        % solve the tracking equation, using Euler approximation
        % the backward tracking equation is converted to a forward, solved to produce
        % v_temp, and then values are flipped to get it back into original v form

        v_temp=zeros(2,length(t)); % prime the v vector with zeros
        v_temp(:,1)=-C'*Pi_f*[r(t(end));0];

        % solve the reversed tracking equation
        for i=1:length(t)-1
            v_temp(:,i+1)=v_temp(:,i)+dt*( (A'-PI{i}*B*inv(R)*B')*v_temp(:,i)-C'*Q*[r(t(length(t)-i+1));0]);
        end

        % flip to forward-ize the backwards tracking equation
        for i=1:length(t)
            v(:,i)=v_temp(:,end-i+1);
        end



        % solve the states, using Euler approximation
        % these are forward, so we finally don't have to do any of that awkward
        % backward-forward solving stuff
        x(:,1)=x0;
        for i=1:length(t)-1
            x(:,i+1)=x(:,i)+dt*( (A-B*inv(R)*B'*PI{i})*x(:,i)-B*inv(R)*B'*v(:,i) );
        end

        % plot the solution for this value of Q
        y=C*x;   % pick off the solution vector

        figure
        subplot(2,1,1)
        plot(t,r_vals,t,y(1,:))
        axis tight
        legend('True Trajectory','Estimate')
        xlabel('Time')
        ylabel('Function value')
        text=['Trajectory versus estimate, optimal control, q=',num2str(k),' and R=',num2str(R)];
        title(text)

        subplot(2,1,2)
        plot(t,(y(1,:)-r_vals).^2)
        axis tight
        xlabel('Time')
        ylabel('Residual value')
        title('Residuals, squared')
    end

end

function out=r(t)   % piecewise definition for the trajectory
    if (t>=0 && t<=1)
        out=t;
    elseif (t>1 && t<=2)
        out=1;
    elseif (t>2 && t<=4)
        out=1+sin(2*pi*(t-2));
    elseif (t>4 && t<=5)
        out=1;
    elseif (t>5 && t<=6)
        out=1-(t-5);
    end
end

function out=p_solve(t,p,Q,R)  % solving for the riccati equation
    % these equations were developed by multiplying out the riccati
    % equation, using PI=[p1,p2; p2,p3]
    
    p1=p(1); p2=p(2); p3=p(3);

    p1_dot=(-3*p2-4/R*p2^2+Q(1,1));
    p2_dot=(p1-1/20*p2-3/2*p3-4/R*p2*p3);
    p3_dot=(2*p2-p3/10-4/R*p3^2);

    out=[p1_dot;p2_dot;p3_dot];
end