Can I include an ode45 in a for loop

I'm working on an LQR controller for an autonomous drone and I'm having problems with the ode45 solver. My code goes like this:

T = 20;
dt = 0.1;
X0 = zeros(12,1);
for i=0:dt:T
   timeRange = i:dt:i+dt;
   P = TrajectoryRef(X0);
   U = Command_lqr(X0,P);
   X = EquationSolver(U,X0,timeRange);
   %since ode45 has an independent timestep i register the last value and sign it as the next input

 X0 = X(size(X,1),:);
end

But i keep getting wrong values in the result.

[Walter Roberson]

? It is not obvious to me where you are calling ode45 or where you would want to call ode45 ?

[Mohamed Amine BELYAMANI]

The ode45 is in the EquationSolver function, it include the ode45 solver and the differential equations of the drone.

on this function. I figured since i'm using a step time using a for loop, I'll need the 3 inputs:

• timeRange
• LQR Command Input U
• Initial condition X0

function EquationSolver(U,X0,timeRange)
[t, X] = ode45(@(t,X)state_equation(t,X,u,p),timeRange,X0);

end

function dX = state_equation(t,X,u,p)

p = param();
 g = p(1); m =p(2); l=p(3);
I = Inertia(); Ix = I(1); Iy = I(2); Iz = I(3);     %Inertia

%Dynamics
dX = [  X(4:6,1); 
        u(1)*cos(X(7))*sin(X(8))/m;
        -u(1)*sin(X(7))/m;
        -g + u(1)*cos(X(7))*cos(X(8))/m;
        X(10:12,1);
        l*u(2)/Ix;
        l*u(3)/Iy;
        l*u(4)/Iz];


end

[Walter Roberson]

You define

function EquationSolver(U,X0,timeRange)

but you have

   X = EquationSolver(U,X0,timeRange);

Notice that the EquationSolver you defined does not return any outputs.

[Mohamed Amine BELYAMANI]

Yes you are right, I apologize.

X = EquationSolver(U,X0,timeRange)

the thing is that ode45 has its own time step so it would return for example a X with a size of (49,12), knowing that the time range I associated is i:0.1:i+0.1.

so I figured I would take the last value calculated and apply it as my X0, but then the results aren't making any sense.

If you would like I can share the whole script with. I would appreciate some inputs.

I tried fixing my inputs, normally its working but when I inplement the time step, It doesn't work

[Walter Roberson]

Instead of

timeRange = i:dt:i+dt;

use

timeRange = i + [0, dt/2, dt];

Then because timeRange will be a vector of more than 2 items, the number of results that will be reported will be the same as the number of elements in the vector, and you can index the result at row 3 always (unless it hits a singularity.)

[Mohamed Amine BELYAMANI]

Okay Thank you, now I only get 3 successive step from the ode45 instead of 49.

But regarding the Initial conditions , should I implement that as well or should I fix it.

[Walter Roberson]

Is your general strategy along the line of

loop:

1. use knowledge of your current state and your goals to run a short term ode prediction
2. command the device
3. get feedback from where it actually ended up (e.g. because of wind the flight might not have gone as planned)

You appear to have only three steps in your loop, but it seems to me that you would also prefer to have feedback from sensors so that you could predict the next necessary next step, and it is not obvious to me where that sensor step is. Or is the sensor information included with the information about where you actually end up after a period?

[Mohamed Amine BELYAMANI]

