I'm working on an inverse pendulum project, and am using an adafruit NXP_FXOS8700_FXAS21002 along with an Arduino due, and an esp8266 (two separate projects, same code however). I'm using a Magdwick filter, which works pretty ok, however I've noticed that small angle displacements (less than 0.1 rads) don't seem to show up. Since I'm working on the inverse pendulum, even the smallest of displacements need to be seen and I think my coding and/or filter is likely the culprit, Filtering of course is required, but it can't be too aggressive because it'll add a phase shift and my regulator won't work.

So My question is, is there a better way to filter/acquire my angle displacements/measurements than what I'm currently doing now? I did at one point completely remove the filter and calculate the angle via:

theta = atan2(ay, ax);

However there were angle "spikes" for lack of a better word during quick angle changes that caused the regulator to over compensate and be completely unstable

Here is my truncated code relevant to sensor and the Madgwick filter, however I can include if requested, the entire program.

update As noted in the comments, I am using the the Adafruit implementation of the Madgwick filter. However I am of course completely willing to use another filter, (kalman?) or any other.

On top of that, here is a youtube video of the system hanging in the stable equilibrium, Here the regulator works in the current code implementation, however for small disturbances, the angle doesn't update accurately enough, in this position it doesn't matter, however in the inverse (angle Pi) this is critical and the cause of my initial question.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Madgwick.h>
#define NXP_FXOS8700_FXAS21002
#include <Adafruit_FXAS21002C.h>
#include <Adafruit_FXOS8700.h>
Madgwick filter;
float offsetAngle, offsetOmega;
Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);

// Offsets applied to raw x/y/z mag values
float mag_offsets[3]            = { -1.28F, -39.23F, -1.78F };
// Soft iron error compensation matrix
float mag_softiron_matrix[3][3] = { {  0.940,  0.056,  0.012 },
  {  0.056,  0.932, -0.023 },
  {  0.012, -0.023,  1.146 }
float mag_field_strength        = 42.35F;
// Offsets applied to compensate for gyro zero-drift error for x/y/z
float gyro_zero_offsets[3]      = { 0.0F, 0.0F, 0.0F };

in setup()

  Serial.println(F("Adafruit AHRS Fusion Example")); Serial.println("");
  // Initialize the sensors.
  if (!gyro.begin())
    /* There was a problem detecting the gyro ... check your connections */
    Serial.println("Ooops, no gyro detected ... Check your wiring!");
    while (1);
  if (!accelmag.begin(ACCEL_RANGE_2G))
    Serial.println("Ooops, no FXOS8700 detected ... Check your wiring!");
    while (1);

And in void()

  sensors_event_t gyro_event;
  sensors_event_t accel_event;
  sensors_event_t mag_event;
  accelmag.getEvent(&accel_event, &mag_event);

  float ax = accel_event.acceleration.x;
  float ay = accel_event.acceleration.y;
  float az = accel_event.acceleration.z;

  // Apply mag offset compensation (base values in uTesla)
  float x = mag_event.magnetic.x - mag_offsets[0];
  float y = mag_event.magnetic.y - mag_offsets[1];
  float z = mag_event.magnetic.z - mag_offsets[2];

  // Apply mag soft iron error compensation
  float mx = x * mag_softiron_matrix[0][0] + y * mag_softiron_matrix[0][1] + z * mag_softiron_matrix[0][2];
  float my = x * mag_softiron_matrix[1][0] + y * mag_softiron_matrix[1][1] + z * mag_softiron_matrix[1][2];
  float mz = x * mag_softiron_matrix[2][0] + y * mag_softiron_matrix[2][1] + z * mag_softiron_matrix[2][2];

  // Apply gyro zero-rate error compensation
  float gx = gyro_event.gyro.x + gyro_zero_offsets[0];
  float gy = gyro_event.gyro.y + gyro_zero_offsets[1];
  float gz = gyro_event.gyro.z + gyro_zero_offsets[2];

  // The filter library expects gyro data in degrees/s, but adafruit sensor
  // uses rad/s so we need to convert them first (or adapt the filter lib
  // where they are being converted)
  gx *= 57.2958F;
  gy *= 57.2958F;
  float gzz = gz * 57.2958F;
  // Update the filter
  filter.update(gx, gy, gzz, ax, ay, az, mx, my, mz);
  theta = ((filter.getPitch() * (PI / 180)) - PI / 4);

I greatly appreciate the help!

  • Do you use this: github.com/adafruit/Adafruit_AHRS Please add a link to that in your question. Some sensors have a filter inside to reduce noise. Can you test the raw values from the sensors? All the libraries and the calculations are with float, you gonna lose some accuracy. The Arduino Due can use double precision. There is a weird calculation in invSqrt. Can someone look into that to see if that bit operation on a float is correct? github.com/adafruit/Adafruit_AHRS/blob/master/Madgwick.cpp#L231 Where are the sensors located? Can you show a photo or draw a picture? – Jot Feb 3 at 15:11
  • Ah, yes that was my madgwick implementation, I will add it to the question...I did mention that in the question, I did use raw accel values to calculate the pitch, resulting in a 'spike' for lack of better word before the actual angle is then correctly displayed/calculated. Yes, I will add a photo/video. – morbo Feb 3 at 15:19
  • An accelerometer is sensitive for small changes and vibrations (and is noisy). The gyro drifts. When also a magnetometer is added, the AHRS filter can make a good steady output. The AHRS filter is for objects like a plane. You only need the angle (in one dimension). – Jot Feb 3 at 15:23
  • Ahh, yes, actually that makes sense, my system is a 1 dof system, in reality I only need the angle...the suggestion would be to only filter the resulting calculated angle from atan2? to reduce noise? or filtering the accel ax and ay components then calculating? – morbo Feb 3 at 15:29
  • Is the module powered with 3.3v? Are sda and scl the pink and grey wires? Then please split those wires, the i2c bus has troubles with crosstalk. How long are the wires to the esp8266? If the i2c fails, will that be detected? I think that the main problem is the motor vibrations. The accelerometer is very sensitive for that. The magnetometer might also be influenced by the motor. That adds up to 7 possible problems, and I might have overlooked the real cause. – Jot Feb 4 at 10:08

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.