Position und Ausrichtung eines Bodenfahrzeugs abschätzen
Dieses Beispiel zeigt, wie man die Position und Orientierung von Bodenfahrzeugen durch die Fusion von Daten einer Inertialmesseinheit (IMU) und eines globalen Positionierungssystems (GPS) schätzen kann.
Simulationseinstellungen
Die Abtastraten einstellen. In einem typischen System arbeiten der Beschleunigungsmesser und das Gyroskop in der IMU mit relativ hohen Abtastraten. Die Komplexität der Datenverarbeitung von diesen Sensoren im Fusionsalgorithmus ist relativ gering. Umgekehrt arbeitet das GPS mit einer relativ niedrigen Abtastrate und die damit verbundene Komplexität der Datenverarbeitung ist hoch. Bei diesem Fusionsalgorithmus werden die GPS-Messwerte mit einer niedrigen Rate verarbeitet, während die Beschleunigungsmesser- und Gyroskopmesswerte gleichzeitig mit der gleichen hohen Rate verarbeitet werden.
Um diese Konfiguration zu simulieren, wird die IMU (Beschleunigungsmesser und Gyroskop) mit 100 Hz abgetastet, das GPS hingegen mit 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.');
Fusionsfilter
Erstelle den Filter zur Fusion von IMU- und GPS-Messungen. Der Fusionsfilter verwendet einen erweiterten Kalman-Filter, um Orientierung (als Quaternion), Position, Geschwindigkeit und Sensorabweichungen zu verfolgen.
Das insfilterNonholonomic-Objekt verfügt über zwei Hauptmethoden: predict und fusegps. Die predict-Methode verwendet die Beschleunigungsmesser- und Gyroskop-Messwerte der IMU als Eingabe. Rufen Sie die Methode predict jedes Mal auf, wenn Beschleunigungsmesser und Gyroskop abgetastet werden. Diese Methode sagt die Zustände für einen Zeitschritt in die Zukunft auf Basis des Beschleunigungsmessers und des Gyroskops voraus. In diesem Schritt wird die Fehlerkovarianz des erweiterten Kalman-Filters aktualisiert.
Die fusegps-Methode verwendet die GPS-Messwerte als Eingabe. Bei dieser Methode werden die Filterzustände anhand der GPS-Abtastung aktualisiert, indem eine Kalman-Verstärkung berechnet wird, die die verschiedenen Sensoreingaben entsprechend ihrer Unsicherheit gewichtet. In diesem Schritt wird auch die Fehlerkovarianz aktualisiert, diesmal ebenfalls unter Verwendung des Kalman-Verstärkungsfaktors.
Das insfilterNonholonomic-Objekt hat zwei Haupteigenschaften: IMUSampleRate und DecimationFactor. Für das Bodenfahrzeug gelten zwei Geschwindigkeitsbeschränkungen, die davon ausgehen, dass es nicht vom Boden abprallt oder auf dem Boden rutscht. Diese Nebenbedingungen werden mithilfe der erweiterten Kalman-Filter-Aktualisierungsgleichungen angewendet. Diese Aktualisierungen werden mit einer Rate von IMUSampleRate/DecimationFactor Hz auf die Filterzustände angewendet.
gndFusion = insfilterNonholonomic('ReferenceFrame', 'ENU', ... 'IMUSampleRate', imuFs, ... 'ReferenceLocation', localOrigin, ... 'DecimationFactor', 2);
Bodenfahrzeugtrajektorie erstellen
Das waypointTrajectory-Objekt berechnet die Pose anhand der angegebenen Abtastrate, Wegpunkte, Ankunftszeiten und Orientierung. Legen Sie die Parameter einer Kreisbahn für das Bodenfahrzeug fest.
% 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');
GPS-Empfänger
Stellen Sie das GPS auf die angegebene Abtastrate und den Referenzstandort ein. Die übrigen Parameter steuern die Art des Rauschens im Ausgangssignal.
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;
IMU-Sensoren
Typischerweise verwenden Bodenfahrzeuge einen 6-Achsen-IMU-Sensor zur Lagebestimmung. Um einen IMU-Sensor zu modellieren, definieren Sie ein IMU-Sensormodell, das einen Beschleunigungsmesser und ein Gyroskop enthält. In einer realen Anwendung können die beiden Sensoren von einem einzigen integrierten Schaltkreis oder von separaten Schaltkreisen stammen. Die hier angegebenen Eigenschaftswerte sind typisch für kostengünstige MEMS-Sensoren.
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);
Initialisieren der Zustände von insfilterNonholonomic
Die Zustände sind:
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
Die Ground Truth wird verwendet, um die Filterzustände zu initialisieren, damit der Filter schnell zu guten Ergebnissen konvergiert.
% 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;
Initialisieren der Varianzen von insfilterNonholonomic
Die Messrauschen beschreiben, wie stark das GPS-Messergebnis durch Rauschen verfälscht wird, basierend auf den gpsSensor-Parametern, und wie groß die Unsicherheit im Fahrzeugdynamikmodell ist.
Das Prozessrauschen beschreibt, wie gut die Filtergleichungen die Zustandsentwicklung beschreiben. Prozessrauschen wird empirisch ermittelt, indem Parameter-Sweeps durchgeführt werden, um die Positions- und Orientierungsschätzungen des Filters gemeinsam zu optimieren.
% 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);
Bereiche initialisieren
Der HelperScrollingPlotter-Bereich ermöglicht die grafische Darstellung von Variablen im Zeitverlauf. Es wird hier verwendet, um Fehler in der Pose zu erfassen. Der HelperPoseViewer-Bereich ermöglicht die 3D-Visualisierung der Filterschätzung und der tatsächlichen Pose. Die Oszilloskope können die Simulation verlangsamen. Um einen Gültigkeitsbereich zu deaktivieren, setzen Sie die entsprechende logische Variable auf 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
Simulationsschleife
Die Hauptsimulationsschleife ist eine While-Schleife mit einer darin enthaltenen For-Schleife. Die while-Schleife wird mit der Frequenz gpsFs ausgeführt, was der GPS-Messrate entspricht. Die verschachtelte for-Schleife wird mit der Abtastrate imuFs ausgeführt, die der IMU-Abtastrate entspricht. Die Messbereiche werden mit der Abtastrate der IMU aktualisiert.
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
Fehlermetrikberechnung
Position und Orientierung wurden während der gesamten Simulation protokolliert. Berechnen Sie nun den gesamten mittleren quadratischen Fehler für Position und Orientierung.
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)
