feat: add ComplementaryFilter, fusing a drifting rate with a noisy absolute reading

[alxkm]

Adds the complementary filter: one estimate out of two sensors that are each wrong in a different way.

The classic pair is an accelerometer and a gyroscope measuring the same tilt. The accelerometer knows where down is and never drifts, but every vibration of the frame shows up in it. The gyroscope is smooth and immune to vibration, but it measures a rate, so using it means integrating, and the smallest bias in that rate integrates into an angle that walks away without limit. Neither is usable alone, and their errors live in different parts of the spectrum, which is exactly the situation this filter is for.

value <- a * (value + rate * dt) + (1 - a) * reference

Read as a pair of filters that add up to one, it is a high pass on the integrated rate and a low pass on the absolute reading: the drift of the first is cut off below the corner frequency and the noise of the second above it. The two transfer functions sum to unity at every frequency, so the true signal passes through untouched whatever a is, and the filter cannot introduce a lag of its own the way a plain low pass on the accelerometer would.

The parameter is easier to reason about as a time constant, tau = a * dt / (1 - a), and ofTimeConstant(tau, dt) sets it that way round. Below tau the answer comes from the gyroscope, above it from the accelerometer, and that fixes the price of the trade exactly: a rate with a constant bias b leaves a steady state error of tau * b and no more, where plain integration would have grown without limit.

Against the KalmanFilter already in the package: the Kalman filter is the right answer when the noise of both sensors is known and worth modelling, and it will beat this one when it is. The complementary filter needs no covariance, no model of the process, two multiplications per sample and one number of state, and it degrades gracefully when the noise is not what anybody assumed. That is why it is what actually runs on small flight controllers, and both Javadocs say so.

ComplementaryFilterTest covers 31 cases, and four of them check the theory rather than the behaviour:

• with nothing to integrate the estimate decays onto the reference as exactly a^n, checked step by step over 200 samples;
• a gyroscope bias of 0.1 rad/s settles at exactly tau * bias and stays there over 5000 samples, while the same bias integrated on its own reaches 5.0;
• white noise on the reference is cut by the factor sqrt((1 - a) / (1 + a)) the theory promises, measured over 20000 samples;
• a ramp is followed with a mean error under 0.02, where a plain low pass of the same a on the reference alone lags by tau * slope, which the test measures and compares.

The rest cover the seeding of the first sample from the reference, the round trip between the coefficient and the time constant, the scaling of the integration by the elapsed time, restarting from a known estimate, and the argument checks.

Checklist

• I have read CONTRIBUTING.md.
• This pull request is all my own work -- I have not plagiarized it.
• All filenames are in PascalCase.
• All functions and variable names follow Java naming conventions.
• All new algorithms have a URL in their comments that points to Wikipedia or other similar explanations.
• All new algorithms include a corresponding test class that validates their functionality.
• All new code is formatted with clang-format -i --style=file path/to/your/file.java

[codecov-commenter]

Codecov Report

✅ All modified and coverable lines are covered by tests.
✅ Project coverage is 81.21%. Comparing base (b2b9d3a) to head (2494062).

Additional details and impacted files

@@             Coverage Diff              @@
##             master    #7614      +/-   ##
============================================
+ Coverage     81.18%   81.21%   +0.02%     
- Complexity     7899     7922      +23     
============================================
  Files           830      831       +1     
  Lines         25012    25056      +44     
  Branches       4882     4885       +3     
============================================
+ Hits          20307    20350      +43     
  Misses         3920     3920              
- Partials        785      786       +1     

☔ View full report in Codecov by Harness.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.