GPS and IMU DATA FUSION FOR POSITION ESTIMATION
32 views (last 30 days)
Show older comments
good morning, everyone.
i am working on a project to reconstruct a route using data from two sensors: gps and imu. I am working with two arduino boards, on one is integrated the imu while the gps is connected to the other board.
The sensor data is saved to two csv files which I then use in matlab.
The matlab code I have developed is as follows: I load the data from the gps and the imu and implement an extended kalman filter with the nonholonomic filter.
clear;
% carico dati del GPS
data_gps = load('C:\Users\alber\OneDrive\Desktop\TESI\PROVE_CAMPUS\14-01 (casa)\output_gga.csv');
% LATITUDINE, LONGITUDINE E ALTITUDINE
latitudine = data_gps(:,1);
longitudine = data_gps(:,2);
altitudine = data_gps(:,3);
time_gps = data_gps(:,4);
origine = [latitudine(1) longitudine(1) altitudine(1)];
%converti in coordinate locali
[xEast, yNorth, zUp] = latlon2local(latitudine, longitudine, altitudine, origine);
% converto il tempo del gps in secondi
tempi_gps_csv = num2str(time_gps);
ore = str2num(tempi_gps_csv(:,1:2));
minuti = str2num(tempi_gps_csv(:,3:4));
secondi = str2num(tempi_gps_csv(:,5:6));
tempi_secondi = ore*3600 + minuti*60 + secondi;
tempo_inizio = tempi_secondi(1);
tempi_gps = tempi_secondi - tempo_inizio;
% carico dati della IMU
data_imu = load('C:\Users\alber\OneDrive\Desktop\TESI\PROVE_CAMPUS\14-01 (casa)\IMU.csv');
N = length(data_imu);
accx_origin = data_imu(1:N,1);
accy_origin = data_imu(1:N,2);
accz_origin = data_imu(1:N,3);
acc_tot_origin = [accx_origin accy_origin accz_origin];
gyrox_origin = data_imu(1:N,4);
gyroy_origin = data_imu(1:N,5);
gyroz_origin = data_imu(1:N,6);
gyro_tot_origin = [gyrox_origin gyroy_origin gyroz_origin];
time_imu = data_imu(1:N,7);
% CONVERTO I DATI DI ACCELEROMETRO E GIROSCOPIO
% converto 'deg/s' to 'rad/s'
gyro_tot_rad = convangvel(gyro_tot_origin, 'deg/s', 'rad/s');
% converto 'g' in 'm/s^2'
acc_tot_meter = convacc(acc_tot_origin, 'G''s', 'm/s^2');
%converto il tempo della imu in secondi
tempi_imu_csv = num2str(floor(time_imu));
ore = str2num(tempi_imu_csv(:,1:2));
minuti = str2num(tempi_imu_csv(:,3:4));
secondi = str2num(tempi_imu_csv(:,5:6));
tempi_imu = ore*3600 + minuti*60 + secondi + mod(time_imu,1);
tempi_imu = tempi_imu - tempo_inizio;
% vettore tempi per l'operazione di interpolazione
tempo_desiderato = (tempi_imu(1):0.01:tempi_imu(end))';
% --------- INTERPOLAZIONE -----------
accx = interp1(tempi_imu, acc_tot_meter(:,1), tempo_desiderato); %la time_imu sarebbe il 'utc.delta_millis' che calcolo
accy = interp1(tempi_imu, acc_tot_meter(:,2), tempo_desiderato);
accz = interp1(tempi_imu, acc_tot_meter(:,3), tempo_desiderato);
acc_tot = [accx accy accz];
% gyrox = interp1(time_imu, gyrox_origin, tempo_desiderato);
% gyroy = interp1(time_imu, gyroy_origin, tempo_desiderato);
% gyroz = interp1(time_imu, gyroz_origin, tempo_desiderato);
gyrox = interp1(tempi_imu, gyro_tot_rad(:,1), tempo_desiderato);
gyroy = interp1(tempi_imu, gyro_tot_rad(:,2), tempo_desiderato);
gyroz = interp1(tempi_imu, gyro_tot_rad(:,3), tempo_desiderato);
gyro_tot = [gyrox gyroy gyroz];
% vado ad interpolare anche le coord del gps
lat_interp = interp1(tempi_gps, latitudine, tempo_desiderato);
lon_interp = interp1(tempi_gps, longitudine, tempo_desiderato);
alt_interp = interp1(tempi_gps, altitudine, tempo_desiderato);
length_imu = length(acc_tot); %numero di elementi sul file csv
length_gps = length(data_gps);
% ------------------ IMU FILTER -----------------------
fuse = imufilter('ReferenceFrame', 'NED','SampleRate', 100,'GyroscopeNoise', 0.0597, 'AccelerometerNoise', 3.8416e-04);
[orientazione ,angVelBodyRecovered] = fuse(acc_tot, gyro_tot);
% Plot Euler angles in degrees
A = eulerd(orientazione, 'ZYX', 'frame');
plot(eulerd(orientazione, 'ZYX', 'frame'));
title('Orientation Estimate');
legend('Z-rotation', 'Y-rotation', 'X-rotation');
ylabel('Degrees');
% --------------- QUA C'E' LA PARTE PER IL FILTRO -----------------------
imuFs_nonHolonomic = 100;
imuFs_imuFilter = 100;
gpsFs = 1;
imuSamplesPerGPS = imuFs_nonHolonomic/gpsFs;
localOrigin = [latitudine(1) longitudine(1) altitudine(1)];
% ------------------- DEFINIZIONE NONHOLONOMIC FILTER ---------------------
filt = insfilterNonholonomic('ReferenceFrame', 'NED');
filt.IMUSampleRate = imuFs_nonHolonomic;
filt.ReferenceLocation = localOrigin;
filt.StateCovariance = 1e-6*eye(16);
% parametri del filtro: noise acc e gyro
filt.AccelerometerNoise = 3.8416e-04;
filt.GyroscopeNoise = 0.0597;
Rpos = 0.001 * eye(3);
% ------- LOOP PRINCIPALE PER IL FILTRO -------------------
numIMUSamples = length(tempo_desiderato);
estOrient = quaternion.ones(numIMUSamples,1);
estPos = zeros(numIMUSamples,3);
estVel = zeros(numIMUSamples,3);
gpsIdx = 1;
correzioni = [];
factor_div = round(numIMUSamples/length_gps);
for idx = 1:numIMUSamples
predict(filt, acc_tot(idx,:), gyro_tot(idx,:)); %Predict filter state
tcorrente = tempo_desiderato(idx);
if tempi_gps(gpsIdx+1)<=tcorrente
% if (mod(idx, factor_div) == 0) %Correct filter state..... fai il fuseGPS ogni 1000 campioni (se simulo su 2 minuti)...
%gpsLLA(gpsIdx,:) = [latitudine(gpsIdx,:) longitudine(gpsIdx,:) altitudine(gpsIdx,:)];
%interp1 per gps su tempocorr
prima = pose(filt);
% fusegps(filt, [lat_interp(gpsIdx,:) lon_interp(gpsIdx,:) altitudine(gpsIdx,:)], Rpos);
fusegps(filt, [latitudine(gpsIdx,:) longitudine(gpsIdx,:) altitudine(gpsIdx,:)], Rpos);
dopo = pose(filt);
correzioni = [correzioni; prima, dopo]; %memorizza correzioni GPS
%[estPos(gpsIdx,:), estOrient(gpsIdx,:), estVel(gpsIdx,:)] = pose(filt); %Log estimated pose
gpsIdx = gpsIdx + 1;
end
[estPos(idx,:), estOrient(idx,:), estVel(gpsIdx,:)] = pose(filt);
end
% plot del risultato
plot(xEast,yNorth,'k');
xlabel('x [m]');
ylabel('y [m]');
hold on
plot(estPos(:,2), estPos(:,1), 'g');
The final plot result should be different from what I get, can anyone tell me why?
I can also share the csv files if needed.
Thank you very much,
alberto.
3 Comments
Aishwarya
on 7 Feb 2024
Hi Alberto,
After running the code with the provided CSV file, I am unable to get the expected plot as shown in "plot.fig" file. Can you update the code, as that will help debug the issue.
Answers (2)
Nedjla
on 28 Apr 2024
hello sir
thank you for your answer
please how did you load the data from the gps and the imu
0 Comments
See Also
Products
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!