Biased corrected gyro from a MPU9255 and RTIMULib

Hello all, l am using RTIMULib which can be found here if required.
https://github.com/RTIMULib/RTIMULib-Arduino

I am trying unsuccessfully to get Gyro Z biased corrected data into my loop below, you can see l have got the raw gyro info.

void loop()
{
  unsigned long now = millis();
  unsigned long delta;
  int loopCount = 1;

  while (imu->IMURead()) {                                // get the latest data if ready yet
    // this flushes remaining data in case we are falling behind
    if (++loopCount >= 10)
      continue;
    fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
    sampleCount++;
    if ((delta = now - lastRate) >= 1000) {
      Serial.print("Sample rate: "); Serial.print(sampleCount);
      if (imu->IMUGyroBiasValid())
        Serial.println(", gyro bias valid");
      else
        Serial.println(", calculating gyro bias");

      sampleCount = 0;
      lastRate = now;
    }


   
   RTVector3 mag = imu->getCompass();

float mx = mag.x();
float my = mag.y();
float mz = mag.z();
// Serial.print("mpu 9255 Mag Raw Data X,Y,Z                              "); Serial.print( mx);Serial.print( my); Serial.println(mz);


//float   mzAV= m_compassAverage.z();

 RTVector3 m_compassAverage;                             
//RTIMU::calibrateAverageCompass();
//float calmz = m_compassAverage.setZ;  
  Serial.print("mag raw x,y,z                                           "); Serial.print(mx);Serial.print("  "); Serial.print(my); Serial.print("  ");Serial.println(mz);//prints in real time   
    
    
    
    RTVector3 vec = fusion.getFusionPose();
    float MPU9255_heading[3];
    MPU9255_heading[2] = (vec.z() * RTMATH_RAD_TO_DEGREE);
      Serial.print("mpu 9255 heading                                             "); Serial.println( MPU9255_heading[2]);//prints in real time

 RTVector3 gyro = imu->getGyro();

float gx = gyro.x();
float gy = gyro.y();
float gz = gyro.z();

gz *= 57.2958F ; // convert RAD to DPS
Serial.print(" gz                                          "); Serial.println(gz);//prints in real time   
 
    
    float MPU9255_gyro[2];
    MPU9255_gyro[2] = imu->getGyro().z();
    MPU9255_gyro[2] *= 57.2958F ; // convert RAD to DPS
      Serial.print("mpu 9255 gz                                          "); Serial.println(MPU9255_gyro[2]);//prints in real time

    if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
      lastDisplay = now;
      //          RTMath::display("Gyro:", (RTVector3&)imu->getGyro());                // gyro data
      //          RTMath::display("Accel:", (RTVector3&)imu->getAccel());              // accel data
      //          RTMath::display("Mag:", (RTVector3&)imu->getCompass());              // compass data
     // RTMath::displayRollPitchYaw("Pose:", (RTVector3&)fusion.getFusionPose()); // fused output
      Serial.println();
    // Serial.print("mpu 9255 gz                                          "); Serial.println(MPU9255_gyro[2]);//prints  3 hz
   //  Serial.print("mpu 9255 heading                                             "); Serial.println( MPU9255_heading[2]);//prints  3 hz
    }
  }
}

