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');
msep = sqrt(mean(posd.^2));
fprintf('\tX: %.2f , Y: %.2f, Z: %.2f (meters)\n\n', msep(1), ...
msep(2), msep(3));
fprintf('Quaternion Distance RMS Error\n');
fprintf('\t%.2f (degrees)\n\n', sqrt(mean(quatd.^2)));
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));
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:
