Solve ode45 system with input calculated inside ode function

Dear MathWorks community,

I have a problem with my ode solving method. I'm trying do solve state-space model in ode45 with input based on actual state calculated inside ode. This input is a bit of random (so probably this is the problem I think) but let me explain.

What I'm trying to do is something like this:

[t, x] = ode45(@(t, x) sys(t, x, plant, MV_bounds, OV_bounds,u_initial), tspan, iniCon);

function  dx = sys(t, x, ss_model, MV_bounds, OV_bounds, u_initial)

persistent u % persisten variable needed to save last input u
persistent kk % persisten variable needed to save sampling instant
if(isempty(kk)||t==0)
    kk=0;
end
 if(isempty(u)||t==0)
     u=u_initial;
 end

Ts = 0.1;
d_ss_model = c2d(ss_model,Ts);
A = ss_model.A; 
B = ss_model.B;
Am = d_ss_model.A; 
Bm = d_ss_model.B; 

%% some parameters for PSO
....

% optimization
if(floor(t/Ts)>=kk)
 % if time reaches optimization sampling time, optimize. 
 % I added such a condition in case the time step of ODE 
 % miss exact time of k*Ts (which im sure it will). 
 % I've tried also set ode option of maxstep to 0.1.

    %% 
    %% PSO optimization that calculates suboptimal input u 
    % based on testing discrete state-space model of the system.
    %%
    u=gbest_p'; % resulted input
    kk= kk+1;
end 
  % if time is between optimization sampling, 
  % hold previous input up to next optimization time
    dx = A*x + B*u;
end

When I run this, ode acts weridly. When I add a breakpoint (for checking timesteps) it seems like ode makes some small steps and then takes a step back (sometimes a big step back) and it takes forever to calculate 10s of timespan (but sometimes it finishes, depends on my patience at the moment). I guess this is a poor design, but I'm really new in constructing such systems and I don't really know about restrictions and conditions I must follow in order to make good system/control design.

What I can add, that PSO calculates input vector pretty well. As I set reference state at 0 (with quadcopter state space model linearized at hover conditions), resulting inputs are very small as suspected.

How can I solve such a problem? Do I need to calculate input outside ODE? How can I handle sampling time for optimization in some better, more reliable way? If the idea of such design is poor and just stupid please be honest, I woud appreciate any constructive critics. Sooooomeday I will be PRO in systems control :)

[Torsten]

As far as I know, control input calculation and integration are decoupled.

Usually, you estimate a control vector u = (u0,u1,...,u_(n-1)) for time intervals [t0:t1],...,[t_(n-1:t_n] solve the state-space model with this vector (usually by integrating from t0 to t1 with input u0, calling the solver anew at t1 with control input u1 and integrating up to t2 and so on). With the response vector x = (x0,...,x_n), you estimate a new control vector u_new =(u0_new,u1_new,...,u_n_new), repeat integration and so on.

[Piotr Pawlowski]

And it's basically what I did now (I think). I took optimization out of the ODE and put ODE into loop (with timesteps 1:Ts:10). In each loop, I perform input optimization for one sampling interval(based on discrete ss_model, with sampling time Ts) and then launch ODE with timespan [t_actual, t_actual+Ts] and "u" as a parameter. After ODE finishes, save final state x, which will be used as initial conditions for next time interval. Am I doin it right now?

Unfortunately solving ode45 100 (as Ts = 0.1) times is a bit long (0.1s every time) so I think I will move to discrete time SS in whole simulation, not only for optimization part. Or there is another way to speed up?

By the way, am I right if I say that it is a bit of MPC control approach?

[Torsten]

And why do you then calculate (or set) u inside the ODE function and not in an external loop in which the ode integrator is called for the next time step ?

[Piotr Pawlowski]

Maybe you just misunderstood me. Now I have something like this:

ss_model = ss(..);
d_ss_model = c2d(ss_model);
Ts = 0.1;
initCon = ..;


for t=0:Ts:10

    % PSO stuff with discrete ss
    ...
    ...
    ...

    u=gbest_p';
   
    [tt, x] = ode45(@(t, x)sys2(t, x,ss_model,u), [t t+Ts], initCon);
    
    initCon = x(length(tt),:)';

end

And

function  dx = sys2(t, x, ss_model,u)

    A = ss_model.A; 
    B = ss_model.B; 
    dx = A*x + B*u;
    
end

Good anyhow?

[Torsten]

That's what I meant.

Although I don't know whether it's necessary to do optimization of single u in the loop. I only know of cases where the complete control vector (for each output time 0:Ts:10) is computed after the loop. Then the loop is executed again with this full new control vector.

[Piotr Pawlowski]

Hmmm, are you sure that it will be computet after loop? Maybe what you meant is that to compute full timespan of 1:Ts:10 for complete vector "u" (without inner partial integration) ant then execute it after loop. Because you know, after the loop there is the end of script in this particular case :)

Right now I just want to test if the approach is correct and if it gives feasible results (just for one step optimization).

My goal is as follows:

Tsim = 0.05;
initCon = ..;
c= 3;
p = 10;
x = iniCon;
for t: Tsim : Tfinal
    
    if(t hits controller sample time)
            
        for tc = t : Ts : c*Ts % c stands for control horizon over which
                                    % we will compute control inputs
            
                % With optimization, compute c<=p control intervals 
                % Optimization is held over p*Ts horizon 
                % with c-control intervals. After c*Ts optimizer holds
                % last computed control interval as constant over rest
                % of prediction horizon.
                
        end

    end
    
    % execute only first control interval
    [tt, x] = ode45(@(t, x)sys2(t, x, ss_model, u(1)), [t : Tsim], initCon);
    %  or with sum, if using discrete mode
    initCon = x(length(tt),:)';
end

How about that? Im tempted to choose ode instead of [ t: Tsim : Tfinal ] for loop but I guess it will be too demanding in computation time, but...