filter nonholonomic - fusegps

Good evening everyone, I am new so I apologise if I am going to be inaccurate.

I am working on gps (latitude and longitude) and IMU (3 axis accelerations and 3 axis gyro) data in order to run a version of the Kalman filter.

I am using a 'nonholonomic' filter and calculating the orientation and angular velocities on the 3 axis; this is the code I wrote:

%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

My question is as follows:

when I do the 'fusegps' operation, res and resCov should be of size 1-by-4 (res) and 4-by-4 (resCov)... from my code they are 1x3 and 3x3.

Is this wrong or is it still OK?

Thank you very much,

Alberto.

Answer 1

[Garmit Pant]

Hello

It is my understanding that you are trying to use the function ‘fusegps’ and the dimensions of the outputs that you are generating are different from the expected output.

The function ‘fusegps’ is a member function of various insfilter classes. For objects of ‘insfilterNonholonomic’, the function ‘fusegps’ can be called as follows:

[res,resCov] = fusegps(FUSE,position,positionCovariance)

[res,resCov] = fusegps(FUSE,position,positionCovariance,velocity,velocityCovariance)

Where:

• FUSE -  insfilterNonholonomic object
• position - 1-by-3 vector of latitude, longitude and altitude
• positionCovariance - scalar, 1-by-3, or 3-by-3 covariance of the NAV position measurement error in m^2

and

• res – 1-by-3 or 1-by-4 position and course residuals in meters (m) and rad/s, respectively
• resCov – 3-by-3 or 4-by-4 residual covariance

The function ‘fusegps’ returns output variables ‘res’ and ‘resCov’ of size 1-by-3 (res) and 3-by-3 (resCov) when only 3 arguments are passed while calling the function (fusegps(FUSE,position,positionCovariance)). Since you are calling the function ‘fusegps’ with the aforementioned function signature, the outputs that you are receiving have the size 1-by-3 (res) and 3-by-3 (resCov).

The following code snippet is an example to test the different output dimensions according to the function signature:

load('loggedGroundVehicleCircle.mat', ... 
    'imuFs', 'localOrigin', ... 
    'initialState', 'initialStateCovariance', ... 
    'accelData', 'gyroData', ... 
    'gpsFs', 'gpsLLA', 'Rpos', 'gpsVel', 'Rvel', ... 
    'trueOrient', 'truePos'); 

% Initialize filter. 
filt = insfilterNonholonomic('IMUSampleRate', imuFs, ... 
    'ReferenceLocation', localOrigin, 'State', initialState, ... 
    'StateCovariance', initialStateCovariance,'ReferenceFrame', 'ENU'); 

imuSamplesPerGPS = imuFs / gpsFs; 

% Log data for final metric computation. 
numIMUSamples = size(accelData, 1); 
estOrient = quaternion.ones(numIMUSamples, 1); 
estPos = zeros(numIMUSamples, 3); 

gpsIdx = 1; 
for idx = 1:numIMUSamples 
    % Use the predict method to estimate the filter state based on the 
    % accelData and gyroData arrays. 
    predict(filt, accelData(idx,:), gyroData(idx,:)); 
    
    if (mod(idx, imuSamplesPerGPS) == 0) 
        % Correct the filter states based on the GPS data. 
        
        % For output size 1x3 and 3x3 
        [res, resCov] = fusegps(filt, gpsLLA(gpsIdx,:), Rpos); 
        
        % For output size 1x4 and 4x4 
        %[res, resCov] = fusegps(filt, gpsLLA(gpsIdx,:), Rpos, gpsVel(gpsIdx,:), Rvel); 
        
        gpsIdx = gpsIdx + 1; 
    end 
    % Log estimated pose. 
    [estPos(idx,:), estOrient(idx,:)] = pose(filt); 
end 

% Calculate RMS errors. 
posd = estPos - truePos; 
quatd = rad2deg(dist(estOrient, trueOrient)); 

% Display RMS errors. 
fprintf('Position RMS Error\n'); 

Position RMS Error

msep = sqrt(mean(posd.^2)); 
fprintf('\tX: %.2f , Y: %.2f, Z: %.2f (meters)\n\n', msep(1), ... 
    msep(2), msep(3)); 

	X: 8.57 , Y: 4.94, Z: 8.43 (meters)

fprintf('Quaternion Distance RMS Error\n'); 

Quaternion Distance RMS Error

fprintf('\t%.2f (degrees)\n\n', sqrt(mean(quatd.^2))); 

	8.85 (degrees)

fprintf('Size of res and resCov\n \t res: %.2f by %.2f\t resCov %.2f by %.2f \n\n', size(res,1),size(res,2),size(resCov,1),size(resCov,2)); 

Size of res and resCov
 	 res: 1.00 by 3.00	 resCov 3.00 by 3.00 

You can run the above code to simulate different behaviours of the function ‘fusegps’.

You can refer to the following documentation pages to learn more:

https://in.mathworks.com/help/fusion/ref/nhconstrainedimugpsfuser.fusegps.html