search

wpilibsuite / allwpilib

Official Repository of WPILibJ and WPILibC
https://wpilib.org/
Other
1.03k stars 606 forks source link

[wpimath] Simplify pose estimator #6705

Closed KangarooKoala closed 6 days ago

KangarooKoala commented 4 weeks ago

Completely non-breaking pose estimator implementation parts of #5473 (that is, excluding the removal of the kinematics parameter of PoseEstimator (which is exposed to custom implementations) and the test unification) updated for the addition of sampleAt(). (Thanks @jlmcmchl for the inspiration!)

Allocations analysis:

Notes:

calcmogul commented 4 weeks ago

Since sampleAt() was merged after 2024 champs, it hasn't been in a release yet, so we can change the behavior to whatever we want.

KangarooKoala commented 4 weeks ago

I didn't think of this earlier, but the information is still needed for addVisionMeasurement() because we sample what the estimate of what the robot pose was at the time of the new vision measurement, but using a single offset would suffice for the main case of vision measurements coming with increasing timestamps. (And from previous discussion in Discord, that's a rare case we could probably drop support for. There's already weird and somewhat unexpected behavior when that happens.)

virtuald commented 4 weeks ago

Sounds like you should write some real code that uses this to make sure it actually does what a user might want before finalizing the PR.

KangarooKoala commented 4 weeks ago

I didn't think of this earlier, but the information is still needed for addVisionMeasurement() because we sample what the estimate of what the robot pose was at the time of the new vision measurement, but using a single offset would suffice for the main case of vision measurements coming with increasing timestamps. (And from previous discussion in Discord, that's a rare case we could probably drop support for. There's already weird and somewhat unexpected behavior when that happens.)

Based on further conversation on Discord, we don't want to add the assumption that vision measurements come in order (which is something that would happen with multiple coprocessors), so we'll keep the vision update buffer.

Sounds like you should write some real code that uses this to make sure it actually does what a user might want before finalizing the PR.

Is there something in addition to the existing tests that you think should be added? (Maybe a simulation of multiple coprocessors?)

KangarooKoala commented 3 weeks ago

What's left for this PR? I'd be happy to add some tests (probably in a separate PR), but I'd need to know which behavior we want to test.

KangarooKoala commented 2 weeks ago

From local testing, the difference between the old implementation and the new implementation is on the order of 1e-12. (First, I changed debug to true in calls to testFollowTrajectory in SwerveDrivePoseEstimatorTest.cpp, then piped stdout from ./gradlew wpimath:testDesktopCpp into a text file, did the same on main (e2893fc), and then ran a Python script on the two text files to find the greatest difference.)

Python code:

from decimal import Decimal, InvalidOperation
import sys

with open(sys.argv[1]) as reader1, open(sys.argv[2]) as reader2:
  max_err: Decimal = Decimal(-1)
  for (lnum, (line1, line2)) in enumerate(zip(reader1, reader2, strict=True), start=1):
    col1: int = 0
    col2: int = 0
    while True:
      # -1 if not found, which using as the end index in a substring cuts off the last one (newline character)
      next1: int = line1.find(',', col1)
      next2: int = line2.find(',', col2)
      part1: str = line1[col1:next1].strip()
      part2: str = line2[col2:next2].strip()
      float1: None | Decimal
      try:
        float1 = Decimal(part1)
      except InvalidOperation:
        float1 = None
      float2: None | Decimal
      try:
        float2 = Decimal(part2)
      except InvalidOperation:
        float2 = None
      if (float1 is None) ^ (float2 is None):
        print(f"mismatched type: {part1 = !r}, {part2 = !r}")
      elif float1 is not None and float2 is not None:
        err: Decimal = abs(float2 - float1)
        if err > max_err:
          max_err = err
          print(f"New max error {err} at {lnum} between {part1} and {part2}")
      if next1 < 0 or next2 < 0:
        break
      col1 = next1 + 1
      col2 = next2 + 1

Output:

New max error 0 at 47 between 0 and 0
New max error 1E-17 at 60 between 0.04050561270097854 and 0.04050561270097855
New max error 3E-16 at 60 between 0.7451136041863254 and 0.7451136041863257
New max error 1.77E-15 at 77 between 0.2627713934009313 and 0.26277139340092953
New max error 1.8E-15 at 81 between 0.3399786876534412 and 0.3399786876534394
New max error 1.20E-14 at 108 between 1.1662427755618154 and 1.1662427755618034
New max error 1.22E-14 at 110 between 1.2420949917467707 and 1.2420949917467585
New max error 5.07E-14 at 590 between 0.7201715575416567 and 0.7201715575417074
New max error 5.79E-14 at 590 between 0.7198043229924798 and 0.7198043229925377
New max error 5.80E-14 at 591 between 0.7361595510115062 and 0.7361595510115642
New max error 1.2005E-13 at 765 between 0.41930745110901085 and 0.4193074511091309
New max error 1.202E-13 at 765 between -0.3931206122099427 and -0.3931206122100629
New max error 7.313E-13 at 3434 between -2.991012647154576 and -2.9910126471538447
New max error 7.314E-13 at 3436 between -2.973068699945149 and -2.9730686999444176
New max error 4.0153E-12 at 4402 between 0.9545146075830452 and 0.9545146075870605
New max error 4.0154E-12 at 4403 between 0.9634643004179145 and 0.9634643004219299
New max error 4.0202E-12 at 8490 between 0.9545729565420277 and 0.9545729565460479
New max error 4.0204E-12 at 8491 between 0.963522649376897 and 0.9635226493809174
New max error 4.0205E-12 at 8494 between 0.9808667964291787 and 0.9808667964331992