function part2b

    values=[100 10000 1000000]; % 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 algebraic riccati equation
        p=zeros(3,1);
        p(2)=R/4*(-3/2+1/2*sqrt(9+16/R*Q(1,1)));
        p(3)=R/4*(-0.05+1/2*sqrt(0.001-16/R*(-2*p(2))));
        p(1)=0.05*p(2)+3/2*p(3)+4/R*p(2)*p(3);

        PI=[p(1),p(2);p(2),p(3)];  % create the constant PI matrix



        % solve the tracking equation  (multiplied out the equation and
        %            solved for the components)
        v(2,:)=Q(1,1)/(-3/2-4*p(2)/R)*r_vals;
        v(1,:)=v(2,:)*(1/20+4*p(3)/R);


        % solve the states, using Euler approximation
        x(:,1)=x0;
        for i=1:length(t)-1
            x(:,i+1)=x(:,i)+dt*( (A-B*inv(R)*B'*PI)*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, sub-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