How can I optimize a cost function under trajectory constraint ?

Hi everyone,

I am trying to run an optimization problem using fmincon and ode solvers and I have an issue: my ODE system is the one of an hexarotor that I can solve using ode solver, then I want to minimize a cost function (sum(abs(u)) with 'u' the square speed of the propeller.

To do so I put my dvar as a 8-long vector with the 6 propellers speed and the 2 tilt angles of these propellers.

The thing is I want to find the best angle for a certain predetermined trajectory (sinus here) and I am supposed to constraint my problem using this sinus, but whatever I try I can't make it work.

Right now I am doing like this:

u is a variable for fmincon, I give it to ode15s which compute the equation dX=f*u+g (f product of matrix and g constant) and I try to constraint the error by setting ceq=[dX(evalTime,columnOfRightState)-sinus(evalTime);...] (I evaluated 9 or more points of my sinus to make it fit with the right time of my ODE).

A list of what I tried:

- ceq=[abs(dX-sin)]

-ceq=[dX-sin;dXother] (I want some of the state to be null and other to follow a sinus path)

I also tried to make step by step resolution by calling ode15s in for-loop and changing the ceq each time..

I saw on other post that it's not possible to constraint an output as long as the solver fully determine the state, but in my case fmincon can play with 'u' which compute the state so I suppose I can tell it to play by respecting some constraint can't I ?

If anyone has an idea it is welcomed cause I really can't figure it out so far and it's been hours and hours...

Thanks :)