Strange output from Optimize command

[David Webster]

Hello, I am a fairly new user trying to solve a fairly simple set of linear matrix equations.

I am determining the optimal feedback for a system with sinusoidal disturbances, requiring that I find matrices P, P1 and P2.

The cost function is:

s'*Q*s + u'*R*u, where s is the continuous state and u in the input.

The optimal control law is determined by three matrices P, P1, and P2, determined by the following equations:

P is the unique, PD solution to: 

A'*P + P*A - P*S*P + Q = 0

P1 is the unique solution to:

A'*P1 + P*D - P2*Omega^2 - P*S*P1 = 0

P2 is the unique solution to:

A'*P2 + P1 - P*S*P2 = 0

Where S = B*R^(-1)*B', and Omega is a diagonal matrix with the magnitudes of the sinusoidal disturbance.

The cost function is set in stone, and I'm really just trying to find the three P matrices. I put the three equations and the PD constraint into Optimize with the cost function as the objective, it runs through 1000 iterations, and near the end begins to produce several lengthy warnings per iteration that the matrices are near singular.

It then terminates the simulation early. However, when I evaluate all three matrices, neither are anywhere near singular.
The program ends up running relatively smoothly, but I'm wondering if I'm somehow poorly stating my constraints that is causing the warnings.
Exact code is below:

A = [0 1 0 0 0 0; -(5*c^2-2)*n^2 0 0 2*n*c 0 0; 0 0 0 1 0 0; 0 -2*n*c 0 0 0 0;...

    0 0 0 0 0 1; 0 0 0 0 -q^2 0;]; 

B = [0;1;0;1;0;1;]; D = eye(6); Q = 1*eye(6); R = 1;

P = sdpvar(6,6); P1 = sdpvar(6,6); P2 = sdpvar(6,6); st = sdpvar(6,1); u = sdpvar(6,1);

S = B*R^(-1)*B'; Omega = eye(6)*q;

constr = [st'*P*st > 0, A'*P + P*A - P*S*P+Q == 0 ; A'*P1+P*D-P2*Omega^2-P*S*P1 == 0 ...

    A'*P2+P1-P*S*P2 == 0;];

obj = [u'*R*u + st'*Q*st];

optimize(constr, obj);

[Johan Löfberg]

You are not solving an LMI (semidefinite program) but a nonconvex quadratically constrained quadratic problem. Most likely you are using the solver fmincon, and the singularity warning has nothing to do with your matrices, but is a warning that the line searches performed during the optimization are ill-conditioned

You will have to make an effort to reformulate this to a linear semidefinite program. It is not clear that it is possible. The first equation is trivial. From the application, I am sure you can relax it to <= instead. After that, you apply a Schur complement to get rid of the quadratic term in P. For the remaining two, it is not as obvious that it is possible to do anything clever.

If this was all mumbo jumbo to you, you have some reading to do
http://stanford.edu/~boyd/lmibook/lmibook.pdf