drone Code :‬

function drone_sim()‬
‭
% === Load and Prepare STL Model ===‬
‭
[faces, vertices] = load_stl(‬
‭ 'drone_frame.stl'‬
‭ );‬
‭
% Center and scale‬
‭
vertices = vertices - mean(vertices, 1);‬ ‭
‭ % center‬
scale = 0.01;‬
‭
vertices = vertices * scale;‬
‭
% === Simulation Parameters ===‬
‭
dt = 0.003;‬
‭
total_time = 12;‬
‭
steps = round(total_time / dt);‬
‭
px = 0; py = 0; pz = 0;‬
‭
flying_up = true; moving_forward = false; landing = false;‬
‭
% === Preallocate trajectory ===‬
‭
traj_x = zeros(1, steps);‬
‭
traj_y = zeros(1, steps);‬
‭
traj_z = zeros(1, steps);‬
‭
% === Create Figure ===‬
‭
figure(‬
‭ 'Color'‬
‭ ,‬
‭ 'w'‬
‭ );‬
‭
axis‬‭
‭ equal‬ ; grid‬‭
‭ on‬
; hold‬‭
‭ on‬
;‬
‭
xlabel(‬
‭ 'X'‬
‭ ); ylabel(‬
‭ 'Y'‬
‭ ); zlabel(‬
‭ 'Z'‬
‭ );‬
‭
view(3);‬
‭
title(‬
‭ '3D Drone Flight Simulation'‬
‭ );‬
‭
xlim([-2 18]); ylim([-5 10]); zlim([-1 8]);‬
‭
% Draw ground‬
‭
fill3([-20 20 20 -20], [-20 -20 20 20], [-0.01 -0.01 -0.01 -0.01], [0.8 0.8 0.8]);‬
‭
% Create drone model‬
‭
drone_model = patch(‬
‭ 'Faces'‬
‭ , faces,‬‭
‭ 'Vertices'‬
,‬‭
‭ vertices, ...‬
'FaceColor'‬
‭ , [0.2 0.5 0.9],‬‭
‭ 'EdgeColor'‬,‬‭
‭ 'none'‬
, ...‬
‭
'FaceLighting'‬
‭ ,‬‭
‭ 'gouraud'‬
);‬
‭
camlight(‬
‭ 'headlight'‬
‭ );‬
‭
lighting‬‭
‭ gouraud‬ ;‬
‭
material‬‭
‭ dull‬ ;‬
‭
% Create trajectory plot‬
‭
trajectory = plot3(0, 0, 0,‬‭
‭ 'r.-'‬,‬‭
‭ 'LineWidth'‬
,‬‭
‭ 1.5);‬
% === Animation Loop ===‬
‭
for i = 1:steps‬
‭
t = i * dt;‬
‭
% Movement Phases‬
‭
if flying_up‬
‭
pz = pz + 1;‬
‭
if pz >= 5‬
‭
flying_up = false;‬
‭
moving_forward = true;‬
‭
end‬
‭
elseif moving_forward‬
‭
px = px + 1;‬
‭
if px >= 10‬
‭
moving_forward = false;‬
‭
landing = true;‬
‭
end‬
‭
elseif landing‬
‭
pz = pz - 1;‬
‭
if pz <= 0‬
‭
pz = 0;‬
‭
landing = false;‬
‭
end‬
‭
end‬
‭
% Update drone model‬
‭
new_vertices = vertices + [px, py, pz];‬
‭
set(drone_model,‬‭
‭ 'Vertices'‬, new_vertices);‬
‭
% Update trajectory data‬
‭
traj_x(i) = px;‬
‭
traj_y(i) = py;‬
‭
traj_z(i) = pz;‬
‭
set(trajectory,‬‭
‭ 'XData'‬, traj_x(1:i),‬‭
‭ 'YData'‬,‬‭
‭ traj_y(1:i),‬‭
'ZData'‬
, traj_z(1:i));‬
‭
drawnow;‬
‭
pause(dt);‬ ‭
‭ % Control speed‬
end‬
‭
‭nd‬
e
% === STL Loader Functions ===‬
‭
function [faces, vertices] = load_stl(filename)‬
‭
fid = fopen(filename,‬‭
‭ 'r'‬);‬
‭
if fid == -1‬
‭
error(‬
‭ 'Cannot open STL file.'‬
‭ );‬
‭
end‬
‭
first_line = fgetl(fid);‬
‭
frewind(fid);‬
‭
if contains(lower(first_line),‬‭
‭ 'solid'‬ )‬
‭
fclose(fid);‬
‭
[faces, vertices] = read_ascii_stl(filename);‬
‭
else‬
‭
fclose(fid);‬
‭
[faces, vertices] = read_binary_stl(filename);‬
‭
end‬
‭
end‬
‭
function [faces, vertices] = read_ascii_stl(filename)‬
‭
fid = fopen(filename,‬‭
‭ 'r'‬);‬
‭
vertices = zeros(100000, 3);‬ ‭
‭ % preallocate large‬‭ buffer‬
faces = zeros(100000, 3);‬
‭
vcount = 0; fcount = 0;‬
‭
while ~feof(fid)‬
‭
tline = fgetl(fid);‬
‭
if contains(tline,‬‭
‭ 'vertex'‬ )‬
‭
vcount = vcount + 1;‬
‭
vert = sscanf(tline,‬‭
‭ ' vertex %f %f %f'‬ );‬
‭
vertices(vcount, :) = vert';‬
‭
elseif contains(tline,‬‭
‭ 'endfacet'‬)‬
‭
fcount = fcount + 1;‬
‭
faces(fcount, :) = [vcount - 2, vcount - 1, vcount];‬
‭
end‬
‭
end‬
‭
fclose(fid);‬
‭
vertices = vertices(1:vcount, :);‬
‭
faces = faces(1:fcount, :);‬
‭
end‬
‭
function [faces, vertices] = read_binary_stl(filename)‬
‭
fid = fopen(filename,‬‭
‭ 'r'‬);‬
‭
fread(fid, 80,‬‭
‭ 'uint8'‬
);‬
‭
numFaces = fread(fid, 1,‬‭
‭ 'uint32'‬ );‬
‭
faces = zeros(numFaces, 3);‬
‭
vertices = zeros(numFaces * 3, 3);‬
‭
for i = 1:numFaces‬
‭
fread(fid, 3,‬‭
‭ 'float'‬);‬
‭
vertices(3*i-2, :) = fread(fid, 3,‬‭
‭ 'float'‬
)';‬
‭
vertices(3*i-1, :) = fread(fid, 3,‬‭
‭ 'float'‬
)';‬
‭
vertices(3*i, :)
‭ = fread(fid, 3,‬‭ 'float'‬
)';‬
‭
fread(fid, 1,‬‭
‭ 'uint16'‬ );‬
‭
faces(i, :) = [3*i-2, 3*i-1, 3*i];‬
‭
end‬
‭
fclose(fid);‬
‭
[vertices, ~, idx] = unique(vertices,‬‭
‭ 'rows'‬);‬
‭
faces = idx(faces);‬
‭
end‬
‭
Drone with Quaternion :‬
‭

