Tilt Correction when it comes to heading?
Closed this issue · 4 comments
I am going to try to provide as much information as possible for some assistance. I've been playing around with this library for a few weeks and am pretty satisfied so far, but I have finally come across a bit of weirdness that I can't seem to solve. It involves the heading of each individual axis while the device is tilted.
Here is my basic setup:
const AHRS = require('ahrs');
this.ahrs = new AHRS({
sampleInterval: 26,
algorithm: 'Mahony',
doInitialisation: true,
kp: 15.0, // Default: 0.5
ki: 0.1, // Default 0.0
});
};After initializing the AHRS, The data is processed this way:
BoxTest.prototype.filterGyroData = function() {
this.dt = this.imu.data.dt; //Calculated each time data is received from the device
this.gyro.copy(this.imu.data.gyro); // Vector3 being created from gyro data
this.gyro.sub(this.state.gyroBias); // Subtract bias
this.acc.copy(this.imu.data.acc); // Vector3 being created from accelerometer data
this.acc.sub(this.state.accBias); // Subtract Bias
this.gyro.mulScalar(70).divScalar(1000).mulScalar(pc.math.DEG_TO_RAD); // Divide by 13.7 (from hardware docs) to be in line with units in degrees and then convert to radians
this.acc.divScalar(8192); // Divide by 8192 (from hardware docs) to make G's
if(this.imu.data.mag) {
// Only send magnetometer data if there has been a update from the chip
this.mag.copy(this.imu.data.mag); // Vector3 being created from magnetometer data
this.mag.sub(this.state.magBias); // Subtract Bias
this.ahrs.update(this.gyro.x, this.gyro.y, this.gyro.z, this.acc.x, this.acc.y, this.acc.z, this.mag.y, -this.mag.x, this.mag.z, this.dt);
}
else {
this.ahrs.update(this.gyro.x, this.gyro.y, this.gyro.z, this.acc.x, this.acc.y, this.acc.z, 0, 0, 0, this.dt);
}
this.state.quat.copy(this.ahrs.getQuaternion()); // Update existing Quaternion from AHRS output
this.flipQuaternion(this.state.quat); // Flip Quaternion axes to match coordinate system
this.state.quat.mul(this.forward); // Rotate 90 degrees to point Z north instead of west
};
BoxTest.prototype.flipQuaternion = function(quat) {
let tempX = quat.y;
let tempY = -quat.z;
let tempZ = quat.x;
let tempW = -quat.w;
this.state.quat.set(tempX, tempY, tempZ, tempW);
};After this data is processed I then apply the rotation to a 3D object. Before adding the magnetometer data I was able to confirm that the basic orientation and rotation of all of the axes occurred as expected. Whenever I would point any of the axes 'down' this would be visually represented by the cube that I'm working with. Of course, the heading was wrong, and the results would drift eventually, especially after some particularly speedy rotations, but overall things were working as expected.
In flipping the Y and X axes for the magnetometer and negating X, I am now able to very consistently have my X and Z point to my virtual 'north' when they point north in the real world. This start to go wonky, however once I move the Y axis closer to horizontal. I find that the direction in which the Y axis points is only accurate when X is pointed straight up. If X is pointed straight down then the Y axis points a full 180 away from the intended direction. This behavior is gradual, so you can see it slowly reach the reversal as X get further away from 'Up'. A identical behavior is observed with pointing the X axis in a direction while the Z axis is not pointed straight down.
Writing this out has me now reconsidering that it is an issue with the mag data being provided to the AHRS system with either order or signs being the culprit, and maybe I am just tired, but I have tried all 48 possible combinations of axis order and signs with no promising results. I have also tried disabling my Quaternion flip and the final rotation of 90 degrees, which did not remove the behavior, making it only appear to happen on different axes due to the coordinate system change.
I am keeping my fingers crossed that it's just a simple mistake that I'm overlooking in my setup, and would deeply appreciate any guidance anyone can give.
From the face of it, I don’t see any issues. Some questions/comments:
- did you read the the debugging notes in README.md?
- Orientation of axes is important, so are the units. When stationary the accel vector should be about 1.0 in length, gyro around 0.0 and mag can be anything.
- I’d start with the Madgwick filter, not the Mahony. It’s slightly simpler and it’s the one I’ve worked with more often.
- focus on getting the correct information out of the filter. So, I would remove the flip part, make sure you get everything to work, then introduce the flip. You may just need to do some mental gymnastics ;).
- Familiarise yourself with the right hand rule for axis, you should only have 6 permutations to test, not 48.
- what IMU are you using?
Hope that helps.
Thanks for the suggestions!
I have checked the debugging notes and followed the directions listed =]
-
Just did a physical examination of the device to confirm the orientation of the axes are aligned on the board in the the suspected way. It is indeed the case that x and y should be swapped and x negated to bring it in line with the gyro/accel.
-
The issue presents itself with both the Madgwick and Mahony filters. Mahony come out a bit smoother for me, but I have been flipping between the two. The same is true after removing the quaternion flip and rotational adjustment.
*The chips being used are a MMC5603NJ for the Magnetometer and a LSM6DS3TR-C for the gyro/ accelerometer
I did find an error in the way I was calculating degrees per second that has been corrected. as edited above. All this research did remind me that the magnetometer was previously exposed to a pretty strong magnetic field never reset. I think this might be the cause and will be researching resetting the magnetometer. I think this might be the cause and will report back soon!
Of course, the heading was wrong, and the results would drift eventually, especially after some particularly speedy rotations, but overall things were working as expected.
I've checked and the orientation of the magnetometer axes obey the right hand rule. So this sounds reasonable.
The issue presents itself with both the Madgwick and Mahony filters
What is your current issue? Is it the drifting when you quickly rotate? This can be caused by incorrect gyro and/or magnetometer. The algorithms work by trusting the gyro more in the short term, because the magnetometer can be slow to respond. So if the gyro input values are incorrect for the AHRS filter then it will listen to the gyro first, then compensate with the magnetometer later, which causes a drift. Does the result end with the x-axis pointing towards magnetic north? Note, magnetic north may be different than actual north, depending where in the world you are.
One other thing, it's worth calibrating your gyro, accelerometer and magnetometer. See the calibrate_*.js files in my mpu9250 repo and example.js on how to do it.
Really appreciate you following up with this! I spent the day studying the reset sequence for the MMC5603NJ and have managed to solve the issue.
Basically, the behavior that was the problem was that with the Y axis pointed upward, X and Z would reliably point to magnetic north. The magnetometer had already been calibrated, but because the magnetometer had been previously exposed to very strong neodymium magnets previously (more than the device's safe maximum), it was slightly magnetized. So, when the Y axis was horizontal, it was always producing an incorrect number in that direction which would cause the quaternion to wobble and eventually turned around because as far as the magnetometer was concerned, it's heading wasn't changing by much.
After running the reset sequence and adding the quaternion flip and forward adjustment the issue seems to have cleared with the original axis rotations and the new gyro calculations. Thank you very much, again, for following up. Closing this issue now.