Failure in initial objective function evaluation in FMINCON function

2 次查看（过去 30 天）
john white
john white 2021-1-17
评论： john white 2021-1-18
hi
I have a code with some functions but when I run the code error is accured!
the code is:
clc
clear all
close all
Ts = 0.01;
Tf = 10;
t = 0:Ts:Tf;
N = numel(t);
x = zeros(6,N);
u = zeros(3,N);
y = zeros(3,N);
A = @(x) [0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
0 0 0 (1.0e5*(cos(x(2))*(cos(x(3))*(5.6e-4*x(6)*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + (x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(5) + x(6)) + (0.13*x(5) + 0.13*x(6))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.025*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.025*x(4)*x(5)*sin(conj(x(2))) - 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6)) + 5.6e-4*x(4)*x(5)*cos(conj(x(2)))*sin(conj(x(3))) - 5.6e-4*x(4)*x(5)*cos(conj(x(3)))*sin(conj(x(2)))) - 1.0*sin(x(3))*(6.8e-3*x(6)*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(x(5) + x(6))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.8e-3*x(4)*x(5)*cos(conj(x(2)))*cos(conj(x(3))) + 6.8e-3*x(4)*x(5)*sin(conj(x(2)))*sin(conj(x(3)))) + (0.13*x(5) + 0.13*x(6))*(0.19*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.19*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.075*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.075*x(4)*x(5)*sin(conj(x(2))) - 0.025*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.025*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.025*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.025*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6))) + sin(x(2))*(cos(x(3))*(6.8e-3*x(6)*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(x(5) + x(6))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.8e-3*x(4)*x(5)*cos(conj(x(2)))*cos(conj(x(3))) + 6.8e-3*x(4)*x(5)*sin(conj(x(2)))*sin(conj(x(3)))) + sin(x(3))*(5.6e-4*x(6)*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + (x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(5) + x(6)) + (0.13*x(5) + 0.13*x(6))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.025*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.025*x(4)*x(5)*sin(conj(x(2))) - 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6)) + 5.6e-4*x(4)*x(5)*cos(conj(x(2)))*sin(conj(x(3))) - 5.6e-4*x(4)*x(5)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.017*x(4)*x(5)*cos(conj(x(2))))))/(x(4)*(7.5e3*cos(conj(x(2)))*cos(x(2)) + 922.0*sin(conj(x(2)))*sin(x(2)) + 2.5e3*cos(conj(x(2)))*cos(x(2))*cos(x(3)) + 2.5e3*cos(conj(x(2)))*sin(x(2))*sin(x(3)) + 2.5e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2)) + 2.5e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*sin(x(2))*sin(x(3)) + 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*sin(x(3)) - 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(3))*sin(x(2)) - 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(2))*sin(x(3)) + 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(3))*sin(x(2)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*sin(x(2))*sin(x(3)) + 33.0)) 0 0
0 0 0 0 ((8.5e4*(1.1*cos(conj(x(2))) + 0.3*sin(x(3))*(0.65*cos(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + (0.13*x(5) + 0.13*x(6))*(0.65*x(5) + 0.65*x(6)) + (0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.65*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.4*cos(conj(x(2)))*sin(conj(x(3))) - 6.4*cos(conj(x(3)))*sin(conj(x(2))) + 0.19*x(4)^2*cos(conj(x(2)))*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.3*cos(x(3))*(6.4*cos(conj(x(2)))*cos(conj(x(3))) - 0.65*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.65*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 6.4*sin(conj(x(2)))*sin(conj(x(3))) + 0.19*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 7.6e-3*x(4)^2*cos(conj(x(2)))*sin(conj(x(2))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) - (5.0e3*(25.0*cos(x(3)) + 17.0)*((6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3))/x(5) 0
0 0 0 0 0 -((5.0e3*(25.0*cos(conj(x(3))) + 17.0)*(1.1*cos(conj(x(2))) + 0.3*sin(x(3))*(0.65*cos(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + (0.13*x(5) + 0.13*x(6))*(0.65*x(5) + 0.65*x(6)) + (0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.65*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.4*cos(conj(x(2)))*sin(conj(x(3))) - 6.4*cos(conj(x(3)))*sin(conj(x(2))) + 0.19*x(4)^2*cos(conj(x(2)))*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.3*cos(x(3))*(6.4*cos(conj(x(2)))*cos(conj(x(3))) - 0.65*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.65*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 6.4*sin(conj(x(2)))*sin(conj(x(3))) + 0.19*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 7.6e-3*x(4)^2*cos(conj(x(2)))*sin(conj(x(2))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) - (1.0e3*(122.0*cos(conj(x(3))) + 133.0*cos(x(3)) + 288.0*cos(conj(x(3)))*cos(x(3)) + 299.0*sin(conj(x(3)))*sin(x(3)) + 211.0)*((6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3))/x(6)];
B = @(x) [0 0 0
0 0 0
0 0 0
1.0e5/(7.5e3*cos(conj(x(2)))*cos(x(2)) + 922.0*sin(conj(x(2)))*sin(x(2)) + 2.5e3*cos(conj(x(2)))*cos(x(2))*cos(x(3)) + 2.5e3*cos(conj(x(2)))*sin(x(2))*sin(x(3)) + 2.5e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2)) + 2.5e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*sin(x(2))*sin(x(3)) + 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*sin(x(3)) - 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(3))*sin(x(2)) - 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(2))*sin(x(3)) + 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(3))*sin(x(2)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*sin(x(2))*sin(x(3)) + 33.0) 0 0
0 8.5e4/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) -(5.0e3*(25.0*cos(x(3)) + 17.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3)
0 -(5.0e3*(25.0*cos(conj(x(3))) + 17.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) (1.0e3*(122.0*cos(conj(x(3))) + 133.0*cos(x(3)) + 288.0*cos(conj(x(3)))*cos(x(3)) + 299.0*sin(conj(x(3)))*sin(x(3)) + 211.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3)];
fc = @(x,u) A(x)*x+B(x)*u;
f = @(x,u) x + Ts*fc(x,u);
Np = 5;
Uopt = zeros(3,Np);
Yref = ones(3,N+Np+1);
for i =1:N-1
FCN = @(U) Cost (x(:,i) , U(:,i) , Yref(:,i+1:i+Np) , Np , Ts)
Uopt = fmincon(FCN ,Uopt , [],[]);
u(:,i) = Uopt(:,1);
x(:,i+1) = f(x(:,i) , u(:,i));
y(i+1) = h(x(:,i+1));
end
plot(t,y)
cost function is :
function J = Cost (xi , U , Yref , Np , Ts)
%%% U = [u(i) u(i+1) u(i+Nc-1)]
A = @(x) [0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
0 0 0 (1.0e5*(cos(x(2))*(cos(x(3))*(5.6e-4*x(6)*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + (x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(5) + x(6)) + (0.13*x(5) + 0.13*x(6))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.025*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.025*x(4)*x(5)*sin(conj(x(2))) - 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6)) + 5.6e-4*x(4)*x(5)*cos(conj(x(2)))*sin(conj(x(3))) - 5.6e-4*x(4)*x(5)*cos(conj(x(3)))*sin(conj(x(2)))) - 1.0*sin(x(3))*(6.8e-3*x(6)*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(x(5) + x(6))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.8e-3*x(4)*x(5)*cos(conj(x(2)))*cos(conj(x(3))) + 6.8e-3*x(4)*x(5)*sin(conj(x(2)))*sin(conj(x(3)))) + (0.13*x(5) + 0.13*x(6))*(0.19*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.19*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.075*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.075*x(4)*x(5)*sin(conj(x(2))) - 0.025*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.025*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.025*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.025*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6))) + sin(x(2))*(cos(x(3))*(6.8e-3*x(6)*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(x(5) + x(6))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.8e-3*x(4)*x(5)*cos(conj(x(2)))*cos(conj(x(3))) + 6.8e-3*x(4)*x(5)*sin(conj(x(2)))*sin(conj(x(3)))) + sin(x(3))*(5.6e-4*x(6)*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + (x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(5) + x(6)) + (0.13*x(5) + 0.13*x(6))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.025*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.025*x(4)*x(5)*sin(conj(x(2))) - 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6)) + 5.6e-4*x(4)*x(5)*cos(conj(x(2)))*sin(conj(x(3))) - 5.6e-4*x(4)*x(5)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.017*x(4)*x(5)*cos(conj(x(2))))))/(x(4)*(7.5e3*cos(conj(x(2)))*cos(x(2)) + 922.0*sin(conj(x(2)))*sin(x(2)) + 2.5e3*cos(conj(x(2)))*cos(x(2))*cos(x(3)) + 2.5e3*cos(conj(x(2)))*sin(x(2))*sin(x(3)) + 2.5e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2)) + 2.5e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*sin(x(2))*sin(x(3)) + 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*sin(x(3)) - 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(3))*sin(x(2)) - 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(2))*sin(x(3)) + 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(3))*sin(x(2)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*sin(x(2))*sin(x(3)) + 33.0)) 0 0
0 0 0 0 ((8.5e4*(1.1*cos(conj(x(2))) + 0.3*sin(x(3))*(0.65*cos(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + (0.13*x(5) + 0.13*x(6))*(0.65*x(5) + 0.65*x(6)) + (0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.65*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.4*cos(conj(x(2)))*sin(conj(x(3))) - 6.4*cos(conj(x(3)))*sin(conj(x(2))) + 0.19*x(4)^2*cos(conj(x(2)))*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.3*cos(x(3))*(6.4*cos(conj(x(2)))*cos(conj(x(3))) - 0.65*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.65*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 6.4*sin(conj(x(2)))*sin(conj(x(3))) + 0.19*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 7.6e-3*x(4)^2*cos(conj(x(2)))*sin(conj(x(2))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) - (5.0e3*(25.0*cos(x(3)) + 17.0)*((6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3))/x(5) 0
0 0 0 0 0 -((5.0e3*(25.0*cos(conj(x(3))) + 17.0)*(1.1*cos(conj(x(2))) + 0.3*sin(x(3))*(0.65*cos(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + (0.13*x(5) + 0.13*x(6))*(0.65*x(5) + 0.65*x(6)) + (0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.65*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.4*cos(conj(x(2)))*sin(conj(x(3))) - 6.4*cos(conj(x(3)))*sin(conj(x(2))) + 0.19*x(4)^2*cos(conj(x(2)))*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.3*cos(x(3))*(6.4*cos(conj(x(2)))*cos(conj(x(3))) - 0.65*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.65*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 6.4*sin(conj(x(2)))*sin(conj(x(3))) + 0.19*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 7.6e-3*x(4)^2*cos(conj(x(2)))*sin(conj(x(2))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) - (1.0e3*(122.0*cos(conj(x(3))) + 133.0*cos(x(3)) + 288.0*cos(conj(x(3)))*cos(x(3)) + 299.0*sin(conj(x(3)))*sin(x(3)) + 211.0)*((6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3))/x(6)];
B = @(x) [0 0 0
0 0 0
0 0 0
1.0e5/(7.5e3*cos(conj(x(2)))*cos(x(2)) + 922.0*sin(conj(x(2)))*sin(x(2)) + 2.5e3*cos(conj(x(2)))*cos(x(2))*cos(x(3)) + 2.5e3*cos(conj(x(2)))*sin(x(2))*sin(x(3)) + 2.5e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2)) + 2.5e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*sin(x(2))*sin(x(3)) + 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*sin(x(3)) - 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(3))*sin(x(2)) - 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(2))*sin(x(3)) + 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(3))*sin(x(2)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*sin(x(2))*sin(x(3)) + 33.0) 0 0
0 8.5e4/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) -(5.0e3*(25.0*cos(x(3)) + 17.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3)
0 -(5.0e3*(25.0*cos(conj(x(3))) + 17.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) (1.0e3*(122.0*cos(conj(x(3))) + 133.0*cos(x(3)) + 288.0*cos(conj(x(3)))*cos(x(3)) + 299.0*sin(conj(x(3)))*sin(x(3)) + 211.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3)];
fc = @(x,u) A(x)*x+B(x)*u;
f = @(x,u) x + Ts*fc(x,u);
x = zeros(6 ,Np);
x = [xi x];
y = zeros(3 , Np);
for i=1:Np
x(: , i+1) = f(x(: , i) , U(:,i));
y(:,i) = h(x(: , i+1));
end
R = 1;
J = sum ((Yref-y).^2) + R*sum (U.^2);
end
and h is:
function y=h(x)
y=[x(1);x(2);x(3)];
end
but I got this error:
FCN =
function_handle with value:
@(U)Cost(x(:,i),U(:,i),Yref(:,i+1:i+Np),Np,Ts)
Index exceeds matrix dimensions.
Error in Cost (line 23)
x(: , i+1) = f(x(: , i) , U(:,i));
Error in MPC>@(U)Cost(x(:,i),U(:,i),Yref(:,i+1:i+Np),Np,Ts)
Error in fmincon (line 536)
initVals.f = feval(funfcn{3},X,varargin{:});
Error in MPC (line 34)
Uopt = fmincon(FCN ,Uopt , [],[]);
Caused by:
Failure in initial objective function evaluation. FMINCON cannot continue.

请先登录，再回答此问题。

回答（1 个）

Matt J
Matt J 2021-1-17
编辑：Matt J 2021-1-17
This loop does not make sense, because in the workspace of Cost(), the variable U only has 1 column.
for i=1:Np
x(: , i+1) = f(x(: , i) , U(:,i));
y(:,i) = h(x(: , i+1));
end
  5 个评论
显示 3更早的评论
Matt J
Matt J 2021-1-18
编辑：Matt J 2021-1-18
I don't know what your purpose is - only you know the form your objective function is supposed to take. However, if your intention was to have all columns of the unknown matrix available for use inside Cost(), then rewriting FCN as,
FCN = @(Z) Cost (x(:,i) , Z , Yref(:,i+1:i+Np) , Np , Ts)
will achieve that.
john white
john white 2021-1-18
you know at the first time I have wriiten my code as you mentioned, I mean FCN = @(U) Cost (x(:,i),U,Yref(:,i+1:i+Np),Np,Ts)
But I got the error about Index of matrix in for loop
x(:,i+1) = f(x(:,i) ,U);
that I changed the code to
x(: , i+1) = f(x(: , i) , U(:,i));
and that error is solved and then I rewrite the FCN as new form
FCN = @(U) Cost (x(:,i) , U(:,i) , Yref(:,i+1:i+Np) , Np , Ts)
but I got this error about Failure in initial objective
actually my purpose is
the code get x nad u and Uopt be the fmincon (minimizing) the cost function , the cost function get u , x ,yref,Np and Ts and y is x dependent and in the loop all of cost function, y and Uopt and x etc will be update

请先登录，再进行评论。

类别

在 Help Center 和 File Exchange 中查找有关 Solver Outputs and Iterative Display 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by