Use MPU9250 to record position and trajectory

Hi,

I am trying to use MPU readings from accelerometer and gyroscope to get pose estimation of the sensor and then display the trajectory of my sensor, but up to now I can't make it. What I did was to record data from the sensor, calculate the orientation using the FUSE function of Matlab to rotate the acceleration into the World Reference Frame. Then I subtracted g, and finally performed a double integration. The problem is that the displacement for the z coordinate are completely wrong. Any idea why?

I am attaching here the code

clc 
clear all
close all

% pos = [0 0 0];
delete(instrfindall);
%serialportlist("available")'
a = serial("COM7", "BaudRate", 9600);
gyroReadings = [0 0 0];
time = 0;
fopen(a);

g = 9.81
accelReadings = [0 0 -9.81];


Fs = 200;
tic;
for i=1:21
data = fscanf(a);
[ax ay az gx gy gz] = strread(data, '%f %f %f %f %f %f ','delimiter',';');
toc;
gx = gx*pi/180;
gy = gy*pi/180;
gz = gz*pi/180;
accelReadings = [accelReadings; ax ay az];
gyroReadings = [gyroReadings; gx gy gz];
time = [time; toc];

end
decim = 1
fuse = imufilter('SampleRate',Fs,'DecimationFactor',decim);
% [orientation,angularVelocity] = FUSE()
q = fuse(accelReadings,gyroReadings)
time_1 = (0:decim:size(accelReadings,1)-1)/Fs;

plot(time_1,eulerd(q,'ZYX','frame'))
title('Orientation Estimate')
legend('Z-axis', 'Y-axis', 'X-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')

%convert Acceleration to World Ref Frame
rotatedAcceleration = rotatepoint(q,accelReadings)

AccelWithoutGravity = [rotatedAcceleration(:,1) rotatedAcceleration(:,2) rotatedAcceleration(:,3)-g];


fs = 200; % Sampling Rate

fc = 0.1/30;  % Cut off Frequency

order = 6; % 6th Order Filter

%%Filter  Acceleration Signals

[b1 a1] = butter(order,fc,'high');

accf=filtfilt(b1,a1,AccelWithoutGravity );

figure (2)

plot(time,accf,'r'); hold on

plot(time,AccelWithoutGravity)

xlabel('Time (sec)')

ylabel('Acceleration (mm/sec^2)')


%First Integration (Acceleration - Veloicty)

velocity=cumtrapz(time,accf);

figure (3)

plot(time,velocity)

xlabel('Time (sec)')

ylabel('Velocity (mm/sec)')
legend('Z-axis', 'Y-axis', 'X-axis')

%%Filter  Veloicty Signals

[b2 a2] = butter(order,fc,'high');

velf = filtfilt(b2,a2,velocity);

%%Second Integration   (Velocity - Displacement)

Displacement=cumtrapz(time, velf);

figure(4)

plot(time,Displacement)

xlabel('Time (sec)')

ylabel('Displacement (mm)');

legend('Z-axis', 'Y-axis', 'X-axis')

 

Thanks!!

[maximilien battistutta]

Hello,

It may be a late answer but I am having the same issue as you right now, i was wondering if you had found a solution?

I also used this way to get rid of the gravity in order to have access to the positions yet i find uncoherent results. I saw it could be due to the accumulation of errors when you integer twice but i am not sure.

[Ioseph]

Maybe this is late too but using a bigger frecuency cutoff (closer to 1Hz) on the filters fixed my issues. The other possible solution is that make sure that you have the IMU correctly calibrated by using a simple average value calculation on the values before doing your real measurements and substract this values known as biases.