how to get position data from acceleration data obtained from an IMU ?

Question

Vasumathi R 2023-5-29

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1974954-how-to-get-position-data-from-acceleration-data-obtained-from-an-imu

回答： arushi 2024-4-4

export_2_rounds.xlsx

I have attached the 3-axis acceleration and roll,pitch,yaw data of a scaled vehicle where an IMU is mounted on it. The vehicle takes two turns in a oval shaped track (basically a line follower). I wnat to get the posotion data from the acceleration and If I plot the X position Vs Y position I should get the two overlapped oval shaped circles. I have tried using klamna filter. As the noise mean of the acceleration daa keeps varying, it was difficult to do kalman filtering. Then I tried Low pass filtering and band pass filtering ( nothing helped). Atlast I tried te code from the following link: https://in.mathworks.com/matlabcentral/fileexchange/97012-acceleration-to-velocity-and-displacement-using-dsp-filters. it worked but now the position data is not accurate. Is there any other already existing filters which can reduce the noise of the data, so that I can get position data?

clear all; close all; clc;

data = readmatrix('export_2_rounds.xlsx');

acc_x = data(:,3); % x acceleration

acc_y = data(:,4); % y acceleratio

acc_z = data(:,5); % z acceleration

roll = data(:,6);

pitch = data(:,7);

yaw = data(:,8);

time = data(:,1); % here it is serial no of the data reading ( I havent used timestamps)

acc_global_x =[];

acc_global_y =[];

acc_global_z =[];

% Convert roll, pitch, and yaw angles to radians

roll_rad = deg2rad(roll);

pitch_rad = deg2rad(pitch);

yaw_rad = deg2rad(yaw);

len = length(acc_x);

for i = 1:len

% Compute the rotation matrix

R_x = [1, 0, 0;

0, cos(roll_rad(i)), -sin(roll_rad(i));

0, sin(roll_rad(i)), cos(roll_rad(i))];

R_y = [cos(pitch_rad(i)), 0, sin(pitch_rad(i));

0, 1, 0;

-sin(pitch_rad(i)), 0, cos(pitch_rad(i))];

R_z = [cos(yaw_rad(i)), -sin(yaw_rad(i)), 0;

sin(yaw_rad(i)), cos(yaw_rad(i)), 0;

0, 0, 1];

% Combine the rotation matrices

R = R_z * R_y * R_x;

% Create the local acceleration vector

acc_local = [acc_x(i), acc_y(i), acc_z(i)]';

% Transform the local acceleration vector to the global frame

acc_global = R_z* acc_local; % only rotational matrix for yaw angle

% Extract the components of the global acceleration

acc_global_x(i) = acc_global(1, :);

acc_global_y(i) = acc_global(2, :);

acc_global_z(i) = acc_global(3, :);

end

ag_xx = [acc_global_x]';

ag_yy = [acc_global_y]';

ag_zz = [acc_global_z]';

accval_x = ag_xx ; % X axis Acceleration after rotation

accval_y = ag_yy ; % Y axis Acceleration after rotation

accval_z = ag_zz ; % Z axis Acceleration after rotation

cut_off_x = [0.2 0.8]; % cut off frequency for x axis - two frequencies for bandpass filter

cut_off_y = [0.2 0.8];

cut_off_z = [0.02 0.4];

alpha = 0.01; % Scaling factor

filtertype = 'bandpass'; % lowpass | highpass | bandpass

%filtermethod = 'fft'; % FFT-fft | FIR-fir

L = size(data,1); % Length of signal

Fs = 100; % Sampling frequency

Ts = 1/Fs;

%%%% Compute the frequency

%--------------------------------------------------------------------------

% Compute the Fourier transform of the un filtered signal.

fft_x = fft(accval_x);

fft_y = fft(accval_x);

fft_z = fft(accval_x);

% Compute the two-sided spectrum P2. Then compute the single-sided spectrum P1

% based on P2 and the even-valued signal length L.

P_x1 = abs(fft_x);

P_y1 = abs(fft_y/L);

P_z1 = abs(fft_z/L);

P_x = P_x1(1:floor(L/2+1));

P_y = P_y1(1:floor(L/2+1));

P_z = P_z1(1:floor(L/2+1));

P_x(2:end-1) = 2*P_x(2:end-1);

