Building a C++20 quadrotor simulation with PID control and an attitude EKF

I built the quadrotor flight-control demo as a portfolio simulation of the inner control loops. It levels a simulated quad, changes altitude, and follows attitude commands using estimated sensor state. It has no hardware drivers and must not run on a real aircraft.

The implementation uses C++20, including the build target and compiler settings. The useful exercise here is following a command through estimation, control, motor allocation, and a simulated vehicle, then checking the pieces independently.

Keep the plant and controller separate

Public interfaces live under include/fc, implementations under src, and the command-line entry point is apps/sim_demo.cpp. FlightController owns the estimators, attitude controller, altitude controller, and mixer. QuadSimulator owns vehicle truth and generates sensor readings.

I use north-east-down coordinates for the world and forward-right-down for the body. A quaternion rotates body vectors into world coordinates. Altitude is positive up, so telemetry negates the world's vertical position. Writing those conventions down matters because a plausible-looking controller can still apply torque in the wrong direction.

There are two execution paths in src/scenario.cpp. runClosedLoop advances everything sequentially with a fixed simulation timestep. That is what the demo executable calls. runThreadedHover separately exercises sensor and control threads through two single-producer, single-consumer rings. The architecture supports that exchange, but the plotted demo does not depend on wall-clock scheduling.

Follow one control cycle

Estimate attitude before asking for torque

In src/flight_controller.cpp, both attitude estimators receive the same IMU sample. The complementary filter is a comparison trace; feedback control uses the EKF attitude and bias-corrected angular rate.

  ekf_.propagate(sample.gyro_rad_s, dt);
  ekf_.updateAccel(sample.accel_m_s2);
  if (sample.mag_valid) {
    ekf_.updateMag(sample.mag_body);
  }
  corrected_gyro_ = ekf_.correctedRate(sample.gyro_rad_s);

The EKF tracks quaternion attitude and gyro bias with a six-dimensional error covariance. Gyro propagation carries the estimate between accelerometer and magnetometer corrections. The complementary filter provides a simpler reference without replacing the EKF in the feedback path.

The vertical filter is separate. I rotate measured specific force into the world frame, convert its vertical component to upward acceleration, and subtract gravity. The altitude Kalman filter predicts altitude and vertical speed from that acceleration and corrects them with the barometer when available.

One important boundary is initialization. The closed-loop scenario supplies the simulator's initial attitude and marks the filters aligned. This run demonstrates an aligned handoff, not recovery from an arbitrary unknown orientation. Separate static bench tests exercise estimator convergence.

Reject accelerometer readings that do not fit the model

An accelerometer measures specific force. On this free-flying model, it sees thrust and drag rather than a direct gravity vector. Treating every sample as gravity would let translational acceleration corrupt tilt estimation.

This excerpt from src/attitude_ekf.cpp shows two gates:

  if (std::abs(accel_norm - config_.gravity_m_s2) > 0.55) {
    consistent_updates_ = 0;
    return false;
  }
  if (aligned_ && norm(last_omega_) > 0.35) {
    return false;
  }
  const Vec3 predicted = q_.rotateInverse(Vec3{0.0, 0.0, -config_.gravity_m_s2});

The following code also checks the residual against the predicted gravity direction. These checks limit when the model is trusted. They do not make the accelerometer a reliable tilt reference during every maneuver. When correction is gated, gyro propagation and magnetic information carry the estimate, and gyro bias becomes less observable.

Turn angle error into rate, then torque

src/controllers.cpp builds a target quaternion and asks for the shortest rotation error. Proportional angle gains turn that error into bounded angular-rate targets. Each axis then uses a rate PID to request torque.

That cascade keeps the roles readable: the outer loop asks how fast to turn, and the inner loop compares that request with measured angular rate. The altitude controller independently adds a PID correction to hover thrust, calculated from vehicle mass and gravity.

In src/pid.cpp, the derivative uses the measurement and can pass through a first-order filter. The output and integrator update are:

  const double unsaturated = gains_.kp * error + integrator_ - gains_.kd * derivative_;
  last_unsaturated_ = unsaturated;
  const double output = std::clamp(unsaturated, gains_.out_min, gains_.out_max);

  integrator_ += (gains_.ki * error + gains_.k_aw * (output - unsaturated)) * dt;

