Проблемы с оптимизацией Ensmallen

Question

Я пытаюсь оптимизировать определенную целевую функцию, используя Ensmallen, библиотеку для математической оптимизации, которую я нашел на Github. К сожалению, примеры, которые я нашел в документации, оказались не очень полезными.

У меня есть функция длины, которая зависит от двух параметров: k_i и k_f, и я хочу минимизировать ее. Эта функция подвергается 4 ограничениям: constraint1, ..., constraint4. Эти ограничения зависят от: k_i, k_f и s, который является параметром, который изменяется от 0 до 1 и представляет параметризацию моей длины.

По сути, это своего рода упрощение проблемы полубесконечного программирования. переменная "s" увеличивается от 0 до 1 с 50 шагами, и для каждого шага я хотел бы иметь свою целевую функцию, которая удовлетворяет моим 4 ограничениям.

x_i, y_i, theta_i, x_f, y_f, theta_f, скорость, диаметр, восходящий, нижний - постоянные. Из оптимизации я хочу найти значения k_i и k_f.

Так что это часть моего кода:

double Evaluate(const arma::mat& x) { 

k_i = x(0,0);
k_f = x(1,0);

return sqrt(k_f*k_f)*(1.0/3.0E1)+sqrt(k_i*k_i)*(1.0/3.0E1)+sqrt(x_f*x_i*-7.2E1-y_f*y_i*7.2E1+k_f*k_f+k_i*k_i+(x_f*x_f)*3.6E1+(x_i*x_i)*3.6E1+(y_f*y_f)*3.6E1+(y_i*y_i)*3.6E1-k_f*x_f*cos(theta_f)*1.2E1+k_f*x_i*cos(theta_f)*1.2E1-k_i*x_f*cos(theta_i)*1.2E1+k_i*x_i*cos(theta_i)*1.2E1-k_f*y_f*sin(theta_f)*1.2E1+k_f*y_i*sin(theta_f)*1.2E1-k_i*y_f*sin(theta_i)*1.2E1+k_i*y_i*sin(theta_i)*1.2E1+k_f*k_i*cos(theta_f-theta_i)*2.0)*(1.0/3.0E1)+sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*4.9E1+(k_i*k_i)*6.4E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2-k_f*x_f*cos(theta_f)*3.36E2+k_f*x_i*cos(theta_f)*3.36E2+k_i*x_f*cos(theta_i)*3.84E2-k_i*x_i*cos(theta_i)*3.84E2-k_f*y_f*sin(theta_f)*3.36E2+k_f*y_i*sin(theta_f)*3.36E2+k_i*y_f*sin(theta_i)*3.84E2-k_i*y_i*sin(theta_i)*3.84E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/3.75E2)+sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*6.4E1+(k_i*k_i)*4.9E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2+k_f*x_f*cos(theta_f)*3.84E2-k_f*x_i*cos(theta_f)*3.84E2-k_i*x_f*cos(theta_i)*3.36E2+k_i*x_i*cos(theta_i)*3.36E2+k_f*y_f*sin(theta_f)*3.84E2-k_f*y_i*sin(theta_f)*3.84E2-k_i*y_f*sin(theta_i)*3.36E2+k_i*y_i*sin(theta_i)*3.36E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/3.75E2)+sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*9.0+(k_i*k_i)*6.4E1+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*2.16E2+k_f*x_i*cos(theta_f)*2.16E2-k_i*x_f*cos(theta_i)*5.76E2+k_i*x_i*cos(theta_i)*5.76E2-k_f*y_f*sin(theta_f)*2.16E2+k_f*y_i*sin(theta_f)*2.16E2-k_i*y_f*sin(theta_i)*5.76E2+k_i*y_i*sin(theta_i)*5.76E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/3.75E2)+sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*6.4E1+(k_i*k_i)*9.0+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*5.76E2+k_f*x_i*cos(theta_f)*5.76E2-k_i*x_f*cos(theta_i)*2.16E2+k_i*x_i*cos(theta_i)*2.16E2-k_f*y_f*sin(theta_f)*5.76E2+k_f*y_i*sin(theta_f)*5.76E2-k_i*y_f*sin(theta_i)*2.16E2+k_i*y_i*sin(theta_i)*2.16E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/3.75E2)+sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*2.89E2+(k_i*k_i)*3.969E3+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3-k_f*x_f*cos(theta_f)*1.836E3+k_f*x_i*cos(theta_f)*1.836E3+k_i*x_f*cos(theta_i)*6.804E3-k_i*x_i*cos(theta_i)*6.804E3-k_f*y_f*sin(theta_f)*1.836E3+k_f*y_i*sin(theta_f)*1.836E3+k_i*y_f*sin(theta_i)*6.804E3-k_i*y_i*sin(theta_i)*6.804E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*(1.0/7.5E2)+sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*3.969E3+(k_i*k_i)*2.89E2+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3+k_f*x_f*cos(theta_f)*6.804E3-k_f*x_i*cos(theta_f)*6.804E3-k_i*x_f*cos(theta_i)*1.836E3+k_i*x_i*cos(theta_i)*1.836E3+k_f*y_f*sin(theta_f)*6.804E3-k_f*y_i*sin(theta_f)*6.804E3-k_i*y_f*sin(theta_i)*1.836E3+k_i*y_i*sin(theta_i)*1.836E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*(1.0/7.5E2)+sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*4.9E1+(k_i*k_i)*1.089E3+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4+k_f*x_f*cos(theta_f)*1.764E3-k_f*x_i*cos(theta_f)*1.764E3-k_i*x_f*cos(theta_i)*8.316E3+k_i*x_i*cos(theta_i)*8.316E3+k_f*y_f*sin(theta_f)*1.764E3-k_f*y_i*sin(theta_f)*1.764E3-k_i*y_f*sin(theta_i)*8.316E3+k_i*y_i*sin(theta_i)*8.316E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*(1.0/7.5E2)+sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*1.089E3+(k_i*k_i)*4.9E1+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4-k_f*x_f*cos(theta_f)*8.316E3+k_f*x_i*cos(theta_f)*8.316E3+k_i*x_f*cos(theta_i)*1.764E3-k_i*x_i*cos(theta_i)*1.764E3-k_f*y_f*sin(theta_f)*8.316E3+k_f*y_i*sin(theta_f)*8.316E3+k_i*y_f*sin(theta_i)*1.764E3-k_i*y_i*sin(theta_i)*1.764E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*(1.0/7.5E2);      
  }

