% Discrete Optimal Gain Interactive Computation function
% Generates Gains for Computer Applications Laboratory
% Optimal Control Laboratory
% By: William Lois
% For: Professor Russell Kraft
% 2/5/2006

clc
clear all
tic

% Change T, the sampling period, for each discrete model
T = 0.1;

%A = [1 2-2*exp(-.5*T);0 exp(-.5*T)];
%B = [T-2+2*exp(-.5*T) 1-exp(-.5*T)];
[A, B] = c2d([0 1; 0 -.5], [0; .5], T);
Q = [2 0; 0 1];
R = .4;
Rneg = (R)^-1;

A, B, Q, R

% Initialize P matrix
P = [100 1; 1 100];

end_while = 0;
num_loops_for_SS = 1;
% While loop to iterate until Steady State solution is found
while end_while == 0
    
    % Generate P Matrix for this iteration
    P_temp_1 = Q + A'*[P^-1 + B*Rneg*B']^-1*A;

    % Calculate delta for this iteration
    i_test = (P_temp_1(1,1)/P(1,1) + P_temp_1(2,2)/P(2,2))/2;
    
    % Normalize difference for +/- changes
    if i_test > 1
        i_test= (P(1,1)/P_temp_1(1,1) + P(2,2)/P_temp_1(2,2))/2;
    end
   
    % Use precision of 0.0000001
    Mark = 0.0000001;
    % Check for Steady State Solution
    if (1 - i_test) < Mark
        end_while = 1;
    end
    
    % Set new P matrix
    P = P_temp_1;
    
    num_loops_for_SS = num_loops_for_SS + 1;

end

num_loops_for_SS
P

% Generate gains from SS P matrix
G = -Rneg*B'*(A.')^-1*[P - Q]

toc