Does anyone have any ideas? I've tried adding in butter(a,b,'low/high') filters of different orders, frequencies, low/high, throughout the code. And while this sometimes seems to help smooth the code, I never get anything resonable for position. I would think, I'm using a rather expensive IMU, Memsense nIMU, that I would at least get something in the ballpark. Is it the filter? Sorry, this seems to be an easy fix to get reasonable data in every other post I've read.
Trouble Filtering and Integrating Acceleration Data
4 次查看(过去 30 天)
显示 更早的评论
Hi,
I've read a few threads about integration of acceleration data and I'm still having trouble getting the results I'm expecting. I was looking at this paper: Slifka, Lance D., and Lance D. Slifka. An Accelerometer based approach to measuring displacement of a vehicle body. Diss. PhD thesis, Horace Rackham School Of Graduate Studies of the University of Michigan, 2004.
to get an idea about high pass filters and using the example code there as a base. I have also tried the iomega function floating around this site forum. My results are still not correct and I'm hoping someone can point me in the right direction.
I have looked at 3 cases. Static, circle, and an arc with a pause between each motion (left 90, pause, right 170, pause, left 80, stop). I am attempting to plot (X,Y) position and hoping it resembles the motion stated.
The code I've utilized from the above thesis (with modifications) is the following: if true % code
filename = 'MovePause.txt';
Test1 = importdata(filename);
XStatic = Test1(1:700,2);
YStatic = Test1(1:700,3);
ZStatic = Test1(1:700,4);
Xmean = mean(XStatic);
Ymean = mean(YStatic);
Zmean = mean(YStatic);
for i=1:1:length(Test1)
Test1(i,2) = (Test1(i,2)-Xmean)*-9.81;
Test1(i,3) = (Test1(i,3)-Ymean)*-9.81;
Test1(i,4) = (Test1(i,4)-Zmean)*-9.81;
end
%Plot the Accelerations
figure(1)
plot(Test1(:,1),Test1(:,2)),xlabel('Time(s)','FontSize',12),ylabel('Acceleration (m/s^2)','FontSize',12);
hold on;
plot(Test1(:,1),Test1(:,3), 'red');
hold off;
%Lets try this other method with filtered and unfiltered data-not so much
%XTest1 = iomega(Test1(:,2), (Test1(length(Test1),1)/length(Test1)),3,1);
%YTest1 = iomega(Test1(:,3),(Test1(length(Test1),1)/length(Test1)),3,1);
%figure(6)
%plot(XTest1,YTest1),xlabel('X Position (m)'),ylabel('YPosition (m)')
%Now the Accels are in m/s and the offset has been removed. Do I remove
%offset if I am high pass filtering?
%Filter Acceleration
Acc_Spect = fft(Test1(:,2),length(Test1));
x = length (Acc_Spect);
%Set the first 15 values constant-why???
Acc_Spect(1) = 0.0775*Acc_Spect(26);
for i=2:25
Acc_Spect(i) = 0.0775*Acc_Spect(26);
Acc_Spect(x-(i-2))=conj(Acc_Spect(i));
end
Test1(:,2) = real(ifft(Acc_Spect));
Acc_SpectY = fft(Test1(:,3),length(Test1));
x = length (Acc_SpectY);
%Set the first 15 values constant-why???
Acc_SpectY(1) = 0.0775*Acc_SpectY(26);
for i=2:25
Acc_SpectY(i) = 0.0775*Acc_SpectY(26);
Acc_SpectY(x-(i-2))=conj(Acc_SpectY(i));
end
Test1(:,3) = real(ifft(Acc_SpectY));
figure(2);
w = [0:2*pi/(x):2*pi-pi/(x)];
plot(w,abs(Acc_Spect));
%First Integration
%I am averaging the time difference here-need to integrate in the
%individual step differences if this goes well
XVel = (Test1(length(Test1),1)/length(Test1))*cumtrapz(Test1(:,2));
XVel_0 = -1*mean(XVel);
Vel_Spect = fft(XVel,length(Test1));
x = length(Vel_Spect);
%Set the first 46 values constant-why?????
Vel_Spect(1) = 0.0775*Vel_Spect(45);
for i =2:45
Vel_Spect(i) = 0.0775*Vel_Spect(45);
Vel_Spect(x-(i-2))=conj(Vel_Spect(i));
end
XVel = real(ifft(Vel_Spect));
YVel = (Test1(length(Test1),1)/length(Test1))*cumtrapz(Test1(:,3));
YVel_0 = -1*mean(YVel);
Vel_SpectY = fft(YVel,length(Test1));
x = length(Vel_SpectY);
%Set the first 46 values constant-why?????
Vel_SpectY(1) = 0.0775*Vel_SpectY(45);
for i =2:45
Vel_SpectY(i) = 0.0775*Vel_SpectY(45);
Vel_SpectY(x-(i-2))=conj(Vel_SpectY(i));
end
YVel = real(ifft(Vel_SpectY));
figure(3)
plot(Test1(:,1),XVel, 'b'), xlabel('Time (s)'),ylabel('Amplitude (m/s)')
hold on
plot(Test1(:,1),YVel, 'r')
hold off
%Second Integration
XPos_ii = (Test1(length(Test1),1)/length(Test1))*cumtrapz(XVel);
Pos_Spect = fft(XPos_ii,length(Test1));
x = length(Pos_Spect);
%Set the first 66 values constant-why?????
Pos_Spect(1) = 0.0775*Pos_Spect(66);
for i=2:65
Pos_Spect(i) = 0.0775*Pos_Spect(66);
Pos_Spect(x-(i-2))=conj(Pos_Spect(i));
end
XPos_ii = real(ifft(Pos_Spect));
YPos_ii = (Test1(length(Test1),1)/length(Test1))*cumtrapz(YVel);
Pos_SpectY = fft(YPos_ii,length(Test1));
x = length(Pos_SpectY);
%Set the first 66 values constant-why?????
Pos_SpectY(1) = 0.0775*Pos_SpectY(66);
for i=2:65
Pos_SpectY(i) = 0.0775*Pos_SpectY(66);
Pos_SpectY(x-(i-2))=conj(Pos_SpectY(i));
end
YPos_ii = real(ifft(Pos_SpectY));
figure(4)
plot(Test1(:,1),XPos_ii,'b'),xlabel('Time(s)'),ylabel('X position (m)');
hold on
plot(Test1(:,1),YPos_ii,'r')
hold off
figure(5)
plot(XPos_ii,YPos_ii),xlabel('X Position(m)'),ylabel('Y position (m)');
end
The circle is about 10 cm in diameter, and the arc should be on the order of 15-20 cm. The accelerometers talk in Gs, so I convert to m/s^2. I also remove the average offset of the signal before it goes into the filter. I'm also a little shaky with my understanding of the filter (for example I have real time intervals that are not always equal, close, but not equal, and I'd love to integrate that in but don't know how.) so if this is a misunderstanding of how that works and someone could explain I'd appreciate it.
Thanks in advance.
回答(1 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Multirate Signal Processing 的更多信息
产品
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!