P_y(2:end-1) = 2*P_y(2:end-1);

P_z(2:end-1) = 2*P_z(2:end-1);

f = Fs*(0:(L/2))/L;

figure;

subplot(3,1,1);

plot(f,P_x, 'LineWidth', 1)

grid on

title('Frequency Spectrum of Acceleration')

xlabel('f(Hz)')

ylabel('|P1(f)|')

subplot(3,1,2);

plot(f,P_y, 'LineWidth', 1)

grid on

title('Frequency Spectrum of Acceleration')

xlabel('f(Hz)')

ylabel('|P1(f)|')

subplot(3,1,3);

plot(f,P_z, 'LineWidth', 1)

grid on

title('Frequency Spectrum of Acceleration')

xlabel('f(Hz)')

ylabel('|P1(f)|')

%% Computing acceleration

[ filtered_disp_x, filtered_vel_x, filtered_acc_x] ...

= func_acc2disp(accval_x , cut_off_x, alpha, filtertype);

[filtered_disp_y, filtered_vel_y, filtered_acc_y] ...

= func_acc2disp(accval_y , cut_off_y, alpha, filtertype);

[filtered_disp_z, filtered_vel_z, filtered_acc_z] = func_acc2disp(accval_z ,cut_off_z, alpha, filtertype);

%% Compute the frequency

%--------------------------------------------------------------------------

% Compute the Fourier transform of the filtered signal.

fft_x = fft(filtered_acc_x);

fft_y = fft(filtered_acc_y);

fft_z = fft(filtered_acc_z);

% Compute the two-sided spectrum P2. Then compute the single-sided spectrum P1

% based on P2 and the even-valued signal length L.

P_x1 = abs(fft_x/L);

P_y1 = abs(fft_y/L);

P_z1 = abs(fft_z/L);

P_x = P_x1(1:floor(L/2+1));

P_y = P_y1(1:floor(L/2+1));

P_z = P_z1(1:floor(L/2+1));

P_x(2:end-1) = 2*P_x(2:end-1);

P_y(2:end-1) = 2*P_y(2:end-1);

P_z(2:end-1) = 2*P_z(2:end-1);

f = Fs*(0:(L/2))/L;

figure;

subplot(3,1,1);

plot(f,P_x, 'LineWidth', 1)

grid on

title('Frequency Spectrum of Acceleration')

xlabel('f(Hz)')

ylabel('|P1(f)|')

subplot(3,1,2);

plot(f,P_y, 'LineWidth', 1)

grid on

title('Frequency Spectrum of Acceleration')

xlabel('f(Hz)')

ylabel('|P1(f)|')

subplot(3,1,3);

plot(f,P_z, 'LineWidth', 1)

grid on

title('Frequency Spectrum of Acceleration')

xlabel('f(Hz)')

ylabel('|P1(f)|')

%% Plot Acceleration vs.time

%--------------------------------------------------------------------------

figure;

subplot(3,1,1)

plot(time,filtered_acc_x, 'LineWidth', 1)

grid on

xlabel('Time ($sec.$)','Interpreter', 'latex');

ylabel('Acceleration ($\frac{m}{s^2}$)','Interpreter', 'latex');

title('Acceleration vs.time (DC bias removed)')

ax4 = subplot(3,1,2);

plot(time,filtered_vel_x, 'LineWidth', 1)

grid on

xlabel('Time ($sec.$)','Interpreter', 'latex');

ylabel('Velocity ($\frac{m}{s}$)','Interpreter', 'latex');

title('Velocity vs.time')

ax5 = subplot(3,1,3);

plot(time,filtered_disp_x, 'LineWidth', 1);

grid on

xlabel('Time ($sec.$)','Interpreter', 'latex');

ylabel('Displacement ($m$)','Interpreter', 'latex');

title('Displacement vs.time')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure;

subplot(3,1,1)

plot(time,filtered_acc_y, 'LineWidth', 1)

grid on

xlabel('Time ($sec.$)','Interpreter', 'latex');

ylabel('Acceleration ($\frac{m}{s^2}$)','Interpreter', 'latex');

title('Acceleration vs.time (DC bias removed)')

subplot(3,1,2)

plot(time,filtered_vel_y, 'LineWidth', 1)

