Trouble removing gravity from calibrated IMU acceleration (dual ICM-20948 + quaternions)

Hi everyone,

I’m working on a project where I collect motion data using two ICM-20948 IMUs connected to an ESP32. I apply a 6-pose accelerometer calibration on the microcontroller and later process the data in MATLAB to obtain linear acceleration in the global frame (gravity-free). However, even after rotating the accelerometer data using the quaternions and subtracting gravity, the results still don’t look correct.

Before assuming my processing is wrong, I want to double-check with the community whether my MATLAB approach for gravity removal is correct — and whether I’m handling the quaternion convention properly.📍 Data Acquisition Pipeline (short summary)

• Hardware: ESP32 + 2× ICM-20948
• I use the ICM20948_WE library
• On the ESP32 I:✅ Read accel & gyro✅ Apply bias + gain from 6-pose calibration✅ Log calibrated accel (in g), calibrated gyro, and quaternions
• No magnetometer used (so yaw may drift, but recordings are 30–120 s only)
• Data is saved as:

q0 q1 q2 q3 ax ay az gx gy gz

🧠 MATLAB Processing (core part of code)

Below is the section where I align the IMU with the global frame and remove gravity. I use the first ~5 s (static) to estimate the gravity direction, align the IMU Z-axis to global Z, rotate accel using the quaternions, and subtract gravity:

%%  CORREÇÃO DA ORIENTAÇÃO INICIAL (PERÍODO PARADO)
fprintf('\n--- Correção da Orientação Inicial (5 s parado) ---\n');

G = 9.80665; % m/s²
Fs_my = 1 / mean(diff(T.t_s)); 
Nrest = round(5 * Fs_my);


% IMU1
a1_rest = [T.a1x(1:Nrest), T.a1y(1:Nrest), T.a1z(1:Nrest)];
g_est1 = mean(a1_rest, 1);
g_est1 = g_est1 / norm(g_est1);  
% direção da gravidade medida


% Quaternion inicial
Q1 = [T.q10, T.q11, T.q12, T.q13];

if mean(abs(Q1(:,1))) < mean(abs(Q1(:,4)))
    Q1 = [Q1(:,4), Q1(:,1:3)]; % tentar garantir [w x y z]
    
end
Q1 = Q1 ./ vecnorm(Q1,2,2);


% Vetor Z global ideal
z_ref = [0 0 1];


% Calcular rotação para alinhar g_est1 -> z_ref
v = cross(g_est1, z_ref);
s = norm(v);

if s ~= 0
    c = dot(g_est1, z_ref);
    v_skew = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];
    R_align = eye(3) + v_skew + v_skew^2 * ((1 - c)/(s^2));
    
else
    R_align = eye(3);
    
end


% Converter aceleração e alinhar
a1 = [T.a1x, T.a1y, T.a1z];
a1_gbl = quatrotate(Q1, a1);
a1_gbl_corr = (R_align * a1_gbl')';


% Remover gravidade
a1_free_gbl = a1_gbl_corr - [0 0 G];

(I apply the same procedure for IMU2 if present.)📎 Example of the issue

When visualizing the acceleration before and after gravity removal, the result still does not look physically correct, despite the 6-pose calibration and quaternion rotation.

Image 1: Before gravity removal

Image 2: After gravity removal — still incorrect)

❓ My Main Questions

1. Is my approach to rotate acceleration into the global frame and remove gravity mathematically correct?(Or am I applying the alignment/rotation in the wrong order?)
2. Should gravity be removed before or after applying R_align?Some posts suggest subtracting gravity in the sensor frame, others in global frame.
3. Quaternion convention:I’m assuming MATLAB’s quatrotate expects [w x y z] (scalar-first).Am I handling the quaternion conversion correctly here, or could this be a mismatch with the ICM-20948 library?
4. Given that I’m not using the magnetometer:For recordings of 30–120 s, is yaw drift likely to significantly affect gravity removal, or should it still work reasonably well?

