Matlab code with Euler approximation:
%%%%%%%%%%%%%%%%%%%%
% Perform a simulation, from t = 0 through t = 10.
% As an argument, take a controller function. This function must accept
% a struct containing the physical parameters of the system and the current
% gyro readings. The controller may use the strust to store persistent state, and
% return this state as a second output value. If no controller is given,
% a simulation is run with some pre-determined inputs.
% The output of this function is a data struct with the following fields, recorded
% at each time-step during the simulation:
%
% data =
%
%         x: [3xN double]
%     theta: [3xN double]
%       vel: [3xN double]
%    angvel: [3xN double]
%         t: [1xN double]
%     input: [4xN double]
%        dt: 0.0050
function result = simulate(controller, tstart, tend, dt)
    % Physical constants.
    g = 9.81;
    m = 0.95;

    d = 0.35;
    h = 0.08;

    I_body = diag([5e-3, 5e-3, 10e-3]);
    I_react=1e-3;
    I_gyro=1e-3;

    I = struct('I_body', I_body, 'I_react', I_react, 'I_gyro', I_gyro);

    kd = 0.25;

    kt = 0.00001*9.8;
    kf = 0.0015*9.8;

    % Simulation times, in seconds.
    if nargin < 4
        tstart = 0;
        tend = 1;
        dt = 0.05;
    end
    ts = tstart:dt:tend;

    % Number of points in the simulation.
    N = numel(ts);

    % Output values, recorded as the simulation runs.
    xout = zeros(3, N);
    xdotout = zeros(3, N);
    thetaout = zeros(3, N);
    thetadotout = zeros(3, N);

    inputout_beta = zeros(2, N);
    inputout_motor_w = zeros(2, N);

    inputout_beta_speed = zeros(2, N);
    inputout_beta_acc = zeros(2, N);

    inputout = struct('inputout_beta', inputout_beta, 'inputout_beta_speed', inputout_beta_speed ,...
        'inputout_beta_acc', inputout_beta_acc,'inputout_motor_w', inputout_motor_w);

    % Struct given to the controller. Controller may store its persistent state in it.
    controller_params = struct('dt', dt, 'I', I, 'kf', kf, 'd', d, 'h', h, 'm', m, 'g', g);

    % Initial system state.
    x = [0; 0; 10];
    xdot = zeros(3, 1);
    theta = zeros(3, 1);

    % If we are running without a controller, do not disturb the system.
    if nargin == 0
        thetadot = zeros(3, 1);
    else
        % With a control, give a random deviation in the angular velocity.
        % Deviation is in degrees/sec.
        deviation = 300;
        thetadot = deg2rad(2 * deviation * rand(3, 1) - deviation);
    end

    ind = 0;
    for t = ts
        ind = ind + 1;

        % Get input from built-in input or controller.
        if nargin == 0
            i = input(t); %built-in generator
        else
            [i, controller_params] = controller(controller_params, thetadot);
        end

                        % Store simulation state for output.
        % don't go through the ground
            if (x(3) < 0)
                x(3) = 0;
                theta(1) = 0; % avoid integration windup
                theta(2) = 0;
            end

        %%%%% Compute forces, torques, and accelerations.
        omega = thetadot2omega(thetadot, theta);
        a = acceleration(i, theta, m, g, kf, kt, kd, xdot);
        omegadot = angular_acceleration(i, omega, I, d, h, kf, kt);

        %%%% Advance system state. Euler method.
        omega = omega + dt * omegadot;
        thetadot = omega2thetadot(omega, theta);

        theta = theta + dt * thetadot;
        xdot = xdot + dt * a;
        x = x + dt * xdot;

        xout(:, ind) = x;
        xdotout(:, ind) = xdot;
        thetaout(:, ind) = theta;
        thetadotout(:, ind) = thetadot;
        inputout(:, ind) = i;
    end

    %% Put all simulation variables into an output struct.

    result = struct('x', xout, 'theta', thetaout, 'vel', xdotout, ...
                    'angvel', thetadotout, 't', ts, 'dt', dt, 'input', inputout);
end

%% Arbitrary test input.
function in = input(t)
inputout_beta= zeros(2, 1);
inputout_motor_w= zeros(2, 1);

    inputout_beta(1) = 0.09;%-0.1*cos(t);
    inputout_beta(2) = 0;%0.1*cos(t);
    inputout_motor_w(1)= 3000*pi/30;%100*pi/30*cos(t) %rpm to radian per second
    inputout_motor_w(2)= 3000*pi/30;%101*pi/30*cos(t) %rpm to radian per second

    inputout_beta_speed(1)=0;
    inputout_beta_speed(2)=inputout_beta_speed(1);
    inputout_beta_acc = zeros(2, 1);

    in = struct('inputout_beta', inputout_beta, 'inputout_beta_speed', inputout_beta_speed ,...
        'inputout_beta_acc', inputout_beta_acc,'inputout_motor_w', inputout_motor_w);

end

