Code Monkey home page Code Monkey logo

Comments (11)

psiphi75 avatar psiphi75 commented on May 29, 2024

It looks like there is no magnetometer (compass). This will stop the angular drift. The reason is that the gyroscope data is very noisy and the values will eventually drift.

Using a magnetometer is the the only way to stop drift entirely. However you can reduce drift by:

  • Calibrating the gyroscope bias (biggest source of drift) and accelerometer, see here and the calibration code and how it's applied. With a well calibrated device it will drift, but much slower. Note that the gyroscope is also temperature sensitive, so it's bias changes with temperature.
  • Play with the beta value for the Madgwick algorithm, or the kp and ki values for the Mahony algorithm. However, by reducing drift you increase latency, or reaction time.

from ahrs.

JohnMcLear avatar JohnMcLear commented on May 29, 2024

After quite a bit of time staring at this code I discovered the drift is not the root cause of the problem. I'm getting pretty much unusable / unreliable values irrespective which means I think I'm piping in incorrect numbers.. I did basic Phi/Theta computation and get semi-usable numbers but I want to use AHRS to get more reliable numbers.. This is my output of a few IMU readings / computation:

{ ax: 0.9912109375,
  ay: 0.1201171875,
  az: 0.1787109375,
  gx: 0.396728515625,
  gy: -0.762939453125,
  gz: -0.152587890625,
  phi: -6.80093969961194,
  theta: -77.7437039898772,
  rawData: { ax: 4061, ay: 492, az: 732, gx: 27, gy: -49, gz: -9 } }
{ angle: 1.9131248874302929,
  x: -0.9782544019855904,
  y: -0.15202728846053132,
  z: -0.1410887258399942 }
{ ax: 0.9921875,
  ay: 0.12158203125,
  az: 0.17578125,
  gx: 0.3662109375,
  gy: -0.91552734375,
  gz: -0.1220703125,
  phi: -6.880070515150009,
  theta: -77.8434152024247,
  rawData: { ax: 4064, ay: 498, az: 721, gx: 25, gy: -60, gz: -8 } }
{ angle: 1.9019002320604812,
  x: -0.9743777948966866,
  y: -0.18268641897718887,
  z: -0.13120055309929682 }

Note that the angles coming from AHRS are nothing like Phi/Theta..

I'm doing:

  madgwick.update(data.gx, data.gy, data.gz, data.ax, data.ay, data.az);
  console.log(madgwick.toVector());

So it makes sense to not get similar values with that so I switch up to

  madgwick.update(data.gx, data.gy, data.gz, data.ax, data.ay, data.az);
  console.log(madgwick.getEulerAngles());

And I get:

{ ax: 0.99169921875,
  ay: 0.12060546875,
  az: 0.1787109375,
  gx: 0.48828125,
  gy: -0.885009765625,
  gz: -0.1220703125,
  phi: -6.825099730316998,
  theta: -77.73450777570605,
  rawData: { ax: 4062, ay: 494, az: 733, gx: 32, gy: -57, gz: -8 } }
{ heading: -3.0117845123070373,
  pitch: -1.17129345147732,
  roll: -1.9609205412222732 }
{ ax: 0.990234375,
  ay: 0.12158203125,
  az: 0.1787109375,
  gx: 0.579833984375,
  gy: -0.6103515625,
  gz: -0.152587890625,
  phi: -6.889597923916833,
  theta: -77.68665587049568,
  rawData: { ax: 4056, ay: 499, az: 733, gx: 38, gy: -39, gz: -10 } }
{ heading: -2.9398821638464008,
  pitch: -1.1969071414667525,
  roll: -1.9943619929444278 }
{ ax: 0.99072265625,
  ay: 0.12255859375,
  az: 0.177734375,
  gx: 0.457763671875,
  gy: -0.67138671875,
  gz: -0.18310546875,
  phi: -6.94229616363923,
  theta: -77.70655383548201,
  rawData: { ax: 4059, ay: 503, az: 729, gx: 30, gy: -43, gz: -12 } }
{ heading: -2.8524759463954314,
  pitch: -1.2210703134796024,
  roll: -2.0496362594401334 }

Note that the heading heading/pitch/roll and roll barely go above 2 degrees..

And with this in mind I feel I'm piping AHRS the wrong values.. I'm passing it the values in the object that isn't the rawData object..

For example..

  ax: 0.99072265625,
  ay: 0.12255859375,
  az: 0.177734375,
  gx: 0.457763671875,
  gy: -0.67138671875,
  gz: -0.18310546875

Any thoughts?