Any help or guidance would be greatly appreciated — especially if someone spots a conceptual mistake or if there’s a more robust approach to handle gravity removal for this sensor.

Thanks in advance!

[Paul]

Hi Maria,

From an editorial perspective:

When entering code into a question, answer, or comment, please use code formatting to make it easier for everyone to read (I've already done it for this question) and to make it easy to copy/paste it for execution. With the cursor at the point you want to enter code, either hit alt-enter or click on the left-most icon in the Code section of ribbon to open a code format section.

You're more likely to get help if you upload T so that people can run the code on the exact data you're using. Save T (and anything else needed to run the code) in a .mat file and add it to your question using the Paperclip icon in the Insert section of the ribbon.

Finally, please add the plotting commands to your code so we can see exactly how the output is obtained.

From a technical perspective ...

What are the units on the horizontal and vertical axes on those plots?

I'm confused about this idea of "gravity removal". An accelerometer in free fall will measure zero, so if the goal is to find the acceleration of the IMU relative to an inertial reference frame, e.g., to integrate to velocity and position, shouldn't the gravity vector be added to the accelerometer measurements?

[Maria Inês]

Thank you for your feedback and for fixing the code formatting — much appreciated.

To clarify the points you raised:

• Units:

The acceleration shown in the plots is expressed in m/s² (converted from g after reading the data). The horizontal axis represents time in seconds (sample index can also be used).

• Objective of “removing gravity”:

My goal is to obtain the linear acceleration of the IMU in the global (earth) frame, i.e., the acceleration due solely to motion, so it can be analyzed over the paddling stroke and compared with other measurement systems.

Since the accelerometer measures specific force (i.e., acceleration minus gravity), my understanding is that to recover the linear acceleration in the inertial frame, the gravitational component needs to be added back — or equivalently, subtracted after rotation into the global frame:

a_linear = a_measured - g

Please let me know if this interpretation is incorrect for this use case.

• Data upload:

As suggested, I am attaching a .mat file containing the variable T so that the code can be run directly. I am also including the plotting commands below.

1. Estimate the gravity direction from the first ~5 s of static data
2. Rotate accelerometer data into the global frame using quaternions ([w x y z], scalar-first)
3. Remove the gravity vector [0 0 g] to obtain linear acceleration in the global frame

%% ============================================================
%  CORREÇÃO DA ORIENTAÇÃO INICIAL (PERÍODO PARADO)
%  - Usa ~5 s iniciais para estimar o vetor gravidade real
%  - Alinha o eixo Z do IMU com o Z global antes de remover g
% ============================================================

fprintf('\n--- Correção da Orientação Inicial (5 s parado) ---\n');

--- Correção da Orientação Inicial (5 s parado) ---

load('T.mat',"T")
G = 9.80665; % m/s²
Fs_my = 1 / mean(diff(T.t_s)); % frequência amostragem real

% Número de amostras parado (5 s iniciais)
Nrest = round(5 * Fs_my);

% IMU1
a1_rest = [T.a1x(1:Nrest), T.a1y(1:Nrest), T.a1z(1:Nrest)];
g_est1 = mean(a1_rest, 1);
g_est1 = g_est1 / norm(g_est1);  % direção da gravidade medida

% Quat inicial
q1_0 = [T.q10(1), T.q11(1), T.q12(1), T.q13(1)];
if abs(q1_0(1)) < abs(q1_0(4))
    q1_0 = [q1_0(4), q1_0(1:3)];
end
q1_0 = q1_0 / norm(q1_0);

% Vetor Z global ideal
z_ref = [0 0 1];

% Calcular rotação de g_est1 → z_ref
v = cross(g_est1, z_ref);
s = norm(v);
if s ~= 0
    c = dot(g_est1, z_ref);
    v_skew = [  0   -v(3)  v(2);
               v(3)   0   -v(1);
              -v(2)  v(1)   0 ];
    R_align = eye(3) + v_skew + v_skew^2 * ((1 - c)/(s^2));
else
    R_align = eye(3);
end

fprintf('✅ IMU1 alinhado ao referencial global\n');

✅ IMU1 alinhado ao referencial global

% Aplicar rotação aos quaternions IMU1
Q1 = [T.q10, T.q11, T.q12, T.q13];
if mean(abs(Q1(:,1))) < mean(abs(Q1(:,4)))
    Q1 = [Q1(:,4), Q1(:,1:3)];
end
Q1 = Q1 ./ vecnorm(Q1,2,2);

% Converter aceleração e alinhar
a1 = [T.a1x, T.a1y, T.a1z];
a1_gbl = quatrotate(Q1, a1);
a1_gbl_corr = (R_align * a1_gbl')';

% Remover gravidade
a1_free_gbl = a1_gbl_corr - [0 0 G];
my_acc1_free_gbl = a1_free_gbl;

fprintf('✅ Gravidade removida e orientação corrigida (IMU1)\n');

✅ Gravidade removida e orientação corrigida (IMU1)

% --- IMU2 (se existir) ---
hasIMU2 = all(ismember({'a2x','a2y','a2z','q20','q21','q22','q23'}, T.Properties.VariableNames));
if hasIMU2
    a2_rest = [T.a2x(1:Nrest), T.a2y(1:Nrest), T.a2z(1:Nrest)];
    g_est2 = mean(a2_rest, 1);
    g_est2 = g_est2 / norm(g_est2);

    q2_0 = [T.q20(1), T.q21(1), T.q22(1), T.q23(1)];
    if abs(q2_0(1)) < abs(q2_0(4))
        q2_0 = [q2_0(4), q2_0(1:3)];
    end
    q2_0 = q2_0 / norm(q2_0);

    v = cross(g_est2, z_ref);
    s = norm(v);
    if s ~= 0
        c = dot(g_est2, z_ref);
        v_skew = [  0   -v(3)  v(2);
                   v(3)   0   -v(1);
                  -v(2)  v(1)   0 ];
        R_align2 = eye(3) + v_skew + v_skew^2 * ((1 - c)/(s^2));
    else
        R_align2 = eye(3);
    end

    Q2 = [T.q20, T.q21, T.q22, T.q23];
    if mean(abs(Q2(:,1))) < mean(abs(Q2(:,4)))
        Q2 = [Q2(:,4), Q2(:,1:3)];
    end
    Q2 = Q2 ./ vecnorm(Q2,2,2);

    a2 = [T.a2x, T.a2y, T.a2z];
    a2_gbl = quatrotate(Q2, a2);
    a2_gbl_corr = (R_align2 * a2_gbl')';
    a2_free_gbl = a2_gbl_corr - [0 0 G];
    my_acc2_free_gbl = a2_free_gbl;

    fprintf('✅ Gravidade removida e orientação corrigida (IMU2)\n');
end

✅ Gravidade removida e orientação corrigida (IMU2)

fprintf('\n✨ Correção da orientação inicial concluída (5 s parado)\n');

✨ Correção da orientação inicial concluída (5 s parado)

%   VISUALIZATION OF THE PIPELINE
%   1) Raw acceleration (sensor frame)
%   2) Rotated to global frame
%   3) Gravity removed (linear acceleration)
% ============================

