Drone Sim V2

Devlog #5
I moved from fixed-duration legs to distance-based dynamic timing, replacing the flat 5-second-per-leg assumption from Devlog #4 with per-leg durations computed from waypoint spacing and a target cruise speed.

I added a speed constant (2 m/s) and used it to compute t_segment as an array instead of a scalar. Each leg’s duration is now distance / speed between its start and end waypoints, so longer legs take proportionally longer instead of every leg getting the same fixed window.
I also updated the interior-waypoint velocity estimate to divide by the sum of the adjacent leg durations (t_segment(i-1) + t_segment(i)) instead of 2 * t_segment, since that shortcut only made sense when every leg was the same length.
I dropped mass to 0.5 kg to better match a real small-quadrotor class, and lowered Kp/Kd as a starting point since neither the old gains nor the old fixed timing apply anymore.

Bugs
I didn’t hit any major bugs during this section.

Where it stands now
Leg duration now reflects actual waypoint spacing instead of an arbitrary constant, which should reduce the unnatural sameness of a drone crawling across a short hop and sprinting across a long one in the same 5 seconds.

Next up
Re-tune Kp/Kd/Kr/Kw against the new mass and dynamic timing before drawing conclusions from the plots. I will also look into MATLAB’s UAV Toolbox with a built-in quadrotor animation/scene viewer that can consume my state trajectory directly.

Devlog #4
I moved from simple point-to-point waypoint chasing to smooth polynomial trajectory generation between waypoints, replacing the threshold-based switching from Devlog #3 with time-based tracking of a continuously moving target.

New machinery
I built cubicTrajectory.m, which solves a 4x4 system of equations for the coefficients of a cubic polynomial connecting a start point to an end point over a fixed duration, with velocity constrained to zero at both ends of each leg.
I also built evalCubic.m, which evaluates that polynomial’s position and velocity at any given time.
I tested both standalone before touching the main dynamics. I stored all the coefficients in one array, coeffs_all, indexed by [coefficient, axis, leg], and built a loop that chains legs together so each leg starts exactly where the previous one ended.

Bugs

1. The very first version of the leg/local-time lookup accidentally dropped global time from the local-time calculation entirely, leaving the “current time within this leg” frozen at a constant for the whole leg instead of counting up. This meant the drone’s target wasn’t moving during a leg at all, it only jumped once per leg, which produced a single sharp overshoot spike instead of a tracked path.
2. The simulation’s total time span hadn’t been updated to match the total trajectory duration, cutting the run short by a full leg.

Where it stands now
With both fixed, the drone tracks a moving position and velocity target through all four waypoints instead of chasing a static point. Spin rate and attitude plots show a clean repeating pattern, a spike at each leg transition that damps out before the next one, rather than instability or runaway spiraling.

Next up
Because every leg currently forces velocity to zero at both ends, the drone still fully stops at every intermediate waypoint before accelerating into the next leg, which is what’s producing the repeating spikes rather than a truly smooth flight. Next is allowing interior waypoints to have nonzero passthrough velocity (estimated from neighboring waypoints), plus adding acceleration feedforward into the force calculation so the controller anticipates the trajectory instead of only reacting to error. After that, the plan is still to move toward true minimum-snap trajectories.

Devlog #3
I replaced the attitude lag from Devlog #2 with real rotational dynamics such as torque, inertia, and actual angular acceleration. Previously, I was assuming that attitude eventually catches up.

New states and new physics
The state vector went from 9 to 12: added p, q, r, the drone’s spin rate around its own x, y, and z axes. These are the rotational equivalent of vx, vy, vz, just measuring how fast orientation is changing instead of how fast position is changing.
I added an inertia matrix, a diagonal 3x3 of made-up placeholder numbers representing how hard it is to spin the drone around each axis, similar to mass.
The output is torque instead of thrust, and it feeds into torque divided by inertia to produce angular acceleration.

Bugs

1. The desired-attitude error vector and the spin-rate vector were stacked in different orders (theta/phi/psi vs. p/q/r, which is roll/pitch/yaw), so the controller was correcting the wrong axis against the wrong spin rate.
2. The drone would approach a waypoint and then spiral into a slowly tightening orbit around it instead of settling, and in one attempt, diverged outward entirely. It reproduced the same spiral with a single fixed target and no waypoint switching at all. I found that the actual fix wasn’t more damping. Increasing the damping gain alone made it worse, not better. The real issue was that the attitude loop needed to react faster overall to keep pace with how aggressively the position loop was demanding tilt changes. Increasing the correction gain substantially fixed it for now.

Where it stands now
With gains rebalanced, the drone flies a full waypoint sequence with real torque-driven attitude dynamics. Spin rates spike on each maneuver and cleanly damp back to zero. One nice side effect of finally having real dynamics is that the path through a waypoint now visibly curves rather than shooting straight there, because aggressive tilting to accelerate sideways steals from the vertical thrust component.

Next up
Everything from here is trajectory generation such as replacing simple point-to-point waypoint chasing with differential-flatness-based polynomial paths. The journey has been difficult so far, especially as this is my first time interacting with vectors and matrices (3b1b’s linear algebra series is the only thing helping me out). Nevertheless, I am persevered to get a finished project.

