using bvp4v for quadrotor differential equations

Hello,

I'm working on simulating an autonomous quadrotor using LQR control algorithm, and since i'm using a step time loop for trajectory following, I'm working with BVP4C as my equation solver.

the problem is that my state space vector X is [12x1] vector, so I would have 12 equations in total. I've seen tutorial on how to use bvp4c, but it's only for one differential equation. I've worked with ode45 before on the same project, but it appears to be a different approach.

for now I'm trying to make my code work for a general solution without using a for loop where the input command coming from the LQR controller and the reference trajectory is changing with each step time.

My question is : How can I implement the bvp4c in my condition ?

The code is as follows:

clc; clear; close all;

% time frame for my simulation 
T = linspace(0,0.2,15);
% Initial time frame given for bvpinit
T_init = linspace(0,0.2,2);

% Initial conditions 
X0 = zeros(12,1);

% My input command is gonna be constant for now as i'm going for take off first.
% U(1) is related to the thrust (Altitude), U(2) and U(3) control the roll and pitch respectively (Attitude), U(4) controls the heading. 
U = [25;0;0;0];

% My trajectory objectif is to reach a 9 m height at a speed of 11 m/s
P_lim = zeros(12,1); P_lim(3) = 9; P_lim(6) = 11;

% BVP init 
init = bvpinit(T_init, P_lim)

% Equation solver
X = bvp4c(@state_equation, @bvp_bc, init);

%% State Equation

function dX = state_equation(t,X,U)

%% Parameters : mass , arm length, Rotor Inertia, Body Inertia ...
p = param();
g = p(1); m =p(2); l=p(3); Jr = p(4);
I = Inertia(); Ix = I(1); Iy = I(2); Iz = I(3);     %Inertia

% since I'm working on take off the relative speed Wr = W1 - W2 + W3 - W4 = 0, since all the actuators are rotating at the same speed.
Wr = 0;
%Dynamics
dX = [  X(4:6,1); 
        U(1)*(cos(X(7))*sin(X(8))*cos(X(9))+ sin(X(7))*sin(X(9)))/m;
        U(1)*(cos(X(7))*sin(X(8))*sin(X(9)) - cos(X(9))*sin(X(7)))/m;
        -g + U(1)*cos(X(7))*cos(X(8))/m;
        X(10:12,1);
        l*U(2)/Ix - Jr*X(11)*Wr/Ix + (Iy-Iz)*X(11)*X(12)/Ix;
        l*U(3)/Iy - Jr*X(10)*Wr/Iy + (Iz-Ix)*X(10)*X(12)/Iy;
        l*U(4)/Iz + (Ix-Iy)*X(10)*X(11)/Iz];


end

%% BVP boundary conditions
function bc = bvp_bc(X0,P_lim)

%X0 present the boundary condition on state vector X on t = 0
%P_lim present the boundary condition on state vector X on t = t_max

bc = [ X0; P_lim];
end