Wrong axis alignment
Opened this issue · 17 comments
Hi!
I just found a bug on your code that make calculation wrong.
in this line:
MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);it must be
MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, -mz);because MPU9250 has magnetic Z axis inverted (arcording to page 38 in datasheet: https://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf )
with the suggested change by @maisonsmd I get meaningful yaw/pitch/roll values.
The sign of mz is STILL wrong in the Sparkfun deposited and the zip file code posted on Sparkfun's tutorial page, and in Kris's Basic_AHRS code as well.
I suppose few people have actually looked to see if the yaw, pitch and roll angles make sense, or if they do, they assume they have made a mistake and give up.
Just swapping X and Y for the magnetometer creates a left handed coordinate system. The yaw, pitch and roll angles are nonsense unless mz is negated, and the magnetometer is properly calibrated.
With the fix I describe, and a properly calibrated magnetometer (I use the Magneto matrix/offset code), the AHRS code works fine. Without the fix, the YPR angles derived from the quaternion are simply nonsense, and have been nonsense for years, evidently.
I suffer from no confusion about NED, ENU, order of applying rotations, etc. and those arguments are completely specious in relation to such major errors as mixing left and right handed coordinate systems.
@jremington How about forking this repository in order to do your own modifications ? Saw your comment and zip files attached on https://forum.arduino.cc/index.php?topic=645527.15
All good but why not forking and sharing your well working solution if you disagree ? Nothing wrong with people disagreeing and debating, but why not formalizing it and showing the way ? Was thinking of doing it myself but I am too new to this AHRS stuff and would need code reviews to make sure things are on the right track.
@jremington would you suggest the newer ICM 20948 from invensense as a replacement?
I didn't even know the mpu 9250 was going to be discontinued. In these conditions it'd be better to move on to something else of course.
Thanks for your help.
Further to this i looked into the 9250 fusion code again and figured out loads of it comes straight from this :
https://github.com/sparkfun/LSM9DS0_Breakout/blob/master/Libraries/Arduino/examples/SparkFun_LSM9DS0_AHRS/SparkFun_LSM9DS0_AHRS.ino
Was adapted from this lib for the LSM9DS0, that does not have the same Z magnetormeter orientation, and updates the quaternion with mz instead of -mz.
Unless im totally wrong I thinks this is where it comes from, now depending on what version of the breakout board (as there are many), the mpu9250 comes from, GY, Sparkfuns, Adafruit, mad in china or mars then the mag z axis, might be in the right direction.
For sure however this would not work on the Sparkfun version and I figured, almost certain won't work on my mpu 92/65 GY either. But now that the issue is identified it's easy to fix it.
Just a shame that people can't talk to each other and just acknowledge the simple fact that : 1) one needs to never blindly trust someone else's software, same in the mountains, as a mountaineer I always double check my mate's navigation choices instead of relying blindly on his judgement. 2 judgements married with a Kalman filter is way better than one !
Also 2) This is where Kris could have done better : now that the issues is known, simply document it in the code, the sketch.ino and the readme. And tell the user to get their sh* together and double check axis orientation themselves, and do a test before assuming it works or not it does not !
On 2020-02-11 15:41, romatou18 wrote: @jremington [1] would you suggest the newer ICM 20948 from invensense as a replacement?
Sorry, I know nothing about it. Pololu and Adafruit keep up to date, and make high quality breakouts for some of the newer sensors, like the LSM9DS1. Their AHRS software remains rather primitive, though.
@jremington
After more research into it, I noted the NXP FXOS8700 Accel + mag combined with the FXAS21002 Gyro. Just like Bosch is doing for the BN)55 NXP provide their own library the NXPMotionSense to both perform the reading of the sensor and the fusion.
I had a quick look to what is an overwhelming amount of math in that lib, but also the sense that the code is very consistent and clean, non withstanding the math the flow of it and comments makes sense. Also the Filtering implemented differs from Mahony or Madgwick and given the constants used, purposefully for these 2 sensors, I guess this high degree of integration between the lib and the sensors can only be beneficial.
I read on the TDK invense website that they provide SDK and code only for OEMs, so not the average Joe, i.e. for their new ICM 20948 that seem to correct some defect of the MP9250.
By the look of it, if Bosch and NXP / Freescale are the only 2 guys that do provide highly integraed libs and fusion algorithm with their sensors, then I guess I will try these 2. It is hard to defeat an army of engineers that work on it full time and are in direct contact with the guys that designed the sensor in the first place.
I used to work in Cadence Design systems (Virtuoso team in Edinburgh), was in the Software R&D and working with the actual chip designers hand in hand, on Schematics, simulation and parasitics. Being in the same office and having a direct and precise answer to questions on the hardware design and the Design Software was hard to beat. Same here, and to be honest I don't understand why the sensor manufacturer don't always provide open source demo code and libs, it's understood they make $$$ not with the IOT tinkers but with the Smartphone and appliance industry. But what would they have to lose in disclosing code that make their hardware more accessible ?
I guess Bosch and NXP got that already. We do the same at Flightcell.com, we give open source code to the adopters of our GPS tracking solution, makes their time to market way shorter and saves us support calls and endless emails if we were (in the past) only giving them a PDF technical manual i.e. the equivalent of "this is my sensor specs, and the sensor register".
Who else than the system designers can design a good library for it ?!
Silly...
The Madgwick filter expects sensor data in NED convention. This depends on which sensor suite one uses as well as which accel axis one chooses as North.
Either NED or ENU, the 3 sensor need to align to each other, X to X, Y to Y, Z to Z. The code I gave was just to align magnetometer's axes to gyroscope and accelerator's.
That worked for me. I agree that anyone having the same issue should fork this repo and make change to their own.
Please post a link to the NPX code; I would be interested. I've used the BNO055 and it does not work very well. The calibration of the sensors is poorly done, and could not work well anyway because only offsets are calculated. Those are updated at mysterious intervals, which results in occasional erroneous jumps in the orientation output. The firmware could be vastly improved, which I would do if it were open source. So I don't agree that the company who wrote the firmware (it was not Bosch) did the best possible job of it. Take a look at RTIMUlib for Arduino. It is a very clean open source implementation of a Kalman filter, and works with the MPU9250 (and others) but is a bit slow on a 16 Mhz cpu. Open source is indeed the best way to go. Together we move forward. Thanks!
…
On 2020-02-12 21:24, romatou18 wrote: > On 2020-02-11 15:41, romatou18 wrote: @jremington [1] [1] would you > suggest the newer ICM 20948 from invensense as a replacement? > Sorry, I know nothing about it. Pololu and Adafruit keep up to date > and make high quality breakouts for some of the newer sensors, like > the LSM9DS1. Their AHRS software remains rather primitive, though. > @jremington [1] > After more research into it, I noted the NXP FXOS8700 Accel + mag > combined with the FXAS21002 Gyro. Just like Bosch is doing for the > BN)55 NXP provide their own library the NXPMotionSense to both > perform the reading of the sensor and the fusion. I had a quick look to what is an overwhelming amount of math in that lib, but also the sense that the code is very consistent and clean, non withstanding the math the flow of it and comments makes sense. Also the Filtering implemented differs from Mahony or Madgwick and given the constants used, purposefully for these 2 sensors, I guess this high degree of integration between the lib and the sensors can only be beneficial. I read on the TDK invense website that they provide SDK and code only for OEMs, so not the average Joe, i.e. for their new ICM 20948 that seem to correct some defect of the MP9250. By the look of it, if Bosch and NXP / Freescale are the only 2 guys that do provide highly integraed libs and fusion algorithm with their sensors, then I guess I will try these 2. It is hard to defeat an army of engineers that work on it full time and are in direct contact with the guys that designed the sensor in the first place. I used to work in Cadence Design systems (Virtuoso team in Edinburgh), was in the Software R&D and working with the actual chip designers hand in hand, on Schematics, simulation and parasitics. Being in the same office and having a direct and precise answer to questions on the hardware design and the Design Software was hard to beat. Same here, and to be honest I don't understand why the sensor manufacturer don't always provide open source demo code and libs, it's understood they make $$$ not with the IOT tinkers but with the Smartphone and appliance industry. But what would they have to lose in disclosing code that make their hardware more accessible ? I guess Bosch and NXP got that already. We do the same at Flightcell.com, we give open source code to the adopters of our GPS tracking solution, makes their time to market way shorter and saves us support calls and endless emails if we were (in the past) only giving them a PDF technical manual i.e. the equivalent of "this is my sensor specs, and the sensor register". Who else than the system designers can design a good library for it ?! Silly... -- You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub [2], or unsubscribe [3]. [ { @.": "http://schema.org", @.": "EmailMessage", "potentialAction": { @.": "ViewAction", "target": "#368?email_source=notifications\u0026email_token=ABKA7LLSA6KGNWFI6BZOIC3RCTKPTA5CNFSM4HMI2LIKYY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOELTOJDA#issuecomment-585557132", "url": "#368?email_source=notifications\u0026email_token=ABKA7LLSA6KGNWFI6BZOIC3RCTKPTA5CNFSM4HMI2LIKYY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOELTOJDA#issuecomment-585557132", "name": "View Issue" }, "description": "View this Issue on GitHub", "publisher": { @.": "Organization", "name": "GitHub", "url": "https://github.com" } } ] Links: ------ [1] https://github.com/jremington [2] #368?email_source=notifications&email_token=ABKA7LLSA6KGNWFI6BZOIC3RCTKPTA5CNFSM4HMI2LIKYY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOELTOJDA#issuecomment-585557132 [3] https://github.com/notifications/unsubscribe-auth/ABKA7LPYODAWHSZBZ4IHW2DRCTKPTANCNFSM4HMI2LIA
The NXPMotionSense lib is demonstrated here + a custom fusion algo by Paul Stoffregen.
https://github.com/PaulStoffregen/NXPMotionSense
I had a further look into the code i downloaded from the SDK from TDK Invense and this is like day and night. Not mentioning that Paul has already demonstrated the use of it, the NXP code is clearly an end user ready to use library and not a full fledged kitchen sink SDK that take 3 days to get started onto and requires their specific dev board, 1) run it 2) write a concise lib out of it 3) test it on another platform like Arduino Esp32 or else... lots and lots of hours.
I won't even try, i'll order a couple of BNo80 see if its better than the 55 then a couple of NXP adafruit board and choose between these and Kris' company solution with the ST chips.
My initial impression of NXP was right, these guys have a very active forum with Mike Stanley one of their IMU and fusion algorithm engineers that's here for support, replies on a regular basis, even request code publications to his management like on this screenshot.
These guys are light years ahead of any other manufacturers when it come to enabling projects and open sourcing production quality code, designs and advices.
https://community.nxp.com/thread/465153
On top of the software and helping people to choose the right algo for their purpose and then debug they also give training videos to get the big picture and solid foundations. I say yes.