Drone Feedback Controller

1# =============================================================================
2# MATHEMATICAL HELPERS
3# =============================================================================
4
5def _clip(v, lo, hi):
6    """Saturate value v to the closed interval [lo, hi]. Used for anti-windup and velocity caps."""
7    return max(lo, min(hi, v))
8
9
10def _state_key(dist, abs_yaw_err, wind):
11    """
12    Discretise the continuous (distance, yaw_error, wind) state into a hashable
13    tuple that indexes the Q-table.
14
15    Naming convention:
16        dist        – 3-D Euclidean distance to target (m).
17        abs_yaw_err – absolute yaw error (rad); always non-negative.
18        wind        – bool converted to int (False→0, True→1).
19
20    Returns (dist_band, yaw_band, wind_int) – all integers.
21    96 total states: 8 dist_band × 6 yaw_band × 2 wind_int.
22    Ref: Lecture 10 – state discretisation for tabular RL.
23    """
24    # dist_band: finer resolution close to target where precision matters.
25    if   dist < 0.08: dist_band = 0
26    elif dist < 0.20: dist_band = 1
27    elif dist < 0.40: dist_band = 2
28    elif dist < 0.70: dist_band = 3
29    elif dist < 1.10: dist_band = 4
30    elif dist < 1.80: dist_band = 5
31    elif dist < 2.80: dist_band = 6
32    else:             dist_band = 7
33
34    # yaw_band: finer resolution near zero (aligned).
35    if   abs_yaw_err < 0.04: yaw_band = 0
36    elif abs_yaw_err < 0.10: yaw_band = 1
37    elif abs_yaw_err < 0.25: yaw_band = 2
38    elif abs_yaw_err < 0.50: yaw_band = 3
39    elif abs_yaw_err < 1.00: yaw_band = 4
40    else:                    yaw_band = 5
41
42    wind_int = int(wind)   # False→0, True→1.
43    return (dist_band, yaw_band, wind_int)
44
45
46# =============================================================================
47# CSV WAYPOINT LOADER
48# =============================================================================
49def _load_csv_targets(csv_path=None):
50    """
51    Loads a sequence of target waypoints from a CSV file.
52    Provides a default square pattern if the file is missing or invalid.
53    
54    Args:
55        csv_path (str): Path to the CSV file containing target coordinates.
56                        If None, defaults to 'targets.csv' in the same directory as this file.
57        
58    Returns:
59        tuple: A tuple of target tuples: ((x1, y1, z1, yaw1), (x2, y2, z2, yaw2), ...)
60    """
61    if csv_path is None:
62        csv_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "targets.csv")
63    # Default fallback flight pattern (a simple 2x2 square at 2m altitude)
64    _DEFAULTS = (
65        ( 2.0,  2.0, 2.0, 0.0),
66        (-2.0,  2.0, 2.0, 1.57),
67        (-2.0, -2.0, 2.0, 3.14),
68        ( 2.0, -2.0, 2.0, 4.71),
69    )
70
71    if not os.path.isfile(csv_path):
72        return _DEFAULTS
73
74    try:
75        targets = []
76        with open(csv_path, newline="") as fh:
77            reader = csv.DictReader(fh)
78            fields = [f.strip() for f in (reader.fieldnames or [])]
79
80            def _col(axis):
81                for c in (f"target_{axis}", axis):
82                    if c in fields:
83                        return c
84                raise ValueError(f"No '{axis}' column in {fields}")
85
86            cx, cy, cz = _col("x"), _col("y"), _col("z")
87            cyaw = next(
88                (c for c in ("target_yaw", "yaw", "heading", "psi") if c in fields),
89                None,
90            )
91            if cyaw is None:
92                raise ValueError("No yaw column found")
93
94            for i, raw in enumerate(reader):
95                row = {k.strip(): v.strip() for k, v in raw.items()}
96                try:
97                    targets.append((float(row[cx]), float(row[cy]),
98                                    float(row[cz]), float(row[cyaw])))
99                except (KeyError, ValueError) as e:
100                    raise ValueError(f"Bad row {i+2}: {row}") from e
101
102        return tuple(targets) if targets else _DEFAULTS
103    except Exception:
104        return _DEFAULTS
105
106
107_CSV_TARGETS = _load_csv_targets()
108
109
110# =============================================================================
111# RL AGENT FUNCTIONS
112# =============================================================================
113
114def _get_errors(state, active_target):
115    """
116    [ADVANCED METHOD – State Estimator & Coordinate Frame Transformation]
117
118    Computes all tracking errors needed by the PID controller and the RL cost.
119
120    NAMING CONVENTION used throughout this function and all callers:
121        ex_w, ey_w  – position errors in the World (inertial) frame (m).
122        ex_b, ey_b  – position errors in the Body frame (m), after R_z(yaw).
123        ez          – altitude error; identical in both frames for level flight.
124        eyaw        – yaw error wrapped to (-π, π] (rad).
125        dist        – 3-D Euclidean distance to target (m).
126        cost        – scalar RL cost used to compute reward signal.
127
128    WHY WORLD AND BODY FRAME ERRORS?
129        PID integration and differentiation use world-frame errors (ex_w, ey_w)
130        to avoid false spikes when the drone yaws. Body-frame errors (ex_b, ey_b)
131        are stored in the return dict for reference but the main PID uses world frame.
132        The final velocity commands are rotated back to body frame in controller()
133        Step 8 before being returned to run.py.
134        Ref: Lecture 6, p.14 – R_z(ψ) rotation matrix.
135
136    ROTATION MATRIX (yaw only):
137        [ ex_b ]   [ cos(yaw)  sin(yaw) ] [ ex_w ]
138        [ ey_b ] = [-sin(yaw)  cos(yaw) ] [ ey_w ]
139
140    COST FUNCTION:
141        cost = dist² + 0.60·eyaw² + prec
142        prec = 5·max(0, 0.10 − dist)²  (precision bonus for sub-10 cm accuracy)
143        Ref: Lecture 10, p.15 – reward shaping.
144    """
145    x, y, z, _, _, yaw = (float(v) for v in state)
146    tx, ty, tz, tyaw   = (float(v) for v in active_target)
147
148    # World-frame position errors.
149    ex_w = tx - x
150    ey_w = ty - y
151    ez   = tz - z
152
153    # Yaw error wrapped to (-π, π]. Ref: Lecture 6, p.11 – angle wrapping.
154    eyaw = math.atan2(math.sin(tyaw - yaw), math.cos(tyaw - yaw))
155
156    # Body-frame XY errors via 2×2 yaw rotation matrix.
157    cos_yaw, sin_yaw = math.cos(yaw), math.sin(yaw)
158    ex_b =  cos_yaw * ex_w + sin_yaw * ey_w
159    ey_b = -sin_yaw * ex_w + cos_yaw * ey_w
160
161    # RL cost function.
162    dist = math.sqrt(ex_w**2 + ey_w**2 + ez**2)
163    prec = 5.0 * max(0.0, 0.10 - dist) ** 2
164    cost = dist**2 + 0.60 * eyaw**2 + prec
165
166    # All keys use the naming convention documented above.
167    return {
168        "dist": dist, "cost": cost,
169        "eyaw": eyaw, "ez":   ez,
170        "ex_w": ex_w, "ey_w": ey_w,   # World-frame: used by PID integral/derivative.
171        "ex_b": ex_b, "ey_b": ey_b,   # Body-frame:  stored for reference.
172    }
173
174
175def _td_update(mem, state_key, action_idx, reward, next_state_key):
176    """
177    [ADVANCED METHOD – Q-learning Temporal Difference Update]
178
179    Naming convention for parameters:
180        state_key      – hashable tuple (dist_band, yaw_band, wind_int) for current state.
181        action_idx     – integer 0-15 index of the action taken.
182        reward         – scalar reward received after the action.
183        next_state_key – hashable tuple for the resulting next state.
184        q_curr         – Q-value list for the current state (16 entries).
185        q_next         – Q-value list for the next state (16 entries).
186
187    UPDATE RULE (Ref: Lecture 10, p.19):
188        Q(state_key, action_idx) +=
189            alpha × [ reward + gamma × max(q_next) − Q(state_key, action_idx) ]
190    """
191    q_curr = mem["q_table"].setdefault(state_key,      [0.0] * len(_ACTION_PROFILES))
192    q_next = mem["q_table"].setdefault(next_state_key, [0.0] * len(_ACTION_PROFILES))
193    q_curr[action_idx] += mem["alpha"] * (
194        reward + mem["gamma"] * max(q_next) - q_curr[action_idx]
195    )
196
197
198def _replay_update(mem):
199    """
200    [ADVANCED METHOD – Experience Replay Mini-Batch Update]
201
202    Samples mem["replay_batch"] past transitions from the replay buffer and
203    applies _td_update to each, using the same parameter naming convention:
204        state_key, action_idx, reward, next_state_key.
205    Ref: Lecture 10, p.21 – experience replay.
206    """
207    if len(mem["replay"]) < mem["replay_batch"]:
208        return
209    for state_key, action_idx, reward, next_state_key in \
210            mem["rng"].sample(mem["replay"], mem["replay_batch"]):
211        _td_update(mem, state_key, action_idx, reward, next_state_key)
212
213
214def _update_learning_agent(mem, errs, dwell_bonus, state_key):
215    """
216    [ADVANCED METHOD – Online Q-learning with Reward Shaping]
217
218    Naming convention:
219        errs         – error dict from _get_errors (keys: dist, cost, eyaw, etc.).
220        dwell_bonus  – float bonus from _handle_dwell (0.0 if not within tolerance).
221        state_key    – current discretised state (dist_band, yaw_band, wind_int).
222        reward       – total shaped reward for the PREVIOUS action.
223        overshoot_penalty – penalty when close but cost is increasing (drone overshooting).
224
225    All reward weight values come from _REWARD_WEIGHTS for full transparency.
226    Update is throttled to every 5th step to save CPU for pybullet physics server.
227    Ref: Lecture 10, p.18-21 – reward shaping, TD(0), experience replay.
228    """
229    if mem["prev_key"] is None:
230        return   # No previous transition on the very first call.
231
232    # Overshoot penalty: cost increase when close means the drone is moving away.
233    overshoot_penalty = (
234        _REWARD_WEIGHTS["overshoot"] * (errs["cost"] - mem["prev_cost"])
235        if (errs["dist"] < 0.6 and errs["cost"] > mem["prev_cost"])
236        else 0.0
237    )
238
239    reward = (
240        (mem["prev_cost"] - errs["cost"])                          # Cost improvement.
241        + dwell_bonus                                              # Arrival/hold bonus.
242        - _REWARD_WEIGHTS["effort"]   * mem["prev_effort"]         # Effort penalty.
243        - _REWARD_WEIGHTS["distance"] * errs["dist"] ** 2          # Distance penalty.
244        - overshoot_penalty                                        # Overshoot penalty.
245    )
246
247    if mem["step_count"] % 5 == 0:
248        _td_update(mem, mem["prev_key"], mem["prev_action"], reward, state_key)
249        mem["replay"].append((mem["prev_key"], mem["prev_action"], reward, state_key))
250        if len(mem["replay"]) > mem["replay_cap"]:
251            mem["replay"].pop(0)
252        _replay_update(mem)
253
254    mem["epsilon"] = max(mem["eps_floor"], mem["epsilon"] * mem["eps_decay"])
255
256
257def _handle_dwell(mem, errs, target_pos):
258    """
259    Manage the dwell-hold phase: count consecutive in-tolerance steps, award RL
260    bonuses, reset integrators, and advance the waypoint index in free-fly mode.
261
262    Naming convention:
263        target_pos   – the raw argument from controller() (None in free-fly mode).
264                       Named identically to the controller() parameter to make the
265                       pass-through explicit: controller receives target_pos from
266                       run.py and passes it here unchanged.
267        dwell_bonus  – float returned to controller() and forwarded to
268                       _update_learning_agent() as dwell_bonus.
269        is_within    – bool: True if BOTH dist and yaw are inside tolerance.
270
271    Ref: Coursework spec – 10 s stabilisation window; Lecture 10, p.16.
272    """
273    is_within   = errs["dist"] < _POS_TOL and abs(errs["eyaw"]) < _YAW_TOL
274    dwell_bonus = 0.0
275
276    if is_within:
277        mem["dwell_steps"] += 1
278        if mem["dwell_steps"] == 1:
279            dwell_bonus = _REWARD_WEIGHTS["dwell_enter"]
280        elif mem["dwell_steps"] >= _HOLD_STEPS:
281            dwell_bonus         = _REWARD_WEIGHTS["dwell_complete"]
282            mem["dwell_steps"]  = 0
283            mem["integral"]     = [0.0, 0.0, 0.0]
284            mem["integral_yaw"] = 0.0
285            if target_pos is None:
286                mem["wp_idx"] = (mem["wp_idx"] + 1) % len(_CSV_TARGETS)
287    else:
288        mem["dwell_steps"] = 0
289
290    return dwell_bonus
291
292
293def _choose_action(mem, state_key, force_exploit=False, mask_coarse=False, mask_fine=False):
294    """
295    [ADVANCED METHOD – ε-greedy Policy with Bi-directional Safety Masking]
296
297    Naming convention:
298        state_key    – current Q-table lookup key (dist_band, yaw_band, wind_int).
299        q_vals       – raw Q-values for state_key from the Q-table (list of 16 floats).
300        q_masked     – Q-values after safety masking (-1e9 for forbidden actions).
301        action_idx   – integer 0-15 index of the selected action profile.
302        eps          – effective epsilon (0.0 if force_exploit, else mem["epsilon"]).
303        best_q       – maximum value in q_masked (used for greedy selection).
304        tied_actions – list of action indices that all share best_q (tie-breaking).
305
306    SAFETY MASKING (Ref: Lecture 10, p.22):
307        mask_coarse (dist < 0.1 m): blocks profiles 0-3 (high-speed).
308        mask_fine   (dist > 1.5 m): blocks profiles 8-15 (slow hover).
309        Forbidden entries set to -1e9 so argmax never selects them.
310    """
311    q_vals = mem["q_table"].setdefault(state_key, [0.0] * len(_ACTION_PROFILES))
312
313    if mask_coarse:
314        q_masked = [v if i >= 4 else -1e9 for i, v in enumerate(q_vals)]
315    elif mask_fine:
316        q_masked = [v if i < 8  else -1e9 for i, v in enumerate(q_vals)]
317    else:
318        q_masked = q_vals
319
320    eps = 0.0 if force_exploit else mem["epsilon"]
321
322    if mem["rng"].random() < eps:
323        if mask_coarse: return mem["rng"].randrange(4, 16)
324        if mask_fine:   return mem["rng"].randrange(0, 8)
325        return mem["rng"].randrange(0, 16)
326
327    best_q       = max(q_masked)
328    tied_actions = [i for i, v in enumerate(q_masked) if v == best_q]
329    action_idx   = tied_actions[mem["rng"].randrange(len(tied_actions))]
330    return action_idx
331
332
333def _init_memory():
334    """
335    Initialise the persistent state dictionary stored as controller._mem.
336
337    Naming convention for all keys:
338        wp_idx       – waypoint index into _CSV_TARGETS (free-fly mode only).
339        q_table      – dict mapping state_key → list of 16 Q-values (action_idx → float).
340        alpha        – TD learning rate (float).
341        gamma        – discount factor (float).
342        epsilon      – current exploration probability (float, decays over time).
343        eps_decay    – per-step multiplicative decay applied to epsilon.
344        eps_floor    – minimum value epsilon can decay to.
345        replay       – list of (state_key, action_idx, reward, next_state_key) tuples.
346        replay_cap   – maximum replay buffer length (FIFO eviction when exceeded).
347        replay_batch – number of transitions sampled per mini-batch update.
348        prev_key     – state_key from the previous controller() call.
349        prev_action  – action_idx from the previous controller() call.
350        prev_cost    – RL cost from the previous controller() call.
351        prev_effort  – total velocity magnitude from the previous controller() call.
352        integral     – [ix, iy, iz]: world-frame PID integral accumulators (m·s).
353        integral_yaw – yaw PID integral accumulator (rad·s).
354        prev_err     – [ex_w, ey_w, ez]: world-frame errors from previous call (for D-term).
355        dwell_steps  – consecutive in-tolerance steps counter.
356        step_count   – total controller() calls (used for TD update throttling).
357        rng          – seeded random.Random instance for reproducibility.
358    """
359    return {
360        "wp_idx":       0,
361        "q_table":      {},
362        "alpha":        0.20,
363        "gamma":        0.94,
364        "epsilon":      0.25,
365        "eps_decay":    0.9985,
366        "eps_floor":    0.04,
367        "replay":       [],
368        "replay_cap":   400,
369        "replay_batch": 16,
370        "prev_key":     None,
371        "prev_action":  None,
372        "prev_cost":    None,
373        "prev_effort":  0.0,
374        "integral":     [0.0, 0.0, 0.0],
375        "integral_yaw": 0.0,
376        "prev_err":     [0.0, 0.0, 0.0],
377        "dwell_steps":  0,
378        "step_count":   0,
379        "rng":          random.Random(42),
380    }
381
382
383# =============================================================================
384# MAIN CONTROLLER – DO NOT modify inputs, outputs, or function name.
385# Ref: Coursework spec p.9 – "DO NOT MODIFY INPUT/OUTPUT variable names."
386# =============================================================================
387
388def controller(state, target_pos, dt, wind_enabled=False):
389    """
390    [ADVANCED METHOD – Cascade RL+PID Controller]
391
392    NAMING CONVENTION used throughout this function:
393        target_pos    – raw argument from run.py (may be None in free-fly mode).
394        active_target – resolved 4-tuple (tx, ty, tz, tyaw) used for all computation.
395        errs          – dict from _get_errors with keys: dist, cost, eyaw, ez,
396                        ex_w, ey_w, ex_b, ey_b.
397        state_key     – discretised RL state (dist_band, yaw_band, wind_int).
398        dwell_bonus   – float reward from _handle_dwell; forwarded to _update_learning_agent.
399        action_idx    – integer 0-15 index of the selected gain profile.
400        kp_xy … yaw_rate_max – gains and caps unpacked from _ACTION_PROFILES[action_idx].
401        integ_limit   – anti-windup clamp value for integral accumulators.
402        d_ex_w, d_ey_w, d_ez – finite-difference derivatives of world-frame errors.
403        vx_w_pid … vz_w_pid   – raw PID outputs in world frame (before speed scaling).
404        v_mag_pid     – 3-D magnitude of the raw PID world-frame velocity vector.
405        v_cruise_max  – safe maximum cruise speed = min(v_xy_max, v_z_max).
406        v_scale       – scalar applied to world-frame vector to enforce speed limit.
407        vx_w … vz_w   – speed-scaled world-frame velocity commands.
408        cos_yaw, sin_yaw – precomputed trig for the body-frame rotation.
409        vx, vy, vz    – final body-frame velocity commands returned to run.py.
410        yr            – yaw rate command returned to run.py.
411
412    CASCADE ARCHITECTURE:
413        OUTER LOOP: Q-learning selects action_idx (gain profile) based on state_key.
414        INNER LOOP: PID uses selected gains to compute vx, vy, vz, yr.
415        Ref: Lecture 10, p.42-43 – cascade control.
416    """
417    if not hasattr(controller, "_mem"):
418        controller._mem = _init_memory()
419    mem = controller._mem
420    mem["step_count"] += 1
421
422    # =========================================================================
423    # STEP 1 – Resolve active_target from target_pos; compute errs and state_key
424    # =========================================================================
425    # target_pos: raw from run.py (marker auto-tester always provides this).
426    # active_target: resolved 4-tuple used for all error computation below.
427    active_target = (
428        tuple(float(v) for v in target_pos)
429        if target_pos is not None
430        else _CSV_TARGETS[mem["wp_idx"]]
431    )
432
433    errs      = _get_errors(state, active_target)
434    state_key = _state_key(errs["dist"], abs(errs["eyaw"]), wind_enabled)
435
436    # =========================================================================
437    # STEP 2 – Dwell-hold and Q-learning update
438    # dwell_bonus forwarded from _handle_dwell → _update_learning_agent.
439    # target_pos passed to _handle_dwell (not active_target) so it can detect
440    # free-fly mode (target_pos is None) vs simulator mode (target_pos provided).
441    # =========================================================================
442    dwell_bonus = _handle_dwell(mem, errs, target_pos)
443    _update_learning_agent(mem, errs, dwell_bonus, state_key)
444
445    # =========================================================================
446    # STEP 3 – Select action_idx via ε-greedy policy with safety masking
447    # =========================================================================
448    action_idx = _choose_action(
449        mem, state_key,
450        force_exploit=(errs["dist"] < 0.5),   # Pure greedy when close.
451        mask_coarse=(errs["dist"] < 0.1),     # Block profiles 0-3 near target.
452        mask_fine=(errs["dist"] > 1.5),       # Block profiles 8-15 far from target.
453    )
454
455    (kp_xy, kp_z, kp_yaw,
456     ki_xy, ki_z, ki_yaw,
457     kd_xy, kd_z,
458     v_xy_max, v_z_max, yaw_rate_max) = _ACTION_PROFILES[action_idx]
459
460    # =========================================================================
461    # STEP 4 – PID integral accumulation (world frame, anti-windup)
462    # Uses ex_w, ey_w (world frame) not ex_b, ey_b (body frame).
463    # World-frame integration avoids false windup when the drone yaws.
464    # Ref: Lecture 10, p.10-11 – I-term and anti-windup clamping.
465    # =========================================================================
466    if dt > 0.0:
467        integ_limit = (
468            0.40 if errs["dist"] < _POS_TOL   # Tight near target.
469            else 1.00 if wind_enabled          # Wide under wind.
470            else 0.60                          # Standard approach.
471        )
472        mem["integral"][0]  = _clip(mem["integral"][0]  + errs["ex_w"] * dt, -integ_limit, integ_limit)
473        mem["integral"][1]  = _clip(mem["integral"][1]  + errs["ey_w"] * dt, -integ_limit, integ_limit)
474        mem["integral"][2]  = _clip(mem["integral"][2]  + errs["ez"]   * dt, -integ_limit, integ_limit)
475        mem["integral_yaw"] = _clip(mem["integral_yaw"] + errs["eyaw"] * dt, -0.30, 0.30)
476
477    # =========================================================================
478    # STEP 5 – PID derivative (world frame, spike-clamped)
479    # d_ex_w, d_ey_w, d_ez: finite differences of world-frame errors.
480    # prev_err stores [ex_w, ey_w, ez] from the previous call (set in Step 9).
481    # Clamped to ±3.0 to suppress sensor noise spikes.
482    # Ref: Lecture 10, p.12 – D-term finite difference.
483    # =========================================================================
484    d_ex_w = _clip((errs["ex_w"] - mem["prev_err"][0]) / dt, -3.0, 3.0) if dt > 0 else 0.0
485    d_ey_w = _clip((errs["ey_w"] - mem["prev_err"][1]) / dt, -3.0, 3.0) if dt > 0 else 0.0
486    d_ez   = _clip((errs["ez"]   - mem["prev_err"][2]) / dt, -3.0, 3.0) if dt > 0 else 0.0
487
488    # =========================================================================
489    # STEP 6 – Raw PID outputs in world frame (vx_w_pid, vy_w_pid, vz_w_pid)
490    # _pid suffix marks these as the direct PID outputs before speed scaling.
491    # v = Kp·e + Ki·∫e·dt + Kd·(de/dt). Ref: Lecture 10, p.8-13.
492    # =========================================================================
493    vx_w_pid = kp_xy * errs["ex_w"] + ki_xy * mem["integral"][0] + kd_xy * d_ex_w
494    vy_w_pid = kp_xy * errs["ey_w"] + ki_xy * mem["integral"][1] + kd_xy * d_ey_w
495    vz_w_pid = kp_z  * errs["ez"]   + ki_z  * mem["integral"][2] + kd_z  * d_ez
496    yr       = _clip(kp_yaw * errs["eyaw"] + ki_yaw * mem["integral_yaw"],
497                     -yaw_rate_max, yaw_rate_max)
498
499    # =========================================================================
500    # STEP 7 – 3-D vector speed scaling (straight-line constant-speed flight)
501    # v_mag_pid: magnitude of the raw PID world-frame velocity vector.
502    # v_cruise_max: safe maximum speed = min(v_xy_max, v_z_max).
503    # v_scale: scalar preserving direction while enforcing the speed limit.
504    # vx_w, vy_w, vz_w: scaled world-frame velocity commands.
505    #
506    # WHY SCALE INSTEAD OF CLIP?
507    #   Independent axis clipping changes the vector direction when any axis
508    #   saturates, causing the drone to follow a curved arc. Uniform scaling
509    #   preserves direction and produces a straight-line approach.
510    # =========================================================================
511    v_mag_pid    = math.sqrt(vx_w_pid**2 + vy_w_pid**2 + vz_w_pid**2)
512    v_cruise_max = min(v_xy_max, v_z_max)
513
514    if v_mag_pid < 0.001:
515        v_scale = 0.0                                      # Essentially stationary.
516    elif errs["dist"] > 0.25:
517        # Cruising Phase: Force the magnitude to EXACTLY v_cruise_max.
518        # This guarantees an "even speed throughout the flight time" while maintaining the
519        # exact straight-line 3D direction prescribed by the World Frame PID.
520        v_scale = v_cruise_max / v_mag_pid                 # Cruising: enforce constant speed.
521    else:
522        # Precision Phase (< 0.25m): Allow the drone to slow down naturally to hold hover.
523        # We simply cap the maximum vector magnitude without forcing a constant speed.
524        v_scale = min(1.0, v_cruise_max / v_mag_pid)       # Precision: cap but allow slowdown.
525
526    vx_w = vx_w_pid * v_scale
527    vy_w = vy_w_pid * v_scale
528    vz_w = vz_w_pid * v_scale
529
530    # =========================================================================
531    # STEP 8 – Rotate world-frame commands into body frame (vx, vy, vz)
532    # cos_yaw, sin_yaw: trig values for the R_z(yaw) rotation matrix.
533    # Ref: Lecture 6, p.14 – R_z(ψ); Coursework spec p.10 – body-frame commands.
534    # =========================================================================
535    cos_yaw, sin_yaw = math.cos(float(state[5])), math.sin(float(state[5]))
536    vx =  cos_yaw * vx_w + sin_yaw * vy_w
537    vy = -sin_yaw * vx_w + cos_yaw * vy_w
538    vz =  vz_w
539
540    # =========================================================================
541    # STEP 9 – Bookkeeping: store this step's values for next call's TD update
542    # prev_key, prev_action, prev_cost, prev_effort, prev_err all follow the
543    # prev_* naming convention consistently throughout _init_memory and callers.
544    # =========================================================================
545    mem["prev_key"]    = state_key
546    mem["prev_action"] = action_idx
547    mem["prev_cost"]   = errs["cost"]
548    mem["prev_effort"] = abs(vx) + abs(vy) + abs(vz) + abs(yr)
549    mem["prev_err"]    = [errs["ex_w"], errs["ey_w"], errs["ez"]]
550
551    # =========================================================================
552    # STEP 10 – Telemetry logging (zero control-loop impact)
553    # =========================================================================
554    _logger.log(
555        dt=dt, pos=state[0:3], euler=state[3:6],
556        vel_cmd=(vx, vy, vz), target=active_target,
557        wind=wind_enabled, profile_idx=action_idx,
558    )
559
560    return (vx, vy, vz, yr)