constr = [P_0 >= 0]
constr = constr + [ P_0 <= R ]
time = 0;
PP = P_0;
i=1;
constr = constr + [ [ (Theta{i} + Q1) (hatA + hatB*U*hatC + PP)' (hatA + hatB*U*hatC )' PP' (U*hatC)' PP*hatE1 ;
(hatA + hatB*U*hatC + PP) -eye(5) zeros(5) zeros(5) zeros(5,3) zeros(5,1);
(hatA + hatB*U*hatC) zeros(5) eye(5) zeros(5) zeros(5,3) zeros(5,1);
PP zeros(5) zeros(5) eye(5) zeros(5,3) zeros(5,1);
(U*hatC) zeros(3,5) zeros(3,5) zeros(3,5) -Q2^-1 zeros(3,1);
(hatE1'* PP) zeros(1,5) zeros(1,5) zeros(1,5) zeros(1,3) -gamma*gamma ] <= 0]
optimize(constr)