void Gradient(const arma::mat& x, arma::mat& g) {    

k_i = x(0,0);
k_f = x(1,0);
g.resize(2,1);

g(0,0) = (k_i*9.8E1-x_f*cos(theta_i)*3.36E2+x_i*cos(theta_i)*3.36E2-y_f*sin(theta_i)*3.36E2+y_i*sin(theta_i)*3.36E2-k_f*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*6.4E1+(k_i*k_i)*4.9E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2+k_f*x_f*cos(theta_f)*3.84E2-k_f*x_i*cos(theta_f)*3.84E2-k_i*x_f*cos(theta_i)*3.36E2+k_i*x_i*cos(theta_i)*3.36E2+k_f*y_f*sin(theta_f)*3.84E2-k_f*y_i*sin(theta_f)*3.84E2-k_i*y_f*sin(theta_i)*3.36E2+k_i*y_i*sin(theta_i)*3.36E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_i*1.28E2+x_f*cos(theta_i)*3.84E2-x_i*cos(theta_i)*3.84E2+y_f*sin(theta_i)*3.84E2-y_i*sin(theta_i)*3.84E2-k_f*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*4.9E1+(k_i*k_i)*6.4E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2-k_f*x_f*cos(theta_f)*3.36E2+k_f*x_i*cos(theta_f)*3.36E2+k_i*x_f*cos(theta_i)*3.84E2-k_i*x_i*cos(theta_i)*3.84E2-k_f*y_f*sin(theta_f)*3.36E2+k_f*y_i*sin(theta_f)*3.36E2+k_i*y_f*sin(theta_i)*3.84E2-k_i*y_i*sin(theta_i)*3.84E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_i*1.8E1-x_f*cos(theta_i)*2.16E2+x_i*cos(theta_i)*2.16E2-y_f*sin(theta_i)*2.16E2+y_i*sin(theta_i)*2.16E2+k_f*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*6.4E1+(k_i*k_i)*9.0+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*5.76E2+k_f*x_i*cos(theta_f)*5.76E2-k_i*x_f*cos(theta_i)*2.16E2+k_i*x_i*cos(theta_i)*2.16E2-k_f*y_f*sin(theta_f)*5.76E2+k_f*y_i*sin(theta_f)*5.76E2-k_i*y_f*sin(theta_i)*2.16E2+k_i*y_i*sin(theta_i)*2.16E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_i*1.28E2-x_f*cos(theta_i)*5.76E2+x_i*cos(theta_i)*5.76E2-y_f*sin(theta_i)*5.76E2+y_i*sin(theta_i)*5.76E2+k_f*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*9.0+(k_i*k_i)*6.4E1+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*2.16E2+k_f*x_i*cos(theta_f)*2.16E2-k_i*x_f*cos(theta_i)*5.76E2+k_i*x_i*cos(theta_i)*5.76E2-k_f*y_f*sin(theta_f)*2.16E2+k_f*y_i*sin(theta_f)*2.16E2-k_i*y_f*sin(theta_i)*5.76E2+k_i*y_i*sin(theta_i)*5.76E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_i*5.78E2-x_f*cos(theta_i)*1.836E3+x_i*cos(theta_i)*1.836E3-y_f*sin(theta_i)*1.836E3+y_i*sin(theta_i)*1.836E3-k_f*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*3.969E3+(k_i*k_i)*2.89E2+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3+k_f*x_f*cos(theta_f)*6.804E3-k_f*x_i*cos(theta_f)*6.804E3-k_i*x_f*cos(theta_i)*1.836E3+k_i*x_i*cos(theta_i)*1.836E3+k_f*y_f*sin(theta_f)*6.804E3-k_f*y_i*sin(theta_f)*6.804E3-k_i*y_f*sin(theta_i)*1.836E3+k_i*y_i*sin(theta_i)*1.836E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_i*7.938E3+x_f*cos(theta_i)*6.804E3-x_i*cos(theta_i)*6.804E3+y_f*sin(theta_i)*6.804E3-y_i*sin(theta_i)*6.804E3-k_f*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*2.89E2+(k_i*k_i)*3.969E3+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3-k_f*x_f*cos(theta_f)*1.836E3+k_f*x_i*cos(theta_f)*1.836E3+k_i*x_f*cos(theta_i)*6.804E3-k_i*x_i*cos(theta_i)*6.804E3-k_f*y_f*sin(theta_f)*1.836E3+k_f*y_i*sin(theta_f)*1.836E3+k_i*y_f*sin(theta_i)*6.804E3-k_i*y_i*sin(theta_i)*6.804E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_i*9.8E1+x_f*cos(theta_i)*1.764E3-x_i*cos(theta_i)*1.764E3+y_f*sin(theta_i)*1.764E3-y_i*sin(theta_i)*1.764E3-k_f*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*1.089E3+(k_i*k_i)*4.9E1+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4-k_f*x_f*cos(theta_f)*8.316E3+k_f*x_i*cos(theta_f)*8.316E3+k_i*x_f*cos(theta_i)*1.764E3-k_i*x_i*cos(theta_i)*1.764E3-k_f*y_f*sin(theta_f)*8.316E3+k_f*y_i*sin(theta_f)*8.316E3+k_i*y_f*sin(theta_i)*1.764E3-k_i*y_i*sin(theta_i)*1.764E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_i*2.178E3-x_f*cos(theta_i)*8.316E3+x_i*cos(theta_i)*8.316E3-y_f*sin(theta_i)*8.316E3+y_i*sin(theta_i)*8.316E3-k_f*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*4.9E1+(k_i*k_i)*1.089E3+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4+k_f*x_f*cos(theta_f)*1.764E3-k_f*x_i*cos(theta_f)*1.764E3-k_i*x_f*cos(theta_i)*8.316E3+k_i*x_i*cos(theta_i)*8.316E3+k_f*y_f*sin(theta_f)*1.764E3-k_f*y_i*sin(theta_f)*1.764E3-k_i*y_f*sin(theta_i)*8.316E3+k_i*y_i*sin(theta_i)*8.316E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_i*2.0-x_f*cos(theta_i)*1.2E1+x_i*cos(theta_i)*1.2E1-y_f*sin(theta_i)*1.2E1+y_i*sin(theta_i)*1.2E1+k_f*cos(theta_f-theta_i)*2.0)*1.0/sqrt(x_f*x_i*-7.2E1-y_f*y_i*7.2E1+k_f*k_f+k_i*k_i+(x_f*x_f)*3.6E1+(x_i*x_i)*3.6E1+(y_f*y_f)*3.6E1+(y_i*y_i)*3.6E1-k_f*x_f*cos(theta_f)*1.2E1+k_f*x_i*cos(theta_f)*1.2E1-k_i*x_f*cos(theta_i)*1.2E1+k_i*x_i*cos(theta_i)*1.2E1-k_f*y_f*sin(theta_f)*1.2E1+k_f*y_i*sin(theta_f)*1.2E1-k_i*y_f*sin(theta_i)*1.2E1+k_i*y_i*sin(theta_i)*1.2E1+k_f*k_i*cos(theta_f-theta_i)*2.0)*(1.0/6.0E1)+k_i*1.0/sqrt(k_i*k_i)*(1.0/3.0E1);

g(1,0) = (k_f*9.8E1-x_f*cos(theta_f)*3.36E2+x_i*cos(theta_f)*3.36E2-y_f*sin(theta_f)*3.36E2+y_i*sin(theta_f)*3.36E2-k_i*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*4.9E1+(k_i*k_i)*6.4E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2-k_f*x_f*cos(theta_f)*3.36E2+k_f*x_i*cos(theta_f)*3.36E2+k_i*x_f*cos(theta_i)*3.84E2-k_i*x_i*cos(theta_i)*3.84E2-k_f*y_f*sin(theta_f)*3.36E2+k_f*y_i*sin(theta_f)*3.36E2+k_i*y_f*sin(theta_i)*3.84E2-k_i*y_i*sin(theta_i)*3.84E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_f*1.28E2+x_f*cos(theta_f)*3.84E2-x_i*cos(theta_f)*3.84E2+y_f*sin(theta_f)*3.84E2-y_i*sin(theta_f)*3.84E2-k_i*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*6.4E1+(k_i*k_i)*4.9E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2+k_f*x_f*cos(theta_f)*3.84E2-k_f*x_i*cos(theta_f)*3.84E2-k_i*x_f*cos(theta_i)*3.36E2+k_i*x_i*cos(theta_i)*3.36E2+k_f*y_f*sin(theta_f)*3.84E2-k_f*y_i*sin(theta_f)*3.84E2-k_i*y_f*sin(theta_i)*3.36E2+k_i*y_i*sin(theta_i)*3.36E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_f*1.8E1-x_f*cos(theta_f)*2.16E2+x_i*cos(theta_f)*2.16E2-y_f*sin(theta_f)*2.16E2+y_i*sin(theta_f)*2.16E2+k_i*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*9.0+(k_i*k_i)*6.4E1+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*2.16E2+k_f*x_i*cos(theta_f)*2.16E2-k_i*x_f*cos(theta_i)*5.76E2+k_i*x_i*cos(theta_i)*5.76E2-k_f*y_f*sin(theta_f)*2.16E2+k_f*y_i*sin(theta_f)*2.16E2-k_i*y_f*sin(theta_i)*5.76E2+k_i*y_i*sin(theta_i)*5.76E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_f*1.28E2-x_f*cos(theta_f)*5.76E2+x_i*cos(theta_f)*5.76E2-y_f*sin(theta_f)*5.76E2+y_i*sin(theta_f)*5.76E2+k_i*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*6.4E1+(k_i*k_i)*9.0+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*5.76E2+k_f*x_i*cos(theta_f)*5.76E2-k_i*x_f*cos(theta_i)*2.16E2+k_i*x_i*cos(theta_i)*2.16E2-k_f*y_f*sin(theta_f)*5.76E2+k_f*y_i*sin(theta_f)*5.76E2-k_i*y_f*sin(theta_i)*2.16E2+k_i*y_i*sin(theta_i)*2.16E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_f*5.78E2-x_f*cos(theta_f)*1.836E3+x_i*cos(theta_f)*1.836E3-y_f*sin(theta_f)*1.836E3+y_i*sin(theta_f)*1.836E3-k_i*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*2.89E2+(k_i*k_i)*3.969E3+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3-k_f*x_f*cos(theta_f)*1.836E3+k_f*x_i*cos(theta_f)*1.836E3+k_i*x_f*cos(theta_i)*6.804E3-k_i*x_i*cos(theta_i)*6.804E3-k_f*y_f*sin(theta_f)*1.836E3+k_f*y_i*sin(theta_f)*1.836E3+k_i*y_f*sin(theta_i)*6.804E3-k_i*y_i*sin(theta_i)*6.804E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_f*7.938E3+x_f*cos(theta_f)*6.804E3-x_i*cos(theta_f)*6.804E3+y_f*sin(theta_f)*6.804E3-y_i*sin(theta_f)*6.804E3-k_i*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*3.969E3+(k_i*k_i)*2.89E2+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3+k_f*x_f*cos(theta_f)*6.804E3-k_f*x_i*cos(theta_f)*6.804E3-k_i*x_f*cos(theta_i)*1.836E3+k_i*x_i*cos(theta_i)*1.836E3+k_f*y_f*sin(theta_f)*6.804E3-k_f*y_i*sin(theta_f)*6.804E3-k_i*y_f*sin(theta_i)*1.836E3+k_i*y_i*sin(theta_i)*1.836E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_f*9.8E1+x_f*cos(theta_f)*1.764E3-x_i*cos(theta_f)*1.764E3+y_f*sin(theta_f)*1.764E3-y_i*sin(theta_f)*1.764E3-k_i*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*4.9E1+(k_i*k_i)*1.089E3+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4+k_f*x_f*cos(theta_f)*1.764E3-k_f*x_i*cos(theta_f)*1.764E3-k_i*x_f*cos(theta_i)*8.316E3+k_i*x_i*cos(theta_i)*8.316E3+k_f*y_f*sin(theta_f)*1.764E3-k_f*y_i*sin(theta_f)*1.764E3-k_i*y_f*sin(theta_i)*8.316E3+k_i*y_i*sin(theta_i)*8.316E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_f*2.178E3-x_f*cos(theta_f)*8.316E3+x_i*cos(theta_f)*8.316E3-y_f*sin(theta_f)*8.316E3+y_i*sin(theta_f)*8.316E3-k_i*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*1.089E3+(k_i*k_i)*4.9E1+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4-k_f*x_f*cos(theta_f)*8.316E3+k_f*x_i*cos(theta_f)*8.316E3+k_i*x_f*cos(theta_i)*1.764E3-k_i*x_i*cos(theta_i)*1.764E3-k_f*y_f*sin(theta_f)*8.316E3+k_f*y_i*sin(theta_f)*8.316E3+k_i*y_f*sin(theta_i)*1.764E3-k_i*y_i*sin(theta_i)*1.764E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_f*2.0-x_f*cos(theta_f)*1.2E1+x_i*cos(theta_f)*1.2E1-y_f*sin(theta_f)*1.2E1+y_i*sin(theta_f)*1.2E1+k_i*cos(theta_f-theta_i)*2.0)*1.0/sqrt(x_f*x_i*-7.2E1-y_f*y_i*7.2E1+k_f*k_f+k_i*k_i+(x_f*x_f)*3.6E1+(x_i*x_i)*3.6E1+(y_f*y_f)*3.6E1+(y_i*y_i)*3.6E1-k_f*x_f*cos(theta_f)*1.2E1+k_f*x_i*cos(theta_f)*1.2E1-k_i*x_f*cos(theta_i)*1.2E1+k_i*x_i*cos(theta_i)*1.2E1-k_f*y_f*sin(theta_f)*1.2E1+k_f*y_i*sin(theta_f)*1.2E1-k_i*y_f*sin(theta_i)*1.2E1+k_i*y_i*sin(theta_i)*1.2E1+k_f*k_i*cos(theta_f-theta_i)*2.0)*(1.0/6.0E1)+k_f*1.0/sqrt(k_f*k_f)*(1.0/3.0E1); 
 }