Below is the RTIMU.cpp were l believe the corrected gyro data is ( m_gyroBias.setZ), which l wish to put into above loop. But l have been unsuccessful in getting it into the loop due to my limited coding skills.

 if (!m_gyroBiasValid) {
        RTVector3 deltaAccel = m_previousAccel;
        deltaAccel -= m_accel;   // compute difference
        m_previousAccel = m_accel;

        if ((deltaAccel.squareLength() < RTIMU_FUZZY_ACCEL_ZERO_SQUARED) &&
            (m_gyro.squareLength() < RTIMU_FUZZY_GYRO_ZERO_SQUARED)) {
            // what we are seeing on the gyros should be bias only so learn from this
            m_gyroBias.setX((1.0 - m_gyroAlpha) * m_gyroBias.x() + m_gyroAlpha * m_gyro.x());
            m_gyroBias.setY((1.0 - m_gyroAlpha) * m_gyroBias.y() + m_gyroAlpha * m_gyro.y());
            m_gyroBias.setZ((1.0 - m_gyroAlpha) * m_gyroBias.z() + m_gyroAlpha * m_gyro.z());

Any help would be appreciated.
Regards
Kevin

Your indentation is all over the place. Run the auto-format tool in the Arduino IDE. It's very good. If it appears to scramble your code then the code was wrong, not the tool.

You've given us a snippet of a program. If we wanted to run it to test it out, we can't. The other code which you think is unimportant isn't. Often the problem is in the code you didn't post because you didn't understand it.

It appears you may be using some kind of sensor fusion library. This will estimate the biases itself. It may or may not let you see the current bias estimate. But even if it doesn't, its output is corrected with the bias removed.

Thanks for the response, but an answer would be helpful.
I am aware of auto format and just hadn't used it then, sorry.
The other part of the code wouldn't be required but here it is. I gave the download as all the library would have to be added to get it to compile and it is an example sketch.
I am aware the fusion would use biased corrected gyro data, but l wish to use  rate of turn indication in a sketch.


[code
#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h"
#include "CalLib.h"
#include <EEPROM.h>

RTIMU *imu;                                           // the IMU object
RTFusionRTQF fusion;                                  // the fusion object
RTIMUSettings settings;                               // the settings object

//  DISPLAY_INTERVAL sets the rate at which results are displayed

#define DISPLAY_INTERVAL  300                         // interval between pose displays

//  SERIAL_PORT_SPEED defines the speed to use for the debug serial port

#define  SERIAL_PORT_SPEED  115200

unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;

void setup()
{
  int errcode;

  Serial.begin(SERIAL_PORT_SPEED);
  Wire.begin();
  imu = RTIMU::createIMU(&settings);                        // create the imu object

  Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName());
  if ((errcode = imu->IMUInit()) < 0) {
    Serial.print("Failed to init IMU: "); Serial.println(errcode);
  }

  if (imu->getCalibrationValid())
    Serial.println("Using compass calibration");
  else
    Serial.println("No valid compass calibration data");

  lastDisplay = lastRate = millis();
  sampleCount = 0;

  // Slerp power controls the fusion and can be between 0 and 1
  // 0 means that only gyros are used, 1 means that only accels/compass are used
  // In-between gives the fusion mix.

  fusion.setSlerpPower(0.02);//-----------------------------------------------------------------------------default
  //fusion.setSlerpPower(0.99);
  // use of sensors in the fusion algorithm can be controlled here
  // change any of these to false to disable that sensor

  fusion.setGyroEnable(true);
  fusion.setAccelEnable(true);
  fusion.setCompassEnable(true);
}

void loop()
{
  unsigned long now = millis();
  unsigned long delta;
  int loopCount = 1;

  while (imu->IMURead()) {                                // get the latest data if ready yet
    // this flushes remaining data in case we are falling behind
    if (++loopCount >= 10)
      continue;
    fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
    sampleCount++;
    if ((delta = now - lastRate) >= 1000) {
      Serial.print("Sample rate: "); Serial.print(sampleCount);
      if (imu->IMUGyroBiasValid())
        Serial.println(", gyro bias valid");
      else
        Serial.println(", calculating gyro bias");

      sampleCount = 0;
      lastRate = now;
    }


    RTVector3 mag = imu->getCompass();
    float mx = mag.x();
    float my = mag.y();
    float mz = mag.z();
    // Serial.print("mpu 9255 Mag Raw Data X,Y,Z                              "); Serial.print( mx);Serial.print( my); Serial.println(mz);

    float mx_bias = 0;//bias offsets from Maagneto
    float my_bias = 0;
    float mz_bias = 0;
    float mx_scale_a = 0;//hard/soft iron offsets from Magneto
    float mx_scale_b = 0;
    float mx_scale_c = 0;
    float my_scale_a = 0;
    float my_scale_b = 0;
    float my_scale_c = 0;
    float mz_scale_a = 0;
    float mz_scale_b = 0;
    float mz_scale_c = 0;

    // Mag correction via  Magneto software
    float mx_off = mx - mx_bias;//Bias correct raw data
    float my_off = my - my_bias;
    float mz_off = mz - mz_bias;

    float MX_cal =  mx_scale_a * mx_off + mx_scale_b * my_off + mx_scale_c * mz_off;//X axis corrected
    float MY_cal =  my_scale_a * mx_off + my_scale_b * my_off + my_scale_c * mz_off;//Y axis corrected
    float MZ_cal =  mz_scale_a * mx_off + mz_scale_b * my_off + mz_scale_c * mz_off;//Z axis corrected

    // Serial.print("mpu 9255 Mag corrected Data MX,MY,MZ                              "); Serial.print( MX_cal); Serial.print("  ");Serial.print( MY_cal);Serial.print("  "); Serial.println(MZ_cal);


    RTVector3 vec = fusion.getFusionPose();
    float MPU9255_heading[3];
    MPU9255_heading[2] = (vec.z() * RTMATH_RAD_TO_DEGREE);
    Serial.print("mpu 9255 heading                                             "); Serial.println( MPU9255_heading[2]);//prints in real time

 //Raw gyro data   

RTVector3 gyro = imu->getGyro();

    float gx = gyro.x();
    float gy = gyro.y();
    float gz = gyro.z();

    gz *= 57.2958F ; // convert RAD to DPS
    Serial.print(" gz                                          "); Serial.println(gz);//prints in real time


    float MPU9255_gyro[2];
    MPU9255_gyro[2] = imu->getGyro().z();
    MPU9255_gyro[2] *= 57.2958F ; // convert RAD to DPS
    Serial.print("mpu 9255 gz                                          "); Serial.println(MPU9255_gyro[2]);//prints in real time

    if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
      lastDisplay = now;
      //          RTMath::display("Gyro:", (RTVector3&)imu->getGyro());                // gyro data
      //          RTMath::display("Accel:", (RTVector3&)imu->getAccel());              // accel data
      //          RTMath::display("Mag:", (RTVector3&)imu->getCompass());              // compass data
      // RTMath::displayRollPitchYaw("Pose:", (RTVector3&)fusion.getFusionPose()); // fused output
      Serial.println();
      // Serial.print("mpu 9255 gz                                          "); Serial.println(MPU9255_gyro[2]);//prints  3 hz
      //  Serial.print("mpu 9255 heading                                             "); Serial.println( MPU9255_heading[2]);//prints  3 hz
    }
  }
}
]

