Sensor Fusion v5 yaw/compass lag

[mtx512]

I ported the v5 sensor fusion library to run on a board with imx6sx M4 processor plus fxos8700cq and fxas21002. When running 9DOF with NED co-ordinates (at 200Hz and 8 samples)  the roll and pitch (thisSV_9DOF_GBY_KALMAN.fPhiPl & thisSV_9DOF_GBY_KALMAN.fThePl) seem to track fine. However when the board is rotated while flat the yaw (thisSV_9DOF_GBY_KALMAN.fPsiPl) and compass value (thisSV_9DOF_GBY_KALMAN.fRhoPl) lag and take about 30-60 seconds to catch up. Is this expected behavior ?

On this board the fxas21002 is located on the top side and near the center, the fxos8700cq is on the under side of the pcb close to one corner. Does the location of the two sensors effect the algorithm?

[michaelestanley]

fRunMagCalibration would be the right place.

BTY, drift in Yaw can occur if you place a device motionless on a tabletop with the Z axis vertically aligned.  The Kalman filter needs motion in each axis in order to correct terms about that axis. I've worked around that problem in the past by implementing the "fusion standby-mode" discussed in section 4.8 of the user guide.  It puts the whole system to sleep when a no motion state is detected, and just outputs the last computed value.

[michaelestanley]

Jas,

In https://community.nxp.com/message/956708 we noted that we are discontinuing support for Version 5.00.  I highly recommend you switch over to 7.xx.  For M4F, you can get that via either FRDM-K22F or FRDM-K64F SDKs.

That aside, yes, placement and orientation of the sensors definitely have major impacts on whether or not the fusion software (either version) works.  You will need a custom HAL (Hardware Abstraction Layer) to make the appropriate adjustments to your code.  This is covered in some detail in the V7.xx user guide, which comes with the SDK.

Regards,

Mike

[mtx512]

Thanks for the reply, I will try v7.xx however before doing that is it possible to say whether a lag of 30-60 seconds for catch up on the yaw/compass values is the expected behaviour given that roll/pitch adapt a lot quicker?

[michaelestanley]

Jas, you almost certainly have your sensors oriented such that you will be lucky to get ANY agreement over time.  It should track quickly once you get your HAL in place.

[mtx512]

Unfortunately I can't change the sensors location as its not my board design. I guessing we can only rely on functions that use input from one of the two sensors , so F_3DOF_G_BASIC, F_3DOF_B_BASIC, F_3DOF_Y_BASIC and F_6DOF_GB_BASIC should all be possible? Although values from the fxos8700cq will be reversed given the component is up side down.

Many thanks for your help.

[michaelestanley]

Jas,

You misunderstand - you can correct for inconsistent sensor orientations by building a custom HAL software interface.  It is relatively painless to do.  But if you don't do it, you'll never get the software to work properly.  Again, see the V7.00 user guide for details.  You can get it by downloading the SDK for either FRDM-K64F or FRDM-K22F and enabling the ISSDK and FreeRTOS options when you do the build.

Mike

[mtx512]

I have managed port the V7.00 code across and got it running. Since the board has no flash and I'm having to initialise the magnetic calibration values to null ( pthisMagCal->iValidMagCal = 0;). What I'm seeing is the geomagnetic field magnitude value doesn't change much once the initial calibration is done. Sometimes the initial calibration seems to be out and it doesn't seem to correct itself at overtime. My code has this fix.  I've tested with Compass app on my android phone which seems to alter the  geomagnetic field magnitude value very quickly as the phone is moved or rotated. I'm not seeing the same behaviour with the fusion library.

[michaelestanley]

Jas,

Ideally, you would like to think that the geomagnetic field magnitude doesn't change at all once calibrated, as the field magnitude should be the same regardless of the orientation of the phone.  In indoors environments, coupled with movement in XYZ, we can and do see variations due to real life magnetic distortions in the field.  If you are not moving the phone in XYZ, but only rotating it, the field magnitude should be roughly constant once calibrated.  As to that initial calibration that doesn't work, what do the X-Y, Y-Z and Z-X projections look like in the Magnetics tab of the Sensor Fusion Toolbox look like?  Can you post a screen dump of that page?

Mike

[mtx512]

Mike,

Unfortunately I'm developing/testing under Linux so haven't had time to port the stream output code to use with Sensor Fusion Toolbox. Anyway I'm dumping out the magnetic calibration values to serial and what I seeing is that iCalInProgress starts at 0 goes 4 and then stays stuck at 7 even after a couple of minutes doesn't move to 10 or recycle. Below is a small dump of MagCal values a couple of minutes after calibration. I was expecting finvW to have more values?