function drone_sim_quat_stl()‬
‭
% --- Load drone STL model ---‬
‭
[faces, vertices] = load_stl(‬
‭ 'drone_frame.stl'‬
‭ );‬
‭
% Center and scale the model‬
‭
vertices = vertices - mean(vertices, 1);‬ ‭
‭ % center‬‭ around origin‬
scale = 0.01;‬
‭ % adjust‬‭
‭ scale to fit plot nicely‬
vertices = vertices * scale;‬
‭
% --- Initialize state ---‬
‭
pos = [0; 0; 0];‬
‭ % Position‬
‭
q = [1; 0; 0; 0];‬
‭ % Quaternion (w,x,y,z)‬
‭
dt = 0.3;‬
‭
total_time = 12;‬
‭
steps = round(total_time / dt);‬
‭
% --- Setup figure ---‬
‭
figure(‬
‭ 'Color'‬
‭ ,‬
‭ 'w'‬
‭ );‬
‭
axis‬‭
‭ equal‬ ; grid‬‭
‭ on‬
; hold‬‭
‭ on‬;‬
‭
xlabel(‬
‭ 'X'‬
‭ ); ylabel(‬
‭ 'Y'‬
‭ ); zlabel(‬
‭ 'Z'‬
‭ );‬
‭
xlim([-2 12]); ylim([-3 3]); zlim([-1 6]);‬
‭
view(3);‬
‭
title(‬
‭ 'Drone Simulation with Quaternion Rotation'‬
‭ );‬
‭
% --- Draw ground ---‬
‭
fill3([-10 10 10 -10], [-10 -10 10 10], [-0.01 -0.01 -0.01 -0.01], [0.8 0.8 0.8]);‬
‭
% --- Create patch for drone ---‬
‭
drone_patch = patch(‬
‭ 'Faces'‬
‭ , faces,‬‭
‭ 'Vertices'‬,‬‭
‭ vertices, ...‬
'FaceColor'‬
‭ , [0 0.447 0.741],‬‭
‭ 'EdgeColor'‬
,‬‭
‭ 'k'‬, ...‬
‭
'FaceLighting'‬
‭ ,‬‭
‭ 'gouraud'‬,‬‭
‭ 'AmbientStrength'‬ ,0.3);‬
‭
camlight(‬
‭ 'headlight'‬
‭ );‬
‭
lighting‬‭
‭ gouraud‬ ;‬
‭
material‬‭
‭ dull‬ ;‬
‭
% --- Trajectory plot ---‬
‭
traj_x = zeros(1, steps);‬
‭
traj_y = zeros(1, steps);‬
‭
traj_z = zeros(1, steps);‬
‭
traj_plot = plot3(0,0,0,‬
‭ 'r.-'‬
‭ ,‬‭
‭ 'LineWidth'‬, 1.5);‬
‭
% --- Movement flags ---‬
‭
flying_up = true; moving_forward = false; landing = false;‬
‭
for i = 1:steps‬
‭
t = i*dt;‬
‭
% --- Update position & angular velocity ---‬
‭
if flying_up‬
‭
pos(3) = pos(3) + 1 ;‬
‭ % Move up‬
‭
omega = [0.2; 0; 0];‬
‭ % Pitch‬‭
‭ rotation (rad/s)‬
if pos(3) >= 4‬
‭
flying_up = false;‬
‭
moving_forward = true;‬
‭
end‬
‭
elseif moving_forward‬
‭
pos(1) = pos(1) + 1 ;‬
‭ % Move forward‬
‭
omega = [0; 0; 0.3];‬
‭ % Yaw rotation‬‭
‭ (rad/s)‬
if pos(1) >= 8‬
‭
moving_forward = false;‬
‭
landing = true;‬
‭
end‬
‭
elseif landing‬
‭
pos(3) = pos(3) - 1 ;‬
‭ % Move down‬
‭
omega = [-0.2; 0; 0];‬
‭ % Reverse‬‭
‭ pitch rotation‬
if pos(3) <= 0‬
‭
pos(3) = 0;‬
‭
landing = false;‬
‭
omega = [0;0;0];‬
‭
end‬
‭
else‬
‭
omega = [0;0;0];‬
‭
end‬
‭
% --- Quaternion update ---‬
‭
Omega_mat = [ 0
‭ -omega(1) -omega(2) -omega(3);‬
omega(1) 0
‭ omega(3) -omega(2);‬
omega(2) -omega(3) 0
‭ omega(1);‬
omega(3) omega(2) -omega(1) 0
‭ ];‬
 ‭_dot = 0.8 * Omega_mat * q;‬
