IMU filter nonholonomic - fusegps

Buonasera a tutti, sono nuovo quindi mi scuso se dirò delle inesattezze.

Sto lavorando su dati di gps (latitudine e longitudine) e di IMU (accelerazioni sui 3 assi e giroscopio sui 3 assi) al fine di eseguire una versione del filtro di Kalman.

Sto usando un filtro 'nonholonomic' e mi calcolo l'orientazione e le velcità angolari sui 3 assi; questo è il codice che ho scritto:

%carico i dati del GPS
data_gps =load('GPS_meta.csv');
[filename, directory_name] = uigetfile('*.csv','GPS_meta.csv');
fullname = fullfile(directory_name, filename);
data_gps = load(fullname);

% LATITUDINE, LONGITUDINE E ALTITUDINE
latitudine = data_gps(:,1); 
longitudine = data_gps(:,2);
altitudine = repmat(55.0, 1, length(latitudine))';

%carico i dati dell'accelerometro e giroscopio
data_imu = load('DATALOG_meta.csv');
[filename1, directory_name1] = uigetfile('*.csv','DATALOG_meta.csv');
fullname = fullfile(directory_name1, filename1);
data_imu = load(fullname);

accx = data_imu(:,6);
accy = data_imu(:,7);
accz = data_imu(:,8); 
acc_tot = [accx accy accz];
gyrox = data_imu(:,9);
gyroy = data_imu(:,10);
gyroz = data_imu(:,11);
gyro_tot = [gyrox gyroy gyroz];
%temp = data_imu(:,12);

% PLOT MAPPA GPS
max_lat = max(latitudine);
min_lat = min(latitudine);
max_long = max(longitudine);
min_long = min(longitudine);
% VADO A RAPPRESENTARE LE MIE COORDINATE
geoplot(latitudine,longitudine,"-",LineWidth=1.5);
%geoscatter(latitudine,longitudine,"*")
geolimits([min_lat max_lat],[min_long max_long])
geobasemap openstreetmap

%---------------QUA C'E' LA PARTE PER IL FILTRO-----------------------
imuFs = 1;
gpsFs = 1;
localOrigin = [44.76483 10.30858 55]; %'punto di partenza'

%convert deg to rad
gyro_tot_rad = deg2rad(gyro_tot);

%fusion filter
filt = insfilterNonholonomic('ReferenceFrame', 'ENU');
filt.IMUSampleRate = imuFs;
filt.ReferenceLocation = localOrigin;
filt.StateCovariance = 1e-9*eye(16);
Rpos = eye(3);

%imu filter for angular velocity
fuse = imufilter('SampleRate',imuFs, 'GyroscopeDriftNoise', 1e-6);
[orientazione ,angVelBodyRecovered] = fuse(acc_tot, gyro_tot_rad);

%log data
numIMUSamples = length(latitudine);
estOrient = quaternion.ones(numIMUSamples,1);
estPos = zeros(numIMUSamples,3);

gpsIdx = 1;
for idx = 1:numIMUSamples
    predict(filt,acc_tot(idx,:),gyro_tot_rad(idx,:));       %Predict filter state
    
    if (mod(idx,gpsFs) == 0)                   %Correct filter state
        %gpsLLA(gpsIdx,:) = [latitudine(gpsIdx,:) longitudine(gpsIdx,:) altitudine(gpsIdx,:)];
        [res, resCov] = fusegps(filt, [latitudine(gpsIdx,:) longitudine(gpsIdx,:) altitudine(gpsIdx,:)], Rpos);
        gpsIdx = gpsIdx + 1;
    end
    
    [estPos(idx,:),estOrient(idx,:)] = pose(filt); %Log estimated pose--> estPos() è in m/s !!!
end

Il mio DUBBIO è il seguente:

quando faccio l'operazione di 'fusegps', res e resCov dovrebbero essere di dimensione 1-by-4 (res) e 4-by-4 (resCov)... a me risultano 1x3 e 3x3.

E' sbagliato o va bene lo stesso?

Grazie mille,

Alberto.