IMU filter nonholonomic - fusegps
2 ビュー (過去 30 日間)
古いコメントを表示
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.
0 件のコメント
回答 (0 件)
参考
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!