size_t NumConstraints() {
return 50;
}

double EvaluateConstraint(const size_t i, const arma::mat& x){
k_i = x(0,0);
k_f = x(1,0);
double index_ensmallen = i;
double s = index_ensmallen/49;  

double v = sqrt((s*s*s*s)*pow(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i),2.0)*9.0+(s*s)*pow(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0,2.0)*4.0+(k_i*k_i)*pow(cos(theta_i),2.0)+(k_i*k_i)*pow(sin(theta_i),2.0)+(s*s*s*s)*pow(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i),2.0)*9.0+(s*s)*pow(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0,2.0)*4.0-(s*s*s)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*1.2E1-(s*s*s)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*1.2E1-k_i*s*cos(theta_i)*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*4.0-k_i*s*sin(theta_i)*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*4.0+k_i*(s*s)*cos(theta_i)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*6.0+k_i*(s*s)*sin(theta_i)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0);

double w = -(s*(k_i*sin(theta_i)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*6.0-k_i*cos(theta_i)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0)-(s*s)*((x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*6.0-(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0)-k_i*sin(theta_i)*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*2.0+k_i*cos(theta_i)*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*2.0)/(-s*(k_i*sin(theta_i)*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*4.0+k_i*cos(theta_i)*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*4.0)+(k_i*k_i)*pow(cos(theta_i),2.0)+(k_i*k_i)*pow(sin(theta_i),2.0)+(s*s*s*s)*(pow(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i),2.0)*9.0+pow(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i),2.0)*9.0)-(s*s*s)*((x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*1.2E1+(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*1.2E1)+(s*s)*(pow(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0,2.0)*4.0+pow(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0,2.0)*4.0+k_i*sin(theta_i)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0+k_i*cos(theta_i)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*6.0));


double constraint1 = w - tan(uplim)*v;    
double constraint2 = tan(downlim)*v - w;
double constraint3 = v*(speed-1) + diameter*speed*w;  
double constraint4 = v*(speed-1) - diameter*speed*w;

  if ( (constraint1 >0) || (constraint2 >0) || (constraint3 >0) || (constraint4 >0) ){
    return DBL_MAX; //constraint unsatisfied
  }    

return 0;
}//close EvaluateConstraint

void GradientConstraint(const size_t i, const arma::mat& x, arma::mat& g){

g.resize(2,1);
k_i = x(0,0);
k_f = x(1,0);
double s=i/49; 
//g(0,0) = 0
//g(1,0) = 0
 }//close gradientconstraint

Когда я пытаюсь использовать этот оптимизатор, я нахожу, что k_i иk_f невероятно похож на Nan. Но я протестировал его с оптимизатором "fmincon" из Matlab, и я получил некоторые достоверные результаты! Так что я не знаю, что делать, я подумал, что, возможно, я написал что-то не так здесь в методе EvaluateConstraint, потому что, когда я пытаюсь напечатать их значение, я нахожу "Nan".

Редактировать: Повышение значенияначальная точка (например, x0: (500, 500)) «разблокирует» симуляцию, оптимизатор не использует одно и то же значение на каждой итерации, он изменяется, как и должно быть, но все еще не имеет решения.