FWIW my revised logic is:

      // Decode the ArrayBuffer into a typed Array based on the data you expect
      var data = new Uint8Array(buffer);

      // (>> 1) = div with two
      var accX = (new Int16Array([data[0] | (data[1] << 8)]))[0] >> 0;
      var accY = (new Int16Array([data[2] | (data[3] << 8)]))[0] >> 0;
      var accZ = (new Int16Array([data[4] | (data[5] << 8)]))[0] >> 0;

      var gyrX = (new Int16Array([data[6] | (data[7] << 8)]))[0] >> 0;
      var gyrY = (new Int16Array([data[8] | (data[9] << 8)]))[0] >> 0;
      var gyrZ = (new Int16Array([data[10] | (data[11] << 8)]))[0] >> 0;

      var rawData = {
        ax: accX,
        ay: accY,
        az: accZ,
        gx: gyrX,
        gy: gyrY,
        gz: gyrZ
      }

      accX = accX >> 1;
      accY = accY >> 1;
      accZ = accZ >> 1;
      gyrX = gyrX >> 1;
      gyrY = gyrY >> 1;
      gyrZ = gyrZ >> 1;

      var div = 16384; // 14 bits = 8192, 15 bits = 16384, 16 bits = 32768
      var ax = (accX / div)*8;
      var ay = (accY / div)*8;
      var az = (accZ / div)*8;
      var gx = (gyrX / div)*500;
      var gy = (gyrY / div)*500;
      var gz = (gyrZ / div)*500;

      // based on: http://www.nxp.com/assets/documents/data/en/application-notes/AN3461.pdf
      // todo: http://robottini.altervista.org/kalman-filter-vs-complementary-filter
      var phi = Math.atan((-ay) / (Math.sign(az) * Math.sqrt(Math.pow(az,2)+Math.pow(ax,2)))) * 180 / Math.PI;
      var theta = Math.atan((-ax) / (Math.sqrt(Math.pow(ay,2) + Math.pow(az,2)))) * 180 / Math.PI;

      var data = {
        ax: ax,
        ay: ay,
        az: az,
        gx: gx,
        gy: gy,
        gz: gz,
        phi: phi,
        theta: theta,
        rawData: rawData
      }

from ahrs.

psiphi75 avatar psiphi75 commented on May 29, 2024

I have to say I don't know what Theta and Phi do. But it looks like your gyro numbers are very large. The algorithm is expecting a value in radians per second, are your values in degrees per second? It's also really important to adjust for bias on the gyroscope, see my previous comment on this. Bias on the gyro is a huge contributor to rotational drift.

from ahrs.

JohnMcLear avatar JohnMcLear commented on May 29, 2024

Interestingly we got Roll and Pitch stable but Heading is wildly erratic and difficult to understand/debug. For now for my use case I might be able to use Roll/Pitch without Heading so I might be able to survive..

FWIW the fix was as you proposed..

  // Convert Degrees per second to Radians per second
  data.gx = Math.radians(data.gx);
  data.gy = Math.radians(data.gy);
  data.gz = Math.radians(data.gz);

Thanks for your help man it's appreciated! Got any donation methods I can use?

from ahrs.

psiphi75 avatar psiphi75 commented on May 29, 2024

What do you consider "wildly erratic". It's expected the heading will drift after a while without a magnetometer in place.

Thanks for the donation offer, but I don't have anything set up.

from ahrs.

JohnMcLear avatar JohnMcLear commented on May 29, 2024

By wildly erratic I mean it does a 180 degree drift within 87 seconds.

Is this amount of drift within this duration common?

heading_drift
Light blue is drift.

Pretty picture that illustrates event included :)

from ahrs.

psiphi75 avatar psiphi75 commented on May 29, 2024

Have you compensated for bias in the gyroscope? This will reduce your error, see my previous comment. However, without a magnetometer (compass) drift is normal, it's always going to occur. You should also be using the Magwick filter, it works better without a compass that the Mahony filter.

Have a look at the test code here, set lines 26 and 27 both to true. Then run node test from the command line. Two tests will be run, both use the same data, run for around 6 seconds and use Magwick. The tests have real sample data from an IMU, the MPU9250. The first test has the magnetometer enabled, the second test has it disabled. You will notice that the test without the compass drifts by an extra 3.5 degrees over the 6 seconds.

from ahrs.

JohnMcLear avatar JohnMcLear commented on May 29, 2024

oh okay so 0.6 degree drift p/s is normal. That's super interesting, I for some reason assumed drift would be in the order of degrees per minute not dps so that's good to know. I will get on with these changes but probably not in client side software but attempt to make the compensation in firmware.

from ahrs.

psiphi75 avatar psiphi75 commented on May 29, 2024

from ahrs.

JohnMcLear avatar JohnMcLear commented on May 29, 2024

We solved this by doing this

sampleInterval: 1000 / sampleIntervalFromFile,

TLDR; our sampleInterval was completely wrong... We were passing 104 as the sampleIntervalFromFile value

from ahrs.

psiphi75 avatar psiphi75 commented on May 29, 2024

Great stuff. Thanks for the update.

from ahrs.

Related Issues (19)

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.