Fusione IMU e GPS per la navigazione inerziale
Questo esempio mostra come è possibile creare un algoritmo di fusione IMU + GPS adatto ai veicoli aerei senza pilota (UAV) o ai quadricotteri.
In questo esempio vengono utilizzati accelerometri, giroscopi, magnetometri e GPS per determinare l'orientamento e la posizione di un UAV.
Impostazione della simulazione
Imposta le frequenze di campionamento. In un sistema tipico, l'accelerometro e il giroscopio funzionano a frequenze di campionamento relativamente elevate. La complessità dell'elaborazione dei dati provenienti da tali sensori nell'algoritmo di fusione è relativamente bassa. Al contrario, il GPS e, in alcuni casi, il magnetometro, funzionano a frequenze di campionamento relativamente basse e la complessità associata alla loro elaborazione è elevata. In questo algoritmo di fusione, i campioni del magnetometro e del GPS vengono elaborati insieme alla stessa bassa velocità, mentre i campioni dell'accelerometro e del giroscopio vengono elaborati insieme alla stessa alta velocità.
Per simulare questa configurazione, l'IMU (accelerometro, giroscopio e magnetometro) viene campionato a 160 Hz, mentre il GPS viene campionato a 1 Hz. Solo uno su 160 campioni del magnetometro viene fornito all'algoritmo di fusione, quindi in un sistema reale il magnetometro potrebbe essere campionato a una velocità molto più bassa.
imuFs = 160; gpsFs = 1; % Define where on the Earth this simulated scenario takes place using the % latitude, longitude and altitude. refloc = [42.2825 -72.3430 53.0352]; % Validate that the |gpsFs| divides |imuFs|. This allows the sensor sample % rates to be simulated using a nested for loop without complex sample rate % matching. imuSamplesPerGPS = (imuFs/gpsFs); assert(imuSamplesPerGPS == fix(imuSamplesPerGPS), ... 'GPS sampling rate must be an integer factor of IMU sampling rate.');
Filtro di fusione
Crea il filtro per fondere le misurazioni IMU + GPS. Il filtro di fusione utilizza un filtro di Kalman esteso per tracciare l'orientamento (come un quaternione), la velocità, la posizione, le polarizzazioni del sensore e il vettore geomagnetico.
Questo insfilterMARG ha alcuni metodi per elaborare i dati dei sensori, tra cui predict, fusemag e fusegps. Il metodo predict accetta come input i campioni dell'accelerometro e del giroscopio dall'IMU. Chiamare il metodo predict ogni volta che vengono campionati l'accelerometro e il giroscopio. Questo metodo prevede gli stati con un passo avanti temporale basandosi sull'accelerometro e sul giroscopio. La covarianza dell'errore del filtro di Kalman esteso è stata aggiornata qui.
Il metodo fusegps accetta campioni GPS come input. Questo metodo aggiorna gli stati del filtro in base ai campioni GPS calcolando un guadagno di Kalman che pondera i vari input dei sensori in base alla loro incertezza. Anche in questo caso viene aggiornata la covarianza degli errori, utilizzando questa volta anche il guadagno di Kalman.
Il metodo fusemag è simile, ma aggiorna gli stati, il guadagno di Kalman e la covarianza dell'errore in base ai campioni del magnetometro.
Sebbene insfilterMARG accetti come input campioni dell'accelerometro e del giroscopio, questi vengono integrati per calcolare rispettivamente velocità delta e angoli delta. Il filtro traccia la polarizzazione del magnetometro e di questi segnali integrati.
fusionfilt = insfilterMARG; fusionfilt.IMUSampleRate = imuFs; fusionfilt.ReferenceLocation = refloc;
Traiettoria UAV
In questo esempio viene utilizzata come base di partenza una traiettoria salvata e registrata da un UAV. Questa traiettoria viene inviata a diversi simulatori di sensori per elaborare flussi di dati simulati di accelerometri, giroscopi, magnetometri e GPS.
% Load the "ground truth" UAV trajectory. load LoggedQuadcopter.mat trajData; trajOrient = trajData.Orientation; trajVel = trajData.Velocity; trajPos = trajData.Position; trajAcc = trajData.Acceleration; trajAngVel = trajData.AngularVelocity; % Initialize the random number generator used in the simulation of sensor % noise. rng(1)
Sensore GPS
Impostare il GPS alla frequenza di campionamento e alla posizione di riferimento specificate. Gli altri parametri controllano la natura del rumore nel segnale di uscita.
gps = gpsSensor('UpdateRate', gpsFs); gps.ReferenceLocation = refloc; gps.DecayFactor = 0.5; % Random walk noise parameter gps.HorizontalPositionAccuracy = 1.6; gps.VerticalPositionAccuracy = 1.6; gps.VelocityAccuracy = 0.1;
Sensori IMU
In genere, un UAV utilizza un sensore MARG (magnetico, angolare, gravitazionale) integrato per la stima della posa. Per modellare un sensore MARG, definire un modello di sensore IMU contenente un accelerometro, un giroscopio e un magnetometro. In un'applicazione pratica, i tre sensori potrebbero provenire da un singolo circuito integrato o da circuiti separati. I valori delle proprietà qui impostati sono tipici dei sensori MEMS a basso costo.
imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs); imu.MagneticField = [19.5281 -5.0741 48.0067]; % Accelerometer imu.Accelerometer.MeasurementRange = 19.6133; imu.Accelerometer.Resolution = 0.0023928; imu.Accelerometer.ConstantBias = 0.19; imu.Accelerometer.NoiseDensity = 0.0012356; % Gyroscope imu.Gyroscope.MeasurementRange = deg2rad(250); imu.Gyroscope.Resolution = deg2rad(0.0625); imu.Gyroscope.ConstantBias = deg2rad(3.125); imu.Gyroscope.AxesMisalignment = 1.5; imu.Gyroscope.NoiseDensity = deg2rad(0.025); % Magnetometer imu.Magnetometer.MeasurementRange = 1000; imu.Magnetometer.Resolution = 0.1; imu.Magnetometer.ConstantBias = 100; imu.Magnetometer.NoiseDensity = 0.3/ sqrt(50);
Inizializza il vettore di stato di insfilterMARG
insfilterMARG traccia gli stati di posa in un vettore di 22 elementi. Gli stati sono:
State Units State Vector Index Orientation as a quaternion 1:4 Position (NED) m 5:7 Velocity (NED) m/s 8:10 Delta Angle Bias (XYZ) rad 11:13 Delta Velocity Bias (XYZ) m/s 14:16 Geomagnetic Field Vector (NED) uT 17:19 Magnetometer Bias (XYZ) uT 20:22
La verità di base viene utilizzata per aiutare a inizializzare gli stati del filtro, in modo che il filtro converga rapidamente verso buone risposte.
% Initialize the states of the filter
initstate = zeros(22,1);
initstate(1:4) = compact( meanrot(trajOrient(1:100)));
initstate(5:7) = mean( trajPos(1:100,:), 1);
initstate(8:10) = mean( trajVel(1:100,:), 1);
initstate(11:13) = imu.Gyroscope.ConstantBias./imuFs;
initstate(14:16) = imu.Accelerometer.ConstantBias./imuFs;
initstate(17:19) = imu.MagneticField;
initstate(20:22) = imu.Magnetometer.ConstantBias;
fusionfilt.State = initstate;Inizializza le varianze di insfilterMARG
I rumori di misurazione insfilterMARG descrivono la quantità di rumore che altera la lettura del sensore. Questi valori si basano sui parametri imuSensor e gpsSensor.
I rumori di processo descrivono quanto bene le equazioni del filtro descrivono l'evoluzione dello stato. I rumori di processo vengono determinati empiricamente utilizzando lo sweep dei parametri per ottimizzare congiuntamente le stime di posizione e orientamento dal filtro.
% Measurement noises Rmag = 0.0862; % Magnetometer measurement noise Rvel = 0.0051; % GPS Velocity measurement noise Rpos = 5.169; % GPS Position measurement noise % Process noises fusionfilt.AccelerometerBiasNoise = 0.010716; fusionfilt.AccelerometerNoise = 9.7785; fusionfilt.GyroscopeBiasNoise = 1.3436e-14; fusionfilt.GyroscopeNoise = 0.00016528; fusionfilt.MagnetometerBiasNoise = 2.189e-11; fusionfilt.GeomagneticVectorNoise = 7.67e-13; % Initial error covariance fusionfilt.StateCovariance = 1e-9*eye(22);
Inizializza gli ambiti
L'ambito HelperScrollingPlotter consente di tracciare grafici delle variabili nel tempo. Viene utilizzato qui per tracciare gli errori nella posa. L'ambito HelperPoseViewer consente la visualizzazione 3D della stima del filtro e della posa della verità di base. Gli ambiti possono rallentare la simulazione. Per disabilitare un ambito, impostare la variabile logica corrispondente su false .
useErrScope = true; % Turn on the streaming error plot usePoseView = true; % Turn on the 3-D pose viewer if useErrScope errscope = HelperScrollingPlotter(... 'NumInputs', 4, ... 'TimeSpan', 10, ... 'SampleRate', imuFs, ... 'YLabel', {'degrees', ... 'meters', ... 'meters', ... 'meters'}, ... 'Title', {'Quaternion Distance', ... 'Position X Error', ... 'Position Y Error', ... 'Position Z Error'}, ... 'YLimits', ... [ -3, 3 -2, 2 -2 2 -2 2]); end if usePoseView posescope = HelperPoseViewer(... 'XPositionLimits', [-15 15], ... 'YPositionLimits', [-15, 15], ... 'ZPositionLimits', [-10 10]); end
Ciclo di simulazione
Il ciclo di simulazione principale è un ciclo while con un ciclo for annidato. Il ciclo while viene eseguito a gpsFs, che è la frequenza di campionamento del GPS. Il ciclo for annidato viene eseguito a imuFs, che è la frequenza di campionamento dell'IMU. Gli ambiti vengono aggiornati alla frequenza di campionamento dell'IMU.
% Loop setup - |trajData| has about 142 seconds of recorded data. secondsToSimulate = 50; % simulate about 50 seconds numsamples = secondsToSimulate*imuFs; loopBound = floor(numsamples); loopBound = floor(loopBound/imuFs)*imuFs; % ensure enough IMU Samples % Log data for final metric computation. pqorient = quaternion.zeros(loopBound,1); pqpos = zeros(loopBound,3); fcnt = 1; while(fcnt <=loopBound) % |predict| loop at IMU update frequency. for ff=1:imuSamplesPerGPS % Simulate the IMU data from the current pose. [accel, gyro, mag] = imu(trajAcc(fcnt,:), trajAngVel(fcnt, :), ... trajOrient(fcnt)); % Use the |predict| method to estimate the filter state based % on the simulated accelerometer and gyroscope signals. predict(fusionfilt, accel, gyro); % Acquire the current estimate of the filter states. [fusedPos, fusedOrient] = pose(fusionfilt); % Save the position and orientation for post processing. pqorient(fcnt) = fusedOrient; pqpos(fcnt,:) = fusedPos; % Compute the errors and plot. if useErrScope orientErr = rad2deg(dist(fusedOrient, ... trajOrient(fcnt) )); posErr = fusedPos - trajPos(fcnt,:); errscope(orientErr, posErr(1), posErr(2), posErr(3)); end % Update the pose viewer. if usePoseView posescope(pqpos(fcnt,:), pqorient(fcnt), trajPos(fcnt,:), ... trajOrient(fcnt,:) ); end fcnt = fcnt + 1; end % This next step happens at the GPS sample rate. % Simulate the GPS output based on the current pose. [lla, gpsvel] = gps( trajPos(fcnt,:), trajVel(fcnt,:) ); % Correct the filter states based on the GPS data and magnetic % field measurements. fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel); fusemag(fusionfilt, mag, Rmag); end
Calcolo della metrica di errore
Le stime di posizione e orientamento sono state registrate durante tutta la simulazione. Calcola ora l'errore quadratico medio end-to-end sia per la posizione che per l'orientamento.
posd = pqpos(1:loopBound,:) - trajPos( 1:loopBound, :); % For orientation, quaternion distance is a much better alternative to % subtracting Euler angles, which have discontinuities. The quaternion % distance can be computed with the |dist| function, which gives the % angular difference in orientation in radians. Convert to degrees % for display in the command window. quatd = rad2deg(dist(pqorient(1:loopBound), trajOrient(1:loopBound)) ); % Display RMS errors in the command window. fprintf('\n\nEnd-to-End Simulation Position RMS Error\n');
End-to-End Simulation 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: 0.50 , Y: 0.79, Z: 0.65 (meters)
fprintf('End-to-End Quaternion Distance RMS Error (degrees) \n');End-to-End Quaternion Distance RMS Error (degrees)
fprintf('\t%.2f (degrees)\n\n', sqrt(mean(quatd.^2)));1.45 (degrees)
