Incorrect estimation of simulated vehicle attitude from generated inertial sensor data

Hello,

I am using the example " Simulate inertial sensor readings from a driving scenario " by Mathworks, (which can be found at the following link) to generate IMU readings and groundtruth data. In particular, the groundtruth data is being extracted using the following code.

% IMU readings.
accel = [];
gyro = [];
% Wheel encoder readings.
ticks = [];
% GPS readings.
lla = [];
gpsVel = [];
% Define the rate of the GPS compared to the simulation rate.
simSamplesPerGPS = (1/s.SampleTime)/gps.SampleRate;
idx = 0;
posTruth = [] ;
velocityTruth = [];
orientationTruth = [];
angVelTruth = [];
accelTruth = [ ];
while advance(s)
    groundTruth = state(v);
    posTruth(end+1, :) = groundTruth.Position;
    velocityTruth(end+1, :) = groundTruth.Velocity ;
    orientationTruth(end+1, :)  = groundTruth.Orientation;
    angVelTruth(end+1, :)  = groundTruth.AngularVelocity;
    accelTruth(end+1, :)  = groundTruth.Acceleration;
    % Unpack the ground truth struct by converting the orientations from
    % Euler angles to quaternions and converting angular velocities form
    % degrees per second to radians per second.
    posVeh = groundTruth.Position;
    orientVeh = quaternion(fliplr(groundTruth.Orientation), 'eulerd', 'ZYX', 'frame');   % Whichever the IMU orientation is, we have to tell him that the vehicle uses roll pitch yaw convention
    velVeh = groundTruth.Velocity;
    accVeh = groundTruth.Acceleration;
    angvelVeh = deg2rad(groundTruth.AngularVelocity);
    
    % Convert motion quantities from vehicle frame to IMU frame.
    [posIMU,orientIMU,velIMU,accIMU,angvelIMU] = transformMotion( ...
        mountingLocationIMU,orientVeh2IMU, ...
        posVeh,orientVeh,velVeh,accVeh,angvelVeh);
    [accel(end+1,:), gyro(end+1,:)] = imu(accIMU,angvelIMU,orientIMU); 
    
    ticks(end+1,:) = encoder(velVeh, angvelVeh, orientVeh); 
    
    % Only generate a new GPS sample when the simulation has advanced
    % enough.
    if (mod(idx, simSamplesPerGPS) == 0)
        % Convert motion quantities from vehicle frame to GPS frame.
        [posGPS,orientGPS,velGPS,accGPS,angvelGPS] = transformMotion(...
            mountingLocationGPS, orientVeh2GPS,...
            posVeh,orientVeh,velVeh,accVeh,angvelVeh);
        [lla(end+1,:), gpsVel(end+1,:)] = gps(posGPS,velGPS);
    end
    idx = idx + 1;
end

The arrays whose names end in "truth" are being populated via the groundTruth function.

My goal is to estimate the attitude of the vehicle from the gyro data, and in order to do that i am referring to the equations in the book " Principles of GNSS, Inertial and Multisensor navigation " by Paul D. Groves, chapter 5, which i report here :

.

Here the subscripts b represents the body frame (which would be the vehicle frame in my case) and the superscript i represents the ECI frame.

The code that realizes the integration truncates the exp at the first order term, and then the angles are computed using atan2.

Where the indexes - and + indicate the current attitude at the beginning and the end of the update cycle, respectively. The math seems to check out. The code that realizes this is the following:

%% imuclean_bframe is [ax ay az wx wy wz]' 


accels_bframe = imuclean_bframe([1:3]);
angrates_bframe = imuclean_bframe([4:6]);

%%% Attitude estimation
Wbx = [0 , -angrates_bframe(3) ,angrates_bframe(2); angrates_bframe(3) ,0 ,angrates_bframe(1); -angrates_bframe(2) ,angrates_bframe(1) ,0];
C_plus = C_min*(eye(3) + dt*Wbx);
% % 
if norm(eye(3)  -  C_plus*C_plus', 'fro')>tol
    [U,S,V] = svd(C_plus);
    C_plus = U*V';
end

yaw_plus=   atan2(C_plus(2,1),C_plus(1,1));
if ge(abs(yaw_plus), pi)
    yaw_plus = -yaw_plus;
end
roll_plus =  atan2(C_plus(3,2),C_plus(3,3));
if ge(abs(roll_plus), pi)
    roll_plus = -roll_plus;
end
pitch_plus=atan2(-C_plus(3,1), sqrt(C_plus(3,2)^2+C_plus(3,3)^2));

if ge(abs(pitch_plus), 0.5*pi)
    pitch_plus = -pitch_plus;
end

However, whenever the simulation is fed a trajectory which varies in altitude, the following happens:

The roll and pitch estimates are massively off. I suspect that the issue might be related to this line of code.

orientVeh2IMU = quaternion(mountingAnglesIMU,'eulerd','ZYX','frame');

Here it seems that the IMU would follow a ZYX angle convention, as opposite to the vehicle following the Roll, Pitch, Yaw convention (thus making it XYZ ). However, modifying the angle rotation order has no effect on the estimation.

TLDR I wish to obtain a attitude estimation, but the data is being generated in an unexpected way.

Any contribution will be greatly appreciated

Best regards

Giorgio