Sanity Check of mpu9250-derived values? Am I in the right units?
Opened this issue · 19 comments
I really appreciate your work on this project, thanks! On a Heltec ESP32 Wifi OLED and MPU9250 on a windows machine with Thonny IDE, I'm trying to replicate your quaternion compass ported to python - not that I like python, but it's the most successful I've been getting an IDE working for embedded ESP32 development.
I don't want you to debug my code - I'm just trying to get a sanity check the logged/screenshotted values for clues to where my math code is going wrong. I believe I've put in calibration values for accelerometer, gyro, and magnetometer. Here are my values with the device sitting still. Do they seem like they're in the right ranges/units? I'm having trouble finding examples of input ranges for the mahony filter.
The mahony inputs and the evolving q seem pretty stable. The roll is also relatively stable. The pitch and, worse, yaw are oscillating wildly when the device is at rest. This strikes me as some kind of accumulation issue over the samples, possibly because I'm using the wrong units and it's basically modulus 180?
Weirdly, the roll does NOT change proportionally as I reorient the device - and this might be as bad as the wild oscillations. The sensor values do seem to change reasonably proportional to me reorienting the chip.
For what value it might have other tinkerers, here's the hardware setup:
@kriswiner thanks for the quick response. Here I switched some settings (committed and pushed) and I've got accel 0..1, gyro 0..0, and |mag in gauss, maybe| <= .5
q's gone crazy now.
I'm still at it, but there's my update just in case you or anyone has ideas or future travelers hit the same frustration.
Are you feeding the filter with data as NED or ENU convention?
I'm not sure what convention this code is using yet. Not that I'm not trying, but so far this is a copy & paste level of comprehension. Maybe the answer is obvious in the next screenshot below?
an you describe how you calibrate the magnetometer...this is the most
common reason for "jitter" in the AHRS output...
AK8963.py has a calibrate() function. It takes over a minute to run and gives back offset and scale tuples. While it's running I wave the chip all around. I can print those out and hardcode them in for future runs.
mag_offset=(-6.254883, 81.39698, -20.41348),
mag_scale=(0.9332827, 1.106261, 0.976022)
if the device is pointing in the sensor axis direction you have
chosen for absolute north, q should be 0,0,0,1 or 1,0,0,0 depending on the
convention you are using...
If I assume the problem is in my python port of your mahony filter code and focus on stable inputs for that, with only a non-nothing value for mx then I get q = [+1.00 +0.00 +0.00 +0.00] and it is stable:
But if I simulate a constant value off-axis, I get this oscillation which I don't understand. Even with 65.0, 1.0, 0.0 for mag, there's an oscillation. Makes me suspicious of some floating point/data type issue (an unexpected integer rounding making the 65.0, 0.0, 0.0 case clean) but I haven't found it.
Note: I've gathered that numeric precision aside, it doesn't really matter what unit values I pass in because all triplets are normalized to the unit sphere.
regardless of NED/ENU orientation, given constant inputs, mahony's q shouldn't oscillate like that, right?
Certainly could [cause
qto oscillate like that]
I'm having trouble understanding that. If the MARG sensors are providing ideal, noiseless values as input to the filter, why would the filter be expected to drift or oscillate? Please elaborate, if you are willing.
Hmm, what values for accel and gyro should be idempotent with regard to the magnetometer values? Again, I'm hardcoding the mahony filter update inputs at this stage.
I would not be surprised if there are other errors in your python implementation
I expect so. Since I'm getting stable values up until the ported mahony code, I figure there's an error in there. Trying to find clues as to where it's going wrong. Since repeated hard-coded off-axis values are giving oscillations, I'm thinking it's some difference in floating point arithmetic or so. Something giving negative values in python that c doesn't do, or vice versa.
sort out this NED/ENU convention business
I aim to, as soon as I learn what it means specifically. Lots of google results referring to the terms, nothing I've recognized yet explaining to me how they apply to these axes and mahony inputs.
why not test your hardware system against a "known good" Arduino solution to have some quat data to compare to?
I don't have an Arduino nor a local guide. And the c stuff on Windows has been such a a hassle and that's all I've got.
The breakout board itself doesn't indicate axes the same way I've seen pictures of other boards do. I found this picture
So here's my reference frame, with the heading forward of the OLED screen:
From this, I infer this expected NED ordering:
aN, gN: -x; mN: -y
aE, gE: +y; mE: +x
aD, gD: -z; mD: +z
Although my actual measurements confuse me or disagree. For the accelerometer measurements I think I need to do the opposite of expected positive z = 9.8m/s2 at rest. I read in a few places about "gravity positive/negative down" convention which I don't get because a freefalling device should see all values at zero, and a lifted device will experience 1G+ forces. But OK, I can get the values I expect by negating. (as you suggested above: "the output should be 0[g], 0[g], +1[g] when the device is at rest and sensor faces up")
Next up is gyro. It's hard for me to be sure which rotations should be considered positive. Clockwise when looking at the positive axis direction from the perspective of the origin?, but I think the expected NED values work.
Next is magnetometer values. I really don't know what I'm looking for here. I get a mag vector magnitude of ~50uT, but the components are not behaving as I'd expect to see. I expect to see that whatever axis is pointing toward north would have the strongest measurement at 50uT and the other two perpendicular axes ideally at 0uT. I'm not getting that, even after several ~consistent calibrations.
The magnitudes of a, g, and m vectors are irrelevant since they're all normalized, I believe. And I do get stable values from the sensors on all 9 axes.
But putting the sensor values aside for a moment, I'm still getting oscillating signs in q on "ideal" hardcoded values to both Mahony and now also Madgwick. I expect that if I give the same NED oriented values repeatedly, both filters should quickly converge to some stable value. A device at rest would have 0, 0, 1g accelleration, 0, 0, 0 gyro rotation, and not many options for magnetometer values, but they too would be ideally constant for a still device. When I simulate any variation of those constant inputs, the oscillations are present. With Madgwick now, the effect is obviously a sign-flip on two of the components.
This is really the same as my earlier post. Any value for mE other than 0.0 makes q unstable. mD doesn't seem to matter, which feels right..
I must really me missing something. I thought maybe it was a single-precision floating point vs double loss of precision problem, so I ported it again to typescript and made this little demo, all self contained. It has a top chart for the quaternion mahony iterations, and the lower chart shows the resulting roll, pitch, yaw. It behaves the same crazy as the other implementation.
https://codesandbox.io/s/eloquent-cray-xqy0lt?file=/src/mahony.ts
