Clear Filters
Clear Filters

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

67 views (last 30 days)
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
end
sigfrq = SIG_fft;
sigflt = real(ifft(SIG_fft));
end

Answers (1)

arushi
arushi on 4 Apr 2024
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 -
Hope this helps.

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!