Adds S3SigmaPoints based on MerweScaledSigmaPoints. In addition, restructures UnscentedKalmanFilter to support different sigma point generators and provides MerweUKF and S3UKF for convenience when working with either kind of filter.
S3UKFTest is copied from MerweUKFTest (which is a rename of UnscentedKalmanFilterTest). Curiously, however, in Java the original tolerance used in MerweUKFTest.testDriveConvergence() for the final rotation was too low for S3UKFTest, so the tolerance is increased from 0.000005 (5e-6) radians to 0.00015 (1.5e-4) radians. However, the C++ version still uses the original tolerance. (This difference is probably because Java uses a final rotation of 5.846 degrees while C++ uses a final rotation of 5.846 radians)
Closes#8072.
Breaking changes:
- (C++) UnscentedKalmanFilter has a new template parameter for the sigma point generator type.
- (Java) UnscentedKalmanFilter has an additional parameter to every constructor providing an instance of a sigma point generator.
- (C++) int MerweScaledSigmaPoints.NumSigmas() has been replaced with constexpr int MerweScaledSigmaPoints::NumSigmas.
- (C++) The second parameter of SquareRootUnscentedTransform has been changed from States to NumSigmas.
* Move units into API docs instead because suffixes make user code verbose and hard to read
* Rename trackWidth to trackwidth
* Make ultrasonic classes use meters instead of a mix of m, cm, mm, ft,
and inches
Small values of kₐ make the iterative solver ill-conditioned. This
change reverts to the constant-acceleration feedforward in that case. It
gives _very_ bad results (hence why we added the iterative solver in the
first place), but it's better than hanging.
```
TEST(ArmFeedforwardTest, CalculateIllConditioned) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 20_V / 1_rad_per_s;
constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
constexpr auto Kg = 0_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
// Calculate(currentAngle, currentVelocity, nextAngle, dt)
CalculateAndSimulate(armFF, 0_rad, 0_rad_per_s, 2_rad_per_s, 20_ms);
}
```
This produces 1 V and doesn't accelerate the system at all. Using
nextVelocity instead of currentVelocity in the feedforward outputs 41 V
and still only accelerates to 0.4 rad/s of the requested 2 rad/s.
I picked the kₐ cutoff by increasing kₐ until the iterative solver
started converging.
Fixes#7743.
This is easier to type and follows the naming of Pose2d::Nearest().
Since Ellipse2d and Rectangle2d were added for the 2025 season, we don't
need to add deprecation notices.
The Raspberry Pi 5 is fast enough that we no longer need it.
```
Running ./build/DAREBench
Run on (4 X 2400 MHz CPU s)
CPU Caches:
L1 Data 64 KiB (x4)
L1 Instruction 64 KiB (x4)
L2 Unified 512 KiB (x4)
L3 Unified 2048 KiB (x1)
Load Average: 0.47, 0.72, 0.45
***WARNING*** CPU scaling is enabled, the benchmark real time measurements may be noisy and will incur extra overhead.
-------------------------------------------------------------------------------
Benchmark Time CPU Iterations
-------------------------------------------------------------------------------
DARE_WPIMath_Dynamic 34.4 us 34.4 us 20315
DARE_WPIMath_NoPrecondChecks_Dynamic 21.7 us 21.7 us 32266
DARE_WPIMath_Static 15.2 us 15.2 us 45878
DARE_WPIMath_NoPrecondChecks_Static 7.84 us 7.84 us 89316
DARE_SLICOT 79.4 us 79.4 us 8789
DARE_Drake 34.9 us 34.9 us 20074
```
The Google C++ protobuf implementation has issues with dynamic linkage across DLL boundaries because it uses global variables. It also has a compile-time dependency because the protoc version must exactly match the libprotobuf version. Using nanopb with a customized generator fixes both of these issues.
Co-authored-by: Gold856 <117957790+Gold856@users.noreply.github.com>
Splitting the files didn't help readability or save compilation time and
it confused contributors. Merging them is also in line with how C++
modules will be written.
Reading exported data from shared objects on windows is broken. It requires __declspec(dllimport). However, this is problematic, as we use the same static libraries both from a shared and static context. So we can't just blindly apply dllimport.
The linker should have caught this, as data members are exported in a different way. However, due to a bug in native-utils, data member symbols were exposed directly. However, interacting with those data member was completely broken.
The only way we can really solve this is to just not use static data members. We're pretty good about this in WPILib itself. However, protobuf is absolutely terrible at this. There are a ton of inline functions that access global data. For the protobuf library itself, we can solve this easily enough.
However, for the generated protobuf code, this is much more problematic. The member needed to bypass the global data is private. This means using just the stock protobuf code, this problem is not solvable. But, protobuf generated code has insertion points. Those insertion points let us add our own code into the generated code via a protoc plugin. And it just so happens that an insertion point exists to add extra public methodsto the generated protobuf header. There is also an insertion point to let us add to the cpp file.
The methods we need are the getters, for unpacking protobufs. For any protobuf that has a message as a member, we generate a new wpi_x() getter (the existing one is just x(), where x is the field name). We then implement this in the cpp file. A trick we can use is in the cpp file, we can safely call the x() function, as the cpp file is in the same library as the global. Thus we can call that inline method, and not actually need to directly access any internal private state of the protobuf object.
TL;DR, all protobuf classes that have messages as fields now have a wpi_x() accessor that must be used instead of x() if you want the code to work on windows. After wpilibsuite/native-utils#212, the bad code will fail to link, rather then just fail at runtime.
Reverts #6609 since that fix didn't Just Work(tm) on Windows. (edit: or Ubuntu. Seems to have broken everything except macOS.) This PR configures CMake to try and find protobuf-config.cmake first, which allows protobuf to pull in abseil for us. If protobuf-config.cmake is not available (coprocessors which don't have a new enough protobuf installed are a common case), it will fallback to CMake's built-in FindProtobuf module, which is what we were using before.
Add wpi::CreateMessage, a wrapper with an ifdef to switch between Arena::CreateMessage and Arena::Create, since the former is deprecated in newer versions of protobuf. This allows forward compatibility with newer versions of protobuf.
We now use a wrapper (wpi::print) to catch exceptions since we can't patch
std::print() to not throw when we ultimately migrate to it.
fmtlib and std format/print throw the same exceptions and always have. We previously patched fmt::print() to not throw a write failure exception, but we can't do that for std::print(); wpi::print() is the migration plan.
LTVUnicycleController is a drop-in replacement with better tuning knobs.
The RamseteCommand examples were removed instead of retrofitted with
LTVUnicycleController because we're planning on removing the command
controller classes anyway, so it would be wasted effort. The
SimpleDifferentialDriveSimulation example shows direct
LTVUnicycleController usage.