figure('Name','Acceleration Processing Pipeline','Color','w');
t = (0:size(a1,1)-1) / Fs_my; % time axis (s)

% 1) Raw acceleration in the sensor frame
subplot(3,1,1); hold on; grid on;
plot(t, a1(:,1), 'r'); plot(t, a1(:,2), 'g'); plot(t, a1(:,3), 'b');
title('1) Raw Acceleration (Sensor Frame)');
ylabel('m/s²'); legend('X','Y','Z');
xlim([0 t(end)]);

% 2) After rotation into the global frame
subplot(3,1,2); hold on; grid on;
plot(t, a1_gbl(:,1), 'r'); plot(t, a1_gbl(:,2), 'g'); plot(t, a1_gbl(:,3), 'b');
title('2) Acceleration in Global Frame (Before Removing Gravity)');
ylabel('m/s²'); legend('X','Y','Z');
xlim([0 t(end)]);

% 3) After gravity removal
subplot(3,1,3); hold on; grid on;
plot(t, a1_free_gbl(:,1), 'r'); plot(t, a1_free_gbl(:,2), 'g'); plot(t, a1_free_gbl(:,3), 'b');
title('3) Acceleration in Global Frame (After Gravity Removal)');
ylabel('m/s²'); xlabel('Time (s)'); legend('X','Y','Z');
xlim([0 t(end)]);

