MATLAB state-derivative function (compact) — matches your choices

Below is a compact MATLAB function file plus a short ode45 example. It uses state: [rI(3); Vb(3); q(4) scalar-first; omega_b(3)], constant mass/inertia, simple aero defaults (CL, CD), alpha≈w/u, dynamic pressure from body speed |Vb|, quaternion renormalized inside the derivative, and shows an ode45 call.

MATLAB code (copy into an m-file, e.g., missile6dof.m)

function xdot = missile6dof(t,x,inputs,params)
% x: [rI(3); Vb(3); q(4) ; omega_b(3)]
% inputs: struct with fields thrust_body (3x1) and moments_body (3x1)
% params: struct (optional defaults provided below)

% Unpack state
rI = x(1:3);      % inertial position (not used in dynamics besides gravity if needed)
Vb = x(4:6);      % body-frame velocity [u; v; w]
q  = x(7:10);     % quaternion scalar-first [q0; q1; q2; q3]
omega = x(11:13);  % body rates [p; q; r]

% Defaults
if nargin<4 || isempty(params)
    params.m = 100;                    % kg
    params.I = diag([10,12,8]);       % inertia matrix in body axes (3x3)
    params.S = 0.5;                    % reference area (m^2)
    params.c = 1.0;                    % reference length (m)
    params.rho = 1.225;               % air density (kg/m^3)
    % aerodynamic coeff defaults
    params.CL0 = 0.0;
    params.CL_alpha = 2*pi;            % per rad (simple linear approx)
    params.CD0 = 0.02;
    params.k = 0.07;                   % induced drag factor
    params.g = 9.80665;                % gravity (m/s^2)
end

% Inputs defaults
if nargin<3 || isempty(inputs)
    inputs.thrust_body = [0;0;0];
    inputs.moments_body = [0;0;0];
end

m = params.m;
I = params.I;
invI = inv(I);

% Body speed magnitude
u = Vb(1); v = Vb(2); w = Vb(3);
Vmag = norm(Vb);

% Small-angle alpha approximation (assume u>0)
if abs(u) < 1e-6
    alpha = 0;
else
    alpha = w/u; % small-angle
end

% Aerodynamic coefficients (simple)
CL = params.CL0 + params.CL_alpha * alpha;
CD = params.CD0 + params.k * CL^2;

% Dynamic pressure
qbar = 0.5 * params.rho * Vmag^2;

% Aerodynamic forces in body axes (simplified along body-x and -z)
% Lift assumed acting in -z body (positive CL -> positive lift upwards in body frame: -z)
L = CL * qbar * params.S;    % magnitude
D = CD * qbar * params.S;    % magnitude

% Force components (simple axis assumptions)
F_aero_body = [-D; 0; -L];   % body axes: X forward, Z down

% Total forces
F_thrust = inputs.thrust_body;     % body frame
F_gravity_inertial = [0;0;m* -params.g]; % inertial down is -zI

% Convert gravity to body frame via DCM from quaternion
Cbi = quat2dcm(q); % body-to-inertial DCM; we'll implement below consistent with scalar-first
F_gravity_body = Cbi' * F_gravity_inertial; % map inertial to body

F_total_body = F_thrust + F_aero_body + F_gravity_body;

% Translational dynamics (body accelerations)
acc_body = F_total_body / m - cross(omega, Vb);

% Quaternion kinematics (scalar-first q = [q0; qv])
q0 = q(1); qv = q(2:4);
Omega = [0, -omega'; omega, -skew(omega)]; % 4x4 quaternion rate matrix (scalar-first)
qdot = 0.5 * Omega * q;

% Rotational dynamics
M_body = inputs.moments_body;
omega_dot = invI * (M_body - cross(omega, I*omega));

% Position derivative (inertial) from body velocity: rdot_I = Cbi * Vb
rdotI = Cbi * Vb;

% Pack xdot
xdot = zeros(13,1);
xdot(1:3) = rdotI;
xdot(4:6) = acc_body;
xdot(7:10) = qdot;
xdot(11:13) = omega_dot;

% Renormalize quaternion to mitigate integration drift
qnew = xdot(7:10); % note: xdot holds qdot; integrate step may produce non-unit q
% We renormalize the state derivative to push quaternion back toward unit length by
% scaling qdot such that instantaneous derivative is consistent with unit constraint.
% Simpler approach: after computing qdot, ensure q remains unit by projecting q+qdot*dt —
% but since we lack dt here, enforce qdot orthogonal to q: q^T qdot = 0
q = q / norm(q + 0); % ensure current q normalized
qdot = qdot - (q' * qdot) * q; % remove component along q
xdot(7:10) = qdot;

end

% --- helper functions ---
function S = skew(v)
S = [  0   -v(3)  v(2);
      v(3)   0   -v(1);
     -v(2) v(1)    0 ];
end

function C = quat2dcm(q)
% Quaternion (scalar-first) to body-to-inertial DCM (C_bi)
q0 = q(1); q1 = q(2); q2 = q(3); q3 = q(4);
C = [
    q0^2+q1^2-q2^2-q3^2,   2*(q1*q2+q0*q3),     2*(q1*q3-q0*q2);
    2*(q1*q2-q0*q3),       q0^2-q1^2+q2^2-q3^2, 2*(q2*q3+q0*q1);
    2*(q1*q3+q0*q2),       2*(q2*q3-q0*q1),     q0^2-q1^2-q2^2+q3^2
];
end

Note: the quaternion renormalization step above removes the component of qdot along q so q stays unit-length to first order. You may optionally normalize the quaternion state after each integrator step for stronger enforcement.

ode45 example (in a separate script)

% initial state
x0 = zeros(13,1);
% initial position
x0(1:3) = [0;0;0];
% initial body velocities [u;v;w]
x0(4:6) = [300;0;0]; % 300 m/s forward
% initial quaternion (level, scalar-first)
x0(7:10) = [1;0;0;0];
% initial angular rates
x0(11:13) = [0;0;0];

% simple constant thrust and zero moments
inputs.thrust_body = [0;0;0];
inputs.moments_body = [0;0;0];

% call ode45
tspan = [0 60];
opts = odeset('RelTol',1e-6,'AbsTol',1e-8);
[t,x] = ode45(@(t,x) missile6dof(t,x,inputs,[]), tspan, x0, opts);

% simple plot of altitude (inertial z)
figure; plot(t, x(:,3)); xlabel('t (s)'); ylabel('z_I (m)'); title('Altitude');

If you'd like, I can now:

Explain any code block step-by-step (brief).
Modify the aero model to include simple stability derivatives for moments.
Provide a post-step quaternion normalization example (after each integrator step).

Learning: 6DOF Equations