The back-calculation term uses the difference between clamped and requested PID output to unwind the integrator. This handles the PID's own limits. It does not receive a separate saturation error from the motor mixer, so I would not describe it as complete actuator-aware anti-windup.

Preserve collective thrust when torque cannot fit

The Quad-X allocation in src/mixer.cpp starts with equal collective thrust and applies signed torque contributions:

  out.newton[0] = quarter - roll + pitch - yaw;
  out.newton[1] = quarter - roll - pitch + yaw;
  out.newton[2] = quarter + roll - pitch - yaw;
  out.newton[3] = quarter + roll + pitch + yaw;

Motor order is front-right, rear-right, rear-left, front-left. The simulator calculates torque from the same physical layout, and a test reconstructs the resulting wrench to check the allocation.

If a requested combination exceeds motor limits, mix clamps total thrust to the available range and searches for a common torque scale with 24 bisection iterations. It preserves the feasible collective request while reducing torque. Clamping each motor independently would lose that relationship.

Build and inspect the simulation

The build instructions in README.md require CMake 3.25 or newer, Ninja, and a C++20 compiler. GoogleTest is downloaded during configuration. From the repository root:

cmake --preset debug
cmake --build --preset debug
ctest --preset debug --output-on-failure

Those commands are copied from README.md. The preset requirement is higher than the minimum declared in CMakeLists.txt, so I follow the README when using presets.

The README then runs the demo and plotting script:

./build/debug/fc_sim_demo --output telemetry.csv
python3 scripts/plot_telemetry.py telemetry.csv --output docs/closed_loop.png

The plot needs matplotlib, listed in scripts/requirements.txt. The executable defaults to 16 simulated seconds and enables seeded sensor noise. --no-noise removes the random noise while retaining the configured constant gyro bias. --duration changes the simulated length.

The schedule in src/scenario.cpp commands a 0.5 metre climb at four seconds, then roll, pitch, and yaw steps. The CSV includes truth, setpoints, both attitude estimates, raw gyro integration, altitude, gyro bias, and motor thrusts.

The checked-in docs/closed_loop.png is identified by the README as the default noisy run with constant gyro bias. The plot omits the complementary-filter traces for readability, although the CSV contains them. It is a simulation illustration, not a hardware benchmark or a table of measured performance.

Test the signs and the complete loop

I check small pieces before relying on the trajectory. The tests cover quaternion rotations, shortest-path yaw error, PID limits and filtering, mixer wrench reconstruction, estimator convergence, altitude estimation, and simulated hover. A tilted-hover test explicitly checks that horizontal acceleration can coexist with a body-up accelerometer reading.

The full-loop test in tests/test_closed_loop.cpp uses these conditions:

  fc::ScenarioConfig config;
  config.sensor_noise = false;
  config.duration_s = 16.0;
  config.dt_s = 0.001;

It checks recovery and scheduled commands at selected times, including altitude near 0.5 metres within 0.2 metres at six seconds. Those are test tolerances, not reported tracking-error measurements. Another test repeats the seeded noisy scenario and compares final values exactly.

Ring tests cover wrapping, newest-sample selection, and ordered transfer between threads. The short threaded hover checks sample flow and bounded final state. .github/workflows/ci.yml configures GCC and Clang jobs, each with and without AddressSanitizer and UndefinedBehaviorSanitizer. That describes the configured checks; it is not a claim that I ran that matrix for this article.

Limits and the next experiment

The vehicle has diagonal inertia, instantaneous thrust, and a quadratic drag term. It has no motor lag, propwash, flexible modes, GPS, optical flow, or position controller. A tilt produces horizontal motion because nothing commands horizontal position back to a target.

The gains are hand-tuned for one simulated vehicle. The threaded pacer sleeps on the wall clock and rebases after missed ticks; it provides no hard-deadline guarantee. I would next add motor dynamics and vary vehicle parameters, then check how saturation and estimator gating change the trajectory before expanding the control stack.

Read the implementation

The repository contains the simulator, controllers, tests, and telemetry plot script. I would start with src/scenario.cpp, follow one sample through FlightController, then read the corresponding tests before changing a gain or sensor model.

For the supervision side, my Rust drone failsafe tutorial covers an advisory monitor that recommends Hold, RTL or Land from synthetic telemetry, with a no_std core shared by a simulator and a browser demo.