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


clear all
% Continuous optimum control m file

% Change A matrix
A = [0 1;0 -0.5];
% Change B matrix
B = [0; 0.5];
Q = [2 0; 0 1];
R = 0.4;
Rneg = (R)^-1;

P11 = sym('P11');
P12 = sym('P12');
P21 = sym('P21');
P22 = sym('P22');

P = [P11 P12; P12 P22];
% P_temp_1 = [P11 P12; P12 P22];
% xdot = A*x + B*u;
% y = C*x + D*u;

% 0 = -P*A - A.'*P + P*B*R^-1*B.'*P - Q;
% G = -R^-1*B.'* P

P_temp_1 = P * B*Rneg*B.'*P - Q - A.'*P - P*A;
P12_1 = solve(P_temp_1(1,1), P12);
P12 = eval(P12_1(1));

P = [P11 P12; P12 P22];

P_temp_2 = P * B*Rneg *B.'*P-Q -A.'* P - P*A;
P22_1 = solve(P_temp_2(2,2), P22);
P22 = eval(P22_1(1));

P = [P11 P12; P12 P22];

P_temp_3 = P * B*Rneg*B.'*P - Q - A.'*P - P*A;
P11_1 = solve(P_temp_3(1,2), P11);
P11 = eval(P11_1(1));

P = [P11 P12; P12 P22]

G = -Rneg*B.'*P
% P22_1 = solve(P_temp_1(2,2));
% P22= eval(P22_1(1));


% options = optimset('Display','off');  % Turn off Display
% [P12] = fsolve(@myfun,0,options)


% P_temp_2 = Q + A.'* P
% Bu= B.';
% Au = A.';
% P = solve('P * B *Bu*P - (Q + Bu* P)=0')
