drv/pose_estimator: add an EKF on odometry and position-only LH2 fixes

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 \

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

drv/drv.emProject

@@ -182,6 +182,15 @@
     <file file_name="tdma_server.c" />
     <file file_name="../tdma_server.h" />
   </project>
+  <project Name="00drv_pose_estimator">
+    <configuration
+      Name="Common"
+      project_dependencies=""
+      project_directory="."
+      project_type="Library" />
+    <file file_name="pose_estimator/pose_estimator.c" />
+    <file file_name="pose_estimator.h" />
+  </project>
   <project Name="00drv_wheel_control">
     <configuration
       Name="Common"

drv/geometry.h

@@ -56,21 +56,65 @@
 /// because the photodiode sits on the centreline.
 #define DB_LH2_LEVER_ANGLE (0.0f)
 
+/// 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
+/// 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:
 // 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_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