grid on

xlabel('Time ($sec.$)','Interpreter', 'latex');

ylabel('Velocity ($\frac{m}{s}$)','Interpreter', 'latex');

title('Velocity vs.time')

subplot(3,1,3)

plot(time,filtered_disp_y, 'LineWidth', 1)

grid on

xlabel('Time ($sec.$)','Interpreter', 'latex');

ylabel('Displacement ($m$)','Interpreter', 'latex');

title('Displacement vs.time')

figure;

plot(filtered_disp_x, filtered_disp_y);

function [pos, xd_Time, Acc_filt] = func_acc2disp( signalIN, cutoff, alpha, filtertype)

% Input

xdd = signalIN;

dt = 0.01;

Fs = 1/dt;

[Acc_filt,~] = func_DCclean(xdd, cutoff, alpha, Fs, filtertype);

velo = dt * cumtrapz(Acc_filt);

[xd_Time, ~] = func_DCclean(velo, cutoff, alpha, Fs, filtertype);

pos = dt * cumtrapz(xd_Time);

%[pos_fil, ~] = func_DCclean(pos, cutoff, alpha, Fs, filtertype);

end

function [sigflt, sigfrq] = ...

func_DCclean(sig, cutoff, alpha, fs, passtype)

% This function FUNC_DCCLEAN removes the low frequency content (for the

% given cutoff value) and the DC offset

x = length(sig);

k = cutoff*(x/fs);

n = round(k);

SIG_fft = fft(sig, x);

switch passtype

case 'lowpass'

SIG_fft(1) = complex(alpha*abs(SIG_fft(n(1))),0);

SIG_fft(x) = complex(alpha*abs(SIG_fft(n(1))),0);

if (length(n)>1)

error('Too many cutoff frequencies')

end

for i=n:round((x/2)-1)

SIG_fft(i) = alpha*SIG_fft(n);

SIG_fft(x-i) = conj(SIG_fft(i));

end

case 'highpass'

SIG_fft(1) = complex(alpha*abs(SIG_fft(n(1))),0);

if (length(n)>1)

error('Too many cutoff frequencies')

end

for i=2:n-1

SIG_fft(i) = alpha*SIG_fft(n);

SIG_fft(x-(i-2)) = conj(SIG_fft(i));

end

otherwise

if (length(n)>2)

error('Too many cutoff frequencies')

elseif (length(n)<2)

error('Too few cutoff frequencies')

end

SIG_fft(1) = complex(alpha*abs(SIG_fft(n(1))),0);

SIG_fft(x) = complex(alpha*abs(SIG_fft(n(1))),0);

for i = n(2):round((x/2)-1)

SIG_fft(i) = alpha*SIG_fft(n(2));

SIG_fft(x-i) = conj(SIG_fft(i));

end

for i = n(1):-1:2

SIG_fft(i) = alpha*SIG_fft(n(1));

SIG_fft(x-i) = conj(SIG_fft(i));

end

sigfrq = SIG_fft;

sigflt = real(ifft(SIG_fft));

end

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

arushi 2024-4-4

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1974954-how-to-get-position-data-from-acceleration-data-obtained-from-an-imu#answer_1436386

Hi Vasumathi,

I can see that you have already tried with Kalman filtering, low-pass filtering, and band-pass filtering without satisfactory results, and considering the varying noise mean in your acceleration data you may try the following :

1. Complementary Filter

A complementary filter combines the high-frequency components of one signal (e.g., from a gyroscope) with the low-frequency components of another (e.g., from an accelerometer) to produce a more accurate composite signal. If you have gyroscope data in addition to accelerometer data, this approach might help in reducing noise while retaining valuable signal information.

2. Adaptive Filtering

Adaptive filters adjust their parameters in real-time based on the characteristics of the input signal and the noise. This can be useful when dealing with non-stationary noise (noise characteristics that change over time).

3. Smoothing Techniques

Smoothing techniques such as Savitzky-Golay filters can help reduce noise while preserving the shape and features of the underlying signal. These filters are particularly good at preserving signal features like peaks, which might be distorted by other types of filters.

For more information on Savitzky-Golay filter please refer to the documentation -

https://www.mathworks.com/help/signal/ref/sgolayfilt.html

Hope this helps.