iValidMagCal fB fBSq fFitErrorpc fV[CHX] fV[CHY] fV[CHY] finvW[0][0] finvW[0][1] finvW[0][2] finvW[1][0] finvW[1][1] finvW[1][1] finvW[1][2] finvW[2][0] finvW[2][1] finvW[2][2]
7 53.3 2847.8 5.49 26.8 -30.4 -33.1 1.15 0.0 0.0 0.0 0.916 0.0 0.0 0.0 1.74
7 53.3 2847.8 5.49 26.8 -30.4 -33.1 1.15 0.0 0.0 0.0 0.916 0.0 0.0 0.0 1.74
7 53.3 2847.8 5.49 26.8 -30.4 -33.1 1.15 0.0 0.0 0.0 0.916 0.0 0.0 0.0 1.74
7 53.3 2847.8 5.49 26.8 -30.4 -33.1 1.15 0.0 0.0 0.0 0.916 0.0 0.0 0.0 1.74
7 53.3 2847.8 5.49 26.8 -30.4 -33.1 1.15 0.0 0.0 0.0 0.916 0.0 0.0 0.0 1.74
7 53.3 2847.8 5.49 26.8 -30.4 -33.1 1.15 0.0 0.0 0.0 0.916 0.0 0.0 0.0 1.74
7 53.3 2847.8 5.49 26.8 -30.4 -33.1 1.15 0.0 0.0 0.0 0.916 0.0 0.0 0.0 1.74

[michaelestanley]

3X3 for finvW is correct.  Since you are not getting past the 7 element model, the off-diagonal elements are zero as shown in your listing above.

The routines only progress to the 10 element model if that yields better performance.  If there are no significant soft iron effects, it may not be needed.  I use the sensor fusion toolbox's magnetic projections to help me with those tradeoffs.

You can play with the heuristics in the magnetic.c RunMagCalibration() routine if you want to try to tweak out a bit more.

Regards,

Mike

[mtx512]

Still have some issues with calibration, if I calibrate at my desk the best measurements were a 10 element model (as below):

iValidMagCal fB fBSq fFitErrorpc fV[CHX] fV[CHY] fV[CHY] finvW[0][0] finvW[0][1] finvW[0][2] finvW[1][0] finvW[1][1] finvW[1][1] finvW[1][2] finvW[2][0] finvW[2][1] finvW[2][2] 

10 57.8 3349.4 4.76  25.4 -21.7 -28.7  1.38 0.49 0.31 0.49 0.877 0.37 0.31 0.37 1.102

If I calibrate outdoors, I get another 10 element model (fit error is better 3.32%).

iValidMagCal fB fBSq fFitErrorpc fV[CHX] fV[CHY] fV[CHY] finvW[0][0] finvW[0][1] finvW[0][2] finvW[1][0] finvW[1][1] finvW[1][1] finvW[1][2] finvW[2][0] finvW[2][1] finvW[2][2] 

10 45.0 2027.8 3.32  20.4 -21.3 -21.3  0.991 0.28 0.32 0.28 0.925 0.20 0.32 0.20 1.91 

Now if I bring the board back to my desk with outdoor calibration values the yaw/compass values never locks and the values cycles from 0 to 360 regardless if the board is rotated/moved before rest. Would you expect fB to rise back to 57.8 or stay at 45.0 for the yaw value to recover as it doesn't seem to settle once the board is at rest?

[michaelestanley]

Jas,

Are you trying to do a one-time calibration, or doing continuous calibration as per our reference code?

It it expected that you would get a better calibration outdoors.  Indoor magnetic environments are notoriously distorted, and those effects can only throw off calibration as they violate the algorithm's assumption of soft iron fixed spatially with respect the the sensor.

But if you are doing continuous calibration, the system should self-adjust as you go from one environment to the other.

Regards,

Mike

[mtx512]

This is with continuous calibration (as per the reference code), what I'm seeing is that after the initial calibration there is barely any movement in the calibration values when moving between outdoor and indoor environments. The main symptom is the yaw value continually drifts and never settles even after a few minutes when moving environments. As I mentioned previously the compass app on the Android phone seems to update the magnetic field (geomagnetic field magnitude) values in seconds.

[michaelestanley]

Jas,

Did you get your HAL layer straightened out?  All sensors MUST have their axes mapped so that they have a consistent XYZ orientation after the HAL functions are applied.  This goes back to my notes to you earlier in the thread.  Use one of FRDM-K22F or FRDM-K64F as references for comparison using the out-of-box examples in the SDK.  When your port matches those, you have arrived.

Mike

[mtx512]

Yes I changed the HAL to ensure the XYZ orientation were corrected. There was a typo in the my last post, its only the yaw values that continually drifts when moving environments.

I'm thinking should the heuristics in fRunMagCalibration also check to see if there is a large variation in pthisMagCal->fB to accept a new calibration? Unless this is done somewhere else in the code.