% Compute thrust given current inputs and thrust coefficient.
function F = force(w_motor, beta_motor, kf)
     w_l=w_motor(1);
     w_r=w_motor(2);
     beta_l=beta_motor(1);
     beta_r=beta_motor(2);

    %Right power plant force
    f_r=kf*w_r;
    %Left power plant force
    f_l=kf*w_l;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %Left motor force rot mat
    R_Pl_Bm=[cos(beta_l), 0, sin(beta_l);
          0,          1,       0;
         -sin(beta_l),0, cos(beta_l)];
    %Right motor force rot mat
    R_Pr_Bm=[cos(beta_r), 0, sin(beta_r);
          0,          1,       0;
         -sin(beta_r),0, cos(beta_r)] ;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    F_l=[0; 0; f_l];
    F_r=[0; 0; f_r];
    F=R_Pl_Bm*F_l+R_Pr_Bm*F_r;
end

% Compute torques, given current inputs, length, drag coefficient, and thrust coefficient.
function T_Bm = torque(input, d, h, k_f, k_d, I)
     w_l=input.inputout_motor_w(1);
     w_r=input.inputout_motor_w(2);

     beta_l=input.inputout_beta(1);
     beta_r=input.inputout_beta(2);

     beta_l_speed=input.inputout_beta_speed(1);
     beta_r_speed=input.inputout_beta_speed(2);

     beta_l_acc=input.inputout_beta_acc(1);
     beta_r_acc=input.inputout_beta_acc(2);

    %Right power plant torque
    tau_drag_r=k_d*w_r;
    %Left power plant torque
    tau_drag_l=k_d*w_l;

    %Right power plant force
    f_r=k_f*w_r;
    %Left power plant force
    f_l=k_f*w_l;

    F_l=[0; 0; f_l];
    F_r=[0; 0; f_r];

    OlOb=[0;d;h];
    OrOb=[0;-d;h];

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %Left motor force rot mat
    R_Pl_Bm=[cos(beta_l), 0, sin(beta_l);
              0,          1,       0;
             -sin(beta_l),0, cos(beta_l)];
    %Right motor force rot mat
    R_Pr_Bm=[cos(beta_r), 0, sin(beta_r);
             0,          1,       0;
             -sin(beta_r),0, cos(beta_r)] ;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

     I_gyro = I.I_gyro;
     I_react = I.I_react;

     % TORQUES
        %Gyroscopic moments, longitudinal tilting
        Mgyro_Pl_beta_l=[0;-I_gyro*w_l*beta_l_speed;0];
        Mgyro_Pr_beta_r=[0;I_gyro*w_r*beta_r_speed;0];
        %Gyroscopic momentin body frame
        Mgyro_Bm=R_Pl_Bm*Mgyro_Pl_beta_l+R_Pr_Bm*Mgyro_Pr_beta_r;

        %Fan torques due aerodynamic drag of the propeller
        Mdrugr_Pr=[0;0;-tau_drag_r];
        Mdrugl_Pl=[0;0;tau_drag_l];
        %Drag torques in body frame
        Mdrug_Bm=R_Pl_Bm*Mdrugl_Pl+R_Pr_Bm*Mdrugr_Pr;

        %Moment from trust vectoring in body frame
        M_thrv_Bm=cross((R_Pl_Bm*F_l),OlOb)+cross((R_Pr_Bm*F_r),OrOb);

        %Adverse Inertial Reaction moment
        M_react_Pr=[0;-I_react*beta_r_acc;0];
        M_react_Pl=[0;-I_react*beta_l_acc;0];
        %Adverse Inertial Reaction moment, body frame
        M_react_Bm=R_Pr_Bm*M_react_Pr+R_Pl_Bm*M_react_Pl;
        %FULL TORQUE
        %Body frame
        T_Bm=Mgyro_Bm+Mdrug_Bm+M_thrv_Bm+M_react_Bm;
end

% Compute acceleration in inertial reference frame
% Parameters:
%   g: gravity acceleration
%   m: mass of quadcopter
%   kf: thrust coefficient
%   kt: global drag coefficient
function A = acceleration(inputs, angles, m, g, kf, kt, kd, linear_velocity)
    F_g = [0; 0; -g];
    R_Bm_Wi = rotation(angles);
    F_Bm = force(inputs.inputout_motor_w, inputs.inputout_beta, kf);
    %Friction as a force proportional to the linear velocity in each direction.
    F_a = -kd * linear_velocity;
    %mV=R*F
    A = R_Bm_Wi * F_Bm * 1/m + F_g + F_a;
end

% Compute angular acceleration in body frame
% Parameters:
%   I: inertia matrix
function omegad = angular_acceleration(inputs, omega, I, d, h, kf, kt)
    T = torque(inputs, d, h, kf, kt, I);
    omegad = I.I_body(T - cross(omega, I.I_body * omega));
end

% Convert derivatives of roll, pitch, yaw to omega.
function omega = thetadot2omega(thetadot, angles)
    phi = angles(1);
    theta = angles(2);
    psi = angles(3);
    W = [1,   0,                   -sin(phi);
         0    cos(phi),    cos(phi)*sin(phi);
         0   -sin(phi), cos(theta)/cos(phi)];
    omega = W * thetadot;
end

% Convert omega to roll, pitch, yaw derivatives
function thetadot = omega2thetadot(omega, angles)
    phi = angles(1);
    theta = angles(2);
    psi = angles(3);
    W = [1,     sin(phi)*tan(theta),cos(phi)*tan(theta);
         0      cos(phi),                     -sin(phi);
         0      sin(phi)/cos(theta), cos(phi)/cos(phi)];
    thetadot = W * omega;
end