Stima della posizione e dell'orientamento di un veicolo terrestre
Questo esempio mostra come stimare la posizione e l'orientamento dei veicoli terrestri combinando i dati provenienti da un'unità di misura inerziale (IMU) e da un ricevitore del sistema di posizionamento globale (GPS).
Impostazione della simulazione
Imposta le frequenze di campionamento. In un sistema tipico, l'accelerometro e il giroscopio nell'IMU 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 funziona a una frequenza di campionamento relativamente bassa e la complessità associata alla sua elaborazione è elevata. In questo algoritmo di fusione, i campioni GPS vengono elaborati a bassa velocità, mentre i campioni dell'accelerometro e del giroscopio vengono elaborati insieme alla stessa velocità elevata.
Per simulare questa configurazione, l'IMU (accelerometro e giroscopio) viene campionato a 100 Hz e il GPS viene campionato a 10 Hz.
imuFs = 100; gpsFs = 10; % Define where on the Earth this simulation takes place using latitude, % longitude, and altitude (LLA) coordinates. localOrigin = [42.2825 -71.343 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 posizione, la velocità e le distorsioni del sensore.
L'oggetto insfilterNonholonomic ha due metodi principali: predict 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 in avanti di un passo temporale basandosi sull'accelerometro e sul giroscopio. In questo passaggio viene aggiornata la covarianza dell'errore del filtro di Kalman esteso.
Il metodo fusegps accetta i campioni GPS come input. Questo metodo aggiorna gli stati del filtro in base al campione GPS calcolando un guadagno di Kalman che pondera i vari input del sensore in base alla loro incertezza. In questa fase viene aggiornata anche la covarianza dell'errore, questa volta utilizzando anche il guadagno di Kalman.
L'oggetto insfilterNonholonomic ha due proprietà principali: IMUSampleRate e DecimationFactor. Il veicolo terrestre ha due vincoli di velocità che presuppongono che non rimbalzi o scivoli dal terreno. Questi vincoli vengono applicati utilizzando le equazioni di aggiornamento del filtro di Kalman esteso. Questi aggiornamenti vengono applicati agli stati del filtro a una velocità di IMUSampleRate/DecimationFactor Hz.
gndFusion = insfilterNonholonomic('ReferenceFrame', 'ENU', ... 'IMUSampleRate', imuFs, ... 'ReferenceLocation', localOrigin, ... 'DecimationFactor', 2);
Crea la traiettoria del veicolo terrestre
L'oggetto waypointTrajectory calcola la posa in base alla frequenza di campionamento specificata, ai waypoint, agli orari di arrivo e all'orientamento. Specificare i parametri di una traiettoria circolare per il veicolo terrestre.
% Trajectory parameters r = 8.42; % (m) speed = 2.50; % (m/s) center = [0, 0]; % (m) initialYaw = 90; % (degrees) numRevs = 2; % Define angles theta and corresponding times of arrival t. revTime = 2*pi*r / speed; theta = (0:pi/2:2*pi*numRevs).'; t = linspace(0, revTime*numRevs, numel(theta)).'; % Define position. x = r .* cos(theta) + center(1); y = r .* sin(theta) + center(2); z = zeros(size(x)); position = [x, y, z]; % Define orientation. yaw = theta + deg2rad(initialYaw); yaw = mod(yaw, 2*pi); pitch = zeros(size(yaw)); roll = zeros(size(yaw)); orientation = quaternion([yaw, pitch, roll], 'euler', ... 'ZYX', 'frame'); % Generate trajectory. groundTruth = waypointTrajectory('SampleRate', imuFs, ... 'Waypoints', position, ... 'TimeOfArrival', t, ... 'Orientation', orientation); % Initialize the random number generator used to simulate sensor noise. rng('default');
Ricevitore 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, 'ReferenceFrame', 'ENU'); gps.ReferenceLocation = localOrigin; gps.DecayFactor = 0.5; % Random walk noise parameter gps.HorizontalPositionAccuracy = 1.0; gps.VerticalPositionAccuracy = 1.0; gps.VelocityAccuracy = 0.1;
Sensori IMU
In genere, i veicoli terrestri utilizzano un sensore IMU a 6 assi per la stima della posa. Per modellare un sensore IMU, definire un modello di sensore IMU contenente un accelerometro e un giroscopio. In un'applicazione pratica, i due sensori potrebbero provenire da un unico circuito integrato o da circuiti separati. I valori delle proprietà qui impostati sono tipici dei sensori MEMS a basso costo.
imu = imuSensor('accel-gyro', ... 'ReferenceFrame', 'ENU', 'SampleRate', imuFs); % Accelerometer imu.Accelerometer.MeasurementRange = 19.6133; imu.Accelerometer.Resolution = 0.0023928; imu.Accelerometer.NoiseDensity = 0.0012356; % Gyroscope imu.Gyroscope.MeasurementRange = deg2rad(250); imu.Gyroscope.Resolution = deg2rad(0.0625); imu.Gyroscope.NoiseDensity = deg2rad(0.025);
Inizializza gli stati di insfilterNonholonomic
Gli stati sono:
States Units Index Orientation (quaternion parts) 1:4 Gyroscope Bias (XYZ) rad/s 5:7 Position (NED) m 8:10 Velocity (NED) m/s 11:13 Accelerometer Bias (XYZ) m/s^2 14:16
La verità di base viene utilizzata per aiutare a inizializzare gli stati del filtro, in modo che il filtro converga rapidamente verso buone risposte.
% Get the initial ground truth pose from the first sample of the trajectory % and release the ground truth trajectory to ensure the first sample is not % skipped during simulation. [initialPos, initialAtt, initialVel] = groundTruth(); reset(groundTruth); % Initialize the states of the filter gndFusion.State(1:4) = compact(initialAtt).'; gndFusion.State(5:7) = imu.Gyroscope.ConstantBias; gndFusion.State(8:10) = initialPos.'; gndFusion.State(11:13) = initialVel.'; gndFusion.State(14:16) = imu.Accelerometer.ConstantBias;
Inizializza le varianze di insfilterNonholonomic
I rumori di misurazione descrivono quanto rumore altera la lettura GPS in base ai parametri gpsSensor e quanta incertezza c'è nel modello dinamico del veicolo.
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 Rvel = gps.VelocityAccuracy.^2; Rpos = gps.HorizontalPositionAccuracy.^2; % The dynamic model of the ground vehicle for this filter assumes there is % no side slip or skid during movement. This means that the velocity is % constrained to only the forward body axis. The other two velocity axis % readings are corrected with a zero measurement weighted by the % |ZeroVelocityConstraintNoise| parameter. gndFusion.ZeroVelocityConstraintNoise = 1e-2; % Process noises gndFusion.GyroscopeNoise = 4e-6; gndFusion.GyroscopeBiasNoise = 4e-14; gndFusion.AccelerometerNoise = 4.8e-2; gndFusion.AccelerometerBiasNoise = 4e-14; % Initial error covariance gndFusion.StateCovariance = 1e-9*eye(16);
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 3D 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', ... [-1, 1 -1, 1 -1, 1 -1, 1]); end if usePoseView viewer = HelperPoseViewer( ... 'XPositionLimits', [-15, 15], ... 'YPositionLimits', [-15, 15], ... 'ZPositionLimits', [-5, 5], ... 'ReferenceFrame', 'ENU'); 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 misurazione 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.
totalSimTime = 30; % seconds % Log data for final metric computation. numGPSSamples = floor(min(t(end), totalSimTime) * gpsFs); numSamples = numGPSSamples*imuSamplesPerGPS; truePosition = zeros(numSamples,3); trueOrientation = quaternion.zeros(numSamples,1); estPosition = zeros(numSamples,3); estOrientation = quaternion.zeros(numSamples,1); idx = 0; for sampleIdx = 1:numGPSSamples % Predict loop at IMU update frequency. for i = 1:imuSamplesPerGPS if ~isDone(groundTruth) idx = idx + 1; % Simulate the IMU data from the current pose. [truePosition(idx,:), trueOrientation(idx,:), ... trueVel, trueAcc, trueAngVel] = groundTruth(); [accelData, gyroData] = imu(trueAcc, trueAngVel, ... trueOrientation(idx,:)); % Use the predict method to estimate the filter state based % on the accelData and gyroData arrays. predict(gndFusion, accelData, gyroData); % Log the estimated orientation and position. [estPosition(idx,:), estOrientation(idx,:)] = pose(gndFusion); % Compute the errors and plot. if useErrScope orientErr = rad2deg( ... dist(estOrientation(idx,:), trueOrientation(idx,:))); posErr = estPosition(idx,:) - truePosition(idx,:); errscope(orientErr, posErr(1), posErr(2), posErr(3)); end % Update the pose viewer. if usePoseView viewer(estPosition(idx,:), estOrientation(idx,:), ... truePosition(idx,:), trueOrientation(idx,:)); end end end if ~isDone(groundTruth) % This next step happens at the GPS sample rate. % Simulate the GPS output based on the current pose. [lla, gpsVel] = gps(truePosition(idx,:), trueVel); % Update the filter states based on the GPS data. fusegps(gndFusion, lla, Rpos, gpsVel, Rvel); end end
Calcolo della metrica di errore
La posizione e l'orientamento sono stati registrati durante tutta la simulazione. Calcola ora l'errore quadratico medio end-to-end sia per la posizione che per l'orientamento.
posd = estPosition - truePosition; % 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(estOrientation, trueOrientation)); % 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: 1.16 , Y: 0.98, Z: 0.03 (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)));0.11 (degrees)
