Use MPU9250 to record position and trajectory
19 次查看(过去 30 天)
显示 更早的评论
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!!
2 个评论
maximilien battistutta
2021-2-3
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
2021-5-20
编辑:Ioseph
2021-5-20
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.
回答(0 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Arduino Hardware 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!