Of course there is more library’s required.
Regards Kevin

l wish to use rate of turn indication

The library returns the offset corrected rate of rotation about each axis. If this is not what you want, please explain.

Hi Jim, l was hoping you might answer as you have a great knowledge in IMU’s and hopefully RTIMULib as l know you are a convert of it.
Yes l am after the offset corrected gyro data, is that not what the excerpt of the rtimu.cpp is?
While you are looking, you will also see l have added some code in the loop to implement magneto, mag correction.
Again l’m unable to reinsert this into the library so the corrected mag could be used for fusion, have you tried to do this?, or could you show how to do this.
I imagine it would need to be called in setup, then updated back into the library.
Any help appreciated.
Regards
Duckman
Kevin

Yes l am after the offset corrected gyro data

That is exactly what is returned by getGyro() function call.

At least for the sensor I use (LSM303DLHC) the magnetometer corrections are built in to the routine that processes the data. However, the full 3x3 matrix multiplication required to correct off-axis errors requires user input is not implemented.

In the newer version of the library, RTIMUlib2, it is implemented but I have not figured out how to get it to compile with Arduino.

However, if you can, this is the section in routine RTIMU.cpp that you want to activate, by entering the ellipsoid correction matrix data elsewhere and setting m_compassCalEllipsoidValid to true.

        if (m_settings->m_compassCalEllipsoidValid) {
            RTVector3 ev = m_imuData.compass;
            ev -= m_settings->m_compassCalEllipsoidOffset;

            m_imuData.compass.setX(ev.x() * m_settings->m_compassCalEllipsoidCorr[0][0] +
                ev.y() * m_settings->m_compassCalEllipsoidCorr[0][1] +
                ev.z() * m_settings->m_compassCalEllipsoidCorr[0][2]);

            m_imuData.compass.setY(ev.x() * m_settings->m_compassCalEllipsoidCorr[1][0] +
                ev.y() * m_settings->m_compassCalEllipsoidCorr[1][1] +
                ev.z() * m_settings->m_compassCalEllipsoidCorr[1][2]);

            m_imuData.compass.setZ(ev.x() * m_settings->m_compassCalEllipsoidCorr[2][0] +
                ev.y() * m_settings->m_compassCalEllipsoidCorr[2][1] +
                ev.z() * m_settings->m_compassCalEllipsoidCorr[2][2]);
        }

Hi Jim, thanks for that, l thought “get gyros” was raw data not corrected so that fixes that issue.
I’m using RTIMULib Arduino and it doesn’t have the ellipsoid correction in the .cpp, but l will play with putting it in here, removing offset and scale and see what happens.

