2/2 Use odom topic with velocity and add noise model on top - #4
Open
vincenttumminello wants to merge 9 commits into
Open
vincenttumminello wants to merge 9 commits into
vincenttumminello wants to merge 9 commits into
Conversation
rt/odometer_state and rt/odom published the base's ground truth, while the real K1's odometry is the controller's estimate and drifts, so localisation in sim saw better odometry than it will on the robot. OdometryModel corrupts the planar body velocity (vx, vy, wz) with a scale error, white noise and a random-walk bias, and integrates the odometry pose from it, so both topics carry the same consistent estimate. The noise terms are densities, so the drift is independent of the publish rate. Configured in config/odometry.yaml, off by default with untuned placeholder values; with it off the output is unchanged. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_014yZBRrPMFAxdS4A93XWANS
The real K1 publishes nav_msgs/Odometry on rt/odom (kTopicRosOdometer, 1.7.0 firmware): rt/odometer_state's planar pose plus the velocity, which NUbots_K1 now reads for Sensors.vTw. Publish it from the base state at the LowState cadence, following the ROS convention since Booster does not document the frames: pose in "odom", twist in the body frame "base_link". Adds the nav_msgs, std_msgs and builtin_interfaces types and the geometry_msgs twist and covariance types, with every package's msg/ on the fastddsgen include path so an .idl can include across packages; the existing generated code is unchanged. The synthetic state source now reports the velocity of its commanded walk, and the SDK contract test checks rt/odom. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_014yZBRrPMFAxdS4A93XWANS
A random-walk bias grows without bound, so a long sim session drifted more than a short one, and a recording could not pin it down: the bias accumulated since the recording started shows up as its own error growth. Model it instead as a first-order Gauss-Markov process with a standard deviation and a time constant, stationary from the first sample, whose pose error over a window of T seconds has the closed form s^2 T + 2 b^2 tau^2 (T/tau - 1 + exp(-T/tau)) that a recording with motion capture can be fitted to. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_014yZBRrPMFAxdS4A93XWANS
…o/add-odometry-noise-model
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This builds on PR #3 by adding a noise model of the form$v_{est} = scale * v_{true} + bias + white noise$ to make it more true to real.
TODO: Record what the noise is actually like on the real K1 and update the params here. (will probably get to this next week)