q
q = q + q_dot * dt;‬
‭
q = q / norm(q);‬‭
‭ % Normalize quaternion‬
% --- Convert quaternion to rotation matrix‬‭
‭ ---‬
w = q(1); x = q(2); y = q(3); z = q(4);‬
‭
R = [1 - 2*(y^2 + z^2),
‭ 2*(x*y - z*w), 2*(x*z + y*w);‬
2*(x*y + z*w),
‭ 1 - 2*(x^2 + z^2), 2*(y*z - x*w);‬
2*(x*z - y*w),
‭ 2*(y*z + x*w), 1 - 2*(x^2 + y^2)];‬
% --- Rotate and translate vertices ---‬
‭
rotated_vertices = (R * vertices')';‬
‭
transformed_vertices = rotated_vertices + pos';‬
‭
% --- Update patch ---‬
‭
set(drone_patch,‬‭
‭ 'Vertices'‬ , transformed_vertices);‬
‭
% --- Update trajectory ---‬
‭
traj_x(i) = pos(1);‬
‭
traj_y(i) = pos(2);‬
‭
traj_z(i) = pos(3);‬
‭
set(traj_plot,‬‭
‭ 'XData'‬ , traj_x(1:i),‬‭
‭ 'YData'‬,‬‭
‭ traj_y(1:i),‬‭
'ZData'‬
, traj_z(1:i));‬
‭
drawnow;‬
‭
pause(dt);‬
‭
end‬
‭
end‬
‭
% --- Helper function to load STL robustly ---‬
‭
function [faces, vertices] = load_stl(filename)‬
‭
fid = fopen(filename,‬‭
‭ 'r'‬);‬
‭
if fid == -1‬
‭
error(‬
‭ 'Cannot open STL file.'‬
‭ );‬
‭
end‬
‭
first_line = fgetl(fid);‬
‭
frewind(fid);‬
‭
if contains(lower(first_line),‬‭
‭ 'solid'‬ )‬
‭
fclose(fid);‬
‭
[faces, vertices] = read_ascii_stl(filename);‬
‭
else‬
‭
fclose(fid);‬
‭
[faces, vertices] = read_binary_stl(filename);‬
‭
end‬
‭
end‬
‭
function [faces, vertices] = read_ascii_stl(filename)‬
‭
fid = fopen(filename,‬‭
‭ 'r'‬);‬
‭
vertices = zeros(100000, 3);‬ ‭
‭ % preallocate large‬‭ buffer‬
faces = zeros(100000, 3);‬
‭
vcount = 0; fcount = 0;‬
‭
while ~feof(fid)‬
‭
tline = fgetl(fid);‬
‭
if contains(tline,‬‭
‭ 'vertex'‬ )‬
‭
vcount = vcount + 1;‬
‭
vert = sscanf(tline,‬‭
‭ ' vertex %f %f %f'‬ );‬
‭
vertices(vcount, :) = vert';‬
‭
elseif contains(tline,‬‭
‭ 'endfacet'‬)‬
‭
fcount = fcount + 1;‬
‭
faces(fcount, :) = [vcount - 2, vcount - 1, vcount];‬
‭
end‬
‭
end‬
‭
fclose(fid);‬
‭
vertices = vertices(1:vcount, :);‬
‭
faces = faces(1:fcount, :);‬
‭
end‬
‭
function [faces, vertices] = read_binary_stl(filename)‬
‭
fid = fopen(filename,‬‭
‭ 'r'‬);‬
‭
fread(fid, 80,‬‭
‭ 'uint8'‬
);‬
‭
numFaces = fread(fid, 1,‬‭
‭ 'uint32'‬ );‬
‭
faces = zeros(numFaces, 3);‬
‭
vertices = zeros(numFaces * 3, 3);‬
‭
for i = 1:numFaces‬
‭
fread(fid, 3,‬‭
‭ 'float'‬);‬
‭
vertices(3*i-2, :) = fread(fid, 3,‬‭
‭ 'float'‬
)';‬
‭
vertices(3*i-1, :) = fread(fid, 3,‬‭
‭ 'float'‬
)';‬
‭
vertices(3*i, :)
‭ = fread(fid, 3,‬‭ 'float'‬
)';‬
‭
fread(fid, 1,‬‭
‭ 'uint16'‬ );‬
‭
faces(i, :) = [3*i-2, 3*i-1, 3*i];‬
‭
 ‭nd‬
e
fclose(fid);‬
‭
[vertices, ~, idx] = unique(vertices,‬‭
‭ 'rows'‬
);‬
‭
faces = idx(faces);‬
‭
end‬
‭
function drone_sim_quat_stl_physics()‬
‭
% --- Load drone STL model or create simple model‬‭
‭ ---‬
try‬
‭
[faces, vertices] = load_stl(‬
‭ 'drone_frame.stl'‬
‭ );‬
‭
vertices = vertices - mean(vertices, 1);‬ ‭
‭ %‬‭
Center model‬
scale = 0.015;‬
‭ % Scaling factor‬
‭
vertices = vertices * scale;‬
‭
catch‬
‭
[faces, vertices] = create_simple_drone_model();‬‭
‭ % Fallback model‬
end‬
‭
% --- Physical parameters ---‬
‭
mass = 1.0;‬
‭ % kg‬
‭
gravity = 9.81;‬
‭ % m/s^2‬
‭
I = diag([0.02 0.02 0.04]);‬‭
‭ % inertia matrix (kg·m²)‬

‭ --- Initial states ---‬

%
pos = [0; 0; 0];‬
‭ ‭
% position (m)‬
vel = [0; 0; 0];‬
‭ %
‭ velocity (m/s)‬
q = [1; 0; 0; 0];‬
‭ %
‭ quaternion [w;x;y;z]‬
omega = [0; 0; 0];‬
‭ %
‭ angular velocity‬‭
(rad/s)‬

‭ --- Simulation parameters ---‬

%
dt = 0.05;‬
‭ % time step‬
‭
total_time = 12;‬
‭
steps = round(total_time / dt);‬
‭

‭ --- Visualization setup ---‬

%
figure(‬
‭ 'Color'‬
‭ ,‬
‭ 'w'‬
‭ ,‬‭
‭ 'Position'‬, [100 100 1200 800],‬‭
‭ 'Name'‬
,‬‭
‭ 'Drone Simulation'‬
);‬
‭
axis‬‭
‭ equal‬; grid‬‭
‭ on‬
; hold‬‭
‭ on‬
;‬
‭
xlabel(‬
‭ 'X (m)'‬
‭ ); ylabel(‬
‭ 'Y (m)'‬
‭ ); zlabel(‬
‭ 'Z (m)'‬
‭ );‬
‭
xlim([-5 15]); ylim([-5 15]); zlim([0 12]);‬
‭
view(30, 30);‬
‭
title(‬
‭ 'Drone Physics Simulation'‬
‭ );‬
‭

‭ Ground plane with grid‬

%
[X,Y] = meshgrid(-5:1:15, -5:1:15);‬
‭
surf(X, Y, zeros(size(X)),‬‭
‭ 'FaceColor'‬
, [0.8 0.8‬‭
‭ 0.8],‬‭
'EdgeColor'‬
, [0.6 0.6 0.6]);‬
‭

‭ Drone model‬
%
drone_patch = patch(‬
‭ 'Faces'‬
‭ , faces,‬‭
‭ 'Vertices'‬
,‬‭
‭ vertices, ...‬
'FaceColor'‬
‭ , [0 0.447 0.741],‬‭
‭ 'EdgeColor'‬
,‬‭
‭ 'k'‬
, ...‬
‭
'FaceLighting'‬
‭ ,‬‭
‭ 'gouraud'‬);‬
‭

‭ Reference markers‬
%
plot3(0, 0, 0,‬‭
‭ 'ro'‬
,‬‭
‭ 'MarkerSize'‬
, 10,‬‭
‭ 'MarkerFaceColor'‬
,‬‭
‭ 'r'‬
);‬
‭
plot3(10, 0, 2,‬‭
‭ 'go'‬,‬‭
‭ 'MarkerSize'‬
, 10);‬
‭

‭ Trajectory‬
%
traj = plot3(pos(1), pos(2), pos(3),‬‭
‭ 'r-'‬
,‬‭
‭ 'LineWidth'‬
,‬‭
‭ 2);‬

‭ --- Flight control parameters ---‬

%
hover_thrust = mass * gravity;‬
‭
Kp = 8;‬ ‭
‭ % Pitch control gain‬
Kd = 0.8;‬‭
‭ % Damping gain‬

‭ --- Main simulation loop ---‬

%
for i = 1:steps‬
‭
% --- Flight phase control ---‬
‭
if pos(3) < 2.0‬
‭
% Ascend phase‬
‭
thrust_body = [0; 0; hover_thrust + 2.5];‬
‭
desired_pitch = deg2rad(-5);‬
‭
elseif pos(1) < 8.0‬
‭
% Forward flight phase‬
‭
thrust_body = [2.0; 0; hover_thrust];‬
‭
desired_pitch = deg2rad(-15);‬
‭
elseif pos(3) > 0.1‬
‭
% Landing phase‬
‭
thrust_body = [0; 0; hover_thrust - 1.5];‬
‭
desired_pitch = 0;‬
‭
else‬
‭
 ‭ Landed‬
%
thrust_body = [0; 0; hover_thrust];‬
‭
desired_pitch = 0;‬
‭
end‬
‭

‭ --- Attitude control ---‬

%
% Current pitch from quaternion‬
‭
w = q(1); x = q(2); y = q(3); z = q(4);‬
‭
pitch = atan2(2*(w*x + y*z), 1 - 2*(x^2 + y^2));‬
‭

‭ PD control for pitch‬

%
torque = [Kp*(desired_pitch - pitch); 0; 0] - Kd*omega;‬
‭

‭ --- Physics integration ---‬

%
% Quaternion kinematics‬
‭
Omega_mat = [0, -omega(1), -omega(2), -omega(3);‬
‭
omega(1), 0, omega(3), -omega(2);‬
‭
omega(2), -omega(3), 0, omega(1);‬
‭
omega(3), omega(2), -omega(1), 0];‬
‭
q = q + 0.5*Omega_mat*q*dt;‬
‭
q = q/norm(q);‬‭
‭ % Normalize‬

‭ Angular dynamics (using more efficient backslash‬‭

% operator)‬
omega_dot = I \ (torque - cross(omega, I*omega));‬
‭
omega = omega + omega_dot*dt;‬
‭

‭ Linear dynamics‬
%
R = quat_to_rotm(q);‬
‭
acc = (R*thrust_body + [0; 0; -mass*gravity])/mass;‬
‭
vel = vel + acc*dt;‬
‭
pos = pos + vel*dt;‬
‭

‭ --- Position constraints ---‬

%
pos(1) = max(min(pos(1), 10), 0);‬
‭
pos(2) = max(min(pos(2), 3), -3);‬
‭
pos(3) = max(pos(3), 0);‬
‭

‭ --- Update visualization ---‬

%
set(drone_patch,‬‭
‭ 'Vertices'‬
, (R*vertices')'‬‭
‭ + pos');‬
traj.XData(i) = pos(1);‬
‭
traj.YData(i) = pos(2);‬
‭
traj.ZData(i) = pos(3);‬
‭

‭ Update title with status‬

%
if pos(3) < 0.1‬
‭
phase =‬‭
‭ 'Landed'‬;‬
‭
elseif pos(1) > 7.9‬
‭
phase =‬‭
‭ 'Landing'‬;‬
‭
elseif pos(3) > 1.9‬
‭
phase =‬‭
‭ 'Cruising'‬ ;‬
‭
else‬
‭
phase =‬‭
‭ 'Ascending'‬ ;‬
‭
end‬
‭
title(sprintf(‬
‭ '%s | Position: [%.1f, %.1f,‬‭
‭ %.1f]'‬
, phase, pos(1), pos(2), pos(3)));‬
‭

‭rawnow‬‭
d limitrate‬
;‬
‭
pause(0.01);‬
‭
end‬
‭
end‬
‭
% Helper function: quaternion to rotation matrix‬
‭
function R = quat_to_rotm(q)‬
‭
w = q(1); x = q(2); y = q(3); z = q(4);‬
‭
R = [1-2*(y^2+z^2), 2*(x*y-z*w), 2*(x*z+y*w);‬
‭
2*(x*y+z*w), 1-2*(x^2+z^2), 2*(y*z-x*w);‬
‭
2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x^2+y^2)];‬
‭
end‬
‭
% Simple drone model (fallback)‬
‭
function [faces, vertices] = create_simple_drone_model()‬
‭
vertices = [-1 -1 -0.2; 1 -1 -0.2; 1 1 -0.2; -1 1 -0.2;‬ %
‭ ‭ Base‬
-1 -1 0.2; 1 -1 0.2; 1 1 0.2; -1 1 0.2;‬
‭ % Top‬
‭
0 -2 0; 0 2 0; -2 0 0; 2 0 0];‬
‭ % Arms‬
‭
 faces = [1 2 3 4; 5 6 7 8;‬
‭ % Top and bottom‬
‭
1 2 6 5; 2 3 7 6;‬
‭ % Sides‬
‭
3 4 8 7; 4 1 5 8;‬
‭ % More sides‬
‭
1 9 5 NaN; 3 10 7 NaN;‬ ‭
‭ % Arms‬
4 11 8 NaN; 2 12 6 NaN];‬
‭
end‬
‭
function [faces, vertices] = load_stl(filename)‬
‭
fid = fopen(filename,‬‭
‭ 'r'‬);‬
‭
if fid == -1‬
‭
error(‬
‭ 'Cannot open STL file.'‬
‭ );‬
‭
end‬
‭
first_line = fgetl(fid);‬
‭
frewind(fid);‬
‭
if contains(lower(first_line),‬‭
‭ 'solid'‬)‬
‭
fclose(fid);‬
‭
[faces, vertices] = read_ascii_stl(filename);‬
‭
else‬
‭
fclose(fid);‬
‭
[faces, vertices] = read_binary_stl(filename);‬
‭
end‬
‭
end‬
‭
function [faces, vertices] = read_ascii_stl(filename)‬
‭
fid = fopen(filename,‬‭
‭ 'r'‬);‬
‭
vertices = zeros(100000, 3);‬ ‭
‭ % preallocate large‬‭ buffer‬
faces = zeros(100000, 3);‬
‭
vcount = 0; fcount = 0;‬
‭
while ~feof(fid)‬
‭
tline = fgetl(fid);‬
‭
if contains(tline,‬‭
‭ 'vertex'‬ )‬
‭
vcount = vcount + 1;‬
‭
vert = sscanf(tline,‬‭
‭ ' vertex %f %f %f'‬ );‬
‭
vertices(vcount, :) = vert';‬
‭
elseif contains(tline,‬‭
‭ 'endfacet'‬)‬
‭
fcount = fcount + 1;‬
‭
faces(fcount, :) = [vcount - 2, vcount - 1, vcount];‬
‭
end‬
‭
end‬
‭
fclose(fid);‬
‭
vertices = vertices(1:vcount, :);‬
‭
faces = faces(1:fcount, :);‬
‭
end‬
‭
function [faces, vertices] = read_binary_stl(filename)‬
‭
fid = fopen(filename,‬‭
‭ 'r'‬);‬
‭
fread(fid, 80,‬‭
‭ 'uint8'‬
);‬
‭
numFaces = fread(fid, 1,‬‭
‭ 'uint32'‬);‬
‭
faces = zeros(numFaces, 3);‬
‭
vertices = zeros(numFaces * 3, 3);‬
‭
for i = 1:numFaces‬
‭
fread(fid, 3,‬‭
‭ 'float'‬);‬
‭
vertices(3*i-2, :) = fread(fid, 3,‬‭
‭ 'float'‬
)';‬
‭
vertices(3*i-1, :) = fread(fid, 3,‬‭
‭ 'float'‬
)';‬
‭
vertices(3*i, :)
‭ = fread(fid, 3,‬‭ 'float'‬
)';‬
‭
fread(fid, 1,‬‭
‭ 'uint16'‬);‬
‭
faces(i, :) = [3*i-2, 3*i-1, 3*i];‬
‭
end‬
‭
fclose(fid);‬
‭
[vertices, ~, idx] = unique(vertices,‬‭
‭ 'rows'‬);‬
‭
faces = idx(faces);‬
‭
end‬
‭
Concept Overview‬
‭
‭ drone (also called a quadcopter) is a flying machine that moves by adjusting the speed and angle‬
A
of its rotors. This simulation:‬
‭

1.‬ ‭
‭ Loads a 3D drone model‬‭
(from file or fallback).‬

2.‬ ‭
‭ Simulates flight using physics laws‬‭
(like gravity, torque, and acceleration).‬

3.‬ ‭
‭ Controls drone movement‬‭
during takeoff, forward flight, and landing.‬

4.‬ ‭
‭ Animates‬‭
the drone’s path in a 3D environment.‬

🔍
‭ Step-by-Step Code Explanation‬
1.
‭ 🔧 Load or Create Drone Model‬
‭atlab‬
m
CopyEdit‬
‭
try‬
‭
[faces, vertices] = load_stl('drone_frame.stl');‬
‭
vertices = vertices - mean(vertices, 1);
‭ % Center the model‬
scale = 0.015;‬
‭
vertices = vertices * scale;‬
‭
catch‬
‭
[faces, vertices] = create_simple_drone_model();‬
‭
end‬
‭

‭●‬ Purpose‬
‭ .stl‬‭
: Load the 3D shape of a drone from an‬‭
‭ file.‬

‭●‬ vertices‬
‭ : Coordinates of the model points.‬
‭

‭●‬ faces‬
‭ : Define how the vertices are connected (triangles).‬
‭

‭●‬ If the file fails to load, a‬‭

‭ simple backup drone model‬‭
is created.‬

2.
‭ ⚙️ Physical Parameters‬
‭atlab‬
m
CopyEdit‬
‭
mass = 1.0;‬
‭
gravity = 9.81;‬
‭
I = diag([0.02 0.02 0.04]);‬
‭

‭●‬ mass‬
‭ : 1 kg (weight of the drone).‬
‭

‭●‬ gravity‬
‭ : Pulls drone down at 9.81 m/s².‬
‭

‭●‬ I‬
‭ : Inertia matrix—how hard it is to rotate the drone along each axis (X, Y, Z).‬
‭
3.
‭ 🚀 Initial State of the Drone‬
‭atlab‬
m
CopyEdit‬
‭
pos = [0; 0; 0];‬
‭
vel = [0; 0; 0];‬
‭
q = [1; 0; 0; 0];‬
‭
omega = [0; 0; 0];‬
‭

‭●‬ pos‬
‭ : Starting position (x, y, z) at origin.‬
‭

‭●‬ vel‬
‭ : Velocity is zero.‬
‭

‭●‬ q‬
‭ : Quaternion (rotation format instead of Euler angles).‬
‭

‭●‬ omega‬
‭ : No spinning motion at start.‬
‭

4.
‭ ⏱️ Simulation Settings‬
‭atlab‬
m
CopyEdit‬
‭
dt = 0.05;‬
‭
total_time = 12;‬
‭
steps = round(total_time / dt);‬
‭

‭●‬ dt‬
‭ : Small time step (like a frame in animation).‬
‭

‭●‬ steps‬
‭ : Total number of frames (240).‬
‭

5.
‭ 🖼️ Create 3D Scene‬
‭atlab‬
m
CopyEdit‬
‭
figure(...);‬
‭
axis equal; grid on; hold on;‬
‭
xlabel(...); ylabel(...); zlabel(...);‬
‭
...‬
‭
surf(X, Y, zeros(size(X)), ...)‬
‭

‭●‬ This builds a‬‭

‭ 3D world‬‭
with a grid floor.‬

‭●‬ Sets camera angle and axes limits.‬

‭

6.
‭ 📦 Draw Drone & Markers‬
‭atlab‬
m
CopyEdit‬
‭
drone_patch = patch(...);‬
‭
plot3(...);
‭ % Red dot at origin‬
plot3(...);
‭ % Green target point‬
traj = plot3(...);
‭ % Red line shows the drone path‬
 ‭●‬ patch‬
‭ : Draws the 3D drone model.‬
‭

‭●‬ Markers‬
‭ : Help see where the drone starts and moves.‬
‭

7.
‭ 🎮 Flight Control Settings‬
‭atlab‬
m
CopyEdit‬
‭
hover_thrust = mass * gravity;‬
‭
Kp = 8;‬
‭
Kd = 0.8;‬
‭

‭●‬ hover_thrust‬
‭ : The force needed to float in air.‬
‭

‭●‬ Kp‬
‭ Kd‬
,‬‭
‭ : Constants to control how the drone tilts (pitch).‬
‭

🔁
‭ Main Simulation Loop (Core)‬
This is the heart of the program. It runs once for each time step.‬
‭

🚦
‭ Flight Phases‬
‭atlab‬
m
CopyEdit‬
‭
if pos(3) < 2.0‬
‭
thrust_body = [0; 0; hover_thrust + 2.5];‬
‭
desired_pitch = deg2rad(-5);‬
‭
elseif pos(1) < 8.0‬
‭
thrust_body = [2.0; 0; hover_thrust];‬
‭
desired_pitch = deg2rad(-15);‬
‭
elseif pos(3) > 0.1‬
‭
thrust_body = [0; 0; hover_thrust - 1.5];‬
‭
desired_pitch = 0;‬
‭
else‬
‭
thrust_body = [0; 0; hover_thrust];‬
‭
desired_pitch = 0;‬
‭
end‬
‭

‭●‬ Takeoff‬
‭ : Goes up (extra upward thrust).‬
‭

‭●‬ Cruise‬
‭ : Tilts forward and moves forward.‬
‭

‭●‬ Landing‬
‭ : Reduces thrust.‬
‭

‭●‬ Landed‬
‭ : Stops thrust.‬
‭

🎯
‭ Attitude Control (How to Pitch)‬
matlab‬
‭
CopyEdit‬
‭
pitch = atan2(2*(w*x + y*z), 1 - 2*(x^2 + y^2));‬
‭
torque = [Kp*(desired_pitch - pitch); 0; 0] - Kd*omega;‬
‭

‭●‬ Reads the‬‭

‭ current tilt (pitch)‬‭
from the quaternion.‬

‭●‬ Uses‬‭
‭ PD Control‬‭
to adjust rotation to match the desired pitch.‬

📐
‭ Physics Integration‬

a) Quaternion Rotation (orientation update)‬

