入力の最適化において状態の制約を加える

離散化・線形化された以下の方程式

X(k+1) = A*X(k) + B*U(k) + C

において、評価関数Jを最小化するような入力Uを求めたいです。

そこで、Uの制約はもちろんの事、Xもとれる範囲が決まっている為、その制約を最適化に加えたいです。その方法を知っている方がいらっしゃいましたら、教えてください。

<試した方法>

非線形制約 - MATLAB & Simulink - MathWorks 日本

このサイトのように、fmincon関数の[c,ceq]を使用し、非線形制約として方程式を入れ込む方法も試しましたが、「実現不可能な点に到達しました。」というエラーが出てしまいます。

function [c,ceq] = mycon(U,A,B,C,x0)

x_next1 = A*x0' + B*[U(1); U(2); U(3)] + C;
x_next2 = A*x0' + B*[U(4); U(5); U(6)] + C;
x_next3 = A*x0' + B*[U(7); U(8); U(9)] + C;
% %a>0
c(1) = -x_next1(1);
c(2) = -x_next2(1);
c(3) = -x_next3(1);
% %0<e<1
 c(4) = -x_next1(2);
 c(5) = x_next1(2)-1