Add Kalman filter, non-linear sensitivity curve

[lenkaiser]

TL;DR

Based on the YT comments a Kalman filter was suggested. Additionally I added a non-linear sensitivity curve. Anyway feel free to merge these changes (partially) as you see fit.

Summary

• Kalman filter: Replaces the simple low-pass filter with an adaptive per-axis Kalman filter. Automatically balances responsiveness vs smoothness — trusts new measurements more after idle, smooths more once stable. Tunable via KALMAN_Q and KALMAN_R in Config.h.
• Non-linear sensitivity curve: Adds a cubic power curve (SENSITIVITY_EXP = 3.0) so small deflections give fine precision and large deflections give fast motion, matching how commercial spacemice feel. Set to 1.0 to restore linear behavior.
• Sensor read validation: readRaw() now returns bool and rejects frames where any axis exceeds sane range (>500 mT) or a sensor reads all-zeros (I2C failure). Bad frames are skipped in both the motion pipeline and calibration.
• Precomputed constants: Replaces runtime sqrt(3.0) calls and /3.0 divisions with precomputed constants, eliminating redundant FP math per frame on the Cortex-M0+.
• Smooth dead zone transitions: Instead of hard-snapping the Kalman estimate to zero at the dead zone boundary, the estimate now decays gradually.

[ThorstenWirth]

He can't include VID/PID and manufacturer string because of legal reasons, IIRC from his video.
What the user does at home before compiling is not his responsibility.

[lenkaiser]

@ThorstenWirth People indicated that others have done this as well, I assumed they also had something of a public DIY project. If not the PR can partially be merged as I enabled edit by maintainers.

[xorl]

@ThorstenWirth People indicated that others have done this as well, I assumed they also had something of a public DIY project. If not the PR can partially be merged as I enabled edit by maintainers.

Yeah other projects I've tried have the config for Arduino IDE in a separate file with instructions on how to modify it. The same could probably be referenced here to keep it out.

[lenkaiser]

Ok just to help out and prevent any legal issues I removed the 3dconnexion changes, it's also not in the history of the PR. This can now safely be merged imo.

[MoonMan-blip]

He can't include VID/PID and manufacturer string because of legal reasons, IIRC from his video. What the user does at home before compiling is not his responsibility.

in the video is it only the 3 lines at the top that you need to inlcude to mimmic the connexion or is there more i need to find.. newbie here ive only built a few projects so i dont fully understand everything. but im trying

[rain-fade]

in the video is it only the 3 lines at the top that you need to inlcude to mimmic the connexion or is there more i need to find.. newbie here ive only built a few projects so i dont fully understand everything. but im trying

This isn't something that should be discussed here given the concern for potential legal implications.

To clarify, at that point in the video, what is shown is that those three lines are not being included in the repo in order to obviate legal issues.

[MoonMan-blip]

right okay, i follow

[apadua]

The instructables has the following comment:

Before building, keep in mind that as mentioned in the video, the motion processing for this project is still not fully solved and may eventually be replaced entirely. There is currently some bleed between the axes because each one is calculated independently, and the implementation also assumes the sensor readings are linear, which is not true.

With these changes would you say this is resolved and it feels much closer to the real thing?

[rain-fade]

With these changes would you say this is resolved and it feels much closer to the real thing?

most likely, if it ever gets merged.

[apadua]

With these changes would you say this is resolved and it feels much closer to the real thing?

most likely, if it ever gets merged.

Sure, but I can have my own fork and merge it myself, right?

[rain-fade]

Sure, but I can have my own fork and merge it myself, right?

this is the way.

[JUBETORT]

I take it this has been abandoned and won't be merged due to the mentioned legal concerns or whatever. But has anyone made their own fork and merged it? Does anyone feel it does improve the feel and functionality closer to what it was mimicking? @lenkaiser Thank you for doing this work. Have you tried it out and what do you think?

[lenkaiser]

@JUBETORT I updated the PR and completely removed any reference to the spacemouse changes and also removed it from the commit history. So there's no legal reason not to merge this code.

However I see that my code is referenced in multiple PRs / commits so regardless if my code is merged I would love to see all the different improvements come together somewhere.

[spuder]

I take it this has been abandoned and won't be merged due to the mentioned legal concerns or whatever. But has anyone made their own fork and merged it? Does anyone feel it does improve the feel and functionality closer to what it was mimicking? @lenkaiser Thank you for doing this work. Have you tried it out and what do you think?

I've been testing this PR. While I have no experience with a space mount so I don't know what the commercial product feels like, this does feel much smoother and slower around the central axis, and significantly faster at the edges (a little too fast in my opinion).

Overall I'm happy with it. Thanks so much for sharing.

For anyone else wanting to test, here are git commands

git remote add lenkaiser git@github.com:lenkaiser/cad-mouse-mk2.git
git fetch lenkaiser main 
git checkout -b kalman lenkaiser/main

[LeonRein]

First off, I am no expert in this field, but I have been reading up heavily on the topic of Kalman filters over the last few days. Here are my thoughts on the Pull Request:

I am not sure how much sense this implementation of the Kalman filter makes in this context. A Kalman filter really shows its strengths when dealing with a system that can be described mathematically and whose behavior can be simulated. For example, a drone. There, assumptions can be made about how the state of the drone has changed after a certain amount of time. For instance, due to inertia, the drone will maintain its flight path while continuing to rotate around a certain axis and accelerating upwards because we are applying slight throttle. The Kalman filter is a mathematical model that can estimate the margin of error in our calculations and, taking into account various measured variables that are also subject to a certain degree of error, provides a statistically optimal estimation of the drone's current state.

The problem with the CAD mouse, however, is that the state of the system primarily depends on user input, which simply cannot be described mathematically. One could only make assumptions along the lines that sudden, excessively large state changes are unlikely. For example, a hand cannot push the knob to the left with an acceleration of 5G. That means the only assumption we can make is that everything stays the way it is. Which is exactly what is being done here.

Another advantage of the Kalman filter is that it can take covariances between different measured variables and states into account. For instance, noise in the supply voltage of a Hall effect sensor would affect the measurement of all three axes of that single Hall sensor. Unfortunately, this is not being utilized here, as a separate Kalman filter is implemented for each DOF.

Furthermore, a Kalman filter only works for linear systems, otherwise the entire math and estimation of the normally distributed noise breaks down. Unfortunately, Hall sensor technology is anything but linear, since the field strength decreases by a factor of 1/(r^3). Here, it is simply assumed first that there is a linear relationship between distance and field strength; based on this assumption, the position and rotation of the knob are calculated, and these states are then Kalman filtered. And only afterward are these values passed through the sensitivity curve. I don't know if the Kalman filter can even work properly this way.

Therefore, it would make much more sense to implement an Unscented Kalman Filter (or even better, a Square Root Unscented Kalman Filter), which can directly handle the strong non-linearity of the system and use the dipole model directly as the h function. The advantage is that you wouldn't even have to calculate the position of the filter from the field strengths, but rather just the expected field strengths from the position of the knob, which is much simpler. How fast the RP2040 manages to compute this on a single core is another question.

TL;DR: I think this implementation of a Kalman filter makes little sense. You should either implement one filter for all axes/rotations using a dipole model, or use a much simpler filter like a One Euro Filter, which was explicitly developed for applications such as HID devices.