How to use rotation matrix for drift removal

Hi, I am trying to remove drift from my accelerometer data. I am currently trying to do this by calculating the device’s orientation, then finding the effect of gravity on the three axes, and subtracting that from acceleration readings. I used an ahrsfilter to find a rotation matrix for each time step, but am having difficulty with interpreting the output numbers, and how to use them to find the expected accelerations due to gravity. the rotation matrices look like this:

-0.8504 -0.5261 0.0025

0.5259 -0.8498 0.0359

-0.0167 0.0319 0.9994

Any help is greatly appreciated!

Also, is this the best drift removal option for my purposes? The sensor is attached to a hat, and we are trying to find small differences in the amount of head movement between people looking at images.

Here is my code so far:

clc, clearvars

% acc_still = readtable("accelerometerstill/acc_still.csv");
% gyr_still = readtable("accelerometerstill/gyr_still.csv");

% acc_head = readtable("accelerometerheadmovements/acc_head.csv");
% gyr_head = readtable("accelerometerheadmovements/gyr_head.csv");

% acc_body = readtable("accelerometerbodymovements/acc_body.csv");
% gyr_body = readtable("accelerometerbodymovements/gyr_body.csv");

acc_test = readtable("test_acc.csv");
gyr_test = readtable("test_gyr.csv");
mag_test = readtable("test_mag.csv");

% Find the length of the shortest array
acc_h = height(acc_test);
gyr_h = height(gyr_test);
mag_h = height(mag_test);
min_length = min([acc_h,gyr_h,mag_h]);
    
% Resize the arrays to have the same length as the shortest array
acc_test = acc_test(1:min_length,:);
gyr_test = gyr_test(1:min_length,:);
mag_test = mag_test(1:min_length,:);
% [earliest_value, acc_index, gyr_index, mag_index] = getStartTime(acc_test.("timestamp__0700_"),gyr_test.("timestamp__0700_"),mag_test.("timestamp__0700_"));



time_difs = diff(acc_test.("elapsed_s_")); 
avg_time_dif = mean(time_difs);
% average sampling rate for all data: 0.04s or 25 Hz

FUSE = ahrsfilter('SampleRate',25,'OrientationFormat', 'Rotation matrix');

acc_x = acc_test.("x_axis_g_");
acc_y = acc_test.("y_axis_g_");
acc_z = acc_test.("z_axis_g_");
acc_x = acc_x*9.80665;
acc_y = acc_y*9.80665;
acc_z = acc_z*9.80665;

accelReadings = [acc_x,acc_y,acc_z];


gyr_x = gyr_test.("x_axis_deg_s_");
gyr_y = gyr_test.("y_axis_deg_s_");
gyr_z = gyr_test.("z_axis_deg_s_");
gyr_x = gyr_x*(pi/180);
gyr_y = gyr_y*(pi/180);
gyr_z = gyr_z*(pi/180);

gyroReadings = [gyr_x,gyr_y,gyr_z];

mag_x = mag_test.("x_axis_T_");
mag_y = mag_test.("y_axis_T_");
mag_z = mag_test.("z_axis_T_");
mag_x = mag_x * 1e6;
mag_y = mag_y * 1e6;
mag_z = mag_z * 1e6;

magReadings = [mag_x,mag_y,mag_z];

[orientation,angularVelocity] = FUSE(accelReadings,gyroReadings,magReadings);

Answer 1

[Chaitanya]

Hi @Nathaniel,

To interpret the rotation matrix output from the AHRS filter and find the expected accelerations due to gravity, you can follow these steps:

1. The rotation matrix represents the transformation from the device frame to the world frame. Each column of the rotation matrix corresponds to the unit vector in the world frame along the x, y, and z axes, respectively.
2. To find the expected accelerations due to gravity, you can multiply the rotation matrix by the gravity vector [0; 0; -9.80665]. This will give you the gravity vector in the device frame.
3. Subtract the gravity vector from your accelerometer readings to remove the effect of gravity drift. This will give you the linear accelerations in the device frame.

Here's an example of how you can use the rotation matrix to find the expected accelerations due to gravity and remove the drift from your accelerometer data:

% Assuming you have obtained the rotation matrix from the AHRS filter
rotationMatrix = [-0.8504, -0.5261, 0.0025;
                  0.5259, -0.8498, 0.0359;
                 -0.0167,  0.0319, 0.9994];

% Define the gravity vector in the world frame
gravityVector = [0; 0; -9.80665];

% Convert the gravity vector to the device frame
gravityDeviceFrame = rotationMatrix * gravityVector;

% Assuming you have accelerometer readings in accReadings variable
accReadings = [acc_x, acc_y, acc_z]; % Replace with your accelerometer data

% Subtract the gravity vector from the accelerometer readings
linearAccelerations = accReadings - gravityDeviceFrame';

% The linearAccelerations variable now contains the accelerometer readings with gravity removed

Regarding the best drift removal option for your purposes, removing the effect of gravity as you are currently attempting is a common approach. However, the effectiveness of this method depends on various factors such as the accuracy of the sensor, the stability of the orientation estimation, and the specific requirements of your application.

If you find that the drift removal using the rotation matrix does not provide satisfactory results, you may consider exploring other techniques such as sensor fusion algorithms (e.g., Kalman filters) or machine learning-based approaches for drift compensation. The choice of the best method will depend on the specific characteristics of your sensor and the desired level of accuracy for your application.

Hope this helps!