filter nonholonomic - fusegps

4 ビュー (過去 30 日間)
Alberto CIPPELLETTI
Alberto CIPPELLETTI 2023 年 8 月 5 日
回答済み: Garmit Pant 2023 年 8 月 29 日
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.

回答 (1 件)

Garmit Pant
Garmit Pant 2023 年 8 月 29 日
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 functionfusegps 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:

カテゴリ

Help Center および File ExchangeInertial Sensor Fusion についてさらに検索

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by