fmincon solver not working
Afficher commentaires plus anciens
close all;
clear all;
clc;
%u = sym('u', [4,2]);
A = [];
b = [];
Aeq = [];
beq = [];
lb = [-4;4];
ub = [ -4;4];
X0 = [1,1];
xd = [3,3].*ones(5,2);
[p,fval] = fmincon(@(u)running_cost(u(1),u(2),u(3),u(4),u(5),u(6),u(7),u(8)),X0,A,b,Aeq,beq,lb,ub);
function x_next = sysdynamics(current_state, current_input, sampling_time)
x(1) = current_state(1);
x(2) = current_state(2);
T = sampling_time;
v = current_input(1);
theta = current_input(2);
x_next = sym(zeros(1,2));
x_next(1) = x(1) + T*v*cos(theta);
x_next(2) = x(2) + T*v*sin(theta);
%next_state = [x_next(1), x_next(2)];
end
function [cost] = running_cost(u1,u2,u3,u4,u5,u6,u7,u8)
%M = 10; % number of subintervals between each sampling time
current_state=[1,1];
desired_state=[3,3].*ones(5,2);
prediction_horizon= 4;
Np = prediction_horizon;
T = 1; %(1/M);
Xd = desired_state; % size(41x2)
Xk = sym(zeros(Np+1, 2)); % size(41x2)
Xk(1,:) = current_state;
Uk(1,:) = [u1,u2];
Uk(2,:) = [u3,u4];
Uk(3,:) = [u5,u6];
Uk(4,:) = [u7,u8];
cost1 = 0;
cost2 = 0;
cost3 = 0;
cost4 = 0;
for k = 1:Np
Xk(k+1,:) = sysdynamics(Xk(k,:),Uk(k,:), T);
end
for i = 1:Np
cost1 = cost1 + (Xk(i +1, 1) - Xd(i + 1, 1))^2;
cost2 = cost2 + (Xk(i +1, 2) - Xd(i +1, 2))^2;
cost3 = cost3 + (Uk(i, 1))^2;
cost4 = cost4 + (Uk(i, 2))^2;
cost = cost1 + cost2 + cost3 + cost4;
end
end
Réponse acceptée
Plus de réponses (0)
Catégories
En savoir plus sur Solver Outputs and Iterative Display dans Centre d'aide et File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!