void RTIMU::calibrateAverageCompass()
{
    //  calibrate if required

    if (!m_calibrationMode && m_calibrationValid) {
        m_compass.setX((m_compass.x() - m_compassCalOffset[0]) * m_compassCalScale[0]);
        m_compass.setY((m_compass.y() - m_compassCalOffset[1]) * m_compassCalScale[1]);
        m_compass.setZ((m_compass.z() - m_compassCalOffset[2]) * m_compassCalScale[2]);
    }

Regards Duckman

Well in the end l may not be as poor at coding as l thought and have worked it out myself.
Attached is RTIMu.cpp with changes to enable the full monty (soft/hard iron correction) calibration instead of min/max Magnetometer.

If using, you will of course require your own IMU correction data.
All IMU’s supported by RTIMULib Arduino can be corrected via this .cpp change.

Still to test how much it improves the yaw tilt error, but in testing on my boat, it looks like it is less than the ± 5 degrees which the Chinese MPU9255 showed.
Regards
Kevin

RTIMU.cpp (11.6 KB)

The code you posted does not contain the full 3x3 matrix magnetometer correction. Perhaps you posted the wrong file.

Jim, yes wrong 1, attached corrected .cpp, but l may have been a bit quick to say it is working correctly.
Heading not working as hoped, maybe it’s my Magneto implementation or the correction data, hopefully you can see the prob.
The heading looked good before because it was running on the default .cpp

RTIMU.cpp (12.8 KB)

Beware that RTIMUlib in some cases changes the sensor axes (changes signs and/or swaps them).

So, if you calibrated the IMU using the raw data of the magnetometer, that calibration matrix has to be swapped around to match whatever RTIMUlib does.

Or, apply the calibration matrix BEFORE any data manipulations are undertaken by either the sensor-specific code or RTIMU.cpp

Jim, l thought l had put the corrections prior to manipulation.

In some cases there are axis modifications in the specific sensor driver, which precede the code you added.

For example, the following kind of crap, which I have yet to understand, taken from either the MPU9250 and the GD20/LSM303DLHC drivers.

In particular the sign of X acceleration is flipped, which makes the acclerometer coordinate system left handed! This is wrong, but it works, so I assume that the mistake gets corrected by a sign swap elsewhere.

    //  sort out gyro axes

    m_gyro.setY(-m_gyro.y());
    m_gyro.setZ(-m_gyro.z());

    //  sort out accel data;

    m_accel.setX(-m_accel.x());

Thanks Jim, l did see that sign change on the gyros, but l'm not changing that.
Re scatter plots, which type of software (free hopefully) do you use, then l can maybe see something in that, as excel is just stupid to use, with it axis scale changes.

Scilab will do 3D scatter plots and it is free.

My recommendation is to have RTIMU.cpp output the final, modified sensor data, and plot those to see if all the axis manipulations and offset/scale operations are correct and make sense.

In my case they don't completely, because of the X-axis issue mentioned above.

Thanks for the info, so l take it you have previously tried to do the full monty calibration without success.
Regards
Kevin

I haven’t implemented the off axis correction in RTIMUlib, but probably will for a future project.

For my application using RTIMUlib the built in min/max correction worked very well, as the off axis correction was small for that particular sensor in that environment.

I did implement the off axis correction for a simple tilt compensated compass starting with the Pololu code, described here: Correcting the Balboa magnetometer - Robots - Pololu Forum.

In that case it made the difference between “usable” and “not usable”.

Jim, l also did the full monty on the Pololu V3 which only lowered the yaw, max tilt induced error from 13 degrees to 11, so it's most probable not worth the effort.
In my case with the V3, you had said it's a very poor performing Mag.
https://forum.pololu.com/t/how-to-use-cal-data-from-magcal-magneto-into-ahrs/8685
Again thanks for your advise.
PS, Scilab is another big learning curve for me, Excel is a easy scatter plotter, albeit not very good.
Kevin 1961

Went back and reconfirmed correction data via rerunning Magneto on corrected data to see if the bias was zeroed and matrix was 0's and 1's.
For some reason my intial correction bias was wrong. Re did the correction data and put into .cpp
I can confirm my implementation of Magneto, full monty correction works and l have a stable yaw with limited tilt error (on a boat so can only do a rough tilt test as boat is moving a bit).
Still to do a complete test and will post results with a comparison of min/max cal V full monty.

As promised, attached 2010 excel spreadsheet.
sheet 1, Full monty calibration nearly halved compass rose linearity error from max 10 degrees to 6.
Tilt error was also slightly improved, so the hard work has been done so you would be mad not to use it.
Sheet 2, shows data taken over 5 minutes with IMU static on 0 degree heading.
Yaw approx <±0.5 degree drift from 0 . Gyro Z axis drift <0.1 dps drift from 0
Regards Kevin1961

MPU9255 min-max, full monty calibration results.zip (447 KB)