Module 02 showed how UAVs move. This module covers how they are made to move where you want them to go. GNC β Guidance, Navigation, and Control β is the discipline that transforms "fly to GPS coordinate X" into actual motor commands firing at the right time. It is the brain and nervous system of any autonomous UAV.
π REFERENCES: Bestaoui Sebbane Β§4β6 Β· Virell Β§3 (PX4) Β· Sadraey Β§5 Β· MDPI Autonomous Control Ch.2
GNC stands for Guidance, Navigation, and Control β three distinct problems that work together to make a UAV fly where you want it to go. Navigation answers "where am I right now?" (position, speed, heading). Guidance answers "where should I go next?" (generate the path to the destination). Control answers "what do I physically do to get there?" (calculate the motor commands). These three problems run in a continuous loop, thousands of times per second, for the entire duration of the flight.
Imagine driving to an unfamiliar destination at night with poor visibility. Navigation is your GPS telling you "you are here, at this speed." Guidance is the GPS telling you "turn left in 200 metres β the optimal route is..." Control is your hands on the steering wheel and foot on the pedals, physically executing the turn. If any one of these fails, you crash. An autonomous UAV is doing all three simultaneously β but at 100β1000 Hz.
Bestaoui Sebbane defines the three functions precisely and notes that their separation into distinct subsystems is not just conceptual β it is an engineering necessity that allows independent development, testing, and redundancy management. Bestaoui Sebbane Β§4.1
| Function | Input | Output | Update Rate | Failure Consequence |
|---|---|---|---|---|
| Navigation | Raw sensor data (IMU, GPS, camera, barometer) | State estimate: position, velocity, attitude | 50β1000 Hz | All downstream functions use wrong state β catastrophic |
| Guidance | Current state + mission waypoints + obstacle map | Reference trajectory or velocity setpoints | 1β10 Hz | UAV flies toward wrong target or takes unsafe path |
| Control | State estimate + reference setpoints from guidance | Actuator commands (motor PWM, servo angles) | 100β2000 Hz | UAV cannot follow commanded trajectory β instability |
Control works through negative feedback: measure what is actually happening, compare it to what should be happening, and generate a corrective action proportional to the difference. Sadraey Β§5.1
Error signal: $e(t) = r(t) - y(t)$. The controller $C(s)$ takes the error and outputs a control signal $u(t)$ that drives the plant toward the reference. The "plant" in UAV control is the flight dynamics model from Module 02.
PID stands for ProportionalβIntegralβDerivative. It is the most widely used control algorithm in the world β used in everything from industrial ovens to aircraft autopilots to the cruise control in your car. The idea is simple: look at the current error (Proportional), look at how long the error has been building up (Integral), and look at how fast the error is changing (Derivative). Add all three together with tunable weights and you have a surprisingly powerful controller that works well for a huge range of systems.
Each term β what it does and when it activates:
$K_p \cdot e(t)$ β push harder when the error is larger. If the drone is 10 m off course, the correction is 10Γ bigger than when it's 1 m off.
Problem alone: Always leaves a small steady-state error (called "droop"). To eliminate error you'd need infinite gain.
$K_i \cdot \int e\,dt$ β accumulates past errors. Eliminates steady-state error by slowly winding up force until it's exactly zero. Compensates for constant disturbances (constant wind, gravity offset).
Problem: "Integrator windup" β if the error persists too long (motor saturated, physical limit hit), the integrator accumulates huge values that cause overshoot when the limit is released.
$K_d \cdot \dot{e}(t)$ β reacts to how fast the error is changing. If the drone is rushing toward the setpoint, D pushes back to prevent overshoot. It's a "predictive brake."
Problem: Amplifies high-frequency sensor noise (noise looks like a rapidly changing error). Always applied to a low-pass filtered signal in practice.
Taking the Laplace transform of the PID equation gives the transfer function, which reveals how the controller behaves at different frequencies: Sadraey Β§5.2
The $1/s$ term (integrator) gives infinite gain at DC (zero frequency) β this is how steady-state error is eliminated. The $s$ term (differentiator) gives high gain at high frequencies β this is why it amplifies noise. In practice, the D term is always implemented as $K_d s / (\tau_f s + 1)$ with a low-pass filter time constant $\tau_f$ to cap the high-frequency gain. Virell documents this as the standard PX4 implementation. Virell Β§3.2
Real flight computers run at fixed time steps $\Delta t$ (e.g., 2 ms for a 500 Hz loop). The continuous PID must be discretised: Virell Β§3.3
$e[k] = r[k] - y[k]$: error at time step $k$. The integral is accumulated as a running sum. The derivative is a backward difference. Integrator anti-windup: clamp $\sum e[j]$ to $\pm u_{max}/K_i$ to prevent wind-up beyond the actuator saturation limit.
A quadrotor altitude controller with $K_p = 2.5$, $K_i = 0.8$, $K_d = 1.2$ is holding 50 m altitude. A wind gust pushes it down to 47 m. Trace the first three control loop steps at $\Delta t = 0.01$ s (100 Hz).
| Step $k$ | Error $e[k]$ | P term | I term (accumulated) | D term | Total $u[k]$ |
|---|---|---|---|---|---|
| 0 (gust hits) | 3.0 m | 7.50 | 0.024 | 360.0 (large β sudden change) | 367.5 |
| 1 | 2.8 m | 7.00 | 0.046 | β24.0 (error shrinking β D brakes) | β16.95 |
| 2 | 2.5 m | 6.25 | 0.066 | β36.0 | β29.68 |
What to notice: At step 0, the D term dominates β it "sees" the sudden 3 m drop from zero and generates a massive corrective thrust burst to stop the descent. By step 1, as the drone starts recovering and error shrinks, D flips negative (braking the recovery to prevent overshoot). The I term slowly grows to compensate for any persistent wind. This is exactly the qualitative behaviour PID is designed to produce.
Note: The large D value at step 0 illustrates why D gain must be limited β real implementations apply a low-pass filter or use "D-on-measurement" (differentiate the measured output, not the error) to avoid the "derivative kick" on sudden setpoint changes.
Knowing the PID equation is the easy part. The hard part is finding the right values of $K_p$, $K_i$, $K_d$ for a specific UAV. Too low and the drone responds sluggishly. Too high and it oscillates violently or becomes unstable. There are both systematic methods and practical heuristics for getting this right β and military UAV programmes typically use model-based methods validated by flight test.
The ZieglerβNichols method is an empirical procedure: gradually increase $K_p$ (with $K_i = K_d = 0$) until the system oscillates at a constant amplitude. That gain is the ultimate gain $K_u$ and the oscillation period is the ultimate period $T_u$. Sadraey Β§5.3
| Controller Type | $K_p$ | $K_i$ | $K_d$ | Typical Use |
|---|---|---|---|---|
| P only | $0.50\,K_u$ | 0 | 0 | Simple heading hold |
| PI | $0.45\,K_u$ | $0.54\,K_u/T_u$ | 0 | Altitude hold, speed hold |
| PID (classic) | $0.60\,K_u$ | $1.20\,K_u/T_u$ | $0.075\,K_u T_u$ | Attitude control (roll, pitch) |
| PID (some overshoot) | $0.33\,K_u$ | $0.66\,K_u/T_u$ | $0.11\,K_u T_u$ | Position hold (smoother) |
For each PID gain, analyse how the closed-loop poles (eigenvalues of the feedback system) move as the gain changes. The root locus traces this movement: Sadraey Β§5.4
Stable system requires all closed-loop poles in the left-half complex plane (negative real part). Performance specifications: settling time $T_s \approx 4/|\sigma|$ (where $\sigma$ is the real part of the dominant pole), percent overshoot $\%OS = e^{-\pi\sigma/\omega_n} \times 100$ where $\omega_n$ is the natural frequency. Design targets: $\%OS < 10\%$, $T_s < 2$ s for attitude loops, $T_s < 5$ s for position loops on typical Group 3β5 UAVs.
PID is intuitive but manual β you adjust three gains by trial and error. LQR (Linear Quadratic Regulator) is a systematic alternative: you tell the algorithm how much you care about each error (position error vs. control effort), and it mathematically computes the optimal gains that minimise a performance cost. It's the difference between guessing the best route and running a sat-nav optimiser. LQR is standard in high-performance military UAV autopilots where guaranteed stability margins and optimal disturbance rejection are required.
LQR requires the system to be written in state-space form. For the linearised UAV equations from Module 02: Bestaoui Sebbane Β§4.2
$\mathbf{x} \in \mathbb{R}^n$: state vector (e.g., $[\phi, \theta, \psi, p, q, r, u, v, w, x, y, z]^T$); $\mathbf{u} \in \mathbb{R}^m$: control input vector (e.g., $[\delta_e, \delta_a, \delta_r, \delta_T]^T$); $A \in \mathbb{R}^{n\times n}$: system matrix (stability derivatives from Module 02); $B \in \mathbb{R}^{n\times m}$: control input matrix (control effectiveness derivatives).
What this means: Minimise the total squared error and total squared control effort over all future time. The matrices $Q$ and $R$ are the designer's choices:
$Q \in \mathbb{R}^{n\times n}$: symmetric positive semi-definite β how much you penalise each
state deviation. Large $Q_{ii}$ means "I really don't want state $i$ to deviate."
$R \in \mathbb{R}^{m\times m}$: symmetric positive definite β how much you penalise control
effort. Large $R$ means "don't use aggressive actuator commands" (saves battery, reduces structural loads).
The optimal state-feedback control law $\mathbf{u}^* = -K^*\mathbf{x}$ that minimises $J$ is found by:
$$\mathbf{u}^*(t) = -K^*\mathbf{x}(t), \qquad K^* = R^{-1}B^T P$$where $P$ is the unique symmetric positive definite solution to the Continuous-time Algebraic Riccati Equation (CARE):
$$A^T P + PA - PBR^{-1}B^T P + Q = 0$$Key properties of LQR: (1) Guaranteed gain margin $\geq 6$ dB and phase margin $\geq 60Β°$ for each input channel β this is the celebrated "LQR robustness guarantee." (2) The closed-loop system $A - BK^*$ is always stable if $(A, B)$ is controllable and $Q \geq 0$, $R > 0$. (3) The gains are computed offline (before flight) by solving the CARE β real-time computation is just a matrix multiply. Bestaoui Sebbane Β§4.3
The "Bryson's rule" provides a practical starting point for weight matrix selection: Bestaoui Sebbane Β§4.4
$x_{i,\max}$: maximum acceptable deviation in state $i$; $u_{j,\max}$: maximum allowable control input $j$. Example for quadrotor attitude: If you can tolerate at most 0.1 rad (5.7Β°) roll error, set $Q_{\phi\phi} = 1/0.1^2 = 100$. If maximum roll torque is 2 NΒ·m, set $R_{LL} = 1/2^2 = 0.25$. This normalises all states and inputs to the same scale before the optimiser runs.
Design an LQR attitude controller for the 1.5 kg quadrotor from Module 02. State: $\mathbf{x} = [\phi, \theta, p, q]^T$. Control: $\mathbf{u} = [L, M]^T$ (roll and pitch torques).
| Parameter | Value | Rationale |
|---|---|---|
| Max roll/pitch error | 0.1 rad (5.7Β°) | Acceptable attitude deviation |
| Max angular rate error | 0.5 rad/s | Acceptable rate deviation |
| Max roll/pitch torque | 0.5 NΒ·m | Physical limit from rotor allocation |
| $Q$ (Bryson's rule) | diag(100, 100, 4, 4) | $1/0.1^2$ for angles, $1/0.5^2$ for rates |
| $R$ (Bryson's rule) | diag(4, 4) | $1/0.5^2$ for each torque |
The linearised hover dynamics give $A$ with structure: $\dot{\phi} = p$, $\dot{\theta} = q$,
$\dot{p} = L/I_{xx}$, $\dot{q} = M/I_{yy}$ (decoupled at hover). So:
$$A = \begin{bmatrix}0&0&1&0\\0&0&0&1\\0&0&0&0\\0&0&0&0\end{bmatrix}, \quad
B = \begin{bmatrix}0&0\\0&0\\1/I_{xx}&0\\0&1/I_{yy}\end{bmatrix}
= \begin{bmatrix}0&0\\0&0\\28.7&0\\0&28.7\end{bmatrix}$$
Solving the CARE (via scipy.linalg.solve_continuous_are in the Python section)
gives optimal gains approximately:
$K^* \approx \begin{bmatrix}5.0 & 0 & 0.42 & 0 \\ 0 & 5.0 & 0 & 0.42\end{bmatrix}$
meaning: apply 5.0 NΒ·m per radian of roll error and 0.42 NΒ·m per rad/s of roll rate error.
Closed-loop poles at approximately $-3.5 \pm 3.5j$ rad/s β damping ratio $\zeta \approx 0.71$,
natural frequency $\omega_n \approx 4.95$ rad/s, settling time $\approx 1.1$ s. Excellent.
Real UAV autopilots don't use a single PID to go from "GPS coordinates" to "motor commands." That would be like trying to drive a car by directly connecting the steering wheel to the engine β too many steps in between. Instead, they use a cascade of nested control loops. The outer loop controls slow things (position, altitude). Its output becomes the reference for the inner loop, which controls faster things (velocity, attitude). The inner loop's output commands the actuators. Each loop runs at a different speed, with the innermost loop always being the fastest.
PX4 β the open-source flight controller used in numerous military and commercial UAVs β implements a four-level cascade for multirotor position control: Virell Β§3.1
The cascade architecture works only if each inner loop is significantly faster than its outer loop. The rule of thumb: the inner loop bandwidth must be at least 5β10Γ the outer loop bandwidth. Virell Β§3.5
For a two-level cascade (inner loop $C_i$ with bandwidth $\omega_i$, outer loop $C_o$ with bandwidth $\omega_o$), the outer loop sees the inner loop as approximately unity gain if $\omega_o \ll \omega_i$:
$$\text{If } \omega_o \leq \frac{\omega_i}{5}, \quad \text{then } G_{inner}(j\omega_o) \approx 1$$Physical interpretation: The outer position loop "trusts" the inner attitude loop to execute its commands perfectly β because the attitude loop is fast enough to track any position-loop command without lag. If this separation is violated (outer loop too fast), the outer loop "sees" the inner loop delay and destabilises. This is the most common mistake in amateur UAV autopilot design. Bestaoui Sebbane Β§5.1
PX4 numbers: Rate loop: ~1000 Hz (bandwidth ~100 rad/s). Attitude loop: ~250 Hz (bandwidth ~20 rad/s β within 5Γ factor). Velocity loop: ~50 Hz (bandwidth ~4 rad/s). Position loop: ~5 Hz (bandwidth ~0.5 rad/s). Each loop is 5β6Γ slower than its inner loop β exactly meeting the separation criterion.
A guidance law is the algorithm that decides which direction the UAV should fly at any given moment to reach its destination efficiently. It's not enough to know where the waypoint is β you need a rule for how to steer toward it when you're off-course, approaching at speed, or following a curved path. Different guidance laws make different trade-offs between tracking accuracy, flight efficiency, and robustness to disturbances. Military UAVs use multiple guidance laws for different mission phases.
The simplest guidance law: always point directly at the target. Bestaoui Sebbane Β§5.2
$\psi_{cmd}$: commanded heading; $(x_T, y_T)$: target position; $(x_{UAV}, y_{UAV})$: current position. Problem: Pure pursuit causes the UAV to "cut corners" and overshoot when approaching at high speed. In the limit of approaching a moving target head-on, it produces oscillatory motion. Rarely used alone; more useful as a terminal guidance component.
The L1 guidance law is used in PX4 and ArduPilot for fixed-wing path following. Instead of pointing at the waypoint, it aims at a point on the desired path that is a distance $L_1$ ahead of the aircraft: Bestaoui Sebbane Β§5.3
$V$: current airspeed; $L_1$: look-ahead distance [m]; $\eta$: angle between velocity vector and line from aircraft to the $L_1$ reference point on the path. The lateral acceleration is then converted to a bank angle command: $\phi_{cmd} = \arctan(a_{cmd}/g)$.
Key insight: $L_1$ is the tuning parameter. Large $L_1$ β gentle turns, tracks poorly in crosswind. Small $L_1$ β aggressive turns, tracks well but risks oscillation. Typical military fixed-wing UAV: $L_1 = 50$β$150$ m at cruise. Virell Β§3.7
Proportional Navigation is used for intercept missions β loitering munition terminal phase, anti-UAV missiles, and air-to-air missile guidance. The rule: command an acceleration proportional to the rate of change of the line-of-sight (LOS) angle to the target. Bestaoui Sebbane Β§5.4
$N'$: navigation constant (typically 3β5 for homing missiles, 2β4 for loitering munitions); $V_c = -\dot{R}$: closing velocity (rate at which range to target decreases); $\dot{\lambda}$: line-of-sight rate [rad/s].
Geometric result: PN produces a collision course β if $\dot{\lambda} = 0$ (LOS angle is constant), the pursuer and target meet at a single point even if both are manoeuvring. A target can be intercepted with minimum energy expenditure by keeping $\dot{\lambda}$ near zero. Detecting $\dot{\lambda} \neq 0$ is the primary warning sign for a UAV being tracked by an interceptor.
A fixed-wing UAV is 30 m off its planned track, flying at 55 m/s, with $L_1 = 100$ m. Compute the commanded bank angle to return to track.
Step 1 β Find $\eta$: The $L_1$ reference point is 100 m along the track from the nearest on-track point. The cross-track error is 30 m. So: $$\eta = \arcsin\!\left(\frac{30}{100}\right) = \arcsin(0.30) = 17.5Β°= 0.305 \text{ rad}$$ Step 2 β Lateral acceleration: $$a_{cmd} = 2 \times \frac{55^2}{100} \times \sin(17.5Β°) = 2 \times 30.25 \times 0.301 = \mathbf{18.2 \text{ m/s}^2}$$ Step 3 β Bank angle: $$\phi_{cmd} = \arctan\!\left(\frac{18.2}{9.81}\right) = \arctan(1.855) = \mathbf{61.7Β°}$$ Interpretation: 30 m off track at 55 m/s with $L_1 = 100$ m demands a very aggressive 62Β° bank angle β beyond the structural limits of most ISR UAVs (~30Β° bank limit). This tells us $L_1 = 100$ m is too short for this speed β the UAV needs a longer look-ahead. Increasing to $L_1 = 200$ m gives $\eta = 8.6Β°$, $a_{cmd} = 8.9$ m/sΒ², $\phi_{cmd} = 42Β°$ β still aggressive but within limits. This is exactly the kind of analysis that informs $L_1$ parameter selection during ground testing. Virell Β§3.7
PX4 is the open-source flight controller used in both commercial UAVs and military research platforms worldwide. Understanding its architecture gives you a concrete picture of how all the theory so far β PID loops, cascade control, guidance laws β gets implemented in real software running on real hardware. Virell's treatment documents this architecture in enough depth for serious engineering work.
PX4 runs on NuttX RTOS and organises all functionality into modules communicating over a publish-subscribe middleware called uORB. This pub-sub architecture means each module is loosely coupled β you can replace the position controller without touching the attitude controller. Virell Β§3.1
| Module | GNC Function | Update Rate | Key Parameters |
|---|---|---|---|
ekf2 | Navigation β fuses IMU, GPS, baro, mag into state estimate | 250 Hz | Process/measurement noise, GPS delay |
mc_pos_control | Position + velocity control (outer cascade loops) | 50β100 Hz | $K_p, K_i, K_d$ for XY and Z axes |
mc_att_control | Attitude control β quaternion error to rate setpoint | 250 Hz | Roll/pitch/yaw proportional gains |
mc_rate_control | Innermost rate loop β body rates to torque commands | 1000 Hz | $K_p, K_i, K_d$ for $p, q, r$ |
navigator | Guidance β waypoint sequencing, mission management | 10 Hz | Waypoint acceptance radius, loiter radius |
mixer | Control allocation β torque/thrust to motor PWM | 1000 Hz | Geometry matrix, PWM limits |
PX4's attitude controller does not use Euler angle errors (which suffer from singularities and gimbal lock from Module 02). Instead it uses the quaternion error: Virell Β§3.8
$\otimes$: quaternion multiplication; $\mathbf{q}_{ref}$: desired attitude quaternion; $\mathbf{q}_{current}^{-1}$: conjugate of current attitude quaternion. The vector part $[q_{e,1}, q_{e,2}, q_{e,3}]^T$ of the error quaternion represents the rotation axis and (approximately) the rotation angle needed. The $\text{sign}(q_{e,0})$ ensures the shorter of the two rotation paths is taken β without it, the controller might spin 350Β° instead of β10Β°.
pip install numpy scipy
# gnc_controllers.py # βββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ # Implements: PID (Sadraey Β§5.2, Virell Β§3.2β3.3) # LQR (Bestaoui Sebbane Β§4.2β4.4) # Closed-loop simulation (Bestaoui Sebbane Β§5.1) # βββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ import numpy as np from scipy.linalg import solve_continuous_are # ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ # Part 1: PID Controller with Anti-Windup and Derivative Filter # Sadraey Β§5.2, Virell Β§3.2β3.3 # ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ class PIDController: """ Discrete-time PID with: - Anti-windup (integral clamping) - Derivative filter (first-order low-pass) - D-on-measurement (avoids derivative kick on setpoint changes) """ def __init__(self, Kp, Ki, Kd, dt, u_max=np.inf, d_filter_tc=0.02): """ Kp, Ki, Kd : PID gains dt : time step [s] u_max : actuator saturation limit (anti-windup) d_filter_tc : derivative filter time constant [s] """ self.Kp, self.Ki, self.Kd = Kp, Ki, Kd self.dt = dt self.u_max = u_max self.tc = d_filter_tc # Internal state self._integral = 0.0 self._prev_meas = 0.0 # previous measurement (for D-on-measurement) self._deriv_filt = 0.0 # filtered derivative def reset(self): """Reset integrator and filter state (e.g., on mode switch).""" self._integral = 0.0 self._prev_meas = 0.0 self._deriv_filt = 0.0 def step(self, setpoint: float, measurement: float) -> float: """ Compute one PID output step. Uses D-on-measurement to avoid derivative kick on step reference changes. """ error = setpoint - measurement # ββ Proportional ββββββββββββββββββββββββββββββββββββββββββββ P = self.Kp * error # ββ Integral with anti-windup ββββββββββββββββββββββββββββββββ self._integral += error * self.dt # Clamp integrator to prevent windup beyond actuator limit integral_limit = self.u_max / (self.Ki + 1e-9) self._integral = np.clip(self._integral, -integral_limit, integral_limit) I = self.Ki * self._integral # ββ Derivative (on measurement, not error β no kick) βββββββββ # Negative because d/dt(measurement) = -d/dt(error) for constant setpoint raw_deriv = -(measurement - self._prev_meas) / self.dt # First-order low-pass filter: Ο*dy/dt + y = x β discrete approximation alpha = self.dt / (self.tc + self.dt) self._deriv_filt = alpha * raw_deriv + (1 - alpha) * self._deriv_filt D = self.Kd * self._deriv_filt self._prev_meas = measurement # Total output (clamped to actuator limits) u = np.clip(P + I + D, -self.u_max, self.u_max) return u, {'P': P, 'I': I, 'D': D, 'error': error} # ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ # Part 2: LQR Attitude Controller # Bestaoui Sebbane Β§4.2β4.4 # ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ def design_lqr_attitude(Ixx: float, Iyy: float) -> tuple: """ Design LQR attitude controller for quadrotor hover. State: x = [phi, theta, p, q] (roll, pitch, roll rate, pitch rate) Input: u = [L, M] (roll torque, pitch torque) Linearised hover dynamics (decoupled roll/pitch at small angles): phi_dot = p theta_dot = q p_dot = L / Ixx q_dot = M / Iyy Bestaoui Sebbane Β§4.2 """ # System matrices (linearised hover β Bestaoui Sebbane Β§4.2) A = np.array([ [0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0], [0, 0, 0, 0], ], dtype=float) B = np.array([ [0, 0 ], [0, 0 ], [1/Ixx, 0 ], [0, 1/Iyy ], ], dtype=float) # Bryson's rule β Q and R weight matrices (Bestaoui Sebbane Β§4.4) angle_max = 0.10 # [rad] max acceptable attitude error (5.7Β°) rate_max = 0.50 # [rad/s] max acceptable rate error torque_max= 0.50 # [NΒ·m] max control torque Q = np.diag([1/angle_max**2, 1/angle_max**2, 1/rate_max**2, 1/rate_max**2]) R = np.diag([1/torque_max**2, 1/torque_max**2]) # Solve Continuous-time Algebraic Riccati Equation # A^T P + PA - P B R^{-1} B^T P + Q = 0 P = solve_continuous_are(A, B, Q, R) # Optimal feedback gain matrix K = R^{-1} B^T P K = np.linalg.solve(R, B.T @ P) # Closed-loop eigenvalues (stability check) A_cl = A - B @ K eigenvalues = np.linalg.eigvals(A_cl) return K, A_cl, eigenvalues, Q, R # ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ # Part 3: Closed-Loop Attitude Simulation β PID vs LQR # Compare step response to a 10Β° roll command # ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ def simulate_roll_step(K_lqr, Ixx, dt=0.002, T=3.0): """ Simulate roll step response for both PID and LQR controllers. System: phi_ddot = L / Ixx (pure double integrator at hover) Step command: 10Β° (0.1745 rad) roll setpoint at t=0. """ N = int(T / dt) t = np.linspace(0, T, N) phi_ref = np.radians(10) # 10Β° roll step command # ββ LQR simulation βββββββββββββββββββββββββββββββββββββββββββ x_lqr = np.zeros(4) # [phi, theta=0, p, q=0] phi_lqr, p_lqr = np.zeros(N), np.zeros(N) for k in range(N): # Error state relative to reference x_err = x_lqr - np.array([phi_ref, 0, 0, 0]) u_lqr = -K_lqr @ x_err # LQR control: u = -K * (x - x_ref) L = np.clip(u_lqr[0], -0.5, 0.5) # actuator saturation # Euler integration of double integrator phi_dot = x_lqr[2] # p = phi_dot p_dot = L / Ixx # angular acceleration x_lqr[0] += phi_dot * dt x_lqr[2] += p_dot * dt phi_lqr[k] = x_lqr[0] p_lqr[k] = x_lqr[2] # ββ PID simulation βββββββββββββββββββββββββββββββββββββββββββ # Gains from Ziegler-Nichols estimate for this system pid = PIDController(Kp=5.0, Ki=0.5, Kd=0.4, dt=dt, u_max=0.5, d_filter_tc=0.02) phi_pid = np.zeros(N); phi_now = 0.0; p_now = 0.0 for k in range(N): u_pid, _ = pid.step(phi_ref, phi_now) L = np.clip(u_pid, -0.5, 0.5) p_now += (L / Ixx) * dt phi_now += p_now * dt phi_pid[k] = phi_now return t, phi_lqr, phi_pid, phi_ref # ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ # Main: Design, Report, and Simulate # ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ if __name__ == "__main__": Ixx, Iyy = 0.0348, 0.0348 # from Module 02 quadrotor # ββ LQR Design ββββββββββββββββββββββββββββββββββββββββββββββ K, A_cl, eigs, Q, R = design_lqr_attitude(Ixx, Iyy) print("\n" + "="*56) print(" LQR ATTITUDE CONTROLLER DESIGN") print(" Bestaoui Sebbane Β§4.2β4.4") print("="*56) print(f"\n Optimal gain matrix K:") print(f" K[0,:] (roll) = {K[0,:]}") print(f" K[1,:] (pitch) = {K[1,:]}") print(f"\n Closed-loop eigenvalues:") for i, ev in enumerate(eigs): wn = abs(ev) zeta= -ev.real/wn if wn > 0 else 1 Ts = 4/(-ev.real) if ev.real < 0 else np.inf print(f" Ξ»{i+1} = {ev:.3f} | Οn={wn:.2f} rad/s ΞΆ={zeta:.3f} Tsβ{Ts:.2f}s") # ββ Simulation ββββββββββββββββββββββββββββββββββββββββββββββ t, phi_lqr, phi_pid, phi_ref = simulate_roll_step(K, Ixx) # Compute performance metrics def settling_time(t, y, ref, band=0.05): """First time signal enters and stays within Β±band*ref of reference.""" tol = band * abs(ref) settled = np.where(np.abs(y - ref) < tol)[0] return t[settled[0]] if len(settled) > 0 else np.inf Ts_lqr = settling_time(t, phi_lqr, phi_ref) Ts_pid = settling_time(t, phi_pid, phi_ref) OS_lqr = (np.max(phi_lqr) - phi_ref) / phi_ref * 100 OS_pid = (np.max(phi_pid) - phi_ref) / phi_ref * 100 print(f"\n STEP RESPONSE COMPARISON (10Β° roll step)") print(f" {'Metric':<25} {'LQR':>10} {'PID':>10}") print(f" {'-'*45}") print(f" {'Settling time (5% band)':<25} {Ts_lqr:>9.2f}s {Ts_pid:>9.2f}s") print(f" {'Overshoot':<25} {OS_lqr:>9.1f}% {OS_pid:>9.1f}%") print(f"\n β LQR is guaranteed stable by design (CARE solution).") print(f" β PID required manual tuning to achieve comparable performance.") print("="*56 + "\n")