user60010: I am currently working on a university project. My aim is to get a rough estimate of the posture of a person's back. For this I have built a system that collects the data from 4 MPU6050 at a central service. From there it will be visualised. My first idea was to draw 4 lines and take the roll angles for the angles between the lines. My problem is that the data from the MPU6050s drifts a lot. I have already created calibration data and use a complementary filter on the nodeMCUs that read the data from the MPUs.
I am using the Adafruit_MPU6050.h lib to connect to the sensors and get data from it. To calculate roll, pitch and yaw I use the following code:
  sensors_event_t gyro_1;
  sensors_event_t accel_1;
  mpu_gyro_1->getEvent(&gyro_1);
  mpu_accel_1->getEvent(&accel_1);

  acc_1_angle_x = (atan(accel_1.acceleration.y / sqrt(pow(accel_1.acceleration.x, 2) + pow(accel_1.acceleration.z, 2))) * 180 / PI) - mpu_1_acc_err_x;
  acc_1_angle_y = (atan(-1 * accel_1.acceleration.x / sqrt(pow(accel_1.acceleration.y, 2) + pow(accel_1.acceleration.z, 2))) * 180 / PI) - mpu_1_acc_err_y;

  previousTime = currentTime;                        
  currentTime = millis();                           
  elapsedTime = (currentTime - previousTime) / 1000;

  gyro_1_angle_x = gyro_1_angle_x + (gyro_1.gyro.x - mpu_1_gyro_err_x) * elapsedTime; // deg/s * s = deg
  gyro_1_angle_y = gyro_1_angle_y + (gyro_1.gyro.y - mpu_1_gyro_err_y) * elapsedTime;
  yaw_1 = yaw_1 + (gyro_1.gyro.z - mpu_1_gyro_err_z) * elapsedTime;
  roll_1 = 0.96 * gyro_1_angle_x + 0.04 * acc_1_angle_x;
  pitch_1 = 0.96 * gyro_1_angle_y + 0.04 * acc_1_angle_y;


How can I make my data more stable?
I tried playing with the 0.96 and 0.04 to change the amount of data I use from the gyro but it only got worse.
If you are interested, this is my project github:
https://github.com/Trojan13/posture-control
Thanks in advance for your input!

user82347: You have the right idea fusing data from both gyro and accelerometer. However, I would strongly advise against doing the fusion yourself since there is a dedicated Digital Motion Processor (DMP) on your MPU-6050 that does this for you. You just need a library that grabs data from the DMP. As far as I know the only Arduino libraries that do this are ones based on Jeff Rowberg's work.
The data outputted by the DMP is in quaternion format. Jeff converts it to yaw pitch and roll but his conversion is flawed (it produces spurious flips and rotations at north and south poles, called gimbal lock). Don't worry, this can be fixed.
If using Jeff's library (or one based on it) you'll need to make sure the detector is level and still before sending a character over serial to trigger the calibration and you need to do this every time you power on. The library and sketches are somewhat overwhelming/intimidating to new users. This too can be fixed.
If you want to
(a) Learn more about this whole topic
(b) Use the correct conversion from quaternion to yaw pitch roll (one that does not produce gimbal lock)
(c) Hide Jeff's library behind a simple and easy to use interface
(d) Perform the calibration only once and have those results saved to persistent memory on your arduino so that you don't have to repeat the calibration every time you power on the device.
Then I would recommend the following tutorial:
https://youtu.be/k5i-vE5rZR0

user7508: Looking at your code, I see the only place where gyro_1_angle_x is
updated is here:
gyro_1_angle_x = gyro_1_angle_x
    + (gyro_1.gyro.x - mpu_1_gyro_err_x) * elapsedTime;

This is adding to gyro_1_angle_x a quantity that does not depend on
gyro_1_angle_x itself. This means that, if there is an uncorrected
systematic error in the measurement of gyro_1.gyro.x (which there
always is, as you can never achieve perfect calibration), the errors
will pile up, leading to an unbounded drift. Then, you estimate the roll
angle as
roll_1 = 0.96 * gyro_1_angle_x + 0.04 * acc_1_angle_x;

The factor 0.96 slightly reduces the amount of drift (by 4%), but does
not change the qualitative behavior: roll_1 is also prone to unbounded
drift.
Suggested approach
My suggestion will be to forget about angles. Using angles to keep track
of your device's attitude is a bad idea: the formulas needed to update
the angles are complex (and thus easy to get wrong), computationally
expensive, and prone to gimbal lock. For tracking the whole
attitude, it would be more reasonable to use rotation matrices or,
better yet, quaternions.
In your use case, however, you do not need to track the whole attitude.
You have no means to reliably track the yaw angle, and you actually do
not even care about this angle. You are only interested in the
orientation of the device relative to the vertical direction. Thus I
suggest you do no even try to track the attitude, and instead just track
the vertical direction. This direction is given by the coordinates of
the gravity vector in the device's reference frame. Then you just have
one vector to track. No angles, no matrices, no quaternions: just a
simple vector.
Using the accelerometers
The accelerometers are sensitive to the gravity, thus you can use their
readings as an estimate of the gravity vector, as in the following
pseudo-code:
at each time step:
    (accel, gyro) = read_MPU();
    gravity = accel;

This may give you the opposite of the gravity (direction of the zenith).
That's fine, as long as you know which vector you are tracking.
There is an issue with this approach though. The accelerometers are not
only sensitive to the gravity, they are also sensitive to the
acceleration (hence the name). If your subject moves, its acceleration
will cause an error in the estimate of the gravity vector. The estimate
is poor in the short term, but it should be good in the long term: as
your subject is unlikely to maintain sustained acceleration, a time
average of the accelerometer readings should closely match the gravity
vector.
Using the gyroscopes
In order to get rid of the acceleration-induced errors, you can instead
use the gyro readings. Assuming you somehow manage to get an initial
estimate of the gravity vector, you can use the gyro readings to
iteratively update this estimate:
at each time step:
    (accel, gyro) = read_MPU();
    dt = time_since_previous_time_step;
    rotation = - dt * gyro;
    gravity = apply_rotation(rotation, gravity);

here rotation is the rotation vector describing how much
the gravity has rotated since the previous time step. The minus sign in
its formula comes from the fact that the gyro gives you the rotation of
the device relative to a still reference frame, whereas you want the
rotation of a still vector relative to the device's reference frame. The
function apply_rotation() would apply Rodrigues' rotation
formula in order to rotate the gravity vector to its new
orientation.
This approach does not suffer from the acceleration induced errors. It
is, however prone to drift, as combining a large number of small
rotations ends up piling up a large number of small errors.
Combining gyroscope and accelerometer data
The accelerometer approach is prone to short-term errors, while the
gyroscope approach is prone to long-term drift. You can try to combine
these two approaches, and use the accelerometers to determine the
long-term behavior of your gravity estimate, while the gyroscopes
determine its short-term behavior. For this you would, at each time
step:

use the gyro data to rotate the current estimate of the gravity vector
then “stir” it gently towards the accelerometer readings.

In pseudo-code, this would look like:
at each time step:
    (accel, gyro) = read_MPU();
    dt = time_since_previous_time_step;
    rotation = - dt * gyro;
    gravity = apply_rotation(rotation, gravity);
    gravity = gravity + (dt/tau) * (accel - gravity)

The last line has the effect of slightly pushing the gravity estimate
towards the accelerometer readings. Note that this line looks like the
implementation of a first-order low-pass filter with time constant
tau. This time constant defines the boundary between the “short” time
scales, where you trust the gyroscopes more than the accelerometers, and
the “long” time scales, where you trust more the accelerometers. The
value of tau has to be chosen by experiment. It controls the trade-off
between the noise coming from the acceleration and the systematic error
coming from the gyros.
With this formula, the accumulated gyro errors will always stay bound,
with a total error proportional to tau. Since the step-wise rotations
are presumably very small, and since the rotation errors do not pile up
in an unbounded way, you can probably get away with using a
linearization of the Rodrigues' rotation formula (cos θ = 1, sin θ = θ):
gravity = gravity + rotation × gravity

and then you need no trigonometry at all.
It is also worth noting that, with this approach, you can safely
initialize gravity to zero: it will immediately start growing as the
program runs, and its magnitude will eventually converge to the
appropriate value.
Getting the pitch and roll
Once you know the gravity vector, you can easily compute the pitch and
roll. I am not saying that you should completely avoid using angles: you
probably want angles on your user interface. What I am saying is that
you should refrain from using angles for the purpose of keeping track of
the device's orientation. Computing the angles as a post-processing step
is fine.
Actually, you may as well compute those angles in the computer that does
the final processing of the data. The ESPs can simply send their best
estimate of the gravity vector, and let the computer do the
trigonometry.

60010: <p>I am currently working on a university project. My aim is to get a rough estimate of the posture of a person's back. For this I have built a system that collects the data from 4 MPU6050 at a central service. From there it will be visualised. My first idea was to draw 4 lines and take the roll angles for the angles between the lines. My problem is that the data from the MPU6050s drifts a lot. I have already created calibration data and use a complementary filter on the nodeMCUs that read the data from the MPUs.</p>
<p>I am using the Adafruit_MPU6050.h lib to connect to the sensors and get data from it. To calculate roll, pitch and yaw I use the following code:</p>
<pre class="lang-cpp prettyprint-override"><code>  sensors_event_t gyro_1;
  sensors_event_t accel_1;
  mpu_gyro_1-&gt;getEvent(&amp;gyro_1);
  mpu_accel_1-&gt;getEvent(&amp;accel_1);

  acc_1_angle_x = (atan(accel_1.acceleration.y / sqrt(pow(accel_1.acceleration.x, 2) + pow(accel_1.acceleration.z, 2))) * 180 / PI) - mpu_1_acc_err_x;
  acc_1_angle_y = (atan(-1 * accel_1.acceleration.x / sqrt(pow(accel_1.acceleration.y, 2) + pow(accel_1.acceleration.z, 2))) * 180 / PI) - mpu_1_acc_err_y;

  previousTime = currentTime;                        
  currentTime = millis();                           
  elapsedTime = (currentTime - previousTime) / 1000;

  gyro_1_angle_x = gyro_1_angle_x + (gyro_1.gyro.x - mpu_1_gyro_err_x) * elapsedTime; // deg/s * s = deg
  gyro_1_angle_y = gyro_1_angle_y + (gyro_1.gyro.y - mpu_1_gyro_err_y) * elapsedTime;
  yaw_1 = yaw_1 + (gyro_1.gyro.z - mpu_1_gyro_err_z) * elapsedTime;
  roll_1 = 0.96 * gyro_1_angle_x + 0.04 * acc_1_angle_x;
  pitch_1 = 0.96 * gyro_1_angle_y + 0.04 * acc_1_angle_y;

</code></pre>
<p>How can I make my data more stable?
I tried playing with the 0.96 and 0.04 to change the amount of data I use from the gyro but it only got worse.</p>
<p>If you are interested, this is my project github:
<a href="https://github.com/Trojan13/posture-control" rel="nofollow noreferrer">https://github.com/Trojan13/posture-control</a></p>
<p>Thanks in advance for your input!</p>
 82347: <p>You have the right idea fusing data from both gyro and accelerometer. However, I would strongly advise against doing the fusion yourself since there is a dedicated Digital Motion Processor (DMP) on your MPU-6050 that does this for you. You just need a library that grabs data from the DMP. As far as I know the only Arduino libraries that do this are ones based on Jeff Rowberg's work.</p>
<p>The data outputted by the DMP is in quaternion format. Jeff converts it to yaw pitch and roll but his conversion is flawed (it produces spurious flips and rotations at north and south poles, called gimbal lock). Don't worry, this can be fixed.</p>
<p>If using Jeff's library (or one based on it) you'll need to make sure the detector is level and still before sending a character over serial to trigger the calibration and you need to do this every time you power on. The library and sketches are somewhat overwhelming/intimidating to new users. This too can be fixed.</p>
<p>If you want to</p>
<p>(a) Learn more about this whole topic</p>
<p>(b) Use the correct conversion from quaternion to yaw pitch roll (one that does not produce gimbal lock)</p>
<p>(c) Hide Jeff's library behind a simple and easy to use interface</p>
<p>(d) Perform the calibration only once and have those results saved to persistent memory on your arduino so that you don't have to repeat the calibration every time you power on the device.</p>
<p>Then I would recommend the following tutorial:</p>
<p><a href="https://youtu.be/k5i-vE5rZR0" rel="nofollow noreferrer">https://youtu.be/k5i-vE5rZR0</a></p>
 7508: <p>Looking at your code, I see the only place where <code>gyro_1_angle_x</code> is
updated is here:</p>
<pre class="lang-cpp prettyprint-override"><code>gyro_1_angle_x = gyro_1_angle_x
    + (gyro_1.gyro.x - mpu_1_gyro_err_x) * elapsedTime;
</code></pre>
<p>This is adding to <code>gyro_1_angle_x</code> a quantity that does not depend on
<code>gyro_1_angle_x</code> itself. This means that, if there is an uncorrected
systematic error in the measurement of <code>gyro_1.gyro.x</code> (which there
always is, as you can never achieve <em>perfect</em> calibration), the errors
will pile up, leading to an unbounded drift. Then, you estimate the roll
angle as</p>
<pre class="lang-cpp prettyprint-override"><code>roll_1 = 0.96 * gyro_1_angle_x + 0.04 * acc_1_angle_x;
</code></pre>
<p>The factor 0.96 slightly reduces the amount of drift (by 4%), but does
not change the qualitative behavior: <code>roll_1</code> is also prone to unbounded
drift.</p>
<h2>Suggested approach</h2>
<p>My suggestion will be to forget about angles. Using angles to keep track
of your device's attitude is a bad idea: the formulas needed to update
the angles are complex (and thus easy to get wrong), computationally
expensive, and prone to <a href="https://en.wikipedia.org/wiki/Gimbal_lock" rel="nofollow noreferrer">gimbal lock</a>. For tracking the whole
attitude, it would be more reasonable to use rotation matrices or,
better yet, quaternions.</p>
<p>In your use case, however, you do not need to track the whole attitude.
You have no means to reliably track the yaw angle, and you actually do
not even care about this angle. You are only interested in the
orientation of the device relative to the vertical direction. Thus I
suggest you do no even try to track the attitude, and instead just track
the vertical direction. This direction is given by the coordinates of
the gravity vector in the device's reference frame. Then you just have
one vector to track. No angles, no matrices, no quaternions: just a
simple vector.</p>
<h3>Using the accelerometers</h3>
<p>The accelerometers are sensitive to the gravity, thus you can use their
readings as an estimate of the gravity vector, as in the following
pseudo-code:</p>
<pre><code>at each time step:
    (accel, gyro) = read_MPU();
    gravity = accel;
</code></pre>
<p>This may give you the opposite of the gravity (direction of the zenith).
That's fine, as long as you know which vector you are tracking.</p>
<p>There is an issue with this approach though. The accelerometers are not
only sensitive to the gravity, they are also sensitive to the
acceleration (hence the name). If your subject moves, its acceleration
will cause an error in the estimate of the gravity vector. The estimate
is poor in the short term, but it should be good in the long term: as
your subject is unlikely to maintain <em>sustained</em> acceleration, a time
average of the accelerometer readings should closely match the gravity
vector.</p>
<h3>Using the gyroscopes</h3>
<p>In order to get rid of the acceleration-induced errors, you can instead
use the gyro readings. Assuming you somehow manage to get an initial
estimate of the gravity vector, you can use the gyro readings to
iteratively update this estimate:</p>
<pre><code>at each time step:
    (accel, gyro) = read_MPU();
    dt = time_since_previous_time_step;
    rotation = - dt * gyro;
    gravity = apply_rotation(rotation, gravity);
</code></pre>
<p>here <code>rotation</code> is the <a href="https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector" rel="nofollow noreferrer">rotation vector</a> describing how much
the gravity has rotated since the previous time step. The minus sign in
its formula comes from the fact that the gyro gives you the rotation of
the device relative to a still reference frame, whereas you want the
rotation of a still vector relative to the device's reference frame. The
function <code>apply_rotation()</code> would apply <a href="https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula" rel="nofollow noreferrer">Rodrigues' rotation
formula</a> in order to rotate the gravity vector to its new
orientation.</p>
<p>This approach does not suffer from the acceleration induced errors. It
is, however prone to drift, as combining a large number of small
rotations ends up piling up a large number of small errors.</p>
<h3>Combining gyroscope and accelerometer data</h3>
<p>The accelerometer approach is prone to short-term errors, while the
gyroscope approach is prone to long-term drift. You can try to combine
these two approaches, and use the accelerometers to determine the
long-term behavior of your gravity estimate, while the gyroscopes
determine its short-term behavior. For this you would, at each time
step:</p>
<ul>
<li>use the gyro data to rotate the current estimate of the gravity vector</li>
<li>then “stir” it gently towards the accelerometer readings.</li>
</ul>
<p>In pseudo-code, this would look like:</p>
<pre><code>at each time step:
    (accel, gyro) = read_MPU();
    dt = time_since_previous_time_step;
    rotation = - dt * gyro;
    gravity = apply_rotation(rotation, gravity);
    gravity = gravity + (dt/tau) * (accel - gravity)
</code></pre>
<p>The last line has the effect of slightly pushing the gravity estimate
towards the accelerometer readings. Note that this line looks like the
implementation of a first-order low-pass filter with time constant
<code>tau</code>. This time constant defines the boundary between the “short” time
scales, where you trust the gyroscopes more than the accelerometers, and
the “long” time scales, where you trust more the accelerometers. The
value of <code>tau</code> has to be chosen by experiment. It controls the trade-off
between the noise coming from the acceleration and the systematic error
coming from the gyros.</p>
<p>With this formula, the accumulated gyro errors will always stay bound,
with a total error proportional to <code>tau</code>. Since the step-wise rotations
are presumably very small, and since the rotation errors do not pile up
in an unbounded way, you can probably get away with using a
linearization of the Rodrigues' rotation formula (cos θ = 1, sin θ = θ):</p>
<pre><code>gravity = gravity + rotation × gravity
</code></pre>
<p>and then you need no trigonometry at all.</p>
<p>It is also worth noting that, with this approach, you can safely
initialize <code>gravity</code> to zero: it will immediately start growing as the
program runs, and its magnitude will eventually converge to the
appropriate value.</p>
<h3>Getting the pitch and roll</h3>
<p>Once you know the gravity vector, you can easily compute the pitch and
roll. I am not saying that you should completely avoid using angles: you
probably want angles on your user interface. What I am saying is that
you should refrain from using angles for the purpose of keeping track of
the device's orientation. Computing the angles as a post-processing step
is fine.</p>
<p>Actually, you may as well compute those angles in the computer that does
the final processing of the data. The ESPs can simply send their best
estimate of the gravity vector, and let the computer do the
trigonometry.</p>
