Magnetometer calibration and gyroscope offset estimation issues

[woutervos]

Dear reader,

I have created a custom designed board with magnetometer, accelerometer and gyroscope and an Cortex M4 MCU, but without UART. On the MCU I am running the NXP Fusion Engine 7.10 using the 9DOF Kalman filter and a NED reference system. The code is running. The gyroscope offset seems to be calculated. Magnetometer values are stored in the magnetic calibration buffer (when moved around) and the magnetic calibration runs when requested. However:

A) For the Y-axis of the board the gyroscope offset seems to be underestimated and cause a drift. The estimation for the other axes seems just fine;

B) The performance of the system after magnetometer calibration is worse than before. The estimation of the offset and scaling does not seem to be correct. I have attached two measurements doing a rotation (NWSEW) in the XZ plane, when the Y-axis of the board is pointing up. Before the calibration the bias is minor the rotation is circular. However, after calibration the bias is way off and the measurements show an ellipse.

Could you please point me in the direction of the cause?

I checked the following:

1) Sensor signal conditioning concerning the units: accelerometer values in G; gyroscope values in dps; and magnetometer values in uT;

2) Sensor signal conditioning concerning conversion of chip reference system to board reference system (NED): when a board axis is pointing down, then the accelerometer value is positive; when a board axis is pointing north or down, then the magnetometer value is positive; when a right handed rule positive rotation is made around a board axis, then the gyroscope value is positive;

3) sensor values are stored in a FIFO buffer of size 1 in the .i<sensorType>sFIFO[][3] variable. The average over the buffer seems to be calculated correctly and the result gets stored in the .i<sensorType>s, and .f<sensorType>s variables. The calibrated version are stored in the  .i<sensorType>c, and .f<sensorType>c variables (except for the gyroscope sensor of course); 

4) The counter of the magnetometer calibration data buffer (iMagBufferCount) equals 360 measurements and data seems to be present before starting the calibration (setting MagCal.iInitiateMagCal=true). It seems to perform 7-point calibration.

4) calibration is performed by making figure 8 movements in an environment with low magnetic interference.

5) I have defined SIMULATION, so no calibration data is stored in or retrieved from flash and at start up the magnetometer buffer and the calibration algorithm seem to be correctly initialised. The loopcounter is set to zero. <-- it seems to me that there may be a potential problem if (int16_t) loopcounter runs over at the end of positive values; newer magnetometer measurement will have lower loopcounter values as indices. Possibly lower than the initial value of -1.

Regards,

Wouter

[woutervos]

Dear Mike,

Sorry for the late reply. Recently I watched the behaviour of the gyro offsets during movement and stationary conditions. The gyro offset for a given axis seems to converge the quickest (from zero) when the device is stationary and the axis is not pointing downwards/upwards. During movement the convergence is slower. Can you confirm this?

[michaelestanley]

Wouter,

No I can't.  The Kalman filter is optimized for a moving board.  There's no new data to help convergence when it is stationary.

Mike

[woutervos]

Dear Mike,

I do not know the exact implementation of the Kalman filter, but I can imagine that noise in the measurement vector may provide the Kalman filter with information. Otherwise, I cannot explain my observations.

[michaelestanley]

Wouter,

Just to be sure we are using the same terms, what do you mean by convergence?  with respect to expected dynamic or static conditions?  Most people I talk to are using referring to the latter, as they don't have good systems implemented for measuring dynamic "truth".

One idea I can mention is that our filter does not model all possible variables associated with motion.  Think "number of states" for the Kalman filter.  If there are errors associated with unmodelled variables, those will be lumped into those that ARE modeled.  Those can accumulate during motion, but gradually die out when the device goes stationary.

Mike

[woutervos]

Mike,

When I talk of convergence, I mean convergence in static conditions; the long time average of the gyroscope values in static conditions.

[woutervos]

Dear reader,

I resolved issue B). Resetting the calibration settings in MagCalibration struct and not the measurements in the MagBuffer struct made it possible to manually start the magnetometer calibration. The compass heading is now stable.

However, issue A) is still present, but does not seem to have a big effect on the compass heading. There is a difference between the actual bias and the estimated bias of approximately 800 mdps for the y-axis, which seems rather large too me and certainly larger than for the x- and z-axis. What could be the cause of this?

Regards,

Wouter

[woutervos]

Dear reader,

Considering issue A). It seems that the gyro offset estimation for the axis that is facing up- or downward is not updated when the system is stationary. Is this intentional?

Regards,

Wouter

[michaelestanley]

I question whether or not you have the correct sensor alignments, given that for a standard NED build, you should get +1 g in the sensors accelerometer reading for the axis pointing UP.  You don't mention which sensors you are using, I assume non-NXP since it sounds like you've done your own driver.  Note that the adjustments for frame of reference are done in the HAL functions, not the drivers.  Your life will be easier if your drivers have the same behavior as the NXP drivers.  I suggest getting one of our standard boards and doing a reference build for comparison.

As to gyro offsets, if your board is rotating about the vertical axis, with one of the board axes aligned to same, you won't be able to cancel the offset in that axis.  I don't know if that is your problem, but if the motion matches my description, you've come up against a fundamental limitation.  You need motion in all 3 dimensions to be able to properly compute all 3 offsets.

I hope that helps.

Mike

[woutervos]

Hello Mike,

Concerning the accelerometer alignment: From my sensor driver I do get +1 g when one of the sensor axis is pointing up. In the HAL, I negate all accelerometer axes, because the NED reference system requires gravity positive values, not acceleration positive values. If I do that, than the fusion engine outputs the correct compass angle (clockwise positive around the down axis)

Concerning the gyro offsets: I estimate and store the offsets in the flash during a special calibration procedure, so it will have a rough estimate of the offsets at startup. Currently I keep the system stationary for the estimation, but reading your comment I recon it should be dynamic. For your information, when I do keep it stationary, it will not update the vertical axis and it will update the other two axis.

Regards,

Wouter

[michaelestanley]

The Kalman filter computes gyro offsets, and requires that the device be moving in order for there to be new data points to compute the offsets.