At first I started as just getting my feedback from the ode45 since it will hold my data. For me to make the system more realistic as to adding IMU and barometer sensors, EKF and even adding disturbance ( like you've said wind existence will not make the flight go as planned).

But to do so I need to have a running simplified system in which I'm sur It'll be working.

[Walter Roberson]

Please describe your current strategy, since we as outside observerse do not know what TrajectoryRef or Command_lqr do. At the moment it kind of looks to me as if you have steps in the wrong order. What is it that the output of the EquationSolver tells you?

I have not done any drone control work... I would kind of expect the outline to go like

• taking into account current position and velocities, pick a general target. This should probably involve path planning to avoid obstacles
• taking into account current position and velocities, pick a short-term target that you would hope to get to in one time-step. If your sensors and algorithms were really good this could include obstacle avoidance, but you need a lot more experience if you hope to be able to just barely miss a corner knowing that the wind pattern might be different on the other part. The first stage general target would generally try to stay out of "mistakes were made" range of obstacles.
• using the current position and velocities as some of the initial conditions, run an inverse kinematics or Boundary Value Problem to figure out what power to apply to which motors to achieve the short term destination
• apply the predicted needed powers
• get information back about where you ended up after the time step, including updated positions and velocities
• loop back

You seem to be doing a forward ODE rather than a BVP, and it is not clear to me why you are doing that.

Also, I think the ODE should always be run in time 0 to dt, not from elapsed time until now. You are doing corrections as you go, so at any timestep it was never the case that you ran a consistent set of original inputs that would have led you from the origin to where you are now if you had just run the ODE that long. You are now where you are now, and you want to make predictions about times relative to now.

It is possible that my general outline is not practical in the form stated, due to the time required to solve the ode, during which you have to keep flying. Ideally after giving the command you would keep computing unstead of just waiting doing nothing until the device reported back.

Because of the potential cost of running a BVP, I could imagine that it might be necessary in practice to use an approximation ODE that is easier / faster to solve but which perhaps does not take into account all terms, relying on the fact that the reports back about where you really got to imply you have to make corrections anyhow. The implication might be that in still air your simplified ODE might not reflect the most efficient way to fly, but that in changing wind conditions the model errors might be less than the errors imposed by outside force.

[Mohamed Amine BELYAMANI]

I'll look into the BVP you mentionned as I never have worked with it before.

Before taking into account the integration of any sensors, the system works as follows, at a time t:

%Given the feedback from the ode45, let's say X0 
T = i + [0, dt/2, t];
%Trajectory Reference gives the desired position we want the drone to get to
P = TrajectoryRef(X0);

%We take the desired Trajectory P and the feedback X0 as an input for the LQR command

U = Command_lqr(X0,P);

%Now that we have U, we set up the next step for the ode45, we'll need U as input, T as time Range
%and I figured X0 we'll be the initial condition for the next step.
X = EquationSolver(T,X0,U);

%since the ode45 we'll return 3x12 array for X, I take the last row as my X0
X0 = X(size(X,1),:);

for waypoints, let say I want my drone to get from its initial postion [0 0 0] to [ 0 0 5], how can I make my Trajectory reference a set of points so like you said : pick a short-term target that you would hope to get to in one time-step.

[Walter Roberson]

Hmmm... I do not know enough about how that class of drones are controlled. I presume that some of the controls have to do with angles of surfaces, and that other controls have to do with velocities of motors. But I do not know, for example, anything about slew rates, about how quickly controls can be changed, about whether it is necessary to keep track of current acceleration of a control because potentially the rate of change of acceleration might be limited. And I do not know anything about issues such as difficulty in getting a motor started again... I know that in some cases (at least for much larger devices) there are cases when starting a motor is hard enough that it can make sense to have a forward motor and a reverse motor both, with one running at idling speed and not enough to overcome the full power of the other motor, so that neither motor ever needs to stop... https://www.motioncontrolonline.org/content-detail.cfm/Motion-Control-News/Understanding-Backlash-and-Stiction/content_id/310

I have never worked with drones, so I do not know which of these things need to be taken into consideration in practice.

[Mohamed Belyamani]

I'm looking into it as well, for now the ode solver is capable of achieving the takeoff of the drone without any problems, but the real issue lies in controlling the pitch and roll, so i'm looking into implementing BVP or InverseKinematic to see if there is any change.

About the path planning, since i'm working with ultrasonic sensors for now, is it possible to simulate or implement the sensors in a way that would detect obstacles along the way. I've started working with Simscape since I modeled my drone in SolidWorks and I saw fit to actually have the command as PWM. I know that the best solution is working with Gazebo using ROS but I couldn't get to install the software since it is kinda difficult (for me) to install it on Windows.