From f36b2dcd1af87036951a5b189dbda7ed61aae54d Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 21:55:49 +0200 Subject: [PATCH 01/12] drv/geometry: add the effective track and the fitted lever arm for v3 AI-assisted: Claude Opus 5.5 --- drv/geometry.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/drv/geometry.h b/drv/geometry.h index 6812edc..4ebd323 100644 --- a/drv/geometry.h +++ b/drv/geometry.h @@ -56,6 +56,20 @@ /// because the photodiode sits on the centreline. #define DB_LH2_LEVER_ANGLE (0.0f) +/// Track in mm that odometry and the twist mixer divide by: the rotation the +/// robot actually makes on carpet for a given wheel travel difference. Tyre +/// scrub puts it above DB_TRACK, and more so the faster it turns; this is the +/// value for spins at up to about 150 mm/s per wheel. +/// TODO: provisional. Spins against LH2 read 80 to 82 mm up to that speed, +/// rising to 87 and beyond faster, and arcs about 85; replace with the floor fit. +#define DB_TRACK_EFFECTIVE (81.0f) + +/// Lever arm in mm fitted to the LH2 fixes of spins in place: 52.0 mm +/// clockwise and 51.3 mm counter-clockwise, 52 +/- 2 with the LH2 scale +/// error. The estimator uses this one; DB_LH2_LEVER_ARM stays the board +/// figure, which is 2 mm longer. +#define DB_LH2_LEVER_ARM_EFFECTIVE (51.5f) + // DotBot v1 and v2, none of whose dimensions have been measured. These are the // values both drivers carried before the v3 bench run, kept so those boards // behave exactly as they did. A zero lever arm is what the drivers assumed: @@ -68,6 +82,8 @@ #define DB_GEAR_RATIO (50.0f) ///< Motor shaft revolutions per wheel revolution #define DB_LH2_LEVER_ARM (0.0f) ///< Axle midpoint to photodiode, in mm #define DB_LH2_LEVER_ANGLE (0.0f) ///< Direction of that offset, degrees clockwise from forward +#define DB_TRACK_EFFECTIVE DB_TRACK ///< Track odometry divides by, in mm +#define DB_LH2_LEVER_ARM_EFFECTIVE DB_LH2_LEVER_ARM ///< Lever arm the estimator uses, in mm #endif /// mm of wheel travel per encoder count From b5497b804a59d452cd2bab251a08a72568159f26 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 22:01:12 +0200 Subject: [PATCH 02/12] drv/pose_estimator: add an EKF on odometry and position-only LH2 fixes AI-assisted: Claude Opus 5.5 --- Makefile | 7 +- doc/sphinx/drv.md | 1 + drv/drv.emProject | 9 + drv/pose_estimator.h | 206 +++++++++++++++ drv/pose_estimator/pose_estimator.c | 314 +++++++++++++++++++++++ tests/test_pose_estimator.c | 373 ++++++++++++++++++++++++++++ 6 files changed, 909 insertions(+), 1 deletion(-) create mode 100644 drv/pose_estimator.h create mode 100644 drv/pose_estimator/pose_estimator.c create mode 100644 tests/test_pose_estimator.c diff --git a/Makefile b/Makefile index 4e1a97a..5810ab8 100644 --- a/Makefile +++ b/Makefile @@ -128,13 +128,18 @@ HOST_CC ?= cc HOST_CFLAGS ?= -std=gnu11 -Wall -Wextra -Werror -O2 -DBOARD_DOTBOT_V3 -Idrv TEST_BUILD_DIR ?= build/tests -test: $(TEST_BUILD_DIR)/test_wheel_control +test: $(TEST_BUILD_DIR)/test_wheel_control $(TEST_BUILD_DIR)/test_pose_estimator $(TEST_BUILD_DIR)/test_wheel_control + $(TEST_BUILD_DIR)/test_pose_estimator $(TEST_BUILD_DIR)/test_wheel_control: tests/test_wheel_control.c drv/wheel_control/wheel_control.c drv/wheel_control.h drv/geometry.h @mkdir -p $(TEST_BUILD_DIR) $(HOST_CC) $(HOST_CFLAGS) -o $@ tests/test_wheel_control.c drv/wheel_control/wheel_control.c -lm +$(TEST_BUILD_DIR)/test_pose_estimator: tests/test_pose_estimator.c drv/pose_estimator/pose_estimator.c drv/pose_estimator.h drv/geometry.h + @mkdir -p $(TEST_BUILD_DIR) + $(HOST_CC) $(HOST_CFLAGS) -o $@ tests/test_pose_estimator.c drv/pose_estimator/pose_estimator.c -lm + artifacts: $(ARTIFACT_PROJECTS) @mkdir -p artifacts @for artifact in "$(ARTIFACTS)"; do \ diff --git a/doc/sphinx/drv.md b/doc/sphinx/drv.md index bcee157..b7faaf6 100644 --- a/doc/sphinx/drv.md +++ b/doc/sphinx/drv.md @@ -26,6 +26,7 @@ _api/drv_motors _api/drv_move _api/drv_n25q128 _api/drv_pid +_api/drv_pose_estimator _api/drv_protocol _api/drv_rgbled _api/drv_rgbled_pwm diff --git a/drv/drv.emProject b/drv/drv.emProject index 27daf89..3826110 100644 --- a/drv/drv.emProject +++ b/drv/drv.emProject @@ -182,6 +182,15 @@ + + + + + +#include + +//=========================== defines ========================================== + +/// Period of one scheduler tick, the unit of the elapsed_ticks argument +#define DB_POSE_ESTIMATOR_TICK_MS (10U) + +/// LH2 position variance per axis, in mm^2. +/// TODO: placeholder. At rest the solve does not leave one integer mm and spin +/// fixes fit a circle to 1.5 mm rms; set from the moving LH2 noise measurement. +#define DB_POSE_ESTIMATOR_R_POS_MM2 (9.0f) + +/// Position variance added per mm the axle midpoint travels, in mm^2 / mm. +/// TODO: placeholder, sized from 5 to 12 mm endpoint spread over 0.6 m. +#define DB_POSE_ESTIMATOR_Q_POS_MM2_PER_MM (0.1f) + +/// Heading variance added per mm the axle midpoint travels, in deg^2 / mm. +/// TODO: placeholder, sized from 1.8 +/- 1.1 deg of drift per 0.6 m straight. +#define DB_POSE_ESTIMATOR_Q_HEADING_ROLL_DEG2_PER_MM (0.005f) + +/// Heading variance added per mm of wheel travel difference |d_right - d_left|, +/// in deg^2 / mm, at or below the reference turning speed. +/// TODO: placeholder, sized for 5 deg over a 180 deg spin. +#define DB_POSE_ESTIMATOR_Q_HEADING_TURN_DEG2_PER_MM (0.1f) + +/// Wheel speed difference |v_right - v_left|, in mm/s, above which the turn +/// term grows in proportion: a spin at 125 mm/s per wheel. +/// TODO: placeholder, from the effective track holding at 80 to 82 mm up to +/// about 150 mm/s per wheel and rising beyond. +#define DB_POSE_ESTIMATOR_TURN_SPEED_REF_MM_S (250.0f) + +/// Squared Mahalanobis distance above which a fix is rejected: chi-square with +/// 2 degrees of freedom at 99.9 %. TODO: set from the outlier distribution. +#define DB_POSE_ESTIMATOR_GATE (13.8f) + +/// Scheduler ticks without an accepted fix before TRACKING becomes LOST (1 s) +#define DB_POSE_ESTIMATOR_TIMEOUT_TICKS (100U) + +/// Consistent fixes a chain needs before it may seed or reseed the pose +#define DB_POSE_ESTIMATOR_SEED_FIXES (3U) + +/// Largest difference, in mm, between how far a fix lies from the chain's first +/// fix and how far odometry says the photodiode moved, for it to join the chain +#define DB_POSE_ESTIMATOR_SEED_TOLERANCE_MM (20.0f) + +/// Photodiode travel in the body frame, in mm, a chain needs to solve for heading +#define DB_POSE_ESTIMATOR_ACQUIRE_MM (40.0f) + +/// Life-cycle state +typedef enum { + DB_POSE_ESTIMATOR_SEEDING, ///< No pose; collecting a chain of consistent fixes + DB_POSE_ESTIMATOR_TRACKING, ///< Pose valid, fixes gated against it + DB_POSE_ESTIMATOR_LOST, ///< No fix accepted for the timeout; pose predicted only +} db_pose_estimator_status_t; + +/// What an update did with a fix +typedef enum { + DB_POSE_ESTIMATOR_ACCEPTED, ///< Inside the gate, and applied + DB_POSE_ESTIMATOR_REJECTED, ///< Outside the gate, or broke the seed chain + DB_POSE_ESTIMATOR_CHAINED, ///< Joined the seed chain; no pose yet + DB_POSE_ESTIMATOR_SEEDED, ///< Completed a chain, and the pose was set from it +} db_pose_estimator_result_t; + +/// Model and noise; the app holds one, the estimator keeps a pointer to it +typedef struct { + float track_mm; ///< track odometry divides by, mm + float lever_mm; ///< axle midpoint to photodiode, mm + float lever_angle_deg; ///< direction of that offset, deg clockwise from forward + float r_pos_mm2; ///< LH2 variance per axis, mm^2 + float q_pos_mm2_per_mm; ///< position variance per mm travelled + float q_heading_roll_deg2_per_mm; ///< heading variance per mm travelled + float q_heading_turn_deg2_per_mm; ///< heading variance per mm of |d_right - d_left| + float turn_speed_ref_mm_s; ///< |v_right - v_left| above which the turn term scales up, mm/s + float gate; ///< squared Mahalanobis rejection threshold + uint32_t timeout_ticks; ///< ticks without an accepted fix before LOST + uint32_t seed_fixes; ///< fixes a seed chain needs + float seed_tolerance_mm; ///< chain consistency tolerance, mm + float acquire_mm; ///< body-frame photodiode travel needed for heading, mm +} db_pose_estimator_conf_t; + +/// Estimator state +typedef struct { + const db_pose_estimator_conf_t *conf; ///< model and noise, not owned + db_pose_estimator_status_t status; ///< life-cycle state + float x; ///< axle midpoint, mm + float y; ///< axle midpoint, mm + float theta; ///< heading, rad, in [-pi, pi) + float P[3][3]; ///< covariance of [x mm, y mm, theta rad] + uint32_t ticks_since_accept; ///< saturating + float chain_x; ///< first fix of the seed chain, mm + float chain_y; ///< first fix of the seed chain, mm + float chain_bx; ///< axle travel since that fix, mm, in the body frame at that fix + float chain_by; ///< axle travel since that fix, mm, in the body frame at that fix + float chain_dtheta; ///< rotation since that fix, rad + float chain_var_theta; ///< heading variance odometry added since that fix, rad^2 + uint32_t chain_count; ///< fixes in the chain, 0 when none + float last_d2; ///< squared Mahalanobis distance of the last gated fix + uint32_t predicts; ///< predict calls, wraps + uint32_t accepted; ///< fixes applied, wraps + uint32_t rejected; ///< fixes rejected, wraps + uint32_t seeds; ///< pose seeded or reseeded from a chain, wraps +} db_pose_estimator_t; + +//=========================== prototypes ======================================= + +/** + * @brief Bind the estimator to its model and start SEEDING + * + * @param[out] est Estimator state + * @param[in] conf Model and noise, which must outlive the estimator + */ +void db_pose_estimator_init(db_pose_estimator_t *est, const db_pose_estimator_conf_t *conf); + +/** + * @brief Set the pose outright and start TRACKING + * + * @param[in] est Estimator state + * @param[in] x_mm Axle midpoint + * @param[in] y_mm Axle midpoint + * @param[in] heading_deg Heading, 0 along +y, clockwise positive + * @param[in] heading_sd_deg Standard deviation of that heading + */ +void db_pose_estimator_seed(db_pose_estimator_t *est, float x_mm, float y_mm, float heading_deg, float heading_sd_deg); + +/** + * @brief Propagate the pose over one scheduler step of wheel travel + * + * @param[in] est Estimator state + * @param[in] counts_left Left encoder counts since the previous call, signed + * @param[in] counts_right Right encoder counts since the previous call, signed + * @param[in] elapsed_ticks Scheduler ticks since the previous call, for the fix timeout + */ +void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, int32_t counts_right, uint32_t elapsed_ticks); + +/** + * @brief Take one LH2 fix of the photodiode + * + * Call once per new fix sequence: the same fix applied twice shrinks the + * covariance without new information. + * + * @param[in] est Estimator state + * @param[in] x_mm Photodiode position + * @param[in] y_mm Photodiode position + * + * @return what was done with the fix + */ +db_pose_estimator_result_t db_pose_estimator_update(db_pose_estimator_t *est, float x_mm, float y_mm); + +/** + * @brief Heading, while TRACKING + * + * @param[in] est Estimator state + * @param[out] deg Heading in [-180, 180), 0 along +y, clockwise positive + * + * @return true while TRACKING, else false and deg untouched + */ +bool db_pose_estimator_heading_deg(const db_pose_estimator_t *est, float *deg); + +/** + * @brief Estimated photodiode position, while TRACKING + * + * @param[in] est Estimator state + * @param[out] x_mm Photodiode position + * @param[out] y_mm Photodiode position + * + * @return true while TRACKING, else false and the outputs untouched + */ +bool db_pose_estimator_sensor(const db_pose_estimator_t *est, float *x_mm, float *y_mm); + +#endif diff --git a/drv/pose_estimator/pose_estimator.c b/drv/pose_estimator/pose_estimator.c new file mode 100644 index 0000000..21201a8 --- /dev/null +++ b/drv/pose_estimator/pose_estimator.c @@ -0,0 +1,314 @@ +/** + * @file + * @ingroup drv_pose_estimator + * + * @brief Extended Kalman filter on [x, y, heading] + * + * @copyright Inria, 2026 + */ +#include +#include +#include +#include + +#include "geometry.h" +#include "pose_estimator.h" + +//=========================== defines ========================================== + +#define DEG_TO_RAD ((float)M_PI / 180.0f) +#define RAD_TO_DEG (180.0f / (float)M_PI) + +//=========================== private ========================================== + +static float _wrap(float a) { + while (a >= (float)M_PI) { + a -= 2.0f * (float)M_PI; + } + while (a < -(float)M_PI) { + a += 2.0f * (float)M_PI; + } + return a; +} + +/// Photodiode offset from the axle midpoint, in the world frame, at heading theta +static void _lever(const db_pose_estimator_conf_t *conf, float theta, float *lx, float *ly) { + float a = theta + conf->lever_angle_deg * DEG_TO_RAD; + *lx = -conf->lever_mm * sinf(a); + *ly = conf->lever_mm * cosf(a); +} + +static void _chain_start(db_pose_estimator_t *est, float x_mm, float y_mm) { + est->chain_x = x_mm; + est->chain_y = y_mm; + est->chain_bx = 0; + est->chain_by = 0; + est->chain_dtheta = 0; + est->chain_var_theta = 0; + est->chain_count = 1; +} + +/// Adds a fix to the seed chain, and sets the pose once the chain can solve for +/// heading. The fix and the chain's first fix are the photodiode at two times; +/// odometry gives the axle travel b and rotation dtheta between them in the +/// body frame of the first, so z - z0 = Rot(theta0) (b + Rot(dtheta) l - l) +/// with l the lever in the body frame. Lengths on both sides match whatever +/// theta0 is, which is the consistency test; their angles differ by theta0. +static db_pose_estimator_result_t _chain_add(db_pose_estimator_t *est, float x_mm, float y_mm) { + const db_pose_estimator_conf_t *conf = est->conf; + if (est->chain_count == 0) { + _chain_start(est, x_mm, y_mm); + return DB_POSE_ESTIMATOR_CHAINED; + } + + float l0x, l0y, l1x, l1y; + _lever(conf, 0, &l0x, &l0y); + _lever(conf, est->chain_dtheta, &l1x, &l1y); + float vx = est->chain_bx + l1x - l0x; + float vy = est->chain_by + l1y - l0y; + float zx = x_mm - est->chain_x; + float zy = y_mm - est->chain_y; + float body_mm = sqrtf(vx * vx + vy * vy); + float world_mm = sqrtf(zx * zx + zy * zy); + + if (fabsf(world_mm - body_mm) > conf->seed_tolerance_mm) { + _chain_start(est, x_mm, y_mm); + return DB_POSE_ESTIMATOR_REJECTED; + } + if (est->chain_count < UINT32_MAX) { + est->chain_count++; + } + if (est->chain_count < conf->seed_fixes || body_mm < conf->acquire_mm) { + return DB_POSE_ESTIMATOR_CHAINED; + } + + // Body-frame vectors turn to the world frame by Rot(theta), with body-forward + // (0, 1); atan2 differences are therefore heading differences. + float theta0 = atan2f(zy, zx) - atan2f(vy, vx); + float theta = _wrap(theta0 + est->chain_dtheta); + float lx, ly; + _lever(conf, theta, &lx, &ly); + + float var_theta = 2.0f * conf->r_pos_mm2 / (body_mm * body_mm) + est->chain_var_theta; + // d(axle)/d(theta) = -d(lever)/d(theta) = (ly, -lx) + float jx = ly; + float jy = -lx; + + est->x = x_mm - lx; + est->y = y_mm - ly; + est->theta = theta; + est->P[0][0] = conf->r_pos_mm2 + jx * jx * var_theta; + est->P[0][1] = jx * jy * var_theta; + est->P[1][0] = est->P[0][1]; + est->P[1][1] = conf->r_pos_mm2 + jy * jy * var_theta; + est->P[0][2] = jx * var_theta; + est->P[2][0] = est->P[0][2]; + est->P[1][2] = jy * var_theta; + est->P[2][1] = est->P[1][2]; + est->P[2][2] = var_theta; + est->status = DB_POSE_ESTIMATOR_TRACKING; + est->chain_count = 0; + est->ticks_since_accept = 0; + est->seeds++; + return DB_POSE_ESTIMATOR_SEEDED; +} + +/// Gated EKF update with h(x) = axle + lever(theta) +static db_pose_estimator_result_t _gated_update(db_pose_estimator_t *est, float x_mm, float y_mm) { + const db_pose_estimator_conf_t *conf = est->conf; + float (*P)[3] = est->P; + + float lx, ly; + _lever(conf, est->theta, &lx, &ly); + float y0 = x_mm - (est->x + lx); + float y1 = y_mm - (est->y + ly); + + // H = [[1, 0, a], [0, 1, b]] with (a, b) = d(lever)/d(theta) + float a = -ly; + float b = lx; + + float pht[3][2]; + for (int i = 0; i < 3; i++) { + pht[i][0] = P[i][0] + a * P[i][2]; + pht[i][1] = P[i][1] + b * P[i][2]; + } + float s00 = pht[0][0] + a * pht[2][0] + conf->r_pos_mm2; + float s01 = pht[0][1] + a * pht[2][1]; + float s10 = pht[1][0] + b * pht[2][0]; + float s11 = pht[1][1] + b * pht[2][1] + conf->r_pos_mm2; + float det = s00 * s11 - s01 * s10; + if (!(det > 0)) { + return DB_POSE_ESTIMATOR_REJECTED; + } + float i00 = s11 / det; + float i01 = -s01 / det; + float i10 = -s10 / det; + float i11 = s00 / det; + + float d2 = y0 * (i00 * y0 + i01 * y1) + y1 * (i10 * y0 + i11 * y1); + est->last_d2 = d2; + if (d2 > conf->gate) { + return DB_POSE_ESTIMATOR_REJECTED; + } + + float k[3][2]; + for (int i = 0; i < 3; i++) { + k[i][0] = pht[i][0] * i00 + pht[i][1] * i10; + k[i][1] = pht[i][0] * i01 + pht[i][1] * i11; + } + est->x += k[0][0] * y0 + k[0][1] * y1; + est->y += k[1][0] * y0 + k[1][1] * y1; + est->theta = _wrap(est->theta + k[2][0] * y0 + k[2][1] * y1); + + // P -= K (H P) with H P = pht transposed + float np[3][3]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + np[i][j] = P[i][j] - (k[i][0] * pht[j][0] + k[i][1] * pht[j][1]); + } + } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + P[i][j] = 0.5f * (np[i][j] + np[j][i]); + } + } + return DB_POSE_ESTIMATOR_ACCEPTED; +} + +//=========================== public =========================================== + +void db_pose_estimator_init(db_pose_estimator_t *est, const db_pose_estimator_conf_t *conf) { + memset(est, 0, sizeof(*est)); + est->conf = conf; + est->status = DB_POSE_ESTIMATOR_SEEDING; +} + +void db_pose_estimator_seed(db_pose_estimator_t *est, float x_mm, float y_mm, float heading_deg, float heading_sd_deg) { + memset(est->P, 0, sizeof(est->P)); + est->x = x_mm; + est->y = y_mm; + est->theta = _wrap(heading_deg * DEG_TO_RAD); + est->P[0][0] = est->conf->r_pos_mm2; + est->P[1][1] = est->conf->r_pos_mm2; + est->P[2][2] = (heading_sd_deg * DEG_TO_RAD) * (heading_sd_deg * DEG_TO_RAD); + est->status = DB_POSE_ESTIMATOR_TRACKING; + est->chain_count = 0; + est->ticks_since_accept = 0; +} + +void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, int32_t counts_right, uint32_t elapsed_ticks) { + const db_pose_estimator_conf_t *conf = est->conf; + est->predicts++; + + if (UINT32_MAX - est->ticks_since_accept < elapsed_ticks) { + est->ticks_since_accept = UINT32_MAX; + } else { + est->ticks_since_accept += elapsed_ticks; + } + if (est->status == DB_POSE_ESTIMATOR_TRACKING && est->ticks_since_accept > conf->timeout_ticks) { + est->status = DB_POSE_ESTIMATOR_LOST; + est->chain_count = 0; + } + + float d_left = (float)counts_left * DB_MM_PER_COUNT; + float d_right = (float)counts_right * DB_MM_PER_COUNT; + float d = 0.5f * (d_left + d_right); + float dd = fabsf(d_right - d_left); + float dtheta = -(d_right - d_left) / conf->track_mm; + + float turn_scale = 1.0f; + if (dd > 0 && conf->turn_speed_ref_mm_s > 0) { + float dt_s = (float)(elapsed_ticks ? elapsed_ticks : 1U) * (DB_POSE_ESTIMATOR_TICK_MS / 1000.0f); + float ratio = (dd / dt_s) / conf->turn_speed_ref_mm_s; + if (ratio > 1.0f) { + turn_scale = ratio; + } + } + float q_theta = (conf->q_heading_roll_deg2_per_mm * fabsf(d) + conf->q_heading_turn_deg2_per_mm * dd * turn_scale) * DEG_TO_RAD * DEG_TO_RAD; + float q_pos = conf->q_pos_mm2_per_mm * fabsf(d); + + if (est->status != DB_POSE_ESTIMATOR_TRACKING && est->chain_count > 0) { + float mid = est->chain_dtheta + 0.5f * dtheta; + est->chain_bx += -d * sinf(mid); + est->chain_by += d * cosf(mid); + est->chain_dtheta += dtheta; + est->chain_var_theta += q_theta; + } + if (est->status == DB_POSE_ESTIMATOR_SEEDING) { + return; + } + + float mid = est->theta + 0.5f * dtheta; + float c = cosf(mid); + float s = sinf(mid); + est->x += -d * s; + est->y += d * c; + est->theta = _wrap(est->theta + dtheta); + + // F = [[1, 0, f0], [0, 1, f1], [0, 0, 1]] + float(*P)[3] = est->P; + float f0 = -d * c; + float f1 = -d * s; + float fp[3][3]; + for (int j = 0; j < 3; j++) { + fp[0][j] = P[0][j] + f0 * P[2][j]; + fp[1][j] = P[1][j] + f1 * P[2][j]; + fp[2][j] = P[2][j]; + } + for (int i = 0; i < 3; i++) { + P[i][0] = fp[i][0] + fp[i][2] * f0; + P[i][1] = fp[i][1] + fp[i][2] * f1; + P[i][2] = fp[i][2]; + } + P[0][0] += q_pos; + P[1][1] += q_pos; + P[2][2] += q_theta; +} + +db_pose_estimator_result_t db_pose_estimator_update(db_pose_estimator_t *est, float x_mm, float y_mm) { + db_pose_estimator_result_t result; + if (est->status == DB_POSE_ESTIMATOR_SEEDING) { + result = _chain_add(est, x_mm, y_mm); + } else { + result = _gated_update(est, x_mm, y_mm); + if (result == DB_POSE_ESTIMATOR_ACCEPTED) { + est->status = DB_POSE_ESTIMATOR_TRACKING; + est->ticks_since_accept = 0; + est->chain_count = 0; + } else if (est->status == DB_POSE_ESTIMATOR_LOST) { + result = _chain_add(est, x_mm, y_mm); + if (result == DB_POSE_ESTIMATOR_CHAINED) { + result = DB_POSE_ESTIMATOR_REJECTED; + } + } + } + if (result == DB_POSE_ESTIMATOR_ACCEPTED) { + est->accepted++; + } else if (result == DB_POSE_ESTIMATOR_REJECTED) { + est->rejected++; + } + return result; +} + +bool db_pose_estimator_heading_deg(const db_pose_estimator_t *est, float *deg) { + if (est->status != DB_POSE_ESTIMATOR_TRACKING) { + return false; + } + float d = est->theta * RAD_TO_DEG; + if (d >= 180.0f) { + d -= 360.0f; + } + *deg = d; + return true; +} + +bool db_pose_estimator_sensor(const db_pose_estimator_t *est, float *x_mm, float *y_mm) { + if (est->status != DB_POSE_ESTIMATOR_TRACKING) { + return false; + } + float lx, ly; + _lever(est->conf, est->theta, &lx, &ly); + *x_mm = est->x + lx; + *y_mm = est->y + ly; + return true; +} diff --git a/tests/test_pose_estimator.c b/tests/test_pose_estimator.c new file mode 100644 index 0000000..653c577 --- /dev/null +++ b/tests/test_pose_estimator.c @@ -0,0 +1,373 @@ +/** + * @file + * @brief Host test of drv/pose_estimator against a simulated robot + * + * The simulated robot turns over the same track the estimator is given, and its + * photodiode sits on the same lever arm, so these checks prove the filter's + * logic, not the model's constants. + * + * @copyright Inria, 2026 + */ +#include +#include +#include + +#include "geometry.h" +#include "pose_estimator.h" + +//=========================== harness ========================================== + +static int _failed = 0; +static int _passed = 0; + +#define CHECK(cond, ...) \ + do { \ + if (cond) { \ + _passed++; \ + } else { \ + _failed++; \ + printf("FAIL %s:%d: ", __FILE__, __LINE__); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } \ + } while (0) + +#define DEG ((float)M_PI / 180.0f) + +static const db_pose_estimator_conf_t _conf = { + .track_mm = DB_TRACK_EFFECTIVE, + .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, + .lever_angle_deg = DB_LH2_LEVER_ANGLE, + .r_pos_mm2 = DB_POSE_ESTIMATOR_R_POS_MM2, + .q_pos_mm2_per_mm = DB_POSE_ESTIMATOR_Q_POS_MM2_PER_MM, + .q_heading_roll_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_ROLL_DEG2_PER_MM, + .q_heading_turn_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_TURN_DEG2_PER_MM, + .turn_speed_ref_mm_s = DB_POSE_ESTIMATOR_TURN_SPEED_REF_MM_S, + .gate = DB_POSE_ESTIMATOR_GATE, + .timeout_ticks = DB_POSE_ESTIMATOR_TIMEOUT_TICKS, + .seed_fixes = DB_POSE_ESTIMATOR_SEED_FIXES, + .seed_tolerance_mm = DB_POSE_ESTIMATOR_SEED_TOLERANCE_MM, + .acquire_mm = DB_POSE_ESTIMATOR_ACQUIRE_MM, +}; + +//=========================== simulated robot ================================== + +#define SIM_SUBSTEPS (20) +#define TICKS_PER_FIX (10U) +#define LH2_NOISE_SD_MM (1.0f) + +typedef struct { + float x; ///< axle midpoint, mm + float y; ///< axle midpoint, mm + float theta; ///< rad, 0 along +y, clockwise positive + uint32_t seed; ///< noise generator state +} robot_t; + +static void _robot_step(robot_t *r, int32_t counts_left, int32_t counts_right) { + float dl = (float)counts_left * DB_MM_PER_COUNT / SIM_SUBSTEPS; + float dr = (float)counts_right * DB_MM_PER_COUNT / SIM_SUBSTEPS; + for (int i = 0; i < SIM_SUBSTEPS; i++) { + float d = 0.5f * (dl + dr); + float dt = -(dr - dl) / _conf.track_mm; + float m = r->theta + 0.5f * dt; + r->x += -d * sinf(m); + r->y += d * cosf(m); + r->theta += dt; + } +} + +static float _noise(robot_t *r) { + // Box-Muller on a 32-bit LCG: reproducible across hosts + r->seed = r->seed * 1664525U + 1013904223U; + float u1 = ((float)(r->seed >> 8) + 1.0f) / 16777217.0f; + r->seed = r->seed * 1664525U + 1013904223U; + float u2 = (float)(r->seed >> 8) / 16777216.0f; + return sqrtf(-2.0f * logf(u1)) * cosf(2.0f * (float)M_PI * u2); +} + +static void _robot_sensor(robot_t *r, float noise_sd, float *x, float *y) { + *x = r->x - _conf.lever_mm * sinf(r->theta) + noise_sd * _noise(r); + *y = r->y + _conf.lever_mm * cosf(r->theta) + noise_sd * _noise(r); +} + +static float _angle_error_deg(float a_rad, float b_rad) { + float e = fmodf(a_rad - b_rad, 2.0f * (float)M_PI); + if (e >= (float)M_PI) { + e -= 2.0f * (float)M_PI; + } else if (e < -(float)M_PI) { + e += 2.0f * (float)M_PI; + } + return fabsf(e) / DEG; +} + +/// Drive both for a number of ticks, with a fix every TICKS_PER_FIX when fixes is set +static uint32_t _run(db_pose_estimator_t *est, robot_t *r, int32_t cl, int32_t cr, uint32_t ticks, int fixes, float noise_sd) { + uint32_t accepted = 0; + for (uint32_t t = 1; t <= ticks; t++) { + _robot_step(r, cl, cr); + db_pose_estimator_predict(est, cl, cr, 1); + if (fixes && (t % TICKS_PER_FIX) == 0) { + float zx, zy; + _robot_sensor(r, noise_sd, &zx, &zy); + if (db_pose_estimator_update(est, zx, zy) == DB_POSE_ESTIMATOR_ACCEPTED) { + accepted++; + } + } + } + return accepted; +} + +//=========================== tests ============================================ + +static void test_straight_prediction(void) { + db_pose_estimator_t est; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, 1000, 1000, 0, 1); + for (int t = 0; t < 100; t++) { + db_pose_estimator_predict(&est, 50, 50, 1); + } + float d = 5000 * DB_MM_PER_COUNT; + CHECK(fabsf(est.x - 1000) < 1e-3f && fabsf(est.y - (1000 + d)) < 0.05f, "heading 0 drives along +y: got (%.2f, %.2f), want (1000, %.2f)", est.x, est.y, 1000 + d); + + db_pose_estimator_seed(&est, 0, 0, -90, 1); + for (int t = 0; t < 100; t++) { + db_pose_estimator_predict(&est, 50, 50, 1); + } + float h = 0; + db_pose_estimator_heading_deg(&est, &h); + CHECK(fabsf(est.x - d) < 0.05f && fabsf(est.y) < 0.05f && fabsf(h + 90) < 1e-3f, "heading -90 drives along +x: got (%.2f, %.2f) at %.2f deg", est.x, est.y, h); +} + +static void test_arc_prediction(void) { + db_pose_estimator_t est; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, 0, 0, 0, 1); + // Left faster, so the turn is clockwise and heading rises + for (int t = 0; t < 200; t++) { + db_pose_estimator_predict(&est, 20, 10, 1); + } + float distance = 200 * 15 * DB_MM_PER_COUNT; + float turn = 200 * 10 * DB_MM_PER_COUNT / _conf.track_mm; + float k = turn / distance; + float want_x = (cosf(turn) - 1.0f) / k; + float want_y = sinf(turn) / k; + CHECK(fabsf(est.x - want_x) < 0.1f && fabsf(est.y - want_y) < 0.1f, "arc end: got (%.2f, %.2f), want (%.2f, %.2f)", est.x, est.y, want_x, want_y); + CHECK(fabsf(est.theta - turn) < 1e-4f, "arc turn: got %.4f rad, want %.4f", est.theta, turn); +} + +static void test_spin_prediction(void) { + db_pose_estimator_t est; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, 500, 500, 0, 1); + for (int t = 0; t < 100; t++) { + db_pose_estimator_predict(&est, 5, -5, 1); + } + float turn = 100 * 10 * DB_MM_PER_COUNT / _conf.track_mm; + CHECK(fabsf(est.x - 500) < 1e-3f && fabsf(est.y - 500) < 1e-3f, "a spin leaves the axle in place: got (%.3f, %.3f)", est.x, est.y); + CHECK(fabsf(est.theta - turn) < 1e-4f, "a spin turns by the travel difference over the effective track: got %.4f, want %.4f", est.theta, turn); +} + +static void test_lever_arm_sensor(void) { + db_pose_estimator_t est; + db_pose_estimator_init(&est, &_conf); + float x = 0, y = 0; + CHECK(!db_pose_estimator_sensor(&est, &x, &y), "no photodiode estimate before a pose"); + db_pose_estimator_seed(&est, 100, 200, 0, 1); + db_pose_estimator_sensor(&est, &x, &y); + CHECK(fabsf(x - 100) < 1e-3f && fabsf(y - (200 + _conf.lever_mm)) < 1e-3f, "heading 0 puts the photodiode along +y: got (%.2f, %.2f)", x, y); + db_pose_estimator_seed(&est, 100, 200, 90, 1); + db_pose_estimator_sensor(&est, &x, &y); + CHECK(fabsf(x - (100 - _conf.lever_mm)) < 1e-3f && fabsf(y - 200) < 1e-3f, "heading 90 puts the photodiode along -x: got (%.2f, %.2f)", x, y); +} + +static void test_lever_arm_spin_update(void) { + // Turning in place swings the photodiode round a 51.5 mm circle; with the + // lever arm in H every fix fits, and without it the fixes are outliers + db_pose_estimator_t est; + robot_t r = { .x = 1500, .y = 1000, .theta = 30 * DEG, .seed = 1 }; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 30, 2); + uint32_t accepted = _run(&est, &r, 5, -5, 700, 1, LH2_NOISE_SD_MM); + CHECK(accepted == 70, "all 70 fixes of a spin fit the lever arm, got %u", accepted); + CHECK(_angle_error_deg(est.theta, r.theta) < 1.0f, "heading tracks the spin, %.2f deg off", _angle_error_deg(est.theta, r.theta)); + CHECK(hypotf(est.x - r.x, est.y - r.y) < 2.0f, "the axle stays put, %.2f mm off", hypotf(est.x - r.x, est.y - r.y)); + CHECK(est.predicts > est.accepted, "predict runs more often than update: %u against %u", est.predicts, est.accepted); + + db_pose_estimator_conf_t no_lever = _conf; + no_lever.lever_mm = 0; + robot_t r2 = { .x = 1500, .y = 1000, .theta = 30 * DEG, .seed = 1 }; + db_pose_estimator_init(&est, &no_lever); + db_pose_estimator_seed(&est, r2.x, r2.y, 30, 2); + accepted = _run(&est, &r2, 5, -5, 300, 1, LH2_NOISE_SD_MM); + CHECK(accepted < 30, "without the lever arm a spin's fixes do not fit, %u of 30 accepted", accepted); +} + +static void test_converges_from_wrong_heading(void) { + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 40 * DEG, .seed = 2 }; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 0, 60); + _run(&est, &r, 10, 10, 300, 1, LH2_NOISE_SD_MM); + CHECK(_angle_error_deg(est.theta, r.theta) < 3.0f, "40 deg of seed error is gone after 3 s straight, %.2f deg left", _angle_error_deg(est.theta, r.theta)); + CHECK(est.rejected == 0, "no fix is rejected while converging, got %u", est.rejected); +} + +static void test_acquire_heading_from_motion(void) { + db_pose_estimator_t est; + robot_t r = { .x = 2000, .y = 800, .theta = 130 * DEG, .seed = 3 }; + db_pose_estimator_init(&est, &_conf); + _run(&est, &r, 0, 0, 100, 1, LH2_NOISE_SD_MM); + float h = 0; + CHECK(!db_pose_estimator_heading_deg(&est, &h), "no heading from a robot that has not moved"); + CHECK(est.status == DB_POSE_ESTIMATOR_SEEDING, "still seeding at rest, status %d", est.status); + + _run(&est, &r, 10, 10, 100, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING, "tracking after 1 s straight, status %d", est.status); + CHECK(_angle_error_deg(est.theta, r.theta) < 5.0f, "acquired heading within 5 deg, %.2f off", _angle_error_deg(est.theta, r.theta)); + _run(&est, &r, 10, 10, 200, 1, LH2_NOISE_SD_MM); + CHECK(_angle_error_deg(est.theta, r.theta) < 2.0f, "within 2 deg after 2 s more, %.2f off", _angle_error_deg(est.theta, r.theta)); + CHECK(hypotf(est.x - r.x, est.y - r.y) < 3.0f, "axle within 3 mm, %.2f off", hypotf(est.x - r.x, est.y - r.y)); +} + +static void test_acquire_heading_from_spin(void) { + // No translation at all: only the lever arm can reveal the heading + db_pose_estimator_t est; + robot_t r = { .x = 1200, .y = 1200, .theta = -60 * DEG, .seed = 4 }; + db_pose_estimator_init(&est, &_conf); + _run(&est, &r, -5, 5, 300, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING, "a spin in place acquires heading, status %d", est.status); + CHECK(_angle_error_deg(est.theta, r.theta) < 5.0f, "heading from a spin within 5 deg, %.2f off", _angle_error_deg(est.theta, r.theta)); + CHECK(hypotf(est.x - r.x, est.y - r.y) < 3.0f, "axle from a spin within 3 mm, %.2f off", hypotf(est.x - r.x, est.y - r.y)); +} + +static void test_gate_rejects_outlier(void) { + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 5 }; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 0, 2); + _run(&est, &r, 0, 0, 100, 1, LH2_NOISE_SD_MM); + float x = est.x, y = est.y, theta = est.theta; + float zx, zy; + _robot_sensor(&r, 0, &zx, &zy); + CHECK(db_pose_estimator_update(&est, zx + 300, zy) == DB_POSE_ESTIMATOR_REJECTED, "a fix 300 mm off is rejected, d2 %.1f", est.last_d2); + CHECK(est.x == x && est.y == y && est.theta == theta, "a rejected fix leaves the pose alone"); + CHECK(db_pose_estimator_update(&est, zx, zy) == DB_POSE_ESTIMATOR_ACCEPTED, "the next good fix is accepted, d2 %.1f", est.last_d2); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING, "one outlier does not lose the pose, status %d", est.status); +} + +static void test_timeout_reseeds(void) { + // Picked up and put down elsewhere, turned: the encoders never saw it, so + // every fix is outside the gate until the timeout, then a chain reseeds + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 6 }; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 0, 2); + _run(&est, &r, 0, 0, 50, 1, LH2_NOISE_SD_MM); + r.x = 1500; + r.y = 1300; + r.theta = 100 * DEG; + _run(&est, &r, 0, 0, _conf.timeout_ticks, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING, "still tracking until the timeout, status %d", est.status); + CHECK(est.rejected >= 9, "fixes after the move are rejected, %u", est.rejected); + _run(&est, &r, 0, 0, 20, 1, LH2_NOISE_SD_MM); + float h = 0; + CHECK(est.status == DB_POSE_ESTIMATOR_LOST && !db_pose_estimator_heading_deg(&est, &h), "lost after the timeout, with no heading, status %d", est.status); + _run(&est, &r, 10, 10, 150, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.seeds == 1, "reseeded once the robot moves, status %d, seeds %u", est.status, est.seeds); + CHECK(_angle_error_deg(est.theta, r.theta) < 5.0f, "reseeded heading within 5 deg, %.2f off", _angle_error_deg(est.theta, r.theta)); + CHECK(hypotf(est.x - r.x, est.y - r.y) < 5.0f, "reseeded axle within 5 mm, %.2f off", hypotf(est.x - r.x, est.y - r.y)); +} + +static void test_occlusion_keeps_heading(void) { + // No fixes at all for 2 s: lost, but odometry carried the pose, so the + // first fix back is inside the gate and nothing is reseeded + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 20 * DEG, .seed = 7 }; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 20, 2); + _run(&est, &r, 10, 10, 100, 1, LH2_NOISE_SD_MM); + _run(&est, &r, 10, 10, 200, 0, 0); + CHECK(est.status == DB_POSE_ESTIMATOR_LOST, "lost after 2 s without a fix, status %d", est.status); + float zx, zy; + _robot_sensor(&r, LH2_NOISE_SD_MM, &zx, &zy); + CHECK(db_pose_estimator_update(&est, zx, zy) == DB_POSE_ESTIMATOR_ACCEPTED, "the first fix back is accepted, d2 %.1f", est.last_d2); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.seeds == 0, "tracking again without a reseed, status %d, seeds %u", est.status, est.seeds); +} + +static void test_noise_scales_with_distance(void) { + db_pose_estimator_t a, b; + db_pose_estimator_init(&a, &_conf); + db_pose_estimator_seed(&a, 0, 0, 0, 2); + float before = a.P[2][2]; + for (int t = 0; t < 1000; t++) { + db_pose_estimator_predict(&a, 0, 0, 1); + } + CHECK(a.P[0][0] == a.conf->r_pos_mm2 && a.P[2][2] == before, "standing still adds no process noise over 1000 calls"); + + // The same spin in 100 calls or in one + db_pose_estimator_init(&a, &_conf); + db_pose_estimator_init(&b, &_conf); + db_pose_estimator_seed(&a, 0, 0, 0, 2); + db_pose_estimator_seed(&b, 0, 0, 0, 2); + for (int t = 0; t < 100; t++) { + db_pose_estimator_predict(&a, 10, -10, 1); + } + db_pose_estimator_predict(&b, 1000, -1000, 100); + CHECK(fabsf(a.P[2][2] - b.P[2][2]) < 1e-3f * b.P[2][2], "heading noise over a spin is independent of the call rate: %.6g against %.6g", a.P[2][2], b.P[2][2]); + + // The same straight in 100 calls or in one + db_pose_estimator_init(&a, &_conf); + db_pose_estimator_init(&b, &_conf); + db_pose_estimator_seed(&a, 0, 0, 0, 0); + db_pose_estimator_seed(&b, 0, 0, 0, 0); + for (int t = 0; t < 100; t++) { + db_pose_estimator_predict(&a, 10, 10, 1); + } + db_pose_estimator_predict(&b, 1000, 1000, 100); + CHECK(fabsf(a.P[2][2] - b.P[2][2]) < 1e-3f * b.P[2][2], "heading noise over a straight is independent of the call rate: %.6g against %.6g", a.P[2][2], b.P[2][2]); + CHECK(fabsf(a.P[1][1] - b.P[1][1]) < 1e-3f * b.P[1][1], "position noise over a straight is independent of the call rate: %.6g against %.6g", a.P[1][1], b.P[1][1]); + + // Twice the distance, twice the heading noise + db_pose_estimator_init(&b, &_conf); + db_pose_estimator_seed(&b, 0, 0, 0, 0); + db_pose_estimator_predict(&b, 2000, 2000, 200); + CHECK(fabsf(b.P[2][2] - 2.0f * a.P[2][2]) < 1e-3f * b.P[2][2], "heading noise doubles with the distance: %.6g against 2 x %.6g", b.P[2][2], a.P[2][2]); +} + +static void test_turn_noise_grows_with_turn_speed(void) { + // The same wheel travel difference, turned slowly or fast + int32_t counts = 1000; + float dd_mm = 2.0f * counts * DB_MM_PER_COUNT; + uint32_t slow = (uint32_t)ceilf(dd_mm / (0.5f * _conf.turn_speed_ref_mm_s) * 100.0f); + uint32_t slower = 2 * slow; + db_pose_estimator_t a, b, c; + db_pose_estimator_init(&a, &_conf); + db_pose_estimator_init(&b, &_conf); + db_pose_estimator_init(&c, &_conf); + db_pose_estimator_seed(&a, 0, 0, 0, 0); + db_pose_estimator_seed(&b, 0, 0, 0, 0); + db_pose_estimator_seed(&c, 0, 0, 0, 0); + db_pose_estimator_predict(&a, counts, -counts, slower); + db_pose_estimator_predict(&b, counts, -counts, slow); + db_pose_estimator_predict(&c, counts, -counts, slow / 4); + CHECK(fabsf(a.P[2][2] - b.P[2][2]) < 1e-3f * a.P[2][2], "below the reference turn speed the turn speed does not matter: %.6g against %.6g", a.P[2][2], b.P[2][2]); + CHECK(c.P[2][2] > 1.5f * b.P[2][2], "above it, faster turning adds more heading noise: %.6g against %.6g", c.P[2][2], b.P[2][2]); +} + +int main(void) { + test_straight_prediction(); + test_arc_prediction(); + test_spin_prediction(); + test_lever_arm_sensor(); + test_lever_arm_spin_update(); + test_converges_from_wrong_heading(); + test_acquire_heading_from_motion(); + test_acquire_heading_from_spin(); + test_gate_rejects_outlier(); + test_timeout_reseeds(); + test_occlusion_keeps_heading(); + test_noise_scales_with_distance(); + test_turn_noise_grows_with_turn_speed(); + printf("%d passed, %d failed\n", _passed, _failed); + return _failed ? EXIT_FAILURE : EXIT_SUCCESS; +} From 59b11c4a06ca6ecb2150904a4403f75feee9964b Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 22:01:27 +0200 Subject: [PATCH 03/12] drv/wheel_control: mix a body twist over the effective track AI-assisted: Claude Opus 5.5 --- drv/wheel_control.h | 2 +- drv/wheel_control/wheel_control.c | 4 ++-- tests/test_wheel_control.c | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/drv/wheel_control.h b/drv/wheel_control.h index ab4f7a3..02b6ba0 100644 --- a/drv/wheel_control.h +++ b/drv/wheel_control.h @@ -130,7 +130,7 @@ int8_t db_wheel_control_step(db_wheel_control_t *wheel, int32_t delta_counts, ui int32_t db_wheel_control_counts(int32_t acc, uint32_t dbl); /** - * @brief Wheel speeds for a body twist, over the track DB_TRACK + * @brief Wheel speeds for a body twist, over the track DB_TRACK_EFFECTIVE * * Clockwise turns speed the left wheel up: v_left = v + w L/2, v_right = v - w L/2. * diff --git a/drv/wheel_control/wheel_control.c b/drv/wheel_control/wheel_control.c index 13b648e..bc7bc05 100644 --- a/drv/wheel_control/wheel_control.c +++ b/drv/wheel_control/wheel_control.c @@ -154,6 +154,6 @@ int32_t db_wheel_control_counts(int32_t acc, uint32_t dbl) { void db_wheel_control_from_twist(const db_body_twist_t *twist, float *left_mm_s, float *right_mm_s) { float w = twist->omega_deg_s * (float)M_PI / 180.0f; - *left_mm_s = twist->v_mm_s + w * DB_TRACK / 2.0f; - *right_mm_s = twist->v_mm_s - w * DB_TRACK / 2.0f; + *left_mm_s = twist->v_mm_s + w * DB_TRACK_EFFECTIVE / 2.0f; + *right_mm_s = twist->v_mm_s - w * DB_TRACK_EFFECTIVE / 2.0f; } diff --git a/tests/test_wheel_control.c b/tests/test_wheel_control.c index b6cbbcd..bb05dc9 100644 --- a/tests/test_wheel_control.c +++ b/tests/test_wheel_control.c @@ -444,7 +444,7 @@ static void test_twist(void) { CHECK(l == 120 && r == 120, "no turn rate drives both wheels equally, got %.1f %.1f", l, r); db_body_twist_t clockwise = { .v_mm_s = 0, .omega_deg_s = 90 }; db_wheel_control_from_twist(&clockwise, &l, &r); - float half = (float)M_PI / 2.0f * DB_TRACK / 2.0f; + float half = (float)M_PI / 2.0f * DB_TRACK_EFFECTIVE / 2.0f; CHECK(fabsf(l - half) < 1e-3f && fabsf(r + half) < 1e-3f, "clockwise speeds the left wheel up: got %.2f %.2f, want %.2f %.2f", l, r, half, -half); } From a8b8d454688df3e8a71a208a2aaeba5ca0db4f4d Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 22:15:21 +0200 Subject: [PATCH 04/12] drv/geometry: raise the effective track with the turn radius AI-assisted: Claude Opus 5.5 --- drv/geometry.h | 56 +++++++++++++++++++++++-------- drv/wheel_control.h | 2 +- drv/wheel_control/wheel_control.c | 10 ++++-- tests/test_wheel_control.c | 9 +++++ 4 files changed, 60 insertions(+), 17 deletions(-) diff --git a/drv/geometry.h b/drv/geometry.h index 4ebd323..9236a35 100644 --- a/drv/geometry.h +++ b/drv/geometry.h @@ -56,14 +56,18 @@ /// because the photodiode sits on the centreline. #define DB_LH2_LEVER_ANGLE (0.0f) -/// Track in mm that odometry and the twist mixer divide by: the rotation the -/// robot actually makes on carpet for a given wheel travel difference. Tyre -/// scrub puts it above DB_TRACK, and more so the faster it turns; this is the -/// value for spins at up to about 150 mm/s per wheel. -/// TODO: provisional. Spins against LH2 read 80 to 82 mm up to that speed, -/// rising to 87 and beyond faster, and arcs about 85; replace with the floor fit. +/// Effective track in mm for a spin in place: the rotation the robot actually +/// makes on carpet for a given wheel travel difference, fitted against LH2. +/// Good to 1 per cent up to 120 mm/s per wheel; it reads 83 at 200 and 87 at 300. #define DB_TRACK_EFFECTIVE (81.0f) +/// Effective track in mm for arcs of 100 mm radius and wider (fit 84.9, sd 1.5) +#define DB_TRACK_EFFECTIVE_ARC (85.0f) + +/// |v_left + v_right| / |v_right - v_left|, the turn radius over half the +/// track, at which the effective track reaches DB_TRACK_EFFECTIVE_ARC: 100 mm +#define DB_TRACK_EFFECTIVE_ARC_RATIO (2.35f) + /// Lever arm in mm fitted to the LH2 fixes of spins in place: 52.0 mm /// clockwise and 51.3 mm counter-clockwise, 52 +/- 2 with the LH2 scale /// error. The estimator uses this one; DB_LH2_LEVER_ARM stays the board @@ -76,17 +80,41 @@ // the sensor at the point of rotation. Do not "correct" these to the v3 // numbers; they describe different hardware. #else -#define DB_WHEEL_DIAMETER (40.0f) ///< Wheel diameter in mm -#define DB_TRACK (90.0f) ///< Distance between the two wheel mid-planes in mm -#define DB_ENCODER_CPR (12.0f) ///< Quadrature counts per motor shaft revolution -#define DB_GEAR_RATIO (50.0f) ///< Motor shaft revolutions per wheel revolution -#define DB_LH2_LEVER_ARM (0.0f) ///< Axle midpoint to photodiode, in mm -#define DB_LH2_LEVER_ANGLE (0.0f) ///< Direction of that offset, degrees clockwise from forward -#define DB_TRACK_EFFECTIVE DB_TRACK ///< Track odometry divides by, in mm -#define DB_LH2_LEVER_ARM_EFFECTIVE DB_LH2_LEVER_ARM ///< Lever arm the estimator uses, in mm +#define DB_WHEEL_DIAMETER (40.0f) ///< Wheel diameter in mm +#define DB_TRACK (90.0f) ///< Distance between the two wheel mid-planes in mm +#define DB_ENCODER_CPR (12.0f) ///< Quadrature counts per motor shaft revolution +#define DB_GEAR_RATIO (50.0f) ///< Motor shaft revolutions per wheel revolution +#define DB_LH2_LEVER_ARM (0.0f) ///< Axle midpoint to photodiode, in mm +#define DB_LH2_LEVER_ANGLE (0.0f) ///< Direction of that offset, degrees clockwise from forward +#define DB_TRACK_EFFECTIVE DB_TRACK ///< Track odometry divides by, in mm +#define DB_TRACK_EFFECTIVE_ARC DB_TRACK ///< The same for wide arcs, in mm +#define DB_TRACK_EFFECTIVE_ARC_RATIO (1.0f) ///< Unused while the two tracks are equal +#define DB_LH2_LEVER_ARM_EFFECTIVE DB_LH2_LEVER_ARM ///< Lever arm the estimator uses, in mm #endif /// mm of wheel travel per encoder count #define DB_MM_PER_COUNT (((float)M_PI * DB_WHEEL_DIAMETER) / (DB_ENCODER_CPR * DB_GEAR_RATIO)) +/** + * @brief Effective track for a turn, in mm + * + * DB_TRACK_EFFECTIVE in place, rising linearly with |left + right| / + * |right - left| to DB_TRACK_EFFECTIVE_ARC at DB_TRACK_EFFECTIVE_ARC_RATIO and + * held there. Only the ratio counts, so left and right may be speeds or + * distances. Spin speed is not modelled. + * + * @param[in] left Left wheel speed or travel + * @param[in] right Right wheel speed or travel + * + * @return track in mm + */ +static inline float db_track_effective_mm(float left, float right) { + float diff = fabsf(right - left); + float sum = fabsf(right + left); + if (sum >= DB_TRACK_EFFECTIVE_ARC_RATIO * diff) { + return DB_TRACK_EFFECTIVE_ARC; + } + return DB_TRACK_EFFECTIVE + (DB_TRACK_EFFECTIVE_ARC - DB_TRACK_EFFECTIVE) * sum / (DB_TRACK_EFFECTIVE_ARC_RATIO * diff); +} + #endif diff --git a/drv/wheel_control.h b/drv/wheel_control.h index 02b6ba0..bfcd614 100644 --- a/drv/wheel_control.h +++ b/drv/wheel_control.h @@ -130,7 +130,7 @@ int8_t db_wheel_control_step(db_wheel_control_t *wheel, int32_t delta_counts, ui int32_t db_wheel_control_counts(int32_t acc, uint32_t dbl); /** - * @brief Wheel speeds for a body twist, over the track DB_TRACK_EFFECTIVE + * @brief Wheel speeds for a body twist, over the effective track for its turn radius * * Clockwise turns speed the left wheel up: v_left = v + w L/2, v_right = v - w L/2. * diff --git a/drv/wheel_control/wheel_control.c b/drv/wheel_control/wheel_control.c index bc7bc05..b8c0c49 100644 --- a/drv/wheel_control/wheel_control.c +++ b/drv/wheel_control/wheel_control.c @@ -154,6 +154,12 @@ int32_t db_wheel_control_counts(int32_t acc, uint32_t dbl) { void db_wheel_control_from_twist(const db_body_twist_t *twist, float *left_mm_s, float *right_mm_s) { float w = twist->omega_deg_s * (float)M_PI / 180.0f; - *left_mm_s = twist->v_mm_s + w * DB_TRACK_EFFECTIVE / 2.0f; - *right_mm_s = twist->v_mm_s - w * DB_TRACK_EFFECTIVE / 2.0f; + float track = DB_TRACK_EFFECTIVE; + // The track depends on the radius the wheel speeds give, which depends on + // the track; the fixed point is a contraction and settles in a few passes + for (int i = 0; i < 3; i++) { + track = db_track_effective_mm(twist->v_mm_s + w * track / 2.0f, twist->v_mm_s - w * track / 2.0f); + } + *left_mm_s = twist->v_mm_s + w * track / 2.0f; + *right_mm_s = twist->v_mm_s - w * track / 2.0f; } diff --git a/tests/test_wheel_control.c b/tests/test_wheel_control.c index bb05dc9..a690372 100644 --- a/tests/test_wheel_control.c +++ b/tests/test_wheel_control.c @@ -446,6 +446,15 @@ static void test_twist(void) { db_wheel_control_from_twist(&clockwise, &l, &r); float half = (float)M_PI / 2.0f * DB_TRACK_EFFECTIVE / 2.0f; CHECK(fabsf(l - half) < 1e-3f && fabsf(r + half) < 1e-3f, "clockwise speeds the left wheel up: got %.2f %.2f, want %.2f %.2f", l, r, half, -half); + // 150 mm/s at 1 rad/s is a 150 mm radius, on the arc track + db_body_twist_t arc = { .v_mm_s = 150, .omega_deg_s = 180.0f / (float)M_PI }; + db_wheel_control_from_twist(&arc, &l, &r); + CHECK(fabsf(l - r - DB_TRACK_EFFECTIVE_ARC) < 1e-3f && fabsf(l + r - 300) < 1e-3f, "a wide arc turns over the arc track: got %.2f %.2f", l, r); + // A pivot on the right wheel: its track solves track = f(1), consistently + db_body_twist_t pivot = { .v_mm_s = 41, .omega_deg_s = 58 }; + db_wheel_control_from_twist(&pivot, &l, &r); + float want = db_track_effective_mm(l, r); + CHECK(fabsf((l - r) - want * 58 * (float)M_PI / 180.0f) < 1e-2f, "a tight turn uses the track its own wheel speeds give: %.2f %.2f over %.2f", l, r, want); } static void test_integral_zone(void) { From b5d087855299b380b25b27bab28e605f8afee01e Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 22:15:21 +0200 Subject: [PATCH 05/12] drv/pose_estimator: move fixes forward by their age and retune to the floor AI-assisted: Claude Opus 5.5 --- drv/pose_estimator.h | 41 +++++++++--- drv/pose_estimator/pose_estimator.c | 24 ++++++- tests/test_pose_estimator.c | 97 +++++++++++++++++++++++------ 3 files changed, 132 insertions(+), 30 deletions(-) diff --git a/drv/pose_estimator.h b/drv/pose_estimator.h index 2729046..5976050 100644 --- a/drv/pose_estimator.h +++ b/drv/pose_estimator.h @@ -9,10 +9,12 @@ * The state is the wheel-axle midpoint and the heading, in the screen frame * the rest of the system uses: x right, y down, heading 0 facing +y and * positive clockwise, so body-forward is (-sin, +cos). Predict runs on every - * scheduler tick from encoder deltas over DB_TRACK_EFFECTIVE; update runs once + * scheduler tick from encoder deltas over db_track_effective_mm(); update runs once * per new LH2 fix, with a position-only measurement of the photodiode, which * sits a lever arm ahead of the axle. That offset is what makes heading - * observable while the robot turns in place. + * observable while the robot turns in place. A fix is some ticks old when it + * arrives, so it is first moved forward by the photodiode travel odometry saw + * since. * * Process noise grows with distance travelled, never with the call rate, and * the heading's share also with how fast the robot turns, since slip is @@ -41,10 +43,10 @@ /// Period of one scheduler tick, the unit of the elapsed_ticks argument #define DB_POSE_ESTIMATOR_TICK_MS (10U) -/// LH2 position variance per axis, in mm^2. -/// TODO: placeholder. At rest the solve does not leave one integer mm and spin -/// fixes fit a circle to 1.5 mm rms; set from the moving LH2 noise measurement. -#define DB_POSE_ESTIMATOR_R_POS_MM2 (9.0f) +/// LH2 position variance per axis, in mm^2: 2 mm sigma, the top of the 1 to 2 mm +/// measured on moving straights and arcs, which also covers a fix age that is +/// 10 ms off DB_POSE_ESTIMATOR_FIX_AGE_TICKS +#define DB_POSE_ESTIMATOR_R_POS_MM2 (4.0f) /// Position variance added per mm the axle midpoint travels, in mm^2 / mm. /// TODO: placeholder, sized from 5 to 12 mm endpoint spread over 0.6 m. @@ -66,8 +68,21 @@ #define DB_POSE_ESTIMATOR_TURN_SPEED_REF_MM_S (250.0f) /// Squared Mahalanobis distance above which a fix is rejected: chi-square with -/// 2 degrees of freedom at 99.9 %. TODO: set from the outlier distribution. -#define DB_POSE_ESTIMATOR_GATE (13.8f) +/// 2 degrees of freedom at 99.999 %, about 14 to 18 mm per axis while tracking. +/// That passes every fix the floor saw on straights, arcs and spins up to +/// 300 mm/s per wheel (worst 17 mm). At 13.8 (99.9 %) the gate locked out the +/// fixes that would correct a 40 degree heading error. +/// TODO: fast spins showed tails to 32 mm, and no fix was off by more than +/// 50 mm, which argues for a gate near 40 mm; gross outliers are still unmeasured. +#define DB_POSE_ESTIMATOR_GATE (23.0f) + +/// Age of a fix when it reaches the estimator, in scheduler ticks. Each fix is +/// moved forward by the photodiode travel odometry saw over that many predicts. +/// TODO: estimated at 30 to 50 ms; the app can stamp sweep capture itself. +#define DB_POSE_ESTIMATOR_FIX_AGE_TICKS (4U) + +/// Longest fix age the estimator can compensate, in predict calls +#define DB_POSE_ESTIMATOR_FIX_AGE_MAX (8U) /// Scheduler ticks without an accepted fix before TRACKING becomes LOST (1 s) #define DB_POSE_ESTIMATOR_TIMEOUT_TICKS (100U) @@ -99,7 +114,6 @@ typedef enum { /// Model and noise; the app holds one, the estimator keeps a pointer to it typedef struct { - float track_mm; ///< track odometry divides by, mm float lever_mm; ///< axle midpoint to photodiode, mm float lever_angle_deg; ///< direction of that offset, deg clockwise from forward float r_pos_mm2; ///< LH2 variance per axis, mm^2 @@ -108,12 +122,20 @@ typedef struct { float q_heading_turn_deg2_per_mm; ///< heading variance per mm of |d_right - d_left| float turn_speed_ref_mm_s; ///< |v_right - v_left| above which the turn term scales up, mm/s float gate; ///< squared Mahalanobis rejection threshold + uint32_t fix_age_ticks; ///< fix age compensated, at most DB_POSE_ESTIMATOR_FIX_AGE_MAX uint32_t timeout_ticks; ///< ticks without an accepted fix before LOST uint32_t seed_fixes; ///< fixes a seed chain needs float seed_tolerance_mm; ///< chain consistency tolerance, mm float acquire_mm; ///< body-frame photodiode travel needed for heading, mm } db_pose_estimator_conf_t; +/// Photodiode travel over the most recent predicts, newest at head - 1 +typedef struct { + float x[DB_POSE_ESTIMATOR_FIX_AGE_MAX]; ///< mm + float y[DB_POSE_ESTIMATOR_FIX_AGE_MAX]; ///< mm + uint32_t head; ///< next slot +} db_pose_estimator_travel_t; + /// Estimator state typedef struct { const db_pose_estimator_conf_t *conf; ///< model and noise, not owned @@ -123,6 +145,7 @@ typedef struct { float theta; ///< heading, rad, in [-pi, pi) float P[3][3]; ///< covariance of [x mm, y mm, theta rad] uint32_t ticks_since_accept; ///< saturating + db_pose_estimator_travel_t travel; ///< for moving a fix forward by its age float chain_x; ///< first fix of the seed chain, mm float chain_y; ///< first fix of the seed chain, mm float chain_bx; ///< axle travel since that fix, mm, in the body frame at that fix diff --git a/drv/pose_estimator/pose_estimator.c b/drv/pose_estimator/pose_estimator.c index 21201a8..a9c364f 100644 --- a/drv/pose_estimator/pose_estimator.c +++ b/drv/pose_estimator/pose_estimator.c @@ -38,6 +38,10 @@ static void _lever(const db_pose_estimator_conf_t *conf, float theta, float *lx, *ly = conf->lever_mm * cosf(a); } +static void _travel_clear(db_pose_estimator_t *est) { + memset(&est->travel, 0, sizeof(est->travel)); +} + static void _chain_start(db_pose_estimator_t *est, float x_mm, float y_mm) { est->chain_x = x_mm; est->chain_y = y_mm; @@ -109,15 +113,24 @@ static db_pose_estimator_result_t _chain_add(db_pose_estimator_t *est, float x_m est->status = DB_POSE_ESTIMATOR_TRACKING; est->chain_count = 0; est->ticks_since_accept = 0; + _travel_clear(est); est->seeds++; return DB_POSE_ESTIMATOR_SEEDED; } -/// Gated EKF update with h(x) = axle + lever(theta) +/// Gated EKF update with h(x) = axle + lever(theta), on the fix moved forward +/// by the photodiode travel since it was captured static db_pose_estimator_result_t _gated_update(db_pose_estimator_t *est, float x_mm, float y_mm) { const db_pose_estimator_conf_t *conf = est->conf; float (*P)[3] = est->P; + uint32_t age = (conf->fix_age_ticks < DB_POSE_ESTIMATOR_FIX_AGE_MAX) ? conf->fix_age_ticks : DB_POSE_ESTIMATOR_FIX_AGE_MAX; + for (uint32_t i = 1; i <= age; i++) { + uint32_t slot = (est->travel.head + DB_POSE_ESTIMATOR_FIX_AGE_MAX - i) % DB_POSE_ESTIMATOR_FIX_AGE_MAX; + x_mm += est->travel.x[slot]; + y_mm += est->travel.y[slot]; + } + float lx, ly; _lever(conf, est->theta, &lx, &ly); float y0 = x_mm - (est->x + lx); @@ -194,6 +207,7 @@ void db_pose_estimator_seed(db_pose_estimator_t *est, float x_mm, float y_mm, fl est->status = DB_POSE_ESTIMATOR_TRACKING; est->chain_count = 0; est->ticks_since_accept = 0; + _travel_clear(est); } void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, int32_t counts_right, uint32_t elapsed_ticks) { @@ -214,7 +228,7 @@ void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, in float d_right = (float)counts_right * DB_MM_PER_COUNT; float d = 0.5f * (d_left + d_right); float dd = fabsf(d_right - d_left); - float dtheta = -(d_right - d_left) / conf->track_mm; + float dtheta = -(d_right - d_left) / db_track_effective_mm(d_left, d_right); float turn_scale = 1.0f; if (dd > 0 && conf->turn_speed_ref_mm_s > 0) { @@ -238,12 +252,18 @@ void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, in return; } + float lx0, ly0, lx1, ly1; + _lever(conf, est->theta, &lx0, &ly0); float mid = est->theta + 0.5f * dtheta; float c = cosf(mid); float s = sinf(mid); est->x += -d * s; est->y += d * c; est->theta = _wrap(est->theta + dtheta); + _lever(conf, est->theta, &lx1, &ly1); + est->travel.x[est->travel.head] = -d * s + lx1 - lx0; + est->travel.y[est->travel.head] = d * c + ly1 - ly0; + est->travel.head = (est->travel.head + 1) % DB_POSE_ESTIMATOR_FIX_AGE_MAX; // F = [[1, 0, f0], [0, 1, f1], [0, 0, 1]] float(*P)[3] = est->P; diff --git a/tests/test_pose_estimator.c b/tests/test_pose_estimator.c index 653c577..a9a29b0 100644 --- a/tests/test_pose_estimator.c +++ b/tests/test_pose_estimator.c @@ -2,7 +2,7 @@ * @file * @brief Host test of drv/pose_estimator against a simulated robot * - * The simulated robot turns over the same track the estimator is given, and its + * The simulated robot turns over the same effective track the estimator uses, and its * photodiode sits on the same lever arm, so these checks prove the filter's * logic, not the model's constants. * @@ -35,7 +35,6 @@ static int _passed = 0; #define DEG ((float)M_PI / 180.0f) static const db_pose_estimator_conf_t _conf = { - .track_mm = DB_TRACK_EFFECTIVE, .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, .lever_angle_deg = DB_LH2_LEVER_ANGLE, .r_pos_mm2 = DB_POSE_ESTIMATOR_R_POS_MM2, @@ -44,6 +43,7 @@ static const db_pose_estimator_conf_t _conf = { .q_heading_turn_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_TURN_DEG2_PER_MM, .turn_speed_ref_mm_s = DB_POSE_ESTIMATOR_TURN_SPEED_REF_MM_S, .gate = DB_POSE_ESTIMATOR_GATE, + .fix_age_ticks = DB_POSE_ESTIMATOR_FIX_AGE_TICKS, .timeout_ticks = DB_POSE_ESTIMATOR_TIMEOUT_TICKS, .seed_fixes = DB_POSE_ESTIMATOR_SEED_FIXES, .seed_tolerance_mm = DB_POSE_ESTIMATOR_SEED_TOLERANCE_MM, @@ -56,11 +56,22 @@ static const db_pose_estimator_conf_t _conf = { #define TICKS_PER_FIX (10U) #define LH2_NOISE_SD_MM (1.0f) +#define SIM_HISTORY (16U) + +typedef struct { + float x; ///< axle midpoint, mm + float y; ///< axle midpoint, mm + float theta; ///< rad, 0 along +y, clockwise positive +} pose_t; + typedef struct { - float x; ///< axle midpoint, mm - float y; ///< axle midpoint, mm - float theta; ///< rad, 0 along +y, clockwise positive - uint32_t seed; ///< noise generator state + float x; ///< axle midpoint, mm + float y; ///< axle midpoint, mm + float theta; ///< rad, 0 along +y, clockwise positive + uint32_t seed; ///< noise generator state + uint32_t steps; ///< steps taken + pose_t history[SIM_HISTORY]; ///< pose after each recent step + uint32_t fix_age; ///< steps a fix lags the robot by } robot_t; static void _robot_step(robot_t *r, int32_t counts_left, int32_t counts_right) { @@ -68,12 +79,14 @@ static void _robot_step(robot_t *r, int32_t counts_left, int32_t counts_right) { float dr = (float)counts_right * DB_MM_PER_COUNT / SIM_SUBSTEPS; for (int i = 0; i < SIM_SUBSTEPS; i++) { float d = 0.5f * (dl + dr); - float dt = -(dr - dl) / _conf.track_mm; + float dt = -(dr - dl) / db_track_effective_mm(dl, dr); float m = r->theta + 0.5f * dt; r->x += -d * sinf(m); r->y += d * cosf(m); r->theta += dt; } + r->history[r->steps % SIM_HISTORY] = (pose_t){ r->x, r->y, r->theta }; + r->steps++; } static float _noise(robot_t *r) { @@ -85,9 +98,15 @@ static float _noise(robot_t *r) { return sqrtf(-2.0f * logf(u1)) * cosf(2.0f * (float)M_PI * u2); } +/// The photodiode as LH2 reports it: the pose fix_age steps ago, or now +/// before the robot has taken that many static void _robot_sensor(robot_t *r, float noise_sd, float *x, float *y) { - *x = r->x - _conf.lever_mm * sinf(r->theta) + noise_sd * _noise(r); - *y = r->y + _conf.lever_mm * cosf(r->theta) + noise_sd * _noise(r); + pose_t p = { r->x, r->y, r->theta }; + if (r->fix_age > 0 && r->steps > r->fix_age) { + p = r->history[(r->steps - 1 - r->fix_age) % SIM_HISTORY]; + } + *x = p.x - _conf.lever_mm * sinf(p.theta) + noise_sd * _noise(r); + *y = p.y + _conf.lever_mm * cosf(p.theta) + noise_sd * _noise(r); } static float _angle_error_deg(float a_rad, float b_rad) { @@ -147,7 +166,7 @@ static void test_arc_prediction(void) { db_pose_estimator_predict(&est, 20, 10, 1); } float distance = 200 * 15 * DB_MM_PER_COUNT; - float turn = 200 * 10 * DB_MM_PER_COUNT / _conf.track_mm; + float turn = 200 * 10 * DB_MM_PER_COUNT / DB_TRACK_EFFECTIVE_ARC; float k = turn / distance; float want_x = (cosf(turn) - 1.0f) / k; float want_y = sinf(turn) / k; @@ -162,11 +181,23 @@ static void test_spin_prediction(void) { for (int t = 0; t < 100; t++) { db_pose_estimator_predict(&est, 5, -5, 1); } - float turn = 100 * 10 * DB_MM_PER_COUNT / _conf.track_mm; + float turn = 100 * 10 * DB_MM_PER_COUNT / DB_TRACK_EFFECTIVE; CHECK(fabsf(est.x - 500) < 1e-3f && fabsf(est.y - 500) < 1e-3f, "a spin leaves the axle in place: got (%.3f, %.3f)", est.x, est.y); CHECK(fabsf(est.theta - turn) < 1e-4f, "a spin turns by the travel difference over the effective track: got %.4f, want %.4f", est.theta, turn); } +static void test_effective_track(void) { + CHECK(db_track_effective_mm(100, -100) == DB_TRACK_EFFECTIVE, "a spin uses the spin track, got %.2f", db_track_effective_mm(100, -100)); + CHECK(db_track_effective_mm(-100, 100) == DB_TRACK_EFFECTIVE, "either way round, got %.2f", db_track_effective_mm(-100, 100)); + float pivot = DB_TRACK_EFFECTIVE + (DB_TRACK_EFFECTIVE_ARC - DB_TRACK_EFFECTIVE) / DB_TRACK_EFFECTIVE_ARC_RATIO; + CHECK(fabsf(db_track_effective_mm(0, 100) - pivot) < 1e-3f && fabsf(db_track_effective_mm(0, -100) - pivot) < 1e-3f, "a pivot on one wheel is part way, got %.2f, want %.2f", db_track_effective_mm(0, 100), pivot); + CHECK(fabsf(db_track_effective_mm(0, 10) - db_track_effective_mm(0, 1000)) < 1e-3f, "only the ratio counts"); + float r100 = 0.5f * DB_TRACK_EFFECTIVE_ARC_RATIO; + CHECK(fabsf(db_track_effective_mm(r100 + 0.5f, r100 - 0.5f) - DB_TRACK_EFFECTIVE_ARC) < 1e-3f, "a 100 mm radius reaches the arc track, got %.2f", db_track_effective_mm(r100 + 0.5f, r100 - 0.5f)); + CHECK(db_track_effective_mm(300, 200) == DB_TRACK_EFFECTIVE_ARC && db_track_effective_mm(-300, -200) == DB_TRACK_EFFECTIVE_ARC, "wider arcs hold the arc track, forward or back"); + CHECK(db_track_effective_mm(100, 100) == DB_TRACK_EFFECTIVE_ARC && db_track_effective_mm(0, 0) == DB_TRACK_EFFECTIVE_ARC, "no turn does not divide by zero"); +} + static void test_lever_arm_sensor(void) { db_pose_estimator_t est; db_pose_estimator_init(&est, &_conf); @@ -184,7 +215,7 @@ static void test_lever_arm_spin_update(void) { // Turning in place swings the photodiode round a 51.5 mm circle; with the // lever arm in H every fix fits, and without it the fixes are outliers db_pose_estimator_t est; - robot_t r = { .x = 1500, .y = 1000, .theta = 30 * DEG, .seed = 1 }; + robot_t r = { .x = 1500, .y = 1000, .theta = 30 * DEG, .seed = 1, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; db_pose_estimator_init(&est, &_conf); db_pose_estimator_seed(&est, r.x, r.y, 30, 2); uint32_t accepted = _run(&est, &r, 5, -5, 700, 1, LH2_NOISE_SD_MM); @@ -195,7 +226,7 @@ static void test_lever_arm_spin_update(void) { db_pose_estimator_conf_t no_lever = _conf; no_lever.lever_mm = 0; - robot_t r2 = { .x = 1500, .y = 1000, .theta = 30 * DEG, .seed = 1 }; + robot_t r2 = { .x = 1500, .y = 1000, .theta = 30 * DEG, .seed = 1, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; db_pose_estimator_init(&est, &no_lever); db_pose_estimator_seed(&est, r2.x, r2.y, 30, 2); accepted = _run(&est, &r2, 5, -5, 300, 1, LH2_NOISE_SD_MM); @@ -204,7 +235,7 @@ static void test_lever_arm_spin_update(void) { static void test_converges_from_wrong_heading(void) { db_pose_estimator_t est; - robot_t r = { .x = 1000, .y = 1000, .theta = 40 * DEG, .seed = 2 }; + robot_t r = { .x = 1000, .y = 1000, .theta = 40 * DEG, .seed = 2, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; db_pose_estimator_init(&est, &_conf); db_pose_estimator_seed(&est, r.x, r.y, 0, 60); _run(&est, &r, 10, 10, 300, 1, LH2_NOISE_SD_MM); @@ -214,7 +245,7 @@ static void test_converges_from_wrong_heading(void) { static void test_acquire_heading_from_motion(void) { db_pose_estimator_t est; - robot_t r = { .x = 2000, .y = 800, .theta = 130 * DEG, .seed = 3 }; + robot_t r = { .x = 2000, .y = 800, .theta = 130 * DEG, .seed = 3, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; db_pose_estimator_init(&est, &_conf); _run(&est, &r, 0, 0, 100, 1, LH2_NOISE_SD_MM); float h = 0; @@ -232,7 +263,7 @@ static void test_acquire_heading_from_motion(void) { static void test_acquire_heading_from_spin(void) { // No translation at all: only the lever arm can reveal the heading db_pose_estimator_t est; - robot_t r = { .x = 1200, .y = 1200, .theta = -60 * DEG, .seed = 4 }; + robot_t r = { .x = 1200, .y = 1200, .theta = -60 * DEG, .seed = 4, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; db_pose_estimator_init(&est, &_conf); _run(&est, &r, -5, 5, 300, 1, LH2_NOISE_SD_MM); CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING, "a spin in place acquires heading, status %d", est.status); @@ -242,7 +273,7 @@ static void test_acquire_heading_from_spin(void) { static void test_gate_rejects_outlier(void) { db_pose_estimator_t est; - robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 5 }; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 5, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; db_pose_estimator_init(&est, &_conf); db_pose_estimator_seed(&est, r.x, r.y, 0, 2); _run(&est, &r, 0, 0, 100, 1, LH2_NOISE_SD_MM); @@ -259,7 +290,7 @@ static void test_timeout_reseeds(void) { // Picked up and put down elsewhere, turned: the encoders never saw it, so // every fix is outside the gate until the timeout, then a chain reseeds db_pose_estimator_t est; - robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 6 }; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 6, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; db_pose_estimator_init(&est, &_conf); db_pose_estimator_seed(&est, r.x, r.y, 0, 2); _run(&est, &r, 0, 0, 50, 1, LH2_NOISE_SD_MM); @@ -282,7 +313,7 @@ static void test_occlusion_keeps_heading(void) { // No fixes at all for 2 s: lost, but odometry carried the pose, so the // first fix back is inside the gate and nothing is reseeded db_pose_estimator_t est; - robot_t r = { .x = 1000, .y = 1000, .theta = 20 * DEG, .seed = 7 }; + robot_t r = { .x = 1000, .y = 1000, .theta = 20 * DEG, .seed = 7, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; db_pose_estimator_init(&est, &_conf); db_pose_estimator_seed(&est, r.x, r.y, 20, 2); _run(&est, &r, 10, 10, 100, 1, LH2_NOISE_SD_MM); @@ -294,6 +325,32 @@ static void test_occlusion_keeps_heading(void) { CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.seeds == 0, "tracking again without a reseed, status %d, seeds %u", est.status, est.seeds); } +static void test_fix_age_compensated(void) { + // 300 mm/s straight and a 200 mm/s-per-wheel spin, with every fix + // DB_POSE_ESTIMATOR_FIX_AGE_TICKS old + int32_t fast = (int32_t)lroundf(300.0f * 0.01f / DB_MM_PER_COUNT); + int32_t spin = (int32_t)lroundf(200.0f * 0.01f / DB_MM_PER_COUNT); + for (int aged = 1; aged >= 0; aged--) { + db_pose_estimator_conf_t conf = _conf; + conf.fix_age_ticks = aged ? DB_POSE_ESTIMATOR_FIX_AGE_TICKS : 0; + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 8, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + db_pose_estimator_init(&est, &conf); + db_pose_estimator_seed(&est, r.x, r.y, 0, 2); + _run(&est, &r, fast, fast, 300, 1, LH2_NOISE_SD_MM); + float straight_mm = hypotf(est.x - r.x, est.y - r.y); + _run(&est, &r, spin, -spin, 200, 1, LH2_NOISE_SD_MM); + float spin_deg = _angle_error_deg(est.theta, r.theta); + if (aged) { + CHECK(est.rejected == 0, "no fix is rejected at speed once its age is compensated, got %u", est.rejected); + CHECK(straight_mm < 4.0f, "axle within 4 mm at 300 mm/s, %.2f off", straight_mm); + CHECK(spin_deg < 2.0f, "heading within 2 deg in a fast spin, %.2f off", spin_deg); + } else { + CHECK(est.rejected > 0 || straight_mm > 8.0f, "uncompensated, the same fixes lag or are rejected: %u rejected, %.2f mm off", est.rejected, straight_mm); + } + } +} + static void test_noise_scales_with_distance(void) { db_pose_estimator_t a, b; db_pose_estimator_init(&a, &_conf); @@ -358,6 +415,7 @@ int main(void) { test_straight_prediction(); test_arc_prediction(); test_spin_prediction(); + test_effective_track(); test_lever_arm_sensor(); test_lever_arm_spin_update(); test_converges_from_wrong_heading(); @@ -366,6 +424,7 @@ int main(void) { test_gate_rejects_outlier(); test_timeout_reseeds(); test_occlusion_keeps_heading(); + test_fix_age_compensated(); test_noise_scales_with_distance(); test_turn_noise_grows_with_turn_speed(); printf("%d passed, %d failed\n", _passed, _failed); From 80a78391914fea41660179a13161c3c29daf9560 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 06:35:48 +0200 Subject: [PATCH 06/12] drv/pose_estimator: apply clang-format AI-assisted: Claude Opus 5.5 --- drv/pose_estimator/pose_estimator.c | 28 ++++++++++++++-------------- tests/test_pose_estimator.c | 14 +++++++------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/drv/pose_estimator/pose_estimator.c b/drv/pose_estimator/pose_estimator.c index a9c364f..e695d97 100644 --- a/drv/pose_estimator/pose_estimator.c +++ b/drv/pose_estimator/pose_estimator.c @@ -98,19 +98,19 @@ static db_pose_estimator_result_t _chain_add(db_pose_estimator_t *est, float x_m float jx = ly; float jy = -lx; - est->x = x_mm - lx; - est->y = y_mm - ly; - est->theta = theta; - est->P[0][0] = conf->r_pos_mm2 + jx * jx * var_theta; - est->P[0][1] = jx * jy * var_theta; - est->P[1][0] = est->P[0][1]; - est->P[1][1] = conf->r_pos_mm2 + jy * jy * var_theta; - est->P[0][2] = jx * var_theta; - est->P[2][0] = est->P[0][2]; - est->P[1][2] = jy * var_theta; - est->P[2][1] = est->P[1][2]; - est->P[2][2] = var_theta; - est->status = DB_POSE_ESTIMATOR_TRACKING; + est->x = x_mm - lx; + est->y = y_mm - ly; + est->theta = theta; + est->P[0][0] = conf->r_pos_mm2 + jx * jx * var_theta; + est->P[0][1] = jx * jy * var_theta; + est->P[1][0] = est->P[0][1]; + est->P[1][1] = conf->r_pos_mm2 + jy * jy * var_theta; + est->P[0][2] = jx * var_theta; + est->P[2][0] = est->P[0][2]; + est->P[1][2] = jy * var_theta; + est->P[2][1] = est->P[1][2]; + est->P[2][2] = var_theta; + est->status = DB_POSE_ESTIMATOR_TRACKING; est->chain_count = 0; est->ticks_since_accept = 0; _travel_clear(est); @@ -122,7 +122,7 @@ static db_pose_estimator_result_t _chain_add(db_pose_estimator_t *est, float x_m /// by the photodiode travel since it was captured static db_pose_estimator_result_t _gated_update(db_pose_estimator_t *est, float x_mm, float y_mm) { const db_pose_estimator_conf_t *conf = est->conf; - float (*P)[3] = est->P; + float(*P)[3] = est->P; uint32_t age = (conf->fix_age_ticks < DB_POSE_ESTIMATOR_FIX_AGE_MAX) ? conf->fix_age_ticks : DB_POSE_ESTIMATOR_FIX_AGE_MAX; for (uint32_t i = 1; i <= age; i++) { diff --git a/tests/test_pose_estimator.c b/tests/test_pose_estimator.c index a9a29b0..3a3279f 100644 --- a/tests/test_pose_estimator.c +++ b/tests/test_pose_estimator.c @@ -52,9 +52,9 @@ static const db_pose_estimator_conf_t _conf = { //=========================== simulated robot ================================== -#define SIM_SUBSTEPS (20) -#define TICKS_PER_FIX (10U) -#define LH2_NOISE_SD_MM (1.0f) +#define SIM_SUBSTEPS (20) +#define TICKS_PER_FIX (10U) +#define LH2_NOISE_SD_MM (1.0f) #define SIM_HISTORY (16U) @@ -91,10 +91,10 @@ static void _robot_step(robot_t *r, int32_t counts_left, int32_t counts_right) { static float _noise(robot_t *r) { // Box-Muller on a 32-bit LCG: reproducible across hosts - r->seed = r->seed * 1664525U + 1013904223U; - float u1 = ((float)(r->seed >> 8) + 1.0f) / 16777217.0f; - r->seed = r->seed * 1664525U + 1013904223U; - float u2 = (float)(r->seed >> 8) / 16777216.0f; + r->seed = r->seed * 1664525U + 1013904223U; + float u1 = ((float)(r->seed >> 8) + 1.0f) / 16777217.0f; + r->seed = r->seed * 1664525U + 1013904223U; + float u2 = (float)(r->seed >> 8) / 16777216.0f; return sqrtf(-2.0f * logf(u1)) * cosf(2.0f * (float)M_PI * u2); } From 4890492fc14dac92cb5aa0c2ec0f1ca9c8ef0cd7 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 07:17:50 +0200 Subject: [PATCH 07/12] drv/pose_estimator: reseed at once when a still robot is moved by hand AI-assisted: Claude Opus 5.5 --- drv/pose_estimator.h | 20 +++++++++ drv/pose_estimator/pose_estimator.c | 41 ++++++++++++++++- tests/test_pose_estimator.c | 69 ++++++++++++++++++++++++----- 3 files changed, 118 insertions(+), 12 deletions(-) diff --git a/drv/pose_estimator.h b/drv/pose_estimator.h index 5976050..b7e4006 100644 --- a/drv/pose_estimator.h +++ b/drv/pose_estimator.h @@ -27,6 +27,12 @@ * a fix inside the gate returns to TRACKING, and a new consistent chain reseeds * the whole pose. Heading and pose are only valid while TRACKING. * + * Kidnap: while TRACKING, kidnap_fixes rejected fixes in a row that agree + * within seed_tolerance_mm, with at most kidnap_still_mm of wheel travel since + * the first of them, mean the robot was moved by hand. The estimator returns + * to SEEDING at once, with those fixes as the start of its chain, so heading + * stays unknown until motion re-acquires it. + * * No hardware calls, so the module also builds on the host for its tests. * * @{ @@ -97,6 +103,13 @@ /// Photodiode travel in the body frame, in mm, a chain needs to solve for heading #define DB_POSE_ESTIMATOR_ACQUIRE_MM (40.0f) +/// Consistent rejected fixes, with the wheels still, that mean a kidnap: 0.3 s at 10 Hz +#define DB_POSE_ESTIMATOR_KIDNAP_FIXES (3U) + +/// Most wheel travel, in mm, |d_left| + |d_right| summed since the first of +/// those fixes, for the wheels to count as still +#define DB_POSE_ESTIMATOR_KIDNAP_STILL_MM (2.0f) + /// Life-cycle state typedef enum { DB_POSE_ESTIMATOR_SEEDING, ///< No pose; collecting a chain of consistent fixes @@ -127,6 +140,8 @@ typedef struct { uint32_t seed_fixes; ///< fixes a seed chain needs float seed_tolerance_mm; ///< chain consistency tolerance, mm float acquire_mm; ///< body-frame photodiode travel needed for heading, mm + uint32_t kidnap_fixes; ///< consistent rejected fixes with the wheels still that reseed; 0 disables + float kidnap_still_mm; ///< wheel travel |d_left| + |d_right| over those fixes still counted as still, mm } db_pose_estimator_conf_t; /// Photodiode travel over the most recent predicts, newest at head - 1 @@ -153,11 +168,16 @@ typedef struct { float chain_dtheta; ///< rotation since that fix, rad float chain_var_theta; ///< heading variance odometry added since that fix, rad^2 uint32_t chain_count; ///< fixes in the chain, 0 when none + float kidnap_x; ///< first of the consecutive rejected fixes, mm + float kidnap_y; ///< first of the consecutive rejected fixes, mm + float kidnap_travel_mm; ///< wheel travel |d_left| + |d_right| since that fix, mm + uint32_t kidnap_count; ///< consecutive consistent rejected fixes, 0 when none float last_d2; ///< squared Mahalanobis distance of the last gated fix uint32_t predicts; ///< predict calls, wraps uint32_t accepted; ///< fixes applied, wraps uint32_t rejected; ///< fixes rejected, wraps uint32_t seeds; ///< pose seeded or reseeded from a chain, wraps + uint32_t kidnaps; ///< returns to SEEDING on a kidnap, wraps } db_pose_estimator_t; //=========================== prototypes ======================================= diff --git a/drv/pose_estimator/pose_estimator.c b/drv/pose_estimator/pose_estimator.c index e695d97..7e7335f 100644 --- a/drv/pose_estimator/pose_estimator.c +++ b/drv/pose_estimator/pose_estimator.c @@ -112,12 +112,40 @@ static db_pose_estimator_result_t _chain_add(db_pose_estimator_t *est, float x_m est->P[2][2] = var_theta; est->status = DB_POSE_ESTIMATOR_TRACKING; est->chain_count = 0; + est->kidnap_count = 0; est->ticks_since_accept = 0; _travel_clear(est); est->seeds++; return DB_POSE_ESTIMATOR_SEEDED; } +/// Counts a fix the gate rejected while TRACKING toward a kidnap, and on the +/// last one returns to SEEDING with the chain started on these fixes +static void _kidnap_check(db_pose_estimator_t *est, float x_mm, float y_mm) { + const db_pose_estimator_conf_t *conf = est->conf; + if (conf->kidnap_fixes == 0) { + return; + } + float dx = x_mm - est->kidnap_x; + float dy = y_mm - est->kidnap_y; + if (est->kidnap_count == 0 || est->kidnap_travel_mm > conf->kidnap_still_mm || sqrtf(dx * dx + dy * dy) > conf->seed_tolerance_mm) { + est->kidnap_x = x_mm; + est->kidnap_y = y_mm; + est->kidnap_travel_mm = 0; + est->kidnap_count = 1; + } else { + est->kidnap_count++; + } + if (est->kidnap_count < conf->kidnap_fixes) { + return; + } + est->status = DB_POSE_ESTIMATOR_SEEDING; + _chain_start(est, est->kidnap_x, est->kidnap_y); + est->chain_count = est->kidnap_count; + est->kidnap_count = 0; + est->kidnaps++; +} + /// Gated EKF update with h(x) = axle + lever(theta), on the fix moved forward /// by the photodiode travel since it was captured static db_pose_estimator_result_t _gated_update(db_pose_estimator_t *est, float x_mm, float y_mm) { @@ -206,6 +234,7 @@ void db_pose_estimator_seed(db_pose_estimator_t *est, float x_mm, float y_mm, fl est->P[2][2] = (heading_sd_deg * DEG_TO_RAD) * (heading_sd_deg * DEG_TO_RAD); est->status = DB_POSE_ESTIMATOR_TRACKING; est->chain_count = 0; + est->kidnap_count = 0; est->ticks_since_accept = 0; _travel_clear(est); } @@ -220,8 +249,9 @@ void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, in est->ticks_since_accept += elapsed_ticks; } if (est->status == DB_POSE_ESTIMATOR_TRACKING && est->ticks_since_accept > conf->timeout_ticks) { - est->status = DB_POSE_ESTIMATOR_LOST; - est->chain_count = 0; + est->status = DB_POSE_ESTIMATOR_LOST; + est->chain_count = 0; + est->kidnap_count = 0; } float d_left = (float)counts_left * DB_MM_PER_COUNT; @@ -241,6 +271,10 @@ void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, in float q_theta = (conf->q_heading_roll_deg2_per_mm * fabsf(d) + conf->q_heading_turn_deg2_per_mm * dd * turn_scale) * DEG_TO_RAD * DEG_TO_RAD; float q_pos = conf->q_pos_mm2_per_mm * fabsf(d); + if (est->kidnap_count > 0) { + est->kidnap_travel_mm += fabsf(d_left) + fabsf(d_right); + } + if (est->status != DB_POSE_ESTIMATOR_TRACKING && est->chain_count > 0) { float mid = est->chain_dtheta + 0.5f * dtheta; est->chain_bx += -d * sinf(mid); @@ -295,6 +329,9 @@ db_pose_estimator_result_t db_pose_estimator_update(db_pose_estimator_t *est, fl est->status = DB_POSE_ESTIMATOR_TRACKING; est->ticks_since_accept = 0; est->chain_count = 0; + est->kidnap_count = 0; + } else if (est->status == DB_POSE_ESTIMATOR_TRACKING) { + _kidnap_check(est, x_mm, y_mm); } else if (est->status == DB_POSE_ESTIMATOR_LOST) { result = _chain_add(est, x_mm, y_mm); if (result == DB_POSE_ESTIMATOR_CHAINED) { diff --git a/tests/test_pose_estimator.c b/tests/test_pose_estimator.c index 3a3279f..37fe034 100644 --- a/tests/test_pose_estimator.c +++ b/tests/test_pose_estimator.c @@ -48,6 +48,8 @@ static const db_pose_estimator_conf_t _conf = { .seed_fixes = DB_POSE_ESTIMATOR_SEED_FIXES, .seed_tolerance_mm = DB_POSE_ESTIMATOR_SEED_TOLERANCE_MM, .acquire_mm = DB_POSE_ESTIMATOR_ACQUIRE_MM, + .kidnap_fixes = DB_POSE_ESTIMATOR_KIDNAP_FIXES, + .kidnap_still_mm = DB_POSE_ESTIMATOR_KIDNAP_STILL_MM, }; //=========================== simulated robot ================================== @@ -286,9 +288,9 @@ static void test_gate_rejects_outlier(void) { CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING, "one outlier does not lose the pose, status %d", est.status); } -static void test_timeout_reseeds(void) { - // Picked up and put down elsewhere, turned: the encoders never saw it, so - // every fix is outside the gate until the timeout, then a chain reseeds +static void test_kidnap_reseeds(void) { + // Picked up and put down elsewhere, turned, wheels still: the estimator + // gives up the pose after DB_POSE_ESTIMATOR_KIDNAP_FIXES fixes, not the timeout db_pose_estimator_t est; robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 6, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; db_pose_estimator_init(&est, &_conf); @@ -297,18 +299,63 @@ static void test_timeout_reseeds(void) { r.x = 1500; r.y = 1300; r.theta = 100 * DEG; - _run(&est, &r, 0, 0, _conf.timeout_ticks, 1, LH2_NOISE_SD_MM); - CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING, "still tracking until the timeout, status %d", est.status); - CHECK(est.rejected >= 9, "fixes after the move are rejected, %u", est.rejected); - _run(&est, &r, 0, 0, 20, 1, LH2_NOISE_SD_MM); - float h = 0; - CHECK(est.status == DB_POSE_ESTIMATOR_LOST && !db_pose_estimator_heading_deg(&est, &h), "lost after the timeout, with no heading, status %d", est.status); + _run(&est, &r, 0, 0, (_conf.kidnap_fixes - 1) * TICKS_PER_FIX, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.kidnaps == 0, "still tracking one fix short of a kidnap, status %d", est.status); + _run(&est, &r, 0, 0, TICKS_PER_FIX, 1, LH2_NOISE_SD_MM); + float h = 0, sx = 0, sy = 0; + CHECK(est.status == DB_POSE_ESTIMATOR_SEEDING && est.kidnaps == 1, "a kidnap after %u fixes, status %d kidnaps %u", _conf.kidnap_fixes, est.status, est.kidnaps); + CHECK(!db_pose_estimator_heading_deg(&est, &h) && !db_pose_estimator_sensor(&est, &sx, &sy), "no heading and no pose after a kidnap"); + CHECK(hypotf(est.chain_x - (r.x - _conf.lever_mm * sinf(r.theta)), est.chain_y - (r.y + _conf.lever_mm * cosf(r.theta))) < 5.0f, "the chain starts on the new fixes"); + _run(&est, &r, 0, 0, 100, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_SEEDING && est.seeds == 0, "standing still does not guess a heading, status %d", est.status); _run(&est, &r, 10, 10, 150, 1, LH2_NOISE_SD_MM); CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.seeds == 1, "reseeded once the robot moves, status %d, seeds %u", est.status, est.seeds); CHECK(_angle_error_deg(est.theta, r.theta) < 5.0f, "reseeded heading within 5 deg, %.2f off", _angle_error_deg(est.theta, r.theta)); CHECK(hypotf(est.x - r.x, est.y - r.y) < 5.0f, "reseeded axle within 5 mm, %.2f off", hypotf(est.x - r.x, est.y - r.y)); } +static void test_outlier_still_no_kidnap(void) { + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 9, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 0, 2); + _run(&est, &r, 0, 0, 50, 1, LH2_NOISE_SD_MM); + float zx, zy; + _robot_sensor(&r, 0, &zx, &zy); + // Two outliers at the same spot, one short of a kidnap, then a good fix + for (uint32_t i = 0; i + 1 < _conf.kidnap_fixes; i++) { + db_pose_estimator_update(&est, zx + 200, zy); + } + _run(&est, &r, 0, 0, 10 * TICKS_PER_FIX, 1, LH2_NOISE_SD_MM); + db_pose_estimator_update(&est, zx + 200, zy); + _run(&est, &r, 0, 0, 10 * TICKS_PER_FIX, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.kidnaps == 0 && est.seeds == 0, "outliers broken up by good fixes do not reseed, status %d kidnaps %u", est.status, est.kidnaps); + // Rejected fixes that disagree with each other do not reseed either + for (uint32_t i = 0; i < 2 * _conf.kidnap_fixes; i++) { + db_pose_estimator_update(&est, zx + 200 + 50 * (float)i, zy); + } + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.kidnaps == 0, "scattered rejected fixes do not reseed, status %d kidnaps %u", est.status, est.kidnaps); +} + +static void test_rejected_while_driving_times_out(void) { + // Knocked sideways while driving: the wheels turn, so this is not a + // kidnap; the pose is held until the timeout, then a chain reseeds + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 6, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 0, 2); + _run(&est, &r, 10, 10, 50, 1, LH2_NOISE_SD_MM); + r.x += 200; + _run(&est, &r, 10, 10, _conf.timeout_ticks, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.kidnaps == 0, "still tracking until the timeout, status %d kidnaps %u", est.status, est.kidnaps); + CHECK(est.rejected >= 9, "fixes after the knock are rejected, %u", est.rejected); + _run(&est, &r, 10, 10, 20, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_LOST, "lost after the timeout, status %d", est.status); + _run(&est, &r, 10, 10, 150, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.seeds == 1 && est.kidnaps == 0, "reseeded by the chain, status %d, seeds %u", est.status, est.seeds); + CHECK(hypotf(est.x - r.x, est.y - r.y) < 5.0f, "reseeded axle within 5 mm, %.2f off", hypotf(est.x - r.x, est.y - r.y)); +} + static void test_occlusion_keeps_heading(void) { // No fixes at all for 2 s: lost, but odometry carried the pose, so the // first fix back is inside the gate and nothing is reseeded @@ -422,7 +469,9 @@ int main(void) { test_acquire_heading_from_motion(); test_acquire_heading_from_spin(); test_gate_rejects_outlier(); - test_timeout_reseeds(); + test_kidnap_reseeds(); + test_outlier_still_no_kidnap(); + test_rejected_while_driving_times_out(); test_occlusion_keeps_heading(); test_fix_age_compensated(); test_noise_scales_with_distance(); From 7ef545e6e18c5d4b97dbe2a1beaf75cb457f846b Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 09:52:28 +0200 Subject: [PATCH 08/12] drv/pose_estimator: compensate a fix age of 2 ticks, as measured AI-assisted: Claude Opus 5.5 --- drv/pose_estimator.h | 4 ++-- tests/test_pose_estimator.c | 13 +++++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/drv/pose_estimator.h b/drv/pose_estimator.h index b7e4006..0af8dcc 100644 --- a/drv/pose_estimator.h +++ b/drv/pose_estimator.h @@ -84,8 +84,8 @@ /// Age of a fix when it reaches the estimator, in scheduler ticks. Each fix is /// moved forward by the photodiode travel odometry saw over that many predicts. -/// TODO: estimated at 30 to 50 ms; the app can stamp sweep capture itself. -#define DB_POSE_ESTIMATOR_FIX_AGE_TICKS (4U) +/// Measured against the encoders on v3 straights: 1 to 3 ticks, about 2 on average. +#define DB_POSE_ESTIMATOR_FIX_AGE_TICKS (2U) /// Longest fix age the estimator can compensate, in predict calls #define DB_POSE_ESTIMATOR_FIX_AGE_MAX (8U) diff --git a/tests/test_pose_estimator.c b/tests/test_pose_estimator.c index 37fe034..8dd567d 100644 --- a/tests/test_pose_estimator.c +++ b/tests/test_pose_estimator.c @@ -373,15 +373,16 @@ static void test_occlusion_keeps_heading(void) { } static void test_fix_age_compensated(void) { - // 300 mm/s straight and a 200 mm/s-per-wheel spin, with every fix - // DB_POSE_ESTIMATOR_FIX_AGE_TICKS old - int32_t fast = (int32_t)lroundf(300.0f * 0.01f / DB_MM_PER_COUNT); - int32_t spin = (int32_t)lroundf(200.0f * 0.01f / DB_MM_PER_COUNT); + // 300 mm/s straight and a 200 mm/s-per-wheel spin, with every fix 4 ticks + // old, so leaving the age out is large enough to show + const uint32_t age = 4; + int32_t fast = (int32_t)lroundf(300.0f * 0.01f / DB_MM_PER_COUNT); + int32_t spin = (int32_t)lroundf(200.0f * 0.01f / DB_MM_PER_COUNT); for (int aged = 1; aged >= 0; aged--) { db_pose_estimator_conf_t conf = _conf; - conf.fix_age_ticks = aged ? DB_POSE_ESTIMATOR_FIX_AGE_TICKS : 0; + conf.fix_age_ticks = aged ? age : 0; db_pose_estimator_t est; - robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 8, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 8, .fix_age = age }; db_pose_estimator_init(&est, &conf); db_pose_estimator_seed(&est, r.x, r.y, 0, 2); _run(&est, &r, fast, fast, 300, 1, LH2_NOISE_SD_MM); From e2dd56d6e3bdb29d67242ae9694d8bd763786826 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 10:40:27 +0200 Subject: [PATCH 09/12] drv/pose_estimator: update the covariance in Joseph form AI-assisted: Claude Opus 5.5 --- drv/pose_estimator/pose_estimator.c | 24 +++++++++++++++++++----- tests/test_pose_estimator.c | 27 +++++++++++++++++++++++++++ 2 files changed, 46 insertions(+), 5 deletions(-) diff --git a/drv/pose_estimator/pose_estimator.c b/drv/pose_estimator/pose_estimator.c index 7e7335f..7bac10d 100644 --- a/drv/pose_estimator/pose_estimator.c +++ b/drv/pose_estimator/pose_estimator.c @@ -201,16 +201,30 @@ static db_pose_estimator_result_t _gated_update(db_pose_estimator_t *est, float est->y += k[1][0] * y0 + k[1][1] * y1; est->theta = _wrap(est->theta + k[2][0] * y0 + k[2][1] * y1); - // P -= K (H P) with H P = pht transposed - float np[3][3]; + // Joseph form, P = (I - K H) P (I - K H)^T + K R K^T, which stays positive + // definite in float32 as fixes on a still robot shrink P toward rank 1 + float ikh[3][3]; + for (int i = 0; i < 3; i++) { + ikh[i][0] = (i == 0) - k[i][0]; + ikh[i][1] = (i == 1) - k[i][1]; + ikh[i][2] = (i == 2) - (k[i][0] * a + k[i][1] * b); + } + float ap[3][3]; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - np[i][j] = P[i][j] - (k[i][0] * pht[j][0] + k[i][1] * pht[j][1]); + ap[i][j] = ikh[i][0] * P[0][j] + ikh[i][1] * P[1][j] + ikh[i][2] * P[2][j]; + } + } + float np[3][3]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j <= i; j++) { + np[i][j] = ap[i][0] * ikh[j][0] + ap[i][1] * ikh[j][1] + ap[i][2] * ikh[j][2] + conf->r_pos_mm2 * (k[i][0] * k[j][0] + k[i][1] * k[j][1]); } } for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - P[i][j] = 0.5f * (np[i][j] + np[j][i]); + for (int j = 0; j <= i; j++) { + P[i][j] = np[i][j]; + P[j][i] = np[i][j]; } } return DB_POSE_ESTIMATOR_ACCEPTED; diff --git a/tests/test_pose_estimator.c b/tests/test_pose_estimator.c index 8dd567d..8e737f8 100644 --- a/tests/test_pose_estimator.c +++ b/tests/test_pose_estimator.c @@ -459,6 +459,32 @@ static void test_turn_noise_grows_with_turn_speed(void) { CHECK(c.P[2][2] > 1.5f * b.P[2][2], "above it, faster turning adds more heading noise: %.6g against %.6g", c.P[2][2], b.P[2][2]); } +static void test_covariance_stays_positive_definite(void) { + // An hour at rest at 10 Hz: fixes shrink P toward rank 1, which the + // non-Joseph update let go indefinite in float32 + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 12, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 0, 5); + uint32_t indefinite = 0; + for (uint32_t t = 1; t <= 3600U * 100U; t++) { + db_pose_estimator_predict(&est, 0, 0, 1); + if ((t % TICKS_PER_FIX) == 0) { + float zx, zy; + _robot_sensor(&r, LH2_NOISE_SD_MM, &zx, &zy); + db_pose_estimator_update(&est, zx, zy); + const float(*P)[3] = est.P; + double m2 = (double)P[0][0] * P[1][1] - (double)P[0][1] * P[1][0]; + double m3 = P[0][0] * ((double)P[1][1] * P[2][2] - (double)P[1][2] * P[2][1]) - P[0][1] * ((double)P[1][0] * P[2][2] - (double)P[1][2] * P[2][0]) + P[0][2] * ((double)P[1][0] * P[2][1] - (double)P[1][1] * P[2][0]); + if (!(P[0][0] > 0 && m2 > 0 && m3 > 0) || P[0][1] != P[1][0] || P[0][2] != P[2][0] || P[1][2] != P[2][1]) { + indefinite++; + } + } + } + CHECK(indefinite == 0, "P stays symmetric positive definite over an hour at rest, %u updates not", indefinite); + CHECK(est.rejected == 0, "no fix is rejected at rest, got %u", est.rejected); +} + int main(void) { test_straight_prediction(); test_arc_prediction(); @@ -477,6 +503,7 @@ int main(void) { test_fix_age_compensated(); test_noise_scales_with_distance(); test_turn_noise_grows_with_turn_speed(); + test_covariance_stays_positive_definite(); printf("%d passed, %d failed\n", _passed, _failed); return _failed ? EXIT_FAILURE : EXIT_SUCCESS; } From efb7bdf31f1fc41fd7868a5174f1905ab8acee78 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 10:40:37 +0200 Subject: [PATCH 10/12] drv/pose_estimator: seed the pose at the present, not at fix capture The chain's fixes are fix_age ticks old, but its odometry ran from their arrival, so a seeded pose was the one at capture: 6 mm behind at 300 mm/s, 5.6 deg behind in a 200 mm/s spin. The odometry ring now records every predict, the chain consumes steps as they leave the age window, and the seeded pose is carried forward over that window. AI-assisted: Claude Opus 5.5 --- drv/pose_estimator.h | 20 ++-- drv/pose_estimator/pose_estimator.c | 144 +++++++++++++++++----------- tests/test_pose_estimator.c | 27 ++++++ 3 files changed, 128 insertions(+), 63 deletions(-) diff --git a/drv/pose_estimator.h b/drv/pose_estimator.h index 0af8dcc..7bd8049 100644 --- a/drv/pose_estimator.h +++ b/drv/pose_estimator.h @@ -14,7 +14,8 @@ * sits a lever arm ahead of the axle. That offset is what makes heading * observable while the robot turns in place. A fix is some ticks old when it * arrives, so it is first moved forward by the photodiode travel odometry saw - * since. + * since; the seed chain likewise lines its odometry up with capture time, and a + * seeded pose is carried forward to the present. * * Process noise grows with distance travelled, never with the call rate, and * the heading's share also with how fast the robot turns, since slip is @@ -144,11 +145,12 @@ typedef struct { float kidnap_still_mm; ///< wheel travel |d_left| + |d_right| over those fixes still counted as still, mm } db_pose_estimator_conf_t; -/// Photodiode travel over the most recent predicts, newest at head - 1 +/// Odometry of the most recent predicts, in every state, newest at head - 1 typedef struct { - float x[DB_POSE_ESTIMATOR_FIX_AGE_MAX]; ///< mm - float y[DB_POSE_ESTIMATOR_FIX_AGE_MAX]; ///< mm - uint32_t head; ///< next slot + float d[DB_POSE_ESTIMATOR_FIX_AGE_MAX]; ///< axle midpoint travel, mm + float dtheta[DB_POSE_ESTIMATOR_FIX_AGE_MAX]; ///< rotation, rad + float q_theta[DB_POSE_ESTIMATOR_FIX_AGE_MAX]; ///< heading variance added, rad^2 + uint32_t head; ///< next slot } db_pose_estimator_travel_t; /// Estimator state @@ -163,10 +165,10 @@ typedef struct { db_pose_estimator_travel_t travel; ///< for moving a fix forward by its age float chain_x; ///< first fix of the seed chain, mm float chain_y; ///< first fix of the seed chain, mm - float chain_bx; ///< axle travel since that fix, mm, in the body frame at that fix - float chain_by; ///< axle travel since that fix, mm, in the body frame at that fix - float chain_dtheta; ///< rotation since that fix, rad - float chain_var_theta; ///< heading variance odometry added since that fix, rad^2 + float chain_bx; ///< axle travel since that fix was captured, mm, in the body frame then + float chain_by; ///< axle travel since that fix was captured, mm, in the body frame then + float chain_dtheta; ///< rotation since that fix was captured, rad + float chain_var_theta; ///< heading variance odometry added since that fix was captured, rad^2 uint32_t chain_count; ///< fixes in the chain, 0 when none float kidnap_x; ///< first of the consecutive rejected fixes, mm float kidnap_y; ///< first of the consecutive rejected fixes, mm diff --git a/drv/pose_estimator/pose_estimator.c b/drv/pose_estimator/pose_estimator.c index 7bac10d..c48f8f2 100644 --- a/drv/pose_estimator/pose_estimator.c +++ b/drv/pose_estimator/pose_estimator.c @@ -38,8 +38,59 @@ static void _lever(const db_pose_estimator_conf_t *conf, float theta, float *lx, *ly = conf->lever_mm * cosf(a); } -static void _travel_clear(db_pose_estimator_t *est) { - memset(&est->travel, 0, sizeof(est->travel)); +static uint32_t _fix_age(const db_pose_estimator_conf_t *conf) { + return (conf->fix_age_ticks < DB_POSE_ESTIMATOR_FIX_AGE_MAX) ? conf->fix_age_ticks : DB_POSE_ESTIMATOR_FIX_AGE_MAX; +} + +/// Ring slot of the i-th most recent predict, i from 1 +static uint32_t _travel_slot(const db_pose_estimator_t *est, uint32_t i) { + return (est->travel.head + DB_POSE_ESTIMATOR_FIX_AGE_MAX - i) % DB_POSE_ESTIMATOR_FIX_AGE_MAX; +} + +/// Axle travel and rotation over the last age predicts, integrated back from +/// the current heading +static void _travel_recent(const db_pose_estimator_t *est, uint32_t age, float *ax, float *ay, float *dtheta) { + float theta = est->theta; + *ax = 0; + *ay = 0; + for (uint32_t i = 1; i <= age; i++) { + uint32_t slot = _travel_slot(est, i); + theta -= est->travel.dtheta[slot]; + float mid = theta + 0.5f * est->travel.dtheta[slot]; + *ax += -est->travel.d[slot] * sinf(mid); + *ay += est->travel.d[slot] * cosf(mid); + } + *dtheta = est->theta - theta; +} + +/// Moves the pose and its covariance over one odometry step +static void _propagate(db_pose_estimator_t *est, float d, float dtheta, float q_theta) { + float mid = est->theta + 0.5f * dtheta; + float c = cosf(mid); + float s = sinf(mid); + est->x += -d * s; + est->y += d * c; + est->theta = _wrap(est->theta + dtheta); + + // F = [[1, 0, f0], [0, 1, f1], [0, 0, 1]] + float(*P)[3] = est->P; + float f0 = -d * c; + float f1 = -d * s; + float fp[3][3]; + for (int j = 0; j < 3; j++) { + fp[0][j] = P[0][j] + f0 * P[2][j]; + fp[1][j] = P[1][j] + f1 * P[2][j]; + fp[2][j] = P[2][j]; + } + for (int i = 0; i < 3; i++) { + P[i][0] = fp[i][0] + fp[i][2] * f0; + P[i][1] = fp[i][1] + fp[i][2] * f1; + P[i][2] = fp[i][2]; + } + float q_pos = est->conf->q_pos_mm2_per_mm * fabsf(d); + P[0][0] += q_pos; + P[1][1] += q_pos; + P[2][2] += q_theta; } static void _chain_start(db_pose_estimator_t *est, float x_mm, float y_mm) { @@ -53,11 +104,12 @@ static void _chain_start(db_pose_estimator_t *est, float x_mm, float y_mm) { } /// Adds a fix to the seed chain, and sets the pose once the chain can solve for -/// heading. The fix and the chain's first fix are the photodiode at two times; -/// odometry gives the axle travel b and rotation dtheta between them in the -/// body frame of the first, so z - z0 = Rot(theta0) (b + Rot(dtheta) l - l) +/// heading. The fix and the chain's first fix are the photodiode at two capture +/// times; odometry gives the axle travel b and rotation dtheta between them in +/// the body frame of the first, so z - z0 = Rot(theta0) (b + Rot(dtheta) l - l) /// with l the lever in the body frame. Lengths on both sides match whatever /// theta0 is, which is the consistency test; their angles differ by theta0. +/// The pose solved is the one at capture, carried forward over the fix age. static db_pose_estimator_result_t _chain_add(db_pose_estimator_t *est, float x_mm, float y_mm) { const db_pose_estimator_conf_t *conf = est->conf; if (est->chain_count == 0) { @@ -114,7 +166,10 @@ static db_pose_estimator_result_t _chain_add(db_pose_estimator_t *est, float x_m est->chain_count = 0; est->kidnap_count = 0; est->ticks_since_accept = 0; - _travel_clear(est); + for (uint32_t i = _fix_age(conf); i >= 1; i--) { + uint32_t slot = _travel_slot(est, i); + _propagate(est, est->travel.d[slot], est->travel.dtheta[slot], est->travel.q_theta[slot]); + } est->seeds++; return DB_POSE_ESTIMATOR_SEEDED; } @@ -152,15 +207,13 @@ static db_pose_estimator_result_t _gated_update(db_pose_estimator_t *est, float const db_pose_estimator_conf_t *conf = est->conf; float(*P)[3] = est->P; - uint32_t age = (conf->fix_age_ticks < DB_POSE_ESTIMATOR_FIX_AGE_MAX) ? conf->fix_age_ticks : DB_POSE_ESTIMATOR_FIX_AGE_MAX; - for (uint32_t i = 1; i <= age; i++) { - uint32_t slot = (est->travel.head + DB_POSE_ESTIMATOR_FIX_AGE_MAX - i) % DB_POSE_ESTIMATOR_FIX_AGE_MAX; - x_mm += est->travel.x[slot]; - y_mm += est->travel.y[slot]; - } - - float lx, ly; + float ax, ay, dtheta, lx0, ly0, lx, ly; + _travel_recent(est, _fix_age(conf), &ax, &ay, &dtheta); + _lever(conf, est->theta - dtheta, &lx0, &ly0); _lever(conf, est->theta, &lx, &ly); + x_mm += ax + lx - lx0; + y_mm += ay + ly - ly0; + float y0 = x_mm - (est->x + lx); float y1 = y_mm - (est->y + ly); @@ -250,7 +303,6 @@ void db_pose_estimator_seed(db_pose_estimator_t *est, float x_mm, float y_mm, fl est->chain_count = 0; est->kidnap_count = 0; est->ticks_since_accept = 0; - _travel_clear(est); } void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, int32_t counts_right, uint32_t elapsed_ticks) { @@ -283,54 +335,38 @@ void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, in } } float q_theta = (conf->q_heading_roll_deg2_per_mm * fabsf(d) + conf->q_heading_turn_deg2_per_mm * dd * turn_scale) * DEG_TO_RAD * DEG_TO_RAD; - float q_pos = conf->q_pos_mm2_per_mm * fabsf(d); if (est->kidnap_count > 0) { est->kidnap_travel_mm += fabsf(d_left) + fabsf(d_right); } - if (est->status != DB_POSE_ESTIMATOR_TRACKING && est->chain_count > 0) { - float mid = est->chain_dtheta + 0.5f * dtheta; - est->chain_bx += -d * sinf(mid); - est->chain_by += d * cosf(mid); - est->chain_dtheta += dtheta; - est->chain_var_theta += q_theta; + // The chain runs fix_age behind, on the step leaving the age window, so its + // odometry spans the capture times of its fixes + uint32_t age = _fix_age(conf); + float old_d = d; + float old_dtheta = dtheta; + float old_q = q_theta; + if (age > 0) { + uint32_t slot = _travel_slot(est, age); + old_d = est->travel.d[slot]; + old_dtheta = est->travel.dtheta[slot]; + old_q = est->travel.q_theta[slot]; } - if (est->status == DB_POSE_ESTIMATOR_SEEDING) { - return; + if (est->status != DB_POSE_ESTIMATOR_TRACKING && est->chain_count > 0) { + float mid = est->chain_dtheta + 0.5f * old_dtheta; + est->chain_bx += -old_d * sinf(mid); + est->chain_by += old_d * cosf(mid); + est->chain_dtheta += old_dtheta; + est->chain_var_theta += old_q; } + est->travel.d[est->travel.head] = d; + est->travel.dtheta[est->travel.head] = dtheta; + est->travel.q_theta[est->travel.head] = q_theta; + est->travel.head = (est->travel.head + 1) % DB_POSE_ESTIMATOR_FIX_AGE_MAX; - float lx0, ly0, lx1, ly1; - _lever(conf, est->theta, &lx0, &ly0); - float mid = est->theta + 0.5f * dtheta; - float c = cosf(mid); - float s = sinf(mid); - est->x += -d * s; - est->y += d * c; - est->theta = _wrap(est->theta + dtheta); - _lever(conf, est->theta, &lx1, &ly1); - est->travel.x[est->travel.head] = -d * s + lx1 - lx0; - est->travel.y[est->travel.head] = d * c + ly1 - ly0; - est->travel.head = (est->travel.head + 1) % DB_POSE_ESTIMATOR_FIX_AGE_MAX; - - // F = [[1, 0, f0], [0, 1, f1], [0, 0, 1]] - float(*P)[3] = est->P; - float f0 = -d * c; - float f1 = -d * s; - float fp[3][3]; - for (int j = 0; j < 3; j++) { - fp[0][j] = P[0][j] + f0 * P[2][j]; - fp[1][j] = P[1][j] + f1 * P[2][j]; - fp[2][j] = P[2][j]; + if (est->status != DB_POSE_ESTIMATOR_SEEDING) { + _propagate(est, d, dtheta, q_theta); } - for (int i = 0; i < 3; i++) { - P[i][0] = fp[i][0] + fp[i][2] * f0; - P[i][1] = fp[i][1] + fp[i][2] * f1; - P[i][2] = fp[i][2]; - } - P[0][0] += q_pos; - P[1][1] += q_pos; - P[2][2] += q_theta; } db_pose_estimator_result_t db_pose_estimator_update(db_pose_estimator_t *est, float x_mm, float y_mm) { diff --git a/tests/test_pose_estimator.c b/tests/test_pose_estimator.c index 8e737f8..979e9a8 100644 --- a/tests/test_pose_estimator.c +++ b/tests/test_pose_estimator.c @@ -459,6 +459,32 @@ static void test_turn_noise_grows_with_turn_speed(void) { CHECK(c.P[2][2] > 1.5f * b.P[2][2], "above it, faster turning adds more heading noise: %.6g against %.6g", c.P[2][2], b.P[2][2]); } +static void test_seed_carried_to_present(void) { + // A chain seeds from fixes DB_POSE_ESTIMATOR_FIX_AGE_TICKS old; the pose it + // sets is the present one, not the one at capture + int32_t fast = (int32_t)lroundf(300.0f * 0.01f / DB_MM_PER_COUNT); + int32_t spin = (int32_t)lroundf(200.0f * 0.01f / DB_MM_PER_COUNT); + for (int spinning = 0; spinning <= 1; spinning++) { + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 30 * DEG, .seed = 11, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + int32_t cl = spinning ? spin : fast; + int32_t cr = spinning ? -spin : fast; + db_pose_estimator_init(&est, &_conf); + for (uint32_t t = 1; t <= 50 * TICKS_PER_FIX && est.seeds == 0; t++) { + _robot_step(&r, cl, cr); + db_pose_estimator_predict(&est, cl, cr, 1); + if ((t % TICKS_PER_FIX) == 0) { + float zx, zy; + _robot_sensor(&r, 0, &zx, &zy); + db_pose_estimator_update(&est, zx, zy); + } + } + CHECK(est.seeds == 1, "%s seeds, seeds %u", spinning ? "a spin" : "a straight", est.seeds); + CHECK(hypotf(est.x - r.x, est.y - r.y) < 0.5f, "%s seeds the present axle, %.2f mm off", spinning ? "a spin" : "a straight", hypotf(est.x - r.x, est.y - r.y)); + CHECK(_angle_error_deg(est.theta, r.theta) < 0.5f, "%s seeds the present heading, %.2f deg off", spinning ? "a spin" : "a straight", _angle_error_deg(est.theta, r.theta)); + } +} + static void test_covariance_stays_positive_definite(void) { // An hour at rest at 10 Hz: fixes shrink P toward rank 1, which the // non-Joseph update let go indefinite in float32 @@ -503,6 +529,7 @@ int main(void) { test_fix_age_compensated(); test_noise_scales_with_distance(); test_turn_noise_grows_with_turn_speed(); + test_seed_carried_to_present(); test_covariance_stays_positive_definite(); printf("%d passed, %d failed\n", _passed, _failed); return _failed ? EXIT_FAILURE : EXIT_SUCCESS; From e65427f6c8018411601999afd4330c12eb28aae4 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 10:40:37 +0200 Subject: [PATCH 11/12] drv/pose_estimator: cover a broken seed chain and a carry past the timeout AI-assisted: Claude Opus 5.5 --- tests/test_pose_estimator.c | 41 +++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/tests/test_pose_estimator.c b/tests/test_pose_estimator.c index 979e9a8..bfe8a86 100644 --- a/tests/test_pose_estimator.c +++ b/tests/test_pose_estimator.c @@ -485,6 +485,45 @@ static void test_seed_carried_to_present(void) { } } +static void test_chain_survives_outlier(void) { + // One wild fix while acquiring restarts the chain; the heading then + // acquired carries nothing of it + db_pose_estimator_t est; + robot_t r = { .x = 1500, .y = 900, .theta = -120 * DEG, .seed = 13, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + db_pose_estimator_init(&est, &_conf); + _run(&est, &r, 10, 10, 2 * TICKS_PER_FIX, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_SEEDING && est.chain_count == 2, "chaining, status %d count %u", est.status, est.chain_count); + float zx, zy; + _robot_sensor(&r, 0, &zx, &zy); + CHECK(db_pose_estimator_update(&est, zx + 150, zy - 100) == DB_POSE_ESTIMATOR_REJECTED, "a wild fix breaks the chain"); + _run(&est, &r, 10, 10, 150, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.seeds == 1, "seeded after the wild fix, status %d seeds %u", est.status, est.seeds); + CHECK(_angle_error_deg(est.theta, r.theta) < 5.0f, "heading within 5 deg, %.2f off", _angle_error_deg(est.theta, r.theta)); +} + +static void test_long_carry_times_out(void) { + // Carried for longer than the timeout with the wheels still: every fix + // moves more than the seed tolerance, so no kidnap; the pose goes LOST, + // the heading stays unknown at rest, and motion reseeds it + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 14, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 0, 2); + _run(&est, &r, 0, 0, 50, 1, LH2_NOISE_SD_MM); + for (int i = 0; i < 15; i++) { + r.x += 40; + r.theta += 6 * DEG; + _run(&est, &r, 0, 0, TICKS_PER_FIX, 1, LH2_NOISE_SD_MM); + } + CHECK(est.status == DB_POSE_ESTIMATOR_LOST && est.kidnaps == 0, "a carry longer than the timeout is lost, not kidnapped, status %d kidnaps %u", est.status, est.kidnaps); + _run(&est, &r, 0, 0, 100, 1, LH2_NOISE_SD_MM); + float h = 0; + CHECK(!db_pose_estimator_heading_deg(&est, &h) && est.seeds == 0, "no heading at rest after the carry, status %d", est.status); + _run(&est, &r, 10, 10, 150, 1, LH2_NOISE_SD_MM); + CHECK(est.status == DB_POSE_ESTIMATOR_TRACKING && est.seeds == 1, "motion reseeds, status %d seeds %u", est.status, est.seeds); + CHECK(_angle_error_deg(est.theta, r.theta) < 5.0f, "heading within 5 deg, %.2f off", _angle_error_deg(est.theta, r.theta)); +} + static void test_covariance_stays_positive_definite(void) { // An hour at rest at 10 Hz: fixes shrink P toward rank 1, which the // non-Joseph update let go indefinite in float32 @@ -530,6 +569,8 @@ int main(void) { test_noise_scales_with_distance(); test_turn_noise_grows_with_turn_speed(); test_seed_carried_to_present(); + test_chain_survives_outlier(); + test_long_carry_times_out(); test_covariance_stays_positive_definite(); printf("%d passed, %d failed\n", _passed, _failed); return _failed ? EXIT_FAILURE : EXIT_SUCCESS; From d2a88c5d9be771b2a8df3cd9b890abe41c83e913 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 12:23:44 +0200 Subject: [PATCH 12/12] drv/pose_estimator: stop reading a hard stop's slip as a kidnap After a 500 to 700 mm/s sprint stops hard, the wheels slip or skid, so the estimate sits 25 to 45 mm off steady fixes while the wheels stand, and the kidnap rule fired. Process noise now also grows with wheel speed changes, and a kidnap needs the wheels to have stood before the fixes jumped; a small jump soon after driving re-anchors the position and keeps the heading. AI-assisted: Claude Opus 5.5 --- drv/pose_estimator.h | 91 ++++++++++--- drv/pose_estimator/pose_estimator.c | 65 ++++++++- tests/test_pose_estimator.c | 200 +++++++++++++++++++++++++--- 3 files changed, 309 insertions(+), 47 deletions(-) diff --git a/drv/pose_estimator.h b/drv/pose_estimator.h index 7bd8049..02f6b9f 100644 --- a/drv/pose_estimator.h +++ b/drv/pose_estimator.h @@ -19,7 +19,10 @@ * * Process noise grows with distance travelled, never with the call rate, and * the heading's share also with how fast the robot turns, since slip is - * dominated by turning. A fix whose squared Mahalanobis distance exceeds the + * dominated by turning. Both also grow with each change of wheel speed beyond + * a deadband, because a hard start or stop slips or skids the wheels by more + * than distance alone accounts for; wheel speed for that is filtered with a + * time constant of speed_tau_ms. A fix whose squared Mahalanobis distance exceeds the * gate is rejected. * * Life cycle: SEEDING until a chain of consistent fixes has seen the photodiode @@ -30,9 +33,13 @@ * * Kidnap: while TRACKING, kidnap_fixes rejected fixes in a row that agree * within seed_tolerance_mm, with at most kidnap_still_mm of wheel travel since - * the first of them, mean the robot was moved by hand. The estimator returns - * to SEEDING at once, with those fixes as the start of its chain, so heading - * stays unknown until motion re-acquires it. + * the first of them, mean the robot was moved by hand, provided the wheels had + * also stood for kidnap_settle_ticks before the first of them. The estimator + * returns to SEEDING at once, with those fixes as the start of its chain, so + * heading stays unknown until motion re-acquires it. The same fixes arriving + * sooner after motion, within reanchor_mm of the estimate, are slip the + * odometry missed: the position is moved onto them, the heading is kept and + * its variance raised by reanchor_heading_var_deg2. * * No hardware calls, so the module also builds on the host for its tests. * @@ -111,6 +118,39 @@ /// those fixes, for the wheels to count as still #define DB_POSE_ESTIMATOR_KIDNAP_STILL_MM (2.0f) +/// Wheels that stood this long before the first rejected fix of a chain make +/// it a kidnap, ticks: 0.5 s. A robot that drove a moment ago re-anchors instead. +#define DB_POSE_ESTIMATOR_KIDNAP_SETTLE_TICKS (50U) + +/// Both filtered wheel speeds below this count as standing, mm/s: well above +/// what a stray count reads as, well below any commanded speed +#define DB_POSE_ESTIMATOR_STILL_MM_S (20.0f) + +/// Largest jump, in mm, from the estimated photodiode to consistent rejected +/// fixes that a robot fresh from driving re-anchors to rather than reseeding. +/// Hard stops from 500 to 700 mm/s left 25 to 45 mm on the floor. +#define DB_POSE_ESTIMATOR_REANCHOR_MM (60.0f) + +/// Heading variance added on a re-anchor, deg^2: 5 deg sigma +#define DB_POSE_ESTIMATOR_REANCHOR_HEADING_VAR_DEG2 (25.0f) + +/// Position variance per axis added per mm/s of wheel speed change beyond the +/// deadband, summed over both wheels, mm^2 / (mm/s): a hard stop from 600 mm/s +/// adds about 60 mm^2, which lets in the 35 to 40 mm of slip it can leave; +/// larger slips after driving are re-anchored +#define DB_POSE_ESTIMATOR_Q_POS_SLIP_MM2_PER_MM_S (0.06f) + +/// Heading variance added per mm/s of wheel speed change beyond the deadband, +/// deg^2 / (mm/s): a hard stop from 700 mm/s adds about (4 deg)^2 +#define DB_POSE_ESTIMATOR_Q_HEADING_SLIP_DEG2_PER_MM_S (0.012f) + +/// Change of the filtered wheel speed per tick not counted as speed change, +/// mm/s: above the 3 mm/s one count swings it by, below a hard start or stop +#define DB_POSE_ESTIMATOR_SLIP_DEADBAND_MM_S (10.0f) + +/// Time constant of the first-order filter on each wheel's speed, ms +#define DB_POSE_ESTIMATOR_SPEED_TAU_MS (30.0f) + /// Life-cycle state typedef enum { DB_POSE_ESTIMATOR_SEEDING, ///< No pose; collecting a chain of consistent fixes @@ -128,21 +168,29 @@ typedef enum { /// Model and noise; the app holds one, the estimator keeps a pointer to it typedef struct { - float lever_mm; ///< axle midpoint to photodiode, mm - float lever_angle_deg; ///< direction of that offset, deg clockwise from forward - float r_pos_mm2; ///< LH2 variance per axis, mm^2 - float q_pos_mm2_per_mm; ///< position variance per mm travelled - float q_heading_roll_deg2_per_mm; ///< heading variance per mm travelled - float q_heading_turn_deg2_per_mm; ///< heading variance per mm of |d_right - d_left| - float turn_speed_ref_mm_s; ///< |v_right - v_left| above which the turn term scales up, mm/s - float gate; ///< squared Mahalanobis rejection threshold - uint32_t fix_age_ticks; ///< fix age compensated, at most DB_POSE_ESTIMATOR_FIX_AGE_MAX - uint32_t timeout_ticks; ///< ticks without an accepted fix before LOST - uint32_t seed_fixes; ///< fixes a seed chain needs - float seed_tolerance_mm; ///< chain consistency tolerance, mm - float acquire_mm; ///< body-frame photodiode travel needed for heading, mm - uint32_t kidnap_fixes; ///< consistent rejected fixes with the wheels still that reseed; 0 disables - float kidnap_still_mm; ///< wheel travel |d_left| + |d_right| over those fixes still counted as still, mm + float lever_mm; ///< axle midpoint to photodiode, mm + float lever_angle_deg; ///< direction of that offset, deg clockwise from forward + float r_pos_mm2; ///< LH2 variance per axis, mm^2 + float q_pos_mm2_per_mm; ///< position variance per mm travelled + float q_heading_roll_deg2_per_mm; ///< heading variance per mm travelled + float q_heading_turn_deg2_per_mm; ///< heading variance per mm of |d_right - d_left| + float turn_speed_ref_mm_s; ///< |v_right - v_left| above which the turn term scales up, mm/s + float gate; ///< squared Mahalanobis rejection threshold + uint32_t fix_age_ticks; ///< fix age compensated, at most DB_POSE_ESTIMATOR_FIX_AGE_MAX + uint32_t timeout_ticks; ///< ticks without an accepted fix before LOST + uint32_t seed_fixes; ///< fixes a seed chain needs + float seed_tolerance_mm; ///< chain consistency tolerance, mm + float acquire_mm; ///< body-frame photodiode travel needed for heading, mm + uint32_t kidnap_fixes; ///< consistent rejected fixes with the wheels still that reseed; 0 disables + float kidnap_still_mm; ///< wheel travel |d_left| + |d_right| over those fixes still counted as still, mm + uint32_t kidnap_settle_ticks; ///< wheels standing this long before the first of them make it a kidnap; 0 always does + float still_mm_s; ///< both averaged wheel speeds below this count as standing, mm/s + float reanchor_mm; ///< largest jump re-anchored to after driving, mm; 0 disables + float reanchor_heading_var_deg2; ///< heading variance added on a re-anchor, deg^2 + float q_pos_slip_mm2_per_mm_s; ///< position variance per mm/s of wheel speed change past the deadband + float q_heading_slip_deg2_per_mm_s; ///< heading variance per mm/s of wheel speed change past the deadband + float slip_deadband_mm_s; ///< filtered speed change per tick below which nothing is added, mm/s + float speed_tau_ms; ///< wheel speed filter time constant, ms } db_pose_estimator_conf_t; /// Odometry of the most recent predicts, in every state, newest at head - 1 @@ -174,12 +222,17 @@ typedef struct { float kidnap_y; ///< first of the consecutive rejected fixes, mm float kidnap_travel_mm; ///< wheel travel |d_left| + |d_right| since that fix, mm uint32_t kidnap_count; ///< consecutive consistent rejected fixes, 0 when none + bool kidnap_settled; ///< the wheels had stood for the settle time before the first of them + float v_left; ///< filtered left wheel speed, mm/s + float v_right; ///< filtered right wheel speed, mm/s + uint32_t still_ticks; ///< consecutive predicts with both wheels standing, saturating float last_d2; ///< squared Mahalanobis distance of the last gated fix uint32_t predicts; ///< predict calls, wraps uint32_t accepted; ///< fixes applied, wraps uint32_t rejected; ///< fixes rejected, wraps uint32_t seeds; ///< pose seeded or reseeded from a chain, wraps uint32_t kidnaps; ///< returns to SEEDING on a kidnap, wraps + uint32_t reanchors; ///< position moved onto consistent rejected fixes after driving, wraps } db_pose_estimator_t; //=========================== prototypes ======================================= diff --git a/drv/pose_estimator/pose_estimator.c b/drv/pose_estimator/pose_estimator.c index c48f8f2..0920059 100644 --- a/drv/pose_estimator/pose_estimator.c +++ b/drv/pose_estimator/pose_estimator.c @@ -64,7 +64,7 @@ static void _travel_recent(const db_pose_estimator_t *est, uint32_t age, float * } /// Moves the pose and its covariance over one odometry step -static void _propagate(db_pose_estimator_t *est, float d, float dtheta, float q_theta) { +static void _propagate(db_pose_estimator_t *est, float d, float dtheta, float q_theta, float q_pos_slip) { float mid = est->theta + 0.5f * dtheta; float c = cosf(mid); float s = sinf(mid); @@ -87,7 +87,7 @@ static void _propagate(db_pose_estimator_t *est, float d, float dtheta, float q_ P[i][1] = fp[i][1] + fp[i][2] * f1; P[i][2] = fp[i][2]; } - float q_pos = est->conf->q_pos_mm2_per_mm * fabsf(d); + float q_pos = est->conf->q_pos_mm2_per_mm * fabsf(d) + q_pos_slip; P[0][0] += q_pos; P[1][1] += q_pos; P[2][2] += q_theta; @@ -168,14 +168,55 @@ static db_pose_estimator_result_t _chain_add(db_pose_estimator_t *est, float x_m est->ticks_since_accept = 0; for (uint32_t i = _fix_age(conf); i >= 1; i--) { uint32_t slot = _travel_slot(est, i); - _propagate(est, est->travel.d[slot], est->travel.dtheta[slot], est->travel.q_theta[slot]); + _propagate(est, est->travel.d[slot], est->travel.dtheta[slot], est->travel.q_theta[slot], 0); } est->seeds++; return DB_POSE_ESTIMATOR_SEEDED; } -/// Counts a fix the gate rejected while TRACKING toward a kidnap, and on the -/// last one returns to SEEDING with the chain started on these fixes +/// Updates the filtered wheel speeds and the standing count over one predict, +/// and returns the speed change past the deadband, summed over both wheels, mm/s +static float _wheels_step(db_pose_estimator_t *est, float d_left, float d_right, uint32_t elapsed_ticks) { + const db_pose_estimator_conf_t *conf = est->conf; + uint32_t n = elapsed_ticks ? elapsed_ticks : 1U; + float dt_ms = (float)(n * DB_POSE_ESTIMATOR_TICK_MS); + float alpha = (conf->speed_tau_ms > 0) ? 1.0f - expf(-dt_ms / conf->speed_tau_ms) : 1.0f; + float dv_l = alpha * (d_left * 1000.0f / dt_ms - est->v_left); + float dv_r = alpha * (d_right * 1000.0f / dt_ms - est->v_right); + est->v_left += dv_l; + est->v_right += dv_r; + float deadband = conf->slip_deadband_mm_s * (float)n; + float change = fmaxf(0, fabsf(dv_l) - deadband) + fmaxf(0, fabsf(dv_r) - deadband); + + if (fabsf(est->v_left) < conf->still_mm_s && fabsf(est->v_right) < conf->still_mm_s) { + est->still_ticks = (UINT32_MAX - est->still_ticks < n) ? UINT32_MAX : est->still_ticks + n; + } else { + est->still_ticks = 0; + } + return change; +} + +/// Moves the position onto a fix of the photodiode, keeping the heading and +/// raising its variance +static void _reanchor(db_pose_estimator_t *est, float x_mm, float y_mm) { + const db_pose_estimator_conf_t *conf = est->conf; + float lx, ly; + _lever(conf, est->theta, &lx, &ly); + est->x = x_mm - lx; + est->y = y_mm - ly; + float var_theta = est->P[2][2] + conf->reanchor_heading_var_deg2 * DEG_TO_RAD * DEG_TO_RAD; + memset(est->P, 0, sizeof(est->P)); + est->P[0][0] = conf->r_pos_mm2; + est->P[1][1] = conf->r_pos_mm2; + est->P[2][2] = var_theta; + est->kidnap_count = 0; + est->ticks_since_accept = 0; + est->reanchors++; +} + +/// Counts a fix the gate rejected while TRACKING toward a kidnap. On the last +/// one it returns to SEEDING with the chain started on these fixes, unless the +/// wheels were turning shortly before and the fixes lie close: then it re-anchors. static void _kidnap_check(db_pose_estimator_t *est, float x_mm, float y_mm) { const db_pose_estimator_conf_t *conf = est->conf; if (conf->kidnap_fixes == 0) { @@ -188,12 +229,21 @@ static void _kidnap_check(db_pose_estimator_t *est, float x_mm, float y_mm) { est->kidnap_y = y_mm; est->kidnap_travel_mm = 0; est->kidnap_count = 1; + est->kidnap_settled = est->still_ticks >= conf->kidnap_settle_ticks; } else { est->kidnap_count++; } if (est->kidnap_count < conf->kidnap_fixes) { return; } + if (!est->kidnap_settled && conf->reanchor_mm > 0) { + float lx, ly; + _lever(conf, est->theta, &lx, &ly); + if (hypotf(x_mm - (est->x + lx), y_mm - (est->y + ly)) <= conf->reanchor_mm) { + _reanchor(est, x_mm, y_mm); + return; + } + } est->status = DB_POSE_ESTIMATOR_SEEDING; _chain_start(est, est->kidnap_x, est->kidnap_y); est->chain_count = est->kidnap_count; @@ -334,7 +384,8 @@ void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, in turn_scale = ratio; } } - float q_theta = (conf->q_heading_roll_deg2_per_mm * fabsf(d) + conf->q_heading_turn_deg2_per_mm * dd * turn_scale) * DEG_TO_RAD * DEG_TO_RAD; + float slip = _wheels_step(est, d_left, d_right, elapsed_ticks); + float q_theta = (conf->q_heading_roll_deg2_per_mm * fabsf(d) + conf->q_heading_turn_deg2_per_mm * dd * turn_scale + conf->q_heading_slip_deg2_per_mm_s * slip) * DEG_TO_RAD * DEG_TO_RAD; if (est->kidnap_count > 0) { est->kidnap_travel_mm += fabsf(d_left) + fabsf(d_right); @@ -365,7 +416,7 @@ void db_pose_estimator_predict(db_pose_estimator_t *est, int32_t counts_left, in est->travel.head = (est->travel.head + 1) % DB_POSE_ESTIMATOR_FIX_AGE_MAX; if (est->status != DB_POSE_ESTIMATOR_SEEDING) { - _propagate(est, d, dtheta, q_theta); + _propagate(est, d, dtheta, q_theta, conf->q_pos_slip_mm2_per_mm_s * slip); } } diff --git a/tests/test_pose_estimator.c b/tests/test_pose_estimator.c index bfe8a86..18984f9 100644 --- a/tests/test_pose_estimator.c +++ b/tests/test_pose_estimator.c @@ -35,21 +35,29 @@ static int _passed = 0; #define DEG ((float)M_PI / 180.0f) static const db_pose_estimator_conf_t _conf = { - .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, - .lever_angle_deg = DB_LH2_LEVER_ANGLE, - .r_pos_mm2 = DB_POSE_ESTIMATOR_R_POS_MM2, - .q_pos_mm2_per_mm = DB_POSE_ESTIMATOR_Q_POS_MM2_PER_MM, - .q_heading_roll_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_ROLL_DEG2_PER_MM, - .q_heading_turn_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_TURN_DEG2_PER_MM, - .turn_speed_ref_mm_s = DB_POSE_ESTIMATOR_TURN_SPEED_REF_MM_S, - .gate = DB_POSE_ESTIMATOR_GATE, - .fix_age_ticks = DB_POSE_ESTIMATOR_FIX_AGE_TICKS, - .timeout_ticks = DB_POSE_ESTIMATOR_TIMEOUT_TICKS, - .seed_fixes = DB_POSE_ESTIMATOR_SEED_FIXES, - .seed_tolerance_mm = DB_POSE_ESTIMATOR_SEED_TOLERANCE_MM, - .acquire_mm = DB_POSE_ESTIMATOR_ACQUIRE_MM, - .kidnap_fixes = DB_POSE_ESTIMATOR_KIDNAP_FIXES, - .kidnap_still_mm = DB_POSE_ESTIMATOR_KIDNAP_STILL_MM, + .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, + .lever_angle_deg = DB_LH2_LEVER_ANGLE, + .r_pos_mm2 = DB_POSE_ESTIMATOR_R_POS_MM2, + .q_pos_mm2_per_mm = DB_POSE_ESTIMATOR_Q_POS_MM2_PER_MM, + .q_heading_roll_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_ROLL_DEG2_PER_MM, + .q_heading_turn_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_TURN_DEG2_PER_MM, + .turn_speed_ref_mm_s = DB_POSE_ESTIMATOR_TURN_SPEED_REF_MM_S, + .gate = DB_POSE_ESTIMATOR_GATE, + .fix_age_ticks = DB_POSE_ESTIMATOR_FIX_AGE_TICKS, + .timeout_ticks = DB_POSE_ESTIMATOR_TIMEOUT_TICKS, + .seed_fixes = DB_POSE_ESTIMATOR_SEED_FIXES, + .seed_tolerance_mm = DB_POSE_ESTIMATOR_SEED_TOLERANCE_MM, + .acquire_mm = DB_POSE_ESTIMATOR_ACQUIRE_MM, + .kidnap_fixes = DB_POSE_ESTIMATOR_KIDNAP_FIXES, + .kidnap_still_mm = DB_POSE_ESTIMATOR_KIDNAP_STILL_MM, + .kidnap_settle_ticks = DB_POSE_ESTIMATOR_KIDNAP_SETTLE_TICKS, + .still_mm_s = DB_POSE_ESTIMATOR_STILL_MM_S, + .reanchor_mm = DB_POSE_ESTIMATOR_REANCHOR_MM, + .reanchor_heading_var_deg2 = DB_POSE_ESTIMATOR_REANCHOR_HEADING_VAR_DEG2, + .q_pos_slip_mm2_per_mm_s = DB_POSE_ESTIMATOR_Q_POS_SLIP_MM2_PER_MM_S, + .q_heading_slip_deg2_per_mm_s = DB_POSE_ESTIMATOR_Q_HEADING_SLIP_DEG2_PER_MM_S, + .slip_deadband_mm_s = DB_POSE_ESTIMATOR_SLIP_DEADBAND_MM_S, + .speed_tau_ms = DB_POSE_ESTIMATOR_SPEED_TAU_MS, }; //=========================== simulated robot ================================== @@ -400,8 +408,12 @@ static void test_fix_age_compensated(void) { } static void test_noise_scales_with_distance(void) { + // The distance and turn terms alone: the slip term depends on speed changes + db_pose_estimator_conf_t conf = _conf; + conf.q_pos_slip_mm2_per_mm_s = 0; + conf.q_heading_slip_deg2_per_mm_s = 0; db_pose_estimator_t a, b; - db_pose_estimator_init(&a, &_conf); + db_pose_estimator_init(&a, &conf); db_pose_estimator_seed(&a, 0, 0, 0, 2); float before = a.P[2][2]; for (int t = 0; t < 1000; t++) { @@ -410,8 +422,8 @@ static void test_noise_scales_with_distance(void) { CHECK(a.P[0][0] == a.conf->r_pos_mm2 && a.P[2][2] == before, "standing still adds no process noise over 1000 calls"); // The same spin in 100 calls or in one - db_pose_estimator_init(&a, &_conf); - db_pose_estimator_init(&b, &_conf); + db_pose_estimator_init(&a, &conf); + db_pose_estimator_init(&b, &conf); db_pose_estimator_seed(&a, 0, 0, 0, 2); db_pose_estimator_seed(&b, 0, 0, 0, 2); for (int t = 0; t < 100; t++) { @@ -421,8 +433,8 @@ static void test_noise_scales_with_distance(void) { CHECK(fabsf(a.P[2][2] - b.P[2][2]) < 1e-3f * b.P[2][2], "heading noise over a spin is independent of the call rate: %.6g against %.6g", a.P[2][2], b.P[2][2]); // The same straight in 100 calls or in one - db_pose_estimator_init(&a, &_conf); - db_pose_estimator_init(&b, &_conf); + db_pose_estimator_init(&a, &conf); + db_pose_estimator_init(&b, &conf); db_pose_estimator_seed(&a, 0, 0, 0, 0); db_pose_estimator_seed(&b, 0, 0, 0, 0); for (int t = 0; t < 100; t++) { @@ -433,12 +445,154 @@ static void test_noise_scales_with_distance(void) { CHECK(fabsf(a.P[1][1] - b.P[1][1]) < 1e-3f * b.P[1][1], "position noise over a straight is independent of the call rate: %.6g against %.6g", a.P[1][1], b.P[1][1]); // Twice the distance, twice the heading noise - db_pose_estimator_init(&b, &_conf); + db_pose_estimator_init(&b, &conf); db_pose_estimator_seed(&b, 0, 0, 0, 0); db_pose_estimator_predict(&b, 2000, 2000, 200); CHECK(fabsf(b.P[2][2] - 2.0f * a.P[2][2]) < 1e-3f * b.P[2][2], "heading noise doubles with the distance: %.6g against 2 x %.6g", b.P[2][2], a.P[2][2]); } +static void test_slip_noise_on_hard_stop(void) { + // Against the same drive with the slip term off: cruising with count + // jitter adds almost nothing, a stop from 700 mm/s in 100 ms adds a lot + db_pose_estimator_conf_t off = _conf; + off.q_pos_slip_mm2_per_mm_s = 0; + off.q_heading_slip_deg2_per_mm_s = 0; + db_pose_estimator_t est, ref; + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_init(&ref, &off); + db_pose_estimator_seed(&est, 0, 0, 0, 1); + db_pose_estimator_seed(&ref, 0, 0, 0, 1); + int32_t cruise = (int32_t)lroundf(700.0f * 0.01f / DB_MM_PER_COUNT); + for (int t = 0; t < 30; t++) { + db_pose_estimator_predict(&est, cruise, cruise, 1); + db_pose_estimator_predict(&ref, cruise, cruise, 1); + } + float h0 = est.P[2][2] - ref.P[2][2]; + for (int t = 0; t < 100; t++) { + db_pose_estimator_predict(&est, cruise + (t % 2), cruise - (t % 2), 1); + db_pose_estimator_predict(&ref, cruise + (t % 2), cruise - (t % 2), 1); + } + CHECK(est.P[2][2] - ref.P[2][2] - h0 < 0.5f * DEG * DEG, "1 s at 700 mm/s with count jitter adds under 0.5 deg^2 of slip noise, %.2f", (est.P[2][2] - ref.P[2][2] - h0) / (DEG * DEG)); + // Reset the heading variance so the stop's position share is not scaled by the cruise's + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + est.P[i][j] = ref.P[i][j] = (i == j) ? ((i == 2) ? DEG * DEG : 4.0f) : 0; + } + } + for (int t = 0; t < 10; t++) { + int32_t c = cruise * (9 - t) / 10; + db_pose_estimator_predict(&est, c, c, 1); + db_pose_estimator_predict(&ref, c, c, 1); + } + for (int t = 0; t < 20; t++) { + db_pose_estimator_predict(&est, 0, 0, 1); + db_pose_estimator_predict(&ref, 0, 0, 1); + } + CHECK(est.P[0][0] - ref.P[0][0] > 40.0f, "a hard stop from 700 mm/s adds over 40 mm^2 per axis, %.1f", est.P[0][0] - ref.P[0][0]); + CHECK((est.P[2][2] - ref.P[2][2]) / (DEG * DEG) > 9.0f, "and over (3 deg)^2 of heading variance, %.1f deg^2", (est.P[2][2] - ref.P[2][2]) / (DEG * DEG)); +} + +/// A sprint from rest and a hard stop in which the odometry counts slip_mm more +/// travel than the ground saw, along the heading and across it; returns the +/// estimator 0.5 s after the stop +static void _sprint_and_slip(db_pose_estimator_t *est, const db_pose_estimator_conf_t *conf, float speed, float slip_mm, float lateral_mm, robot_t *r) { + db_pose_estimator_init(est, conf); + db_pose_estimator_seed(est, r->x, r->y, r->theta / DEG, 1); + _run(est, r, 0, 0, 100, 1, LH2_NOISE_SD_MM); + int32_t cruise = (int32_t)lroundf(speed * 0.01f / DB_MM_PER_COUNT); + uint32_t tick = 0; + // 0.3 s at speed, then a 10-tick stop over which the odometry over-counts + for (int t = 0; t < 40; t++) { + int32_t c = (t < 30) ? cruise : cruise * (39 - t) / 10; + int32_t extra = (t < 30) ? 0 : (int32_t)lroundf(slip_mm / DB_MM_PER_COUNT / 10.0f) * (speed < 0 ? -1 : 1); + _robot_step(r, c, c); + if (t >= 30) { + r->x += lateral_mm / 10.0f * cosf(r->theta); + r->y += lateral_mm / 10.0f * sinf(r->theta); + } + db_pose_estimator_predict(est, c + extra, c + extra, 1); + if (++tick % TICKS_PER_FIX == 0) { + float zx, zy; + _robot_sensor(r, LH2_NOISE_SD_MM, &zx, &zy); + db_pose_estimator_update(est, zx, zy); + } + } + for (int t = 0; t < 50; t++) { + _robot_step(r, 0, 0); + db_pose_estimator_predict(est, 0, 0, 1); + if (++tick % TICKS_PER_FIX == 0) { + float zx, zy; + _robot_sensor(r, LH2_NOISE_SD_MM, &zx, &zy); + db_pose_estimator_update(est, zx, zy); + } + } +} + +static void test_hard_stop_slip_is_not_a_kidnap(void) { + // As on the floor: after a 500 to 600 mm/s sprint and a hard stop, the + // estimate sat 25 to 45 mm off steady fixes with the wheels still + const float speeds[] = { 600, -600, 500, -500 }; + const float slips[] = { 35, 40, 30, 45 }; + const float lateral[] = { 0, 0, 25, -20 }; + for (unsigned i = 0; i < 4; i++) { + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 90 * DEG, .seed = 30 + i, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + _sprint_and_slip(&est, &_conf, speeds[i], slips[i], lateral[i], &r); + float sx = 0, sy = 0, px, py; + _robot_sensor(&r, 0, &px, &py); + bool tracking = db_pose_estimator_sensor(&est, &sx, &sy); + CHECK(tracking && est.kidnaps == 0 && est.seeds == 0, "%+.0f mm/s, %.0f mm slip: still tracking, no kidnap, status %d kidnaps %u", speeds[i], slips[i], est.status, est.kidnaps); + if (lateral[i] == 0) { + CHECK(est.reanchors == 0, "%+.0f mm/s, %.0f mm slip: the slip noise lets the fixes in, no re-anchor needed, %u", speeds[i], slips[i], est.reanchors); + } + CHECK(hypotf(sx - px, sy - py) < 5.0f, "%+.0f mm/s, %.0f mm slip: on the fixes 0.5 s after the stop, %.1f mm off", speeds[i], slips[i], hypotf(sx - px, sy - py)); + // A sideways slide is partly read as a turn until the robot moves again + float heading_tol = (lateral[i] == 0) ? 4.0f : 8.0f; + CHECK(_angle_error_deg(est.theta, r.theta) < heading_tol, "%+.0f mm/s, %.0f mm slip: heading kept, %.1f deg off", speeds[i], slips[i], _angle_error_deg(est.theta, r.theta)); + } +} + +static void test_reanchor_after_driving(void) { + // Without the slip noise the gate rejects the fixes; the robot drove a + // moment ago, so it re-anchors, keeping the heading, instead of a kidnap + db_pose_estimator_conf_t conf = _conf; + conf.q_pos_slip_mm2_per_mm_s = 0; + conf.q_heading_slip_deg2_per_mm_s = 0; + const float speeds[] = { 600, -600 }; + for (unsigned i = 0; i < 2; i++) { + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 90 * DEG, .seed = 40 + i, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + _sprint_and_slip(&est, &conf, speeds[i], 40, 0, &r); + float sx = 0, sy = 0, px, py; + _robot_sensor(&r, 0, &px, &py); + bool tracking = db_pose_estimator_sensor(&est, &sx, &sy); + CHECK(tracking && est.kidnaps == 0 && est.reanchors == 1, "%+.0f mm/s, no slip noise: re-anchored, not kidnapped, status %d kidnaps %u reanchors %u", speeds[i], est.status, est.kidnaps, est.reanchors); + CHECK(hypotf(sx - px, sy - py) < 5.0f, "%+.0f mm/s, no slip noise: on the fixes, %.1f mm off", speeds[i], hypotf(sx - px, sy - py)); + CHECK(_angle_error_deg(est.theta, r.theta) < 4.0f, "%+.0f mm/s, no slip noise: heading kept, %.1f deg off", speeds[i], _angle_error_deg(est.theta, r.theta)); + } + // With neither, as before the fix, the same stop is taken for a kidnap + conf.kidnap_settle_ticks = 0; + conf.reanchor_mm = 0; + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 90 * DEG, .seed = 42, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + _sprint_and_slip(&est, &conf, -600, 40, 0, &r); + CHECK(est.kidnaps == 1, "without the guard the stop reads as a kidnap, kidnaps %u", est.kidnaps); +} + +static void test_moved_right_after_driving(void) { + // Carried 150 mm the moment it stopped: too far to be slip, so still a kidnap + db_pose_estimator_t est; + robot_t r = { .x = 1000, .y = 1000, .theta = 0, .seed = 44, .fix_age = DB_POSE_ESTIMATOR_FIX_AGE_TICKS }; + int32_t run = (int32_t)lroundf(300.0f * 0.01f / DB_MM_PER_COUNT); + db_pose_estimator_init(&est, &_conf); + db_pose_estimator_seed(&est, r.x, r.y, 0, 1); + _run(&est, &r, run, run, 50, 1, LH2_NOISE_SD_MM); + _run(&est, &r, 0, 0, 5, 1, LH2_NOISE_SD_MM); + r.x += 150; + _run(&est, &r, 0, 0, 45, 1, LH2_NOISE_SD_MM); + CHECK(est.kidnaps == 1 && est.reanchors == 0, "a 150 mm move right after driving is a kidnap, kidnaps %u reanchors %u", est.kidnaps, est.reanchors); +} + static void test_turn_noise_grows_with_turn_speed(void) { // The same wheel travel difference, turned slowly or fast int32_t counts = 1000; @@ -567,6 +721,10 @@ int main(void) { test_occlusion_keeps_heading(); test_fix_age_compensated(); test_noise_scales_with_distance(); + test_slip_noise_on_hard_stop(); + test_hard_stop_slip_is_not_a_kidnap(); + test_reanchor_after_driving(); + test_moved_right_after_driving(); test_turn_noise_grows_with_turn_speed(); test_seed_carried_to_present(); test_chain_survives_outlier();