●

Thank you again for your guidance — it is very helpful.

[Paul]

Hi Maria,

I see that the new plots do not show the same results as the old plots. In the new plots, the difference between plots 2 and 3 over the first five seconds clearly show an effect of the operations on the data. In particular, plot 3 shows zero-ish acceleration for the first few seconds (though the X-component still seems to be a bit off). Do you still have concerns about the process?

"Since the accelerometer measures specific force (i.e., acceleration minus gravity), my understanding is that to recover the linear acceleration in the inertial frame, the gravitational component needs to be added back"

I believe that statement is correct, i.e.,

a_measured = a - g (1)

where a is the translational acceleration relative to an inertial frame and all three quantities are vectors. So vector g needs to be added to vector a_measured to construct the acceleration you wish to obtain (as I understand what you're trying to do).

" — or equivalently, subtracted after rotation into the global frame:"

Equation (1) is true (conceptually, not accounting for measurement and misalignment error) regardless of coordinate frame, so I don't see why the gravity vector would ever be subtracted from a_measured, regardless of coordinate frame in which the operations are taking place.

Looking at equation (1) and assuming the IMU is at rest relative to your inertial coordinate frame for the first few seconds, we have

a_measured = a - g = 0 - g. = -g

Looing at the first plot, we have roughly

a_measured in the sensor frame = -g in the sensor frame = [0 ; 0; 9.8] in the sensor frame, therefore,

g in the sensor frame = [0 ; 0; -9.8]

which implies that for those first few seconds, the z-axis of the IMU is pointing (approximately) up. Is that how your experiment is set up?

Having said all of that, I think I now see why you're "removing gravity." Based on this line

% v = cross(g_est1, z_ref);

it looks like you're defining the z-axis of the inertial frame as pointing up because vector g_est1, the raw output of the IMU, should be ideally equal the negative of the gravity vector, which points up.

But in this line

% a1_free_gbl = a1_gbl_corr - [0 0 G];

so that's the same as a1_free_gb1 = a1_gb1_corr + [0, 0, -G], i.e., a = a_measured + g, where the rightmost term is the gravity vector in our inertial frame (with z-axis pointed up and G > 0).

So it looks to me like you're subtracting (removing) the gravity vector that is resolved in a different inertial frame than the inertial frame in which the rest of the processing is defined, though the process yields the (conceptually) correct result (two negatives make a positive, i.e., a = a_measured - (-g) ).

Looking at the actual data

load('T.mat',"T")
G = 9.80665; % m/s²
Fs_my = 1 / mean(diff(T.t_s)); % frequência amostragem real
% Número de amostras parado (5 s iniciais)
Nrest = round(5 * Fs_my);
% IMU1
a1_rest = [T.a1x(1:Nrest), T.a1y(1:Nrest), T.a1z(1:Nrest)];

We see a bit of a transient, particularly in x for the first second or so. Is that expected?

figure,plot((0:Nrest-1)/Fs_my,a1_rest),grid

If we ignore that transient, we get

a1_rest = [T.a1x(200:Nrest), T.a1y(200:Nrest), T.a1z(200:Nrest)];
g_est1 = mean(a1_rest, 1);
g_est1, norm(g_est1)

g_est1 = 1×3

-1.5002 1.1581 9.9774

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

ans = 10.1558

figure
plot((200:Nrest)/Fs_my,a1_rest),grid,yline(g_est1)

Apparently g_est1 reflects other measurement errors (scale factor, bias, etc.) besides the misalignment that you are trying to estimate.

[Maria Inês]

Thank you for the detailed breakdown — this was very helpful.

I now understand the key point you highlighted: even before applying any rotation or gravity compensation, the accelerometer readings during the rest period still contain non-negligible bias and scale errors, which explains why the gravity vector is not 009.810 0 9.81009.81 and why the norm is ~10.16 m/s². That makes sense, and it clarifies why the gravity removal step cannot behave correctly afterwards.

Just to confirm that I’m following your reasoning correctly:

• The issue at this stage is not the gravity-removal code itself, but the quality of the calibrated accelerometer data being fed into it.
• So, to improve the outcome, I should first refine the accelerometer calibration (beyond the simple bias/scale that I currently apply on the device), so that the rest-vector ‖a_rest‖ is as close as possible to 9.81 m/s² and aligned with the expected axis.

Before I move to correcting the calibration, may I ask for your input on one point so that I apply the right fix:

Would you recommend performing a full 6-point accelerometer calibration in MATLAB (solving for the 3×3 scale/misalignment matrix), and then applying those calibrated parameters to the raw data, or would you advise that I bake that calibration into the firmware on the device so that the logged data is already corrected?

My final goal is to obtain reliable short-window linear acceleration (and possibly velocity) during cyclic motion (paddle strokes), not long-term tracking, so I want to ensure that I improve the calibration in the most appropriate way for that context.

Thanks again for your time — your comments have been extremely valuable.

[Paul]

I'm not sure the entire code is correct, but haven't thought about it in too much detail. In particular, I'm not 100% convinced of the process to come up with R_align. My concern there is that v and c are computed from cross and dot products of two vectors that are not resolved in the same frame.

As for the quality of the measurements, from the original post it sounds like you're already doing a 6-point calibration? So I'm not sure what additional 6-point calibration you're considering. Regardless, it sounds like you're seeing that even after the calibration that you're already doing that the IMU accelerometer output is still showing significant error? Keep in mind that IMU errors can change with time/temperature; don't know if that's a concern for you. Disclaimer: I'm not all that familiar with IMU calibration processes, certainly not as much as with the IMU theory of operation.

As for misalignment in general, I think there are at least three sources: 1) each sensor axis can be misaligned relative to the IMU case, presumably the vendor has a spec sheet that controls this, 2) the IMU case can be misaligned relative to the axes of the platform for which the motion is is to be measured, 3) the platform could be misaligned relative to the vertical during those first few seconds when the platform is at rest. Presumably (2) and/or (3) are what you're trying to measure and compensate?