Devlog #2

I added real orientation states (φ, θ, ψ) and made thrust act along a single body axis instead of the free-vector cheat I’d been using. This is the point where the sim actually starts being a quadrotor.

Building the rotation matrix
Before touching the dynamics at all, I built a rotation matrix that converts “straight up, from the drone’s own perspective” into “which way that is, in world coordinates.” A quadrotor only ever pushes along its own up-direction, so the only way it moves sideways is by tilting, which redirects that one push.

I built the matrix as three separate small rotations (yaw, roll, pitch) multiplied together, one for each axis, in a fixed order. I tested it with a small function, bodyZAxis(phi, theta, psi), against three hand-checks:

Level (phi=theta=psi=0) → straight up, unchanged.
Small roll → a small tilt in y, x untouched.
Small pitch → a small tilt in x, y untouched.

All three passed, which confirmed the rotation math was doing what it was supposed to before wiring it into anything else.

Wiring it into droneDynamics

The state vector went from 6 to 9: position, velocity, and now phi/theta/psi. The control loop has split into:
Outer loop (same PID idea as before) computes a desired force vector, F_des, from position/velocity error plus a gravity feedforward term.

That desired force gets converted into a desired tilt angle, using a small-angle approximation (the sideways component of a tilted thrust vector is roughly thrust * angle, solved backwards for angle given how much sideways force we want). Clamped to ±0.5 rad so a big position error can’t force a nonsensical tilt.

The actual attitude states lag toward that desired tilt with a simple first-order lag (same idea as the motor lag in my old Python project, just applied to orientation instead of thrust).

Actual thrust direction is computed from the real (possibly still-lagging) attitude, not the desired one.

Bugs, in the order I found them

1. Sign error on the gravity feedforward in F_des, which had gravity pulling the “cancel gravity” term the wrong way.

2. Sign error on the final acceleration line, where I used the wrong sign on gravity there too, which sent the drone climbing to hundreds of meters instead of hovering.

3. A big position error was asking for tilts of multiple radians (past 90°), which is physically meaningless and was part of what caused the runaway climb. Fixed with the clamp mentioned above.

Where it stands now
With the signs fixed and the tilt clamped, the drone tracks through the same waypoint list as before, but now via real (if simplified) attitude dynamics. I can see roll and pitch spike up during each leg of the trip and settle back down as it arrives at a waypoint, instead of the instant, direction-agnostic thrust from the old model.

Next up
This 9-state model is still leaning on a simplified attitude lag instead of real rotational physics, with no torques, no moment of inertia, no angular velocity states. The next real step is adding those (a 12-state model), replacing the lag with actual torque-driven rotation. After that, there is differential flatness and real trajectory generation, which is the actual point of this whole project

Devlog #1

I’ve built a drone sim before, such as a Python project with Plotly animations, PD control, motor lag, drag, the works. It looked good, but it had a problem where the “drone” was really just a point that could apply force in any direction, instantly. Real quadrotors can’t do that, because they only push one way (straight up from their own frame) and have to physically rotate to redirect that push.

I’m now pushing myself to build a research-style quadrotor simulator in MATLAB.

The plan:
Start with a 9-state model: position, velocity, and orientation (roll/pitch/yaw).
Verify a hover with a basic PID controller first.
Later, move to trajectory planning using differential flatness (generating smooth polynomial paths and computing the forces needed to fly them)
Eventually stress-test trajectories by speeding them up / slowing them down and watching what that does to the required forces.

In this devlog, I covered:
Milestone 1: 1D hover
Before touching 3D, I built the simplest possible version, a single point that can only move up and down, controlled by:
thrust = massgravity + Kperror - Kd*velocity

Using MATLAB’s ode45 to integrate [z; vz] forward in time. I learned what an ODE solver does.

Milestone 2: 3D
Next step: extend to a full 3D position [x,y,z,vx,vy,vz], but still letting the controller output force in any direction. This is the same “free vector” simplification my old Python sim used, kept on purpose this time as an isolated stepping stone. The goal was to prove the PID math generalizes to 3D before adding the complexity of real attitude dynamics on top.

In my first test, the drone was pointed at a single fixed target. It gave a perfectly straight line, and it took me a second to realize that wasn’t a bug: since x, y, and z all use identical gains and start at rest, they move in lockstep, always in the same ratio to the target.

Milestone 3: chasing waypoints
I added a list of waypoints instead of one static target, with the controller switching to the next point once it gets within a 0.5 m of the current one. This needed a persistent variable inside the dynamics function to track which waypoint index it’s on, since ode45 calls that function many times per step and nothing about waypoint progress lives in the physical state vector itself.

This gave path that actually bends at each waypoint instead of a straight shot. The corners are sharp however, instead of being smooth.

Next up
I’m going to be adding adding real orientation states (φ, θ, ψ) and constraining thrust to act along a single body axis, the way an actual quadrotor works. That’s the point where this stops being a point-mass simulation and starts being a quadrotor simulation.