task_observer module

task_observer.py — Cooperative state-observer task for a 2-wheeled differential-drive robot.

Implements a discrete-time Luenberger observer that estimates the full robot state from sensor measurements each control cycle. The task is designed for MicroPython’s cooperative scheduler: calling run() returns a generator that yields control back to the scheduler after every update.

State vector x_hat (4 elements):

[center_distance, heading, omega_left, omega_right]

Input vector u (6 elements):

[voltage_left, voltage_right, dist_left, dist_right, heading, heading_rate]

Output vector y_hat (4 elements):

[dist_left, dist_right, heading, heading_rate]

class task_observer.task_observer(goFlag, distLeft, distRight, voltageLeft, voltageRight, heading, headingRate, observerCenterDistance, observerHeading, observerHeadingRate, observerOmegaLeft, observerOmegaRight, observerDistanceLeft, observerDistanceRight)

Bases: object

Cooperative Luenberger observer task for a differential-drive robot.

Each call to run() yields a generator suitable for use with a round-robin cooperative scheduler. On every active cycle the observer propagates the state estimate forward one step and publishes the results to shared inter-task variables.

All matrix operations use pre-allocated array.array buffers and flat tuple indexing to avoid heap allocation in the 50 Hz hot path.

run()

Cooperative function for scheduler

update(u_L, u_R, s_L, s_R, psi, psi_dot)

Advance the observer by one discrete time step.

Reads sensor values from the caller, packs them into the reusable input buffer, propagates x_hat via A_D and B_D, then computes the output estimate y_hat via C_D. All arithmetic uses pre-allocated buffers — no objects are created on the heap.

Parameters

u_L : float Left-motor voltage (V). u_R : float Right-motor voltage (V). s_L : float Left-wheel distance (m). s_R : float Right-wheel distance (m). psi : float IMU heading angle (rad). psi_dot : float IMU heading rate (rad/s).