‭
‭atlab‬
m
CopyEdit‬
‭
Omega_mat = ...;‬
‭
q = q + 0.5*Omega_mat*q*dt;‬
‭
q = q / norm(q);‬
‭

‭●‬ Updates rotation based on angular speed.‬

‭

‭●‬ Ensures quaternion stays normalized (unit length).‬

‭

b) Angular Motion‬
‭
‭atlab‬
m
CopyEdit‬
‭
omega_dot = I \ (torque - cross(omega, I*omega));‬
‭
omega = omega + omega_dot*dt;‬
‭

‭●‬ Calculates how rotation changes over time.‬

‭

c) Linear Motion‬
‭
‭atlab‬
m
CopyEdit‬
‭
R = quat_to_rotm(q);‬
‭
acc = (R*thrust_body + [0; 0; -mass*gravity])/mass;‬
‭
vel = vel + acc*dt;‬
‭
pos = pos + vel*dt;‬
‭

‭●‬ ‭ R‬
Converts orientation to rotation matrix‬‭ .‬
‭

‭●‬ Applies forces and updates position.‬

‭

⛓️
‭ Position Limits (Boundaries)‬
‭atlab‬
m
CopyEdit‬
‭
pos(1) = max(min(pos(1), 10), 0);‬
‭
pos(2) = max(min(pos(2), 3), -3);‬
‭
pos(3) = max(pos(3), 0);‬
‭
 ‭●‬ Stops the drone from flying out of the visible area.‬
