Go Down

Topic: compass heading changes on tilt of device. (Read 1 time) previous topic - next topic

Idahowalker

At this point you may want to take the roll, pitch, yaw and send it through a simple Kalman Filter, like this one #include <SimpleKalmanFilter.h>, which you can get from here https://github.com/denyssene/SimpleKalmanFilter. You'll use deltat for the q: Process Variance, which should be updated before each update.

Miniflyer

I thought the calman filter was just to smooth out jittery results? Right now they are not jittery, they are not responding to my movement and drifting randomly. Tried some other start and end points for the delta t peasurement. All not working....

Idahowalker

#17
Nov 07, 2019, 04:46 pm Last Edit: Nov 07, 2019, 04:47 pm by Idahowalker
I thought the calman filter was just to smooth out jittery results? Right now they are not jittery, they are not responding to my movement and drifting randomly. Tried some other start and end points for the delta t peasurement. All not working....

From the description of the problem you are having, I'd say the issue is on line 2356 of your code.

I put in code that allows me to read the raw values from the IMU on the screen, when I am troubleshooting:
Code: [Select]

void fGetIMU( void *pvParameters )
{
  if ( fInitializeDevice( ) )
  {
    log_i( " SPI device init " );
    if ( fInitializeAG() )
    {
      log_i( "AG init" );
      if ( fDO_AG_ID() )
      {
        log_i( " AG self ID'd " );
        if ( fInitializeM() )
        {
          log_i( "Init Magnetometer" );
          if ( fDO_M_ID() )
          {
            log_i( " M self ID'd: &d", getMAG_ID_OK() );
            fReboot();
            vTaskDelay( 50 );
            if ( fEnableGandA() == false ) // enable gyros and accelerometers
            {
              log_i( " Fail: Enable Gyro and Accelerometer " );
            }
            if ( fEnableM() == false ) // enable gyros and accelerometers
            {
              Serial.println( " Fail: Enable Magnetometer" );
            }
            if ( setupAccelScale( LSM9DS1_ACCELRANGE_8G ) )
            {
              log_i( " Fail: SetupAccelScale" );
            }
            if ( setupGyroScale ( LSM9DS1_GYROSCALE_2000DPS ) )
            {
              Serial.println( " Fail: setupGyroScale" );
            }
            if (setupMagScale( LSM9DS1_MAGGAIN_12GAUSS ) )
            {
              Serial.println( "FailL setupMagScale" );
            }
          } // if ( fDO_M_ID )
          else
          {
            log_i( "fDO_M_ID, fail: %d", returnHighBits() ); //each LSMDS1 may have its own ID
          }
        } // if ( fInitializeM() )
      } // if ( fDO_AG_ID() )
      else
      {
        log_i( "fDO_AG_ID, fail: %d", returnHighBits() );
      }
    } // if ( fInitializeAG() )
  } // if ( fInitializeDevice( ) )

  boolean DidBias = false;
  // boolean DidBias = true; // for testing
  float TimePast = get_ESP32_uS();
  float TimeNow = get_ESP32_uS();
  TickType_t xLastWakeTime = xTaskGetTickCount();
  TickType_t xFrequency = 25;
   for (;;)
  {
    vTaskDelayUntil( &xLastWakeTime, xFrequency );

    // read the sensor
    if ( DidBias )
    {
      TimeNow = get_ESP32_uS();
      deltat = ( TimeNow - TimePast);
      if (  getLSM9DS1_ID_OK() &&  getMAG_ID_OK() ) // then do things
      {
        fReadAccelerometers();
        fReadGyros();
        fReadMagnetometer();
        // fPrintIMU();
        MahonyQuaternionUpdate ( get_aX(), get_aY(), get_aZ(), get_gX(), get_gY(), get_gZ(), get_mX(), get_mY(), get_mZ() );
        yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
        pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
        roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
        pitch = ( (pitch * 180.0f) / PI);
        yaw  *= 180.0f / PI;
        yaw   -= 14.0f; // Declination
        roll = ( (roll * 180.0f) / PI);
        // Serial.print( roll );
        // Serial.print( ", " );
        // Serial.print( pitch );
        // Serial.print( ", " );
        // Serial.print( yaw );
        // Serial.print( ", " );
        // Serial.println( );
        if ( xSemaphoreTake( sema_X, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
        {
          xQueueOverwrite( xQ_X_INFO, (void *) &roll );
          xEventGroupSetBits( eg, evtTweakServoX );
        }
        if ( xSemaphoreTake( sema_Y, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, do not wait
        {
          xQueueOverwrite( xQ_Y_INFO, (void *) &pitch );
          xEventGroupSetBits( eg, evtTweakServoY );
        }
        TimePast = TimeNow;
      } // if ( LSM9DS1_ID_OK && M_ID_OK ) // then do things
      else
      {
        Serial.print ( " LSM9DS1_ID_NOT_OK ");
        Serial.print ( getLSM9DS1_ID_OK() );
        Serial.print ( " or MAG_ID_OK not OK " );
        Serial.println(  getMAG_ID_OK() );
      }
    } // if ( DidBias )
    else
    {
      calibrate(); // accelerometer and gyro
      // log_i( " aXbias = %f, aYbias = %f, aZbias = %f", get_aXbias(), get_aYbias(), get_aZbias() );
      // log_i( " gXbias = %f, gYbias = %f, gZbias = %f", get_gXbias(), get_gYbias(), get_gZbias() );
      fCalculateM_offset( );
      //      Serial.print( "calibration values ");
      //      Serial.print( ", ax ");
      //      Serial.print ( mpu9250.getAXbias(), 6 );
      //      Serial.print( ", ay ");
      //      Serial.print ( mpu9250.getAYbias(), 6 );
      //      Serial.print( ", az ");
      //      Serial.print ( mpu9250.getAZbias(), 6 );
      //      Serial.print( ", gx ");
      //      Serial.print ( mpu9250.getGXbias(), 6 );
      //      Serial.print( ", gy ");
      //      Serial.print ( mpu9250.getGYbias(), 6 );
      //      Serial.print( ", gz ");
      //      Serial.print ( mpu9250.getGZbias(), 6 );
      //      Serial.print( ", mx ");
      //      Serial.print ( mpu9250.getMXbias(), 6 );
      //      Serial.print( ", my ");
      //      Serial.print ( mpu9250.getMYbias(), 6 );
      //      Serial.print( ", mz ");
      //      Serial.print ( mpu9250.getMZbias(), 6 );
      //      Serial.println();
      DidBias = true;
      ////        /* setting the interrupt */
      xEventGroupSetBits( eg, evtLIDAR_ServoAspectChange ); // start the LIDAR scanning
    }
    //    Serial.print( "fGetIMU " );
    //    Serial.print(uxTaskGetStackHighWaterMark( NULL ));
    //    Serial.println();
    //    Serial.flush();
    xLastWakeTime = xTaskGetTickCount();;
  } //  for (;;)
  vTaskDelete( NULL );
} // void fGetIMU( void *pvParameters )
If the input values are not changing the output values will not...

Anyways, consider your descriptions of your issues and how would the descriptions help someone else help you.

Miniflyer

From the description of the problem you are having, I'd say the issue is on line 2356 of your code.

I doubt that.....it is flawless and clean.
But scroll up a bit, you will see that i too have added prints of the raw data, they are just commented out on this status of code....they sense fine it seems.

Anyways, consider your descriptions of your issues and how would the descriptions help someone else help you.

I posted full codes since development starts, i posted problem description, changes made, things i tried and a description of results i am getting. ....

Miniflyer

OK i have taken sample readings after getting noplace with the coding. The error must be in the integration, or in the quaternion code itself.
Convention:

Line 1: Accelleration x;y;z; Gyro x;y;z; Magnetometer x;y;z;
Line 2: q0, q1, q2, q3
Line 3: pitch, roll, yaw.

What i see: Accelx and y show slight readings where they should be near 0. They are not far off though. The Z value is with 9.3 roughly in the ballpark.
Gyros are all near 0, which is correct also
Magnetometer values are also keeping roughly constant


The quaternion dates i cannot interpret, but pitch roll and yaw are nowhere near where they should be with these raw inputs......

Quote
1.087023;0.962518;-9.337862;-0.000272;-0.000341;0.000172;13.573575;-26.966167;19.024675;
0.00;0.86;-0.45;-0.25
25.61;165.40;-45.68

1.091812;0.943364;-9.347440;-0.000804;-0.000341;0.000172;13.573575;-26.966167;19.024675;
-0.00;-0.98;0.19;0.03
3.06;179.41;-8.52

1.082234;0.972095;-9.352229;0.000794;-0.000341;-0.000361;12.125727;-26.604206;19.373752;
0.00;0.69;-0.57;0.44
-37.55;-140.62;-79.82

1.072657;0.957729;-9.361806;-0.000272;0.000192;0.000172;12.125727;-26.604206;19.373752;
-0.00;-0.17;0.91;0.37
7.37;137.01;-148.28

1.082234;0.957729;-9.342652;0.000261;0.000192;0.000704;13.030632;-27.147150;19.897367;
0.00;0.72;-0.59;-0.36
31.27;150.41;-73.54

1.091812;0.972095;-9.357017;0.000261;0.000192;0.000704;13.030632;-27.147150;19.897367;
-0.00;-0.99;0.07;0.08
8.69;179.35;4.58

1.087023;0.962518;-9.342652;-0.000804;0.000724;-0.000361;13.935537;-26.604206;18.675600;
0.00;0.68;-0.20;0.71
-73.69;-87.86;-95.22

MorganS

What time period does that data cover? Is it possible that you actually did roll from 179 degrees to -87 degrees between the last two readings?

The roll seems to be mostly around -180. That would indicate that the Mahoney integration thinks the device is upside-down. Check the sign on all the inputs - if the accelerometers are upside down but the gyro isn't then it will be getting conflicting data and the output will be crazy. Also check the axes agree with what the library expects: gyro "X" might be rotation around the X axis or an XY plane, which would be the Z axis.
"The problem is in the code you didn't post."

Miniflyer

#21
Nov 09, 2019, 09:29 pm Last Edit: Nov 09, 2019, 09:33 pm by Miniflyer
A roll or such of any kind should not occur, all samples are taken with the device sitting still on the desk....this is confirmed by gyro raw data.

I will test gyro orientation shortly.....

I have added time stamps of micros() and deltat:

Quote
1.403074;-0.028732;-10.147145;-3.509010;-0.049212;-0.222653;9.591993;-16.831232;29.496974;
-0.00;-0.85;0.47;-0.23
-22.58;-166.75;-47.26
17799680;9048.00

1.403074;-0.028732;-10.147145;-3.509010;-0.049212;-0.222653;9.591993;-16.831232;29.496974;
0.00;0.85;-0.47;0.22
-22.49;-166.74;-47.51
17814896;9072.00

1.407862;0.196335;-9.644336;-2.654123;-0.031102;-0.176313;9.591993;-16.831232;29.496974;
-0.00;-0.85;0.47;-0.22
-22.34;-166.81;-47.57
17830076;9028.00

1.407862;0.603370;-9.270822;-2.369693;-0.038027;-0.180574;9.591993;-16.831232;29.496974;
0.00;0.85;-0.48;0.22
-22.19;-166.84;-47.80
17845224;9012.00

1.163641;1.178007;-11.167126;-2.436273;0.006183;-0.182705;10.496898;-30.404808;24.435363;
-0.00;-0.85;0.48;-0.23
-22.61;-166.46;-48.26
17860480;9104.00

1.163641;1.178007;-11.167126;-2.436273;0.006183;-0.182705;10.496898;-30.404808;24.435363;
0.00;0.85;-0.48;0.23
-22.57;-166.40;-48.59
17875736;9116.00

0.732663;0.694354;-10.214185;-1.688978;0.196335;0.041537;10.496898;-30.404808;24.435363;
-0.00;-0.85;0.48;-0.23
-22.64;-166.32;-48.72
17890972;9076.00

1.067868;0.579426;-9.424058;-0.353649;0.045065;0.284421;10.496898;-30.404808;24.435363;
0.00;0.85;-0.48;0.23
-22.63;-166.35;-48.65
17906148;9020.00

1.245048;0.933786;-9.347440;-0.082003;0.009378;0.118769;10.496898;-30.404808;24.435363;
-0.00;-0.85;0.48;-0.23
-22.52;-166.36;-48.86
17921284;8972.00

1.245048;0.933786;-9.347440;-0.082003;0.009378;0.118769;10.496898;-30.404808;24.435363;
0.00;0.85;-0.48;0.23
-22.49;-166.30;-49.14
17936420;9000.00

1.264203;0.952941;-9.342652;-0.003172;0.002454;0.076158;10.496898;-30.404808;24.435363;
-0.00;-0.84;0.49;-0.23
-22.43;-166.27;-49.41
17951576;9004.00

1.292935;1.024770;-9.342652;-0.025543;0.005117;0.072430;10.496898;-30.404808;24.435363;
0.00;0.84;-0.49;0.23
-22.39;-166.22;-49.70
17966728;9012.00

1.302512;1.067868;-9.333074;-0.051642;0.006183;0.092670;10.496898;-30.404808;24.435363;
-0.00;-0.84;0.49;-0.23
-22.36;-166.16;-49.99
17981876;8988.00

1.302512;1.067868;-9.333074;-0.051642;0.006183;0.092670;10.496898;-30.404808;24.435363;
0.00;0.84;-0.49;0.23
-22.31;-166.11;-50.28
17997024;9000.00

1.312089;0.699142;-9.342652;-0.031402;0.006183;0.133683;11.401803;-32.757560;24.958978;
-0.00;-0.84;0.49;-0.23
-22.33;-166.03;-50.51
18012148;8976.00

1.288146;0.636890;-9.366595;-0.022880;0.002987;0.064440;11.401803;-32.757560;24.958978;
0.00;0.84;-0.50;0.23
-22.28;-166.00;-50.78
18027256;8972.00

1.278569;0.914632;-9.333074;-0.039391;0.000856;-0.014391;11.401803;-32.757560;24.958978;
-0.00;-0.84;0.50;-0.23
-22.19;-165.97;-51.11
18042448;9020.00

1.297723;0.962518;-9.313920;-0.030337;-0.001274;-0.025043;11.401803;-32.757560;24.958978;
0.00;0.84;-0.50;0.23
-22.13;-165.93;-51.39
18057640;9020.00

1.297723;0.962518;-9.313920;-0.030337;-0.001274;-0.025043;11.401803;-32.757560;24.958978;
-0.00;-0.84;0.50;-0.23
-22.08;-165.88;-51.67
18072832;9016.00

1.307301;1.010405;-9.318708;-0.015423;-0.000209;-0.021315;11.401803;-32.757560;24.958978;
0.00;0.83;-0.50;0.22
-22.02;-165.84;-51.96
18087992;8996.00

1.283357;1.043925;-9.323497;0.009079;-0.000209;-0.031968;11.401803;-32.757560;24.958978;
-0.00;-0.83;0.51;-0.22
-21.96;-165.81;-52.25
18103168;8984.00

1.273780;0.976884;-9.357017;0.008546;-0.000742;-0.063394;11.763765;-32.757560;24.958978;
0.00;0.83;-0.51;0.22
-21.87;-165.79;-52.53
18118356;9020.00

1.273780;0.976884;-9.357017;0.008546;-0.000742;-0.063394;11.763765;-32.757560;24.958978;
-0.00;-0.83;0.51;-0.22
-21.81;-165.75;-52.81
18133544;8996.00

1.302512;0.852379;-9.337862;0.001089;-0.000209;-0.080971;11.763765;-32.757560;24.958978;
0.00;0.83;-0.51;0.22
-21.74;-165.73;-53.07
18148736;9000.00

1.288146;0.756606;-9.318708;0.002155;-0.000209;-0.067122;11.763765;-32.757560;24.958978;
-0.00;-0.83;0.51;-0.22
-21.69;-165.69;-53.33
18163912;8996.00

Taking out the delay has taken out the jumping, but has re-introduced the drift. You can see it especially in the heading. Device is still sitting still

Miniflyer

As expected:

GyroX gives movement around y axis (x moving up and down)
GyroY gives movement around x axis (y moving up down)
GyroZ gives rotation around the Z axis, so turning device flat on the table

What i did notice was the output is in rads/s iso Deg/sec. I will see what unit flows into the quaternion....i believe it should be deg/s

Miniflyer

#23
Nov 09, 2019, 10:31 pm Last Edit: Nov 09, 2019, 10:33 pm by Miniflyer
OK, i converted rad to deg. No noticeable change (foreseeable, as gyro rates were extremely small anyways). Took this screenshot to show the "drifting around". The sensor is sitting still on the table....


MorganS

Good job with the data and charts. Now you need to check if the raw acceleration and gyros are all working the right way. Place it on the desk sideway, upside down and so on to check each accelerometer with gravity.

Then twist it around each axis and check the correct gyro responds in the correct direction.

You can check the magnetometer similar to acceleration.

All of this seems like overkill for a simple one-chip device but you must check each axis works before you can go to the next step.
"The problem is in the code you didn't post."

Idahowalker

#25
Nov 09, 2019, 11:19 pm Last Edit: Nov 09, 2019, 11:28 pm by Idahowalker
Code: [Select]
#include <SimpleKalmanFilter.h

void some_task()
SimpleKalmanFilter X_KF( (float)iX_Posit90, (float)iX_Posit90, float(ServoDelay12mS) / 1000.0f );

 while (1)
  {
    xEventGroupWaitBits (eg, evtTweakServoX, pdTRUE, pdTRUE, portMAX_DELAY) ;
    xQueueReceive ( xQ_X_INFO, &temp, QueueReceiveDelayTime );

 X_KalmanFiltered = X_KF.updateEstimate(temp);

blah
blah
blah


something about The Great Circle and the values moving the center point of The Great Circle, which is what you might be looking for...

The graph showing slam from -180 to + 180 shows axis roll over.

Quite normal looking graph representing the correct output.

Miniflyer

#26
Nov 09, 2019, 11:55 pm Last Edit: Nov 09, 2019, 11:55 pm by Miniflyer
OK, Raw data output seems OK.

Pointing an Axis straight up gives on that axis a positive output of about 9, the other 2 axis each near 0.
Z gives a negative value sitting on the table, and positive when turning the breadboard upside down. This corresponds to the definition given in the quaternions, where the Z axis is pointing downwards.

The Gyro outputs i've put in the following graphic.



@idaho: Yes, the output seems ok......if the sensor was moving. It is not. It is sitting dead on the table and should not drift around the world.
Your kalman filter will smooth jittery movements....it will not catch slow drift, will it? I need a somewhat useable output before thinking about smoothing and filtering it....and im not getting anything thats usable at this time....

Go Up