Estimación de la orientación mediante mediciones de orientación derivadas del GPS
Este ejemplo muestra cómo definir y utilizar un modelo de sensor personalizado para el objeto insEKF
junto con los modelos de sensor integrados. Utilizando un sensor de ángulo de guiñada personalizado, un acelerómetro y un giroscopio, este ejemplo utiliza el objeto insEKF
para determinar la orientación de un vehículo. Utiliza la velocidad de un receptor GPS para calcular la orientación del vehículo. Siguiendo un enfoque similar al que se muestra en este ejemplo, puede desarrollar modelos de sensores personalizados para sus propias aplicaciones de fusión de sensores.
Este ejemplo requiere la Sensor Fusion and Tracking Toolbox o la Navigation Toolbox. Este ejemplo también utiliza opcionalmente MATLAB Coder para acelerar el ajuste del filtro.
Descripción del problema
Está intentando estimar la orientación de un vehículo mientras está en movimiento. Los únicos sensores disponibles son un acelerómetro, un giroscopio y un receptor GPS que genera una estimación de velocidad. No se puede utilizar un magnetómetro porque hay una gran cantidad de interferencia magnética en el vehículo.
Acercarse
Hay varias herramientas disponibles en la toolbox para determinar la orientación, incluidas
ecompass
imufilter
ahrsfilter
complementaryFilter
Sin embargo, estos filtros requieren alguna combinación de un acelerómetro, un giroscopio y/o un magnetómetro. Si necesita determinar la orientación absoluta (en relación con el norte verdadero) y no tiene un magnetómetro disponible, ninguno de estos filtros es ideal. Los objetos imufilter y complementarioFilter fusionan datos de acelerómetro y giroscopio, pero solo proporcionan una orientación relativa. Además, si bien estos filtros pueden estimar y compensar la polarización del giroscopio, no hay ningún sensor adicional para ayudar a rastrear la polarización del giroscopio para el ángulo de guiñada, lo que puede producir una estimación subóptima. Normalmente, se utiliza un magnetómetro para lograr esto. Sin embargo, como se mencionó, los magnetómetros no se pueden utilizar en situaciones con perturbaciones magnéticas grandes y muy variables. (Tenga en cuenta que el objeto ahrsfilter
puede soportar perturbaciones magnéticas leves durante un corto período de tiempo).
En este ejemplo, para obtener la orientación absoluta, utiliza la estimación de velocidad del GPS para determinar el ángulo de guiñada. Esta estimación del ángulo de guiñada puede tener el mismo propósito que un magnetómetro sin problemas de perturbaciones magnéticas. Construirá un sensor personalizado en el marco insEKF
para modelar este sensor sin procesar basado en GPS.
Trayectoria
Primero, crea una trayectoria ground-truth y simula el sensor usando las funciones exampleHelperMakeTrajectory
y exampleHelperMakeIMUGPSData
adjuntas a este ejemplo.
groundTruth = exampleHelperMakeTrajectory; originalSensorData = exampleHelperMakeIMUGPSData(groundTruth);
Reajustados como un calendario, los datos del sensor original incluyen el acelerómetro, el giroscopio y los datos de velocidad del GPS. Transforme los datos de velocidad del GPS en una estimación del ángulo de guiñada.
velyaw = atan2(originalSensorData.GPSVelocity(:,2), originalSensorData.GPSVelocity(:,1));
Los resultados anteriores suponen restricciones no holonómicas. Es decir, suponen que el vehículo apunta en la dirección del movimiento. Esta suposición es generalmente cierta para algunos vehículos, como un automóvil, pero no para otros vehículos, como un cuadricóptero.
Crear datos del sensor de ángulo de guiñada sintético
Es difícil trabajar con el ángulo de guiñada porque el ángulo de guiñada se ajusta a y . El ángulo que salta en el límite de envoltura podría causar divergencia del filtro de Kalman extendido. Para evitar este problema, convierta el ángulo de guiñada en un cuaternión.
velyaw(:,2) = 0; % set pitch and roll to 0 velyaw(:,3) = 0; qyaw = quaternion(velyaw, 'euler', 'ZYX', 'frame');
Una convención común es forzar que el cuaternión tenga una parte real positiva.
isNegQuat = parts(qyaw) < 0; % Find quaternions with a non-negative real part qyaw(isNegQuat) = -qyaw(isNegQuat); % Invert the negative quaternions.
Tenga en cuenta que cuando el objeto insEKF rastrea un estado Orientation
de 4 elementos como en este ejemplo, asume que el estado Orientation
es un cuaternión y obliga a que el cuaternión se normalice y tener una parte real positiva. Puede desactivar esta regla llamando al estado de otra manera, como "Actitud".
Ahora puede crear el cronograma de los datos del sensor que se fusionarán.
sensorData = removevars(originalSensorData, 'GPSVelocity'); sensorData = addvars(sensorData,compact(qyaw),'NewVariableNames','YawSensor');
Cree un sensor insEKF
para fusionar el ángulo de guiñada
Para fusionar el cuaternión del ángulo de guiñada, personaliza un modelo de sensor y lo utiliza junto con los insAccelerometer
y insGyroscope
en el objeto insEKF
. . Para personalizar el modelo del sensor, crea una clase que hereda de la clase de interfaz positioning.INSSensorModel
e implementa el método de medición. Nombra la clase exampleHelperYawSensor
classdef exampleHelperYawSensor < positioning.INSSensorModel %EXAMPLEHELPERYAWSENSOR Yaw angle as quaternion sensor % This class is for internal use only. It may be removed in the future. % Copyright 2021 The MathWorks, Inc. methods function z = measurement(~, filt) % Return an estimate of the measurement based on the % state vector of the filter saved in filt.State. % % The measurement is just the quaternion converted from the yaw % of the orientation estimate, assuming roll=0 and pitch=0. q = quaternion(stateparts(filt, "Orientation")); eul = euler(q, 'ZYX', 'frame'); yawquat = quaternion([eul(1) 0 0], 'euler', 'ZYX', 'frame'); % Enforce a positive quaternion convention if parts(yawquat) < 0 yawquat = -yawquat; end % Return a compacted quaternion z = compact(yawquat); end end end
Ahora use un objeto exampleHelperYawSensor
junto con un objeto insAccelerometer
y un objeto insGyroscope
para construir el objeto insEKF
.
opts = insOptions(SensorNamesSource='property', SensorNames={'Accelerometer', 'Gyroscope', 'YawSensor'}); filt = insEKF(insAccelerometer, insGyroscope, exampleHelperYawSensor, insMotionOrientation, opts);
Inicialice el filtro utilizando las funciones de objeto stateparts
y statecovparts
.
stateparts(filt, 'Orientation', sensorData.YawSensor(1,:)); statecovparts(filt,'Accelerometer_Bias', 1e-3); statecovparts(filt,'Gyroscope_Bias', 1e-3);
Ajuste de filtro
Puede utilizar la función de objeto tune
para encontrar los parámetros de ruido óptimos para el objeto insEKF
. Puede llamar directamente a la función de objeto tune
, o puede usar una función de coste acelerada por MEX con MATLAB Coder.
% Trim ground truth to just contain the Orientation for tuning. trimmedGroundTruth = timetable(groundTruth.Orientation, ... SampleRate=groundTruth.Properties.SampleRate, ... VariableNames={'Orientation'}); % Use MATLAB Coder to accelerate tuning by MEXing the cost function. % To run the MATLAB Coder accelerated path, prior to running the example, % type: % exampleHelperUseAcceleratedTuning(true); % To avoid using MATLAB Coder, prior to the example, type: % exampleHelperUseAcceleratedTuning(false); % By default, the example will not tune the filter live and will not use % MATLAB Coder. % Select the accelerated tuning option. acceleratedTuning = exampleHelperUseAcceleratedTuning(); if acceleratedTuning createTunerCostTemplate(filt); % A new cost function in the editor % Save and close the file doc = matlab.desktop.editor.getActive; doc.saveAs(fullfile(pwd, 'tunercost.m')); doc.close; % Find the first parameter for codegen p = tunerCostFcnParam(filt); %#ok<NASGU> % Now generate a mex file codegen tunercost.m -args {p, sensorData, trimmedGroundTruth}; % Use the Custom Cost Function and run the tuner for 20 iterations tunerIters = 20; cfg = tunerconfig(filt, MaxIterations=tunerIters, ... Cost='custom', CustomCostFcn=@tunercost_mex, ... StepForward=1.5, ... ObjectiveLimit=0.0001, ... FunctionTolerance=1e-6, ... Display='none'); mnoise = tunernoise(filt); tunedmn = tune(filt, mnoise, sensorData, trimmedGroundTruth, cfg); else % Use optimal parameters obtained separately. tunedmn = struct(... 'AccelerometerNoise', 0.7786515625000000, ... 'GyroscopeNoise', 167.8674323338237, ... 'YawSensorNoise', 1.003122320344434); adp = diag([... 1.265000000000000; 1.181989791398048; 0.735171900658607; 0.765000000000000; 0.026248409763699; 0.154586330266264; 31.823154516336434; 0.000546245218270; 5.517012554348883; 0.809085937500000; 0.139035477206961; 41.340145917279159; 0.592875000000000]); filt.AdditiveProcessNoise = adp; end
Fusionar datos y compararlos con Ground Truth
Fusione por lotes los datos con la función de objeto estimateStates
.
est = estimateStates(filt, sensorData, tunedmn)
est=13500×5 timetable
Time Orientation AngularVelocity Accelerometer_Bias Gyroscope_Bias StateCovariance
________ ______________ ______________________________________ _________________________________________ ______________________________________ _______________
0 sec 1x1 quaternion 0.00029964 0.0003257 0.00033221 3.1017e-08 -3.4836e-24 -1.2416e-09 2.9964e-07 3.257e-07 3.3221e-07 {13x13 double}
0.01 sec 1x1 quaternion 0.00058811 0.0010514 0.00077335 -1.7319e-10 -1.9136e-05 1.5274e-06 1.0727e-06 0.00013317 2.6591e-06 {13x13 double}
0.02 sec 1x1 quaternion 0.0010153 0.0011293 0.0012812 1.1835e-08 5.0402e-05 4.4661e-06 2.2303e-06 0.0003694 6.6701e-06 {13x13 double}
0.03 sec 1x1 quaternion 0.0013953 0.0014522 0.0019019 1.0275e-08 9.5573e-05 2.0662e-06 3.7974e-06 0.00073969 1.2675e-05 {13x13 double}
0.04 sec 1x1 quaternion 0.0017109 0.0015025 0.0025803 2.3834e-08 0.00010594 6.3287e-06 5.7708e-06 0.0012283 2.0107e-05 {13x13 double}
0.05 sec 1x1 quaternion 0.00205 0.0019622 0.0033511 1.4978e-08 0.00012105 5.2769e-06 8.2924e-06 0.0018652 2.9309e-05 {13x13 double}
0.06 sec 1x1 quaternion 0.0020261 0.0023534 0.0041042 8.8652e-09 -6.6667e-05 1.5446e-06 1.1182e-05 0.0025908 3.8877e-05 {13x13 double}
0.07 sec 1x1 quaternion 0.0026568 0.0026599 0.0050163 8.3994e-09 0.00012693 4.528e-06 1.4392e-05 0.0034661 5.0998e-05 {13x13 double}
0.08 sec 1x1 quaternion 0.0027766 0.0029837 0.0058779 2.101e-09 3.3626e-05 1.1473e-05 1.7889e-05 0.0042033 6.2921e-05 {13x13 double}
0.09 sec 1x1 quaternion 0.0032068 0.0030261 0.0068784 1.3912e-08 0.00011826 1.7097e-05 2.1816e-05 0.0051831 7.7173e-05 {13x13 double}
0.1 sec 1x1 quaternion 0.0031981 0.0032051 0.007876 1.643e-08 -7.5163e-05 1.2083e-05 2.6806e-05 0.0061659 9.1711e-05 {13x13 double}
0.11 sec 1x1 quaternion 0.0036603 0.0032823 0.008879 2.4021e-08 3.2796e-05 1.2375e-05 3.1437e-05 0.00716 0.00010671 {13x13 double}
0.12 sec 1x1 quaternion 0.003734 0.0034359 0.0099138 2.7443e-08 -9.4583e-05 8.6765e-06 3.6787e-05 0.0083283 0.0001224 {13x13 double}
0.13 sec 1x1 quaternion 0.0039583 0.0036587 0.011055 2.6296e-08 -0.00012251 3.6091e-06 4.2167e-05 0.0095194 0.00014002 {13x13 double}
0.14 sec 1x1 quaternion 0.0040944 0.0040022 0.012244 1.7306e-08 -0.0002173 2.531e-06 4.8518e-05 0.01074 0.00015864 {13x13 double}
0.15 sec 1x1 quaternion 0.0044477 0.0040462 0.013476 2.4033e-08 -0.00015984 8.1767e-06 5.4321e-05 0.011837 0.00017817 {13x13 double}
⋮
Compare la orientación estimada con la orientación ground-truth. Trazar la distancia del cuaternión entre las orientaciones estimada y verdadera.
% Convert compacted quaternions to regular quaternions estOrient = quaternion(est.Orientation); d = rad2deg(dist(estOrient, groundTruth.Orientation)); plot(groundTruth.Time, d); xlabel('Time (s)') ylabel('Orientation Error (deg)'); title('Estimated Orientation Error');
snapnow;
A partir de los resultados, el error de orientación estimado es bajo en general. Hay un pequeño aumento en el error alrededor de los 100 segundos, lo que probablemente se debe a una ligera inflexión en el ángulo de guiñada ground-truth en ese momento. Pero en general, este es un buen resultado de estimación de orientación para muchas aplicaciones. Puedes visualizar la orientación estimada usando la función poseplot
.
p = poseplot(estOrient(1)); for ii=2:numel(estOrient) p.Orientation = estOrient(ii); drawnow limitrate; end
Conclusión
En este ejemplo, aprendió cómo personalizar un sensor y agregarlo al marco insEKF
. Los sensores personalizados se pueden integrar con los sensores integrados como insAccelerometer
y insGyroscope
. También aprendió a utilizar la función de ajuste de objetos para encontrar parámetros de ruido óptimos y mejorar los resultados del filtrado.