‭

🖼️
‭ Update Graphics‬
‭atlab‬
m
CopyEdit‬
‭
set(drone_patch, 'Vertices', (R*vertices')' + pos');‬
‭
traj.XData(i) = pos(1); ...‬
‭

‭●‬ Rotates and moves the drone model.‬

‭

‭●‬ Adds a red trail showing the path.‬

‭

🧭
‭ Flight Status Text‬
‭atlab‬
m
CopyEdit‬
‭
if pos(3) < 0.1‬
‭
phase = 'Landed';‬
‭
elseif ...‬
‭
title(sprintf('%s | Position: ...'));‬
‭

‭●‬ Displays whether the drone is taking off, cruising, landing, or landed.‬
‭

🧩
‭ Helper Functions‬
1. Quaternion → Rotation Matrix‬
‭
‭atlab‬
m
CopyEdit‬
‭
function R = quat_to_rotm(q)‬
‭

Converts drone rotation from quaternion to a matrix that can rotate 3D points.‬
‭

2. Simple Backup Drone Model‬

‭
‭atlab‬
m
CopyEdit‬
‭
function [faces, vertices] = create_simple_drone_model()‬
‭

Creates a basic cube-shaped drone if the STL file is missing.‬

‭

3. STL File Loaders‬

‭
 ‭●‬ load_stl()‬‭
‭ picks ASCII or Binary STL.‬

‭●‬ read_ascii_stl()‬‭
‭ parses human-readable STL.‬

‭●‬ read_binary_stl()‬‭
‭ parses binary STL format.‬

🧠
‭ Summary of What You Learn‬
‭●‬ Basics of‬‭
‭ drone physics‬‭
(thrust, torque, orientation).‬

‭●‬ How to use‬‭

‭ quaternions‬‭
to avoid rotation problems.‬

‭●‬ How to build a‬‭

‭ 3D animation‬‭
in MATLAB.‬

‭●‬ How to simulate and visualize‬‭

‭ real-world motion‬‭
step by step.‬

✅
‭ What Happens When You Run the Code‬
1.‬ ‭
‭ A window opens showing a gray grid (ground) and a 3D drone.‬

2.‬ ‭
‭ The drone‬‭
takes off‬
, then‬‭
‭ tilts forward and flies‬
, then‬‭
‭ lands‬
.‬
‭

3.‬ ‭
‭ A‬‭
red trail‬‭
marks its flight path.‬

4.‬ ‭
‭ The simulation updates in real-time with clear 3D visuals and status.‬