IMU- und GPS-Fusion für die Trägheitsnavigation
Dieses Beispiel zeigt, wie Sie einen IMU + GPS-Fusionsalgorithmus erstellen können, der für unbemannte Luftfahrzeuge (UAVs) oder Quadcopter geeignet ist.
In diesem Beispiel werden Beschleunigungsmesser, Gyroskope, Magnetometer und GPS verwendet, um die Ausrichtung und Position eines UAV zu bestimmen.
Simulationsaufbau
Stellen Sie die Abtastraten ein. In einem typischen System laufen der Beschleunigungsmesser und das Gyroskop mit relativ hohen Abtastraten. Die Komplexität der Verarbeitung der Daten dieser Sensoren im Fusionsalgorithmus ist relativ gering. Im Gegensatz dazu arbeiten das GPS und in manchen Fällen auch das Magnetometer mit relativ niedrigen Abtastraten, und die mit ihrer Verarbeitung verbundene Komplexität ist hoch. Bei diesem Fusionsalgorithmus werden die Magnetometer- und GPS-Samples zusammen mit derselben niedrigen Rate und die Beschleunigungsmesser- und Gyroskop-Samples zusammen mit derselben hohen Rate verarbeitet.
Um diese Konfiguration zu simulieren, werden die IMU (Beschleunigungsmesser, Gyroskop und Magnetometer) mit 160 Hz und das GPS mit 1 Hz abgetastet. Da nur eine von 160 Abtastungen des Magnetometers an den Fusionsalgorithmus weitergegeben wird, könnte die Abtastung des Magnetometers in einem realen System mit einer viel geringeren Rate erfolgen.
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.');
Fusion Filter
Erstellen Sie den Filter, um IMU- und GPS-Messungen zu kombinieren. Der Fusionsfilter verwendet einen erweiterten Kalman-Filter, um Ausrichtung (als Quaternion), Geschwindigkeit, Position, Sensorverzerrungen und den geomagnetischen Vektor zu verfolgen.
Dieses insfilterMARG verfügt über einige Methoden zur Verarbeitung von Sensordaten, darunter predict, fusemag und fusegps. Die Methode predict verwendet die Beschleunigungsmesser- und Gyroskopproben der IMU als Eingaben. Rufen Sie die Methode predict jedes Mal auf, wenn der Beschleunigungsmesser und das Gyroskop abgetastet werden. Diese Methode sagt die Zustände auf Grundlage des Beschleunigungsmessers und des Gyroskops einen Zeitschritt im Voraus voraus. Hier wird die Fehlerkovarianz des erweiterten Kalman-Filters aktualisiert.
Die Methode fusegps verwendet GPS-Samples als Eingabe. Diese Methode aktualisiert die Filterzustände basierend auf GPS-Samples, indem sie einen Kalman-Gewinn berechnet, der die verschiedenen Sensoreingaben entsprechend ihrer Unsicherheit gewichtet. Auch hier wird eine Fehlerkovarianz aktualisiert, diesmal ebenfalls unter Verwendung der Kalman-Verstärkung.
Die fusemag-Methode ist ähnlich, aktualisiert jedoch die Zustände, die Kalman-Verstärkung und die Fehlerkovarianz basierend auf den Magnetometerproben.
Obwohl insfilterMARG Beschleunigungsmesser- und Gyroskopproben als Eingaben verwendet, werden diese integriert, um jeweils Deltageschwindigkeiten und Deltawinkel zu berechnen. Der Filter verfolgt die Vorspannung des Magnetometers und diese integrierten Signale.
fusionfilt = insfilterMARG; fusionfilt.IMUSampleRate = imuFs; fusionfilt.ReferenceLocation = refloc;
UAV-Flugbahn
Dieses Beispiel verwendet eine gespeicherte, von einem UAV aufgezeichnete Flugbahn als Grundwahrheit. Diese Flugbahn wird mehreren Sensorsimulatoren zugeführt, um simulierte Datenströme von Beschleunigungsmesser, Gyroskop, Magnetometer und GPS zu berechnen.
% 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)
GPS-Sensor
Richten Sie das GPS auf die angegebene Abtastrate und den angegebenen Referenzort ein. Die anderen Parameter steuern die Art des Rauschens im Ausgangssignal.
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;
IMU-Sensoren
Normalerweise verwendet ein UAV einen integrierten MARG-Sensor (Magnetic, Angular Rate, Gravity) zur Lagebestimmung. Um einen MARG-Sensor zu modellieren, definieren Sie ein IMU-Sensormodell, das einen Beschleunigungsmesser, ein Gyroskop und ein Magnetometer enthält. In einer realen Anwendung könnten die drei Sensoren aus einem einzigen integrierten Schaltkreis oder aus separaten Schaltkreisen stammen. Die hier eingestellten Eigenschaftswerte sind typisch für kostengünstige MEMS-Sensoren.
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);
Initialisieren Sie den Zustandsvektor des insfilterMARG
Das insfilterMARG verfolgt die Pose-Zustände in einem 22-Element-Vektor. Die Zustände sind:
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
Die Ground Truth wird zur Initialisierung der Filterzustände verwendet, sodass der Filter schnell zu guten Antworten konvergiert.
% 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;Initialisieren Sie die Varianzen von insfilterMARG
Die insfilterMARG-Messgeräusche beschreiben, wie viel Rauschen die Sensorwerte verfälscht. Diese Werte basieren auf den Parametern imuSensor und gpsSensor.
Das Prozessrauschen beschreibt, wie gut die Filtergleichungen die Zustandsentwicklung beschreiben. Prozessgeräusche werden empirisch durch Parameter-Sweepen ermittelt, um die Positions- und Orientierungsschätzungen des Filters gemeinsam zu optimieren.
% 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);
Bereiche initialisieren
Der HelperScrollingPlotter-Bereich ermöglicht die Darstellung von Variablen im Zeitverlauf. Es wird hier verwendet, um Posenfehler zu verfolgen. Der HelperPoseViewer-Bereich ermöglicht eine 3D-Visualisierung der Filterschätzung und der Ground-Truth-Pose. Die Zielfernrohre können die Simulation verlangsamen. Um einen Bereich zu deaktivieren, setzen Sie die entsprechende logische Variable auf 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
Simulationsschleife
Die Hauptsimulationsschleife ist eine While-Schleife mit einer verschachtelten For-Schleife. Die While-Schleife wird bei gpsFs ausgeführt, was der GPS-Abtastrate entspricht. Die verschachtelte For-Schleife wird bei imuFs ausgeführt, was der IMU-Abtastrate entspricht. Die Oszilloskope werden mit der IMU-Abtastrate aktualisiert.
% 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
Berechnung der Fehlermetrik
Positions- und Orientierungsschätzungen wurden während der gesamten Simulation protokolliert. Berechnen Sie nun den mittleren quadrierten Fehler von Ende zu Ende für Position und Ausrichtung.
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)