I have no opinion as for whether or not the cal should be programmed into the firmware or applied to the sensor outputs.

[Maria Inês]

To make sure I understand the key takeaway correctly:

• The issue I’m seeing now is not caused by the gravity-removal step itself, but rather because the accelerometer data already contains residual bias/scale/misalignment errors before any alignment or gravity compensation is applied. • Also, the way I computed R_align may involve a frame-mixing issue, since the vectors used in the cross/dot products must be expressed in the same frame.

That makes sense, and I will correct the calibration and alignment order accordingly.

Since my goal at this stage is to ensure a conceptually correct MATLAB pipeline, could I please ask for your view on the following point — this would help me move forward:

👉 From a purely frame-consistent point of view, which of the two approaches below is the most correct for applying the static alignment correction?

A) Compute the small corrective rotation (from rest gravity vector to [0 0 1]) in the global frame and apply it directly to the rotated acceleration data, after converting the raw measurements to the global frame using the quaternions.

B) Convert that corrective rotation into a quaternion and pre-multiply it with the original quaternions, so that all data is rotated with the corrected quaternions from the start (and then subtract [0 0 g] in the global frame).

I’m not asking for code here — just which approach is conceptually more rigorous/clean in terms of reference frames, assuming that the accelerometer readings will be properly calibrated (full 3×3 matrix) before this step.

