GPS and IMU DATA FUSION FOR POSITION ESTIMATION

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.

[Garmit Pant]

Hi Alberto

Can you please share the CSV files and the output plot that you are getting and also the ideal plot that you should be getting for reference?

[Alberto CIPPELLETTI]

Attached are the two csv files and the plot I obtain.

the black line is the gps plot (I use latitude and longitude coordinates), the green one is the output of the filter.

my goal is to get the green line more linear (more accurate) than the black gps line.

I also have a question: what does the value of Rpos indicate? I set it to 0.001 but I'm not sure if that's right.

The GyroscopeNoise and AccelerometerNoise values I calculated from the data on the IMU datasheet

[Aishwarya]

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.

Answer 1

[Nedjla]

hello does it worked with

Answer 2

[Nedjla]

hello sir

thank you for your answer

please how did you load the data from the gps and the imu