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.