A short clarification on whether A or B is the preferable direction would already be extremely helpful for me to proceed with a clean implementation in MATLAB.

[Paul]

Upon reflection, I had a misunderstanding regarding the computations of v and c (because I misunderstood the intended meaning of z_ref). But I still have concerns about the process.

For illustration assume three quite large angles that define the misalignment of the IMU relative to the platform frame, and assume the platform is initially at rest with its z-axis aligned with the local vertical.

angles = [20,-10,15]*pi/180;

Define the direction cosine matrix (DCM) from the platform to the IMU

C = angle2dcm(angles(1),angles(2),angles(3),'ZYX');

Define the unit vector in the direction of -g as measured by the IMU

% Vetor Z global ideal
z_ref = [0 0 1];

g_est1 = (C*9.81*z_ref.').';
g_est1 = g_est1 / norm(g_est1)  % direção da gravidade medida

g_est1 = 1×3

0.1736 0.2549 0.9513

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

Now compute R_align

% Calcular rotação de g_est1 → z_ref
v = cross(g_est1, z_ref);
s = norm(v);
if s ~= 0
    c = dot(g_est1, z_ref);
    v_skew = [  0   -v(3)  v(2);
               v(3)   0   -v(1);
              -v(2)  v(1)   0 ];
    R_align = eye(3) + v_skew + v_skew^2 * ((1 - c)/(s^2));
else
    R_align = eye(3);
end

R_align is the DCM from the IMU frame to the inertial frame.

R_align does recover the measured gravity vector

(R_align.'*z_ref.').'

ans = 1×3

0.1736 0.2549 0.9513

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

However, if we extract the angles from R_align', we see that only the second and third match the angles we started with

[angle1,angle2,angle3] = dcm2angle(R_align.','ZYX');
[angle1,angle2,angle3]*180/pi

ans = 1×3

-1.3198 -10.0000 15.0000

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

What are the implications of this mismatch?

Suppose the platorm is accelerating with a specific force of [1,2,3] resolved in the platform frame, but the platform orientation hasn't changed from when it was at rest. The measurement in the sensor frame will then be

a1 = (C*[1,2,3].').'

a1 = 1×3

2.1200 2.1767 2.1835

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

The platform orientation hasn't changed, so the reported quaternion should be [1,0,0,0]

a1_gbl = quatrotate([1,0,0,0], a1);

Now we apply R_align to get back from the IMU frame to the platform frame.

a1_gbl_corr = (R_align * a1_gbl')'

a1_gbl_corr = 1×3

1.6587 1.4996 3.0000

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

The result is in error in the first two elements.

Note that we can recover the pitch and roll angles for this setup more simply as

theta = asind(-g_est1(1))

theta = -10

phi = atan2d(g_est1(2),g_est1(3))

phi = 15.0000

But it appears we can't recover the yaw angle with this setup. I think we'd have to rotate the platform by a known amount relative to the inertial frame to make the yaw angle "observable" through a measurement of the gravity vector.

As for questions (A) and (B), I'm not sure what the difference is between them. Maybe you can clarify with equations or code to illustrate the two methods. In any event, you can always test your process, whatever it may be, by using known inputs to construct simulated sensor measurements, and then see if whatever process you use recovers those known inputs from the simulated sensor measurements.

[Maria Inês]

Thank you again, this has been extremely valuable.

I’ll implement the steps you highlighted:

• improve the accelerometer calibration (including scale + misalignment),

• apply tilt-only gravity removal (rather than treating the rotation as a full DCM), and

• run tests with both controlled movements and synthetic data to verify the pipeline before moving forward.

I don’t want to take more of your time at this stage. I’ll apply these changes, analyse the results, and if it’s okay, I may share a short update later just to confirm that I’m interpreting the outcomes correctly.

Your guidance has genuinely helped me correct the conceptual foundation of this part of the project. Thank you for that.

[Paul]

You're quite welcome. Good luck with your project.