Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Filtro Design Fusion para sensores personalizados

Este ejemplo presenta cómo personalizar los modelos de sensores utilizados con un objeto insEKF .

Usando el objeto insEKF, puede fusionar datos de medición de múltiples tipos de sensores usando los modelos de sensores INS integrados, incluido el insAccelerometer, insGyrospace, insMagnetomer y insGPS objetos. Aunque estos objetos de sensores cubren una variedad de modelos de sensores INS, es posible que desee fusionar datos de medición de un tipo diferente de sensor. Usando el objeto insEKF a través de un marco extendido, puede definir sus propios modelos de sensores y modelos de movimiento utilizados por el filtro. En este ejemplo, aprenderá a personalizar tres modelos de sensores en unos pocos pasos. Puede aplicar pasos similares para definir un modelo de movimiento.

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

Considere que está intentando estimar la posición de un objeto que se mueve en una dimensión. Para estimar la posición, se utiliza un sensor de velocidad y se fusionan los datos de ese sensor para determinar la posición. Normalmente, la aplicación de este enfoque puede conducir a una estimación deficiente de la posición porque pequeños errores en la estimación de la velocidad pueden integrarse para formar errores más grandes en la estimación de la posición. Como se muestra más adelante en este ejemplo, combinar mediciones de múltiples sensores puede mejorar los resultados.

Para comenzar, defina una trayectoria ground-truth básica simple para el objeto y visualice cómo se mueve.

groundTruth = exampleHelperMakeGroundTruth();
plot(groundTruth.Time, groundTruth.Position);
xlabel("Time (s)");
ylabel("Position (m)");
title("Ground Truth");

snapnow;

Sensor de velocidad sencillo

Considere los datos de un sensor de velocidad simple que mide la velocidad pero está corrompido por un pequeño sesgo y un ruido blanco gaussiano aditivo.

velBias = exampleHelperVelocityWithBias(groundTruth);

Para el enfoque más simple, cree un filtro que trate las mediciones de velocidad simplemente como velocidad más ruido blanco. Es probable que esto no produzca resultados deseables, pero vale la pena probar primero el modelo más simple. Para crear un modelo de sensor de velocidad para el objeto insEKF , debe definir una clase que herede de la clase de interfaz positioning.INSSensorModel . Como mínimo, debe implementar un método measurement que toma el objeto sensor y el objeto de filtro como entradas. El método measurement devuelve un vector z as una estimación de la medición del sensor, basada en variables de estado.

classdef exampleHelperVelocitySensor < positioning.INSSensorModel
%EXAMPLEHELPERVELOCITYSENSOR A simple velocity sensor for insEKF
% This class is for internal use only. It may be removed in the future.

%   Copyright 2021 The MathWorks, Inc.    

    methods 
        function z = measurement(~, filt)
            % The measurement is just the velocity estimate
            z = stateparts(filt, 'Velocity');          
        end
    end
end

Este es el mínimo requerido para definir un sensor que funcione con el insEKF object. Opcionalmente, puede definir la medida jacobiana en el método measurementJacobian , que puede mejorar la velocidad de cálculo y posiblemente mejorar la precisión de la estimación. Sin implementar el método measurementJacobian , estás confiando en el insEKF para calcular numéricamente el jacobiano.

El filtro se construye con un modelo de movimiento de velocidad constante 1-D simple definido usando una clase BasicConstantVelocityMotion . Esta clase es un ejemplo de cómo implementar sus propios modelos de movimiento personalizados heredando de la clase de interfaz positioning.INSMotionModel . La definición de modelos de movimiento personalizados requiere implementar solo un subconjunto de los métodos necesarios para implementar un modelo de sensor personalizado.

Ahora puede crear el filtro, ajustar sus parámetros de ruido y ver cómo funciona. Puede nombrar el sensor de velocidad "VelocityWithBias" usando el objeto insOptions .

basicOpts = insOptions(SensorNamesSource="property", SensorNames={'VelocityWithBias'});
basicfilt = insEKF(exampleHelperVelocitySensor, exampleHelperConstantVelocityMotion, ...
    basicOpts);

El objeto comienza en reposo con una posición cero. Establezca la covarianza de estado para la posición y la velocidad en un valor más bajo antes de realizar el ajuste.

statecovparts(basicfilt, "Position", 1e-2);
statecovparts(basicfilt, "Velocity", 1e-2);

Ajusteel filtro después de especificar una estructura de ruido y un objeto tunerconfig .

mn = tunernoise(basicfilt);
tunerIters = 30;
cfg = tunerconfig(basicfilt, MaxIterations=tunerIters, Display='none');
tunedmn = tune(basicfilt, mn, velBias, groundTruth, cfg);

estBasic = estimateStates(basicfilt, velBias, tunedmn);
figure;
plot(groundTruth.Time, groundTruth.Position, 'g', estBasic.Time, ...
    estBasic.Position, 'k')
title("Basic Velocity Sensor Estimate");
err = sqrt(mean((estBasic.Position - groundTruth.Position).^2));
subtitle("RMS Error = " + err);
legend("Ground Truth", "Basic Sensor Estimate");

snapnow;

Los resultados de la estimación son deficientes como se esperaba, ya que el filtro no tiene en cuenta el sesgo. Cuando el sesgo de velocidad no contabilizado se integra en la posición, la estimación diverge rápidamente de ground-truth.

Sensor de velocidad con polarización

Ahora puedes explorar un modelo de sensor más complejo que tenga en cuenta el sesgo del sensor. Para hacer esto, define un método sensorstates para informar al insEKF que rastree el sesgo del sensor en su vector de estado, guardado en el State propiedad del filtro. Puede consultar el sesgo del sensor con la función de objeto stateparts del filtro y utilizar la estimación de sesgo en la función measurement .

classdef exampleHelperVelocitySensorWithBias < positioning.INSSensorModel
%EXAMPLEHELPERVELOCITYSENSORWITHBIAS Velocity sensor assuming constant bias
% This class is for internal use only. It may be removed in the future.

%   Copyright 2021 The MathWorks, Inc.    

    methods 
        function s = sensorstates(~,~)
            % Assume there is a constant bias corrupting the velocity
            % measurement. Estimate that bias as part of the filter
            % computation.
            s = struct('Bias', 0);
        end        
        function z = measurement(sensor, filt)
            % Measurement is velocity plus bias. Obtain the velocity from
            % the filter state.
            velocity = stateparts(filt, 'Velocity');

            % Obtain the sensor bias from the filter state by directly
            % using the sensor object as an input to the stateparts object
            % function. In this way, knowledge of the SensorName associated
            % with this sensor is not required. See the reference page for
            % stateparts for more details.
            bias = stateparts(filt, sensor, 'Bias');
          
            % Form the measurement
            z = velocity + bias;
        end        
        function dzdx = measurementJacobian(sensor, filt)
            % Compute the Jacobian as the partial derivates of the Bias
            % state relative to all other states.
            N = numel(filt.State);  % Number of states

            % Initialize a matrix of partial derivatives. This matrix has
            % one row because the sensor state is a scalar.
            dzdx = zeros(1,N);  
 
            % The partial derivative of z with respect to the Bias is 1. So
            % put a 1 at the Bias index in the dzdx matrix:
            bidx = stateinfo(filt, sensor, 'Bias');
            dzdx(:,bidx) = 1;

            % The partial derivative of z with respect to the Velocity is
            % also 1. So put a 1 at the Bias index in the dzdx matrix:
            vidx = stateinfo(filt, 'Velocity');
            dzdx(:,vidx) = 1;
        end
    end

end

En lo anterior, implementó el método measurementJacobian en lugar de depender de un jacobiano numérico. La matriz jacobiana contiene las derivadas parciales de la medición del sensor con respecto al estado del filtro, la cual es una matriz M-por- N , donde M es el número de elementos en el vector devuelto por el método measurement y N es el número de estados del filtro. Ahora puedes crear un filtro con el sensor personalizado.

sensorVelWithBias = exampleHelperVelocitySensorWithBias;
velWithBiasFilt = insEKF(sensorVelWithBias, ...
    exampleHelperConstantVelocityMotion, basicOpts);

Puede establecer el estado de polarización inicial del filtro con una estimación de la polarización, basada en los datos recopilados mientras el sensor está estacionario, utilizando la función de objeto stateparts .

stationaryLen = 1e7;
stationary = timetable(zeros(stationaryLen,1), zeros(stationaryLen,1), ...
    TimeStep=groundTruth.Properties.TimeStep, ...
    VariableNames={'Position', 'Velocity'});
biasCalibrationData = exampleHelperVelocityWithBias(stationary);

stateparts(velWithBiasFilt,  sensorVelWithBias, "Bias", ...
    mean(biasCalibrationData.VelocityWithBias));

Nuevamente, establezca la covarianza de estado para estos estados en valores más pequeños antes de realizar el ajuste.

statecovparts(velWithBiasFilt, "Position", 1e-2);
statecovparts(velWithBiasFilt, "Velocity", 1e-2);

Puede ajustar este filtro como antes, pero también puede ajustarlo más rápido usando MATLAB Coder. Si bien la función de objeto tune no admite la generación de código, puede usar una función de coste acelerado por MEX para aumentar considerablemente la velocidad de ajuste. Puede especificar la función tune para usar una función de coste personalizada a través de la entrada tunerconfig . El objeto insEKF tiene funciones de objeto para ayudar a crear una función de coste personalizada. La función de objeto createTunerCostTemplate crea una función de coste, que intenta minimizar el error RMS de las estimaciones de estado, en un nuevo documento en el Editor. Luego puede generar código para esa función con la ayuda de la función de objeto tunerCostFcnParam , que crea un ejemplo del primer argumento de una función de coste.

% Use MATLAB Coder to accelerate tuning by MEXing the cost function.
% To run the MATLAB Coder accelerated path, prior to running the example,
% type:
%   exampleHelperUseCodegenForCost(true);
% To avoid using MATLAB Coder, prior to the example, type:
%   exampleHelperUseCodegenForCost(false);

useCodegenForTuning = exampleHelperUseCodegenForCost();

if useCodegenForTuning
    createTunerCostTemplate(velWithBiasFilt); % A new cost function in the editor
    exampleHelperSaveCostFunction;
    p = tunerCostFcnParam(velWithBiasFilt); 
    % Now generate a mex file
    codegen tunercost.m -args {p, velBias, groundTruth};
    % Use the Custom Cost Function
    cfg2 = tunerconfig(velWithBiasFilt, MaxIterations=tunerIters, ...
        Cost="custom", CustomCostFcn=@tunercost_mex, Display='none');
else
    % Use the default cost function
    cfg2 = tunerconfig(velWithBiasFilt, MaxIterations=tunerIters, ...
        Display='none'); %#ok<*UNRCH> 
end
mn = tunernoise(velWithBiasFilt);
tunedmn = tune(velWithBiasFilt, mn, velBias, groundTruth, cfg2);
estWithBias = estimateStates(velWithBiasFilt, velBias, tunedmn);
figure;
plot(groundTruth.Time, groundTruth.Position, 'k', estWithBias.Time, ...
    estWithBias.Position, 'b')
title("Velocity Sensor Estimate With Bias Tracking");
err = sqrt(mean((estWithBias.Position - groundTruth.Position).^2));
subtitle("RMS Error = " + err);

snapnow;

Según los resultados, la estimación de la posición ha mejorado pero aún no es la ideal. El filtro estima el sesgo, pero cualquier error en la estimación del sesgo se trata como velocidad real, lo que corrompe la estimación de la posición. Un modelo de sensor más sofisticado puede mejorar aún más los resultados.

Sensor de velocidad con ruido Gauss-Markov

Considere un nuevo modelo de sensor de velocidad que está corrompido con ruido Gauss-Markov de primer orden.

velGM = exampleHelperVelocityWithGaussMarkov(groundTruth);
figure
plot(velGM.Time, velGM.VelocityWithGM, 'r', groundTruth.Time, ...
    groundTruth.Velocity, 'k');
title("Velocity Sensor With Gauss-Markov Noise");
xlabel("Time (s)");
ylabel("Velocity (m/s)");
legend("True Velocity", "Velocity With G-M Noise");

snapnow;

El ruido de Gauss-Markov de primer orden se puede modelar en tiempo discreto como:

$${\mathit{x}}_{\mathit{k}}=\left(1-\beta *\mathrm{dt}\right){\mathit{x}}_{\mathit{k}-1}+{\mathit{w}}_{\mathit{k}}$$

donde ${\mathit{w}}_{\mathit{k}}$ es ruido blanco. La formulación de tiempo continuo equivalente es:

$$\frac{\mathrm{d}}{\mathrm{d}\mathit{t}}\mathit{x}=-\beta \mathit{x}\left(\mathit{t}\right)+\mathit{w}\left(\mathit{t}\right)$$

donde $\beta$ es la constante de tiempo del proceso, y w(t) y x(t) son las versiones en tiempo continuo del estado discreto ${\mathit{x}}_{\mathit{k}}$ y ruido de proceso ${\mathit{w}}_{\mathit{k}}$, respectivamente.

Puede comprobar que se trata de un par discreto-continuo válido aplicando la integración de Euler en la segunda ecuación entre el tiempo k y k-1 para obtener la primera ecuación. Consulte la Referencia [1] para obtener una explicación más detallada. Para simplificar, establezca $\beta$ como 0,002 para este ejemplo. Dado que el proceso Gauss-Markov evoluciona con el tiempo, es necesario implementarlo en el método stateTransition de la clase positioning.INSSensorModel . El método stateTransition describe cómo los elementos de estado descritos en sensorstates evolucionan con el tiempo. La función stateTransition genera una estructura con los mismos campos que la estructura de salida del método sensorstates , y cada campo contiene la derivada del estado con respecto al tiempo. Cuando el método stateTransition no se implementa explícitamente, el objeto insEKF asume que el estado es constante en el tiempo.

classdef exampleHelperVelocitySensorWithGM < positioning.INSSensorModel
%EXAMPLEHELPERVELOCITYSENSORWITHGM Velocity sensor with Gauss-Markov noise
% This class is for internal use only. It may be removed in the future.

%   Copyright 2021 The MathWorks, Inc.    

    properties (Constant)
        Beta =0.002; % First-order Gauss-Markov time constant
    end
    methods
        function s = sensorstates(~,~)
            % Assume the velocity measurement is corrupted by a first-order
            % Gauss-Markov random process. Estimate the state of the
            % Gauss-Markov process as part of the filtering.
            s = struct('GMProc', 0);
        end
        function z = measurement(sensor, filt)
            % Measurement of velocity plus Gauss-Markov process noise
            velocity = stateparts(filt, 'Velocity');
            gm = stateparts(filt, sensor, 'GMProc');
          
            z = velocity  + gm;
        end
        function dhdx = measurementJacobian(sensor, filt)
            % Compute the Jacobian of partial derivates of the measurement
            % relative to all states.
            N = numel(filt.State);  % Number of states

            % Initialize a matrix of partial derivatives. The matrix only
            % has one row because the sensor state is a scalar.
            dhdx = zeros(1,N);  

            % Get the indices of the Velocity and GMProc states in the
            % state vector.
            vidx = stateinfo(filt, 'Velocity');
            gmidx = stateinfo(filt, sensor, 'GMProc');
            dhdx(:,gmidx) = 1;
            dhdx(:,vidx) = 1;
        end
        function sdot = stateTransition(sensor, filt, ~, varargin)
            % Define the state transition function for each sensorstates
            % defined in this class. Since the insEKF is a
            % continuous-discrete EKF, the output is the derivative of the
            % state in the form of a structure with field names matching
            % the field names of the output structure of sensorstates. The
            % first-order Gauss-Markov process has a state transition of
            %      sdot = -1*beta*s
            % where beta is the reciprocal of the time constant of the
            % process.
            sdot.GMProc = -1*(sensor.Beta) * ...
                stateparts(filt, sensor, 'GMProc');                
        end
        function dfdx = stateTransitionJacobian(sensor, filt, ~, varargin)
            % Compute the Jacobian of the partial derivates of the GMProc
            % state transition relative to all states.

            % Find the number of states and initialize an array of the same
            % size with all elements equal to zero.
            N = numel(filt.State);
            dfdx.GMProc = zeros(1,N);
         
            % Find the index of the Gauss-Markov state, GMProc, in the
            % state vector.
            gmidx = stateinfo(filt, sensor, 'GMProc');

            % The derivative of the GMProc stateTransition function with
            % respect to GMProc is -1*Beta
            dfdx.GMProc(gmidx) = -1*(sensor.Beta);
        end
    end
end

Tenga en cuenta que aquí también se implementa un método stateTranstionJacobian . El método genera una estructura con sus campos que contienen las derivadas parciales de sensorstates con respecto al vector de estado del filtro. Si esta función no está implementada, el insEKF calcula estas matrices numéricamente. Puede intentar comentar esta función para ver los efectos del uso del jacobiano numérico.

gmOpts = insOptions(SensorNamesSource="property", SensorNames={'VelocityWithGM'});
filtWithGM = insEKF(exampleHelperVelocitySensorWithGM, ...
    exampleHelperConstantVelocityMotion, gmOpts);

Puede volver a ajustar el filtro siguiendo cualquiera de los procesos anteriores. Calcule el estado utilizando la función de objeto estimateStates .

statecovparts(filtWithGM, "Position", 1e-2);
statecovparts(filtWithGM, "Velocity", 1e-2);
gmMeasNoise = tunernoise(filtWithGM);
cfg3 =  tunerconfig(filtWithGM, MaxIterations=tunerIters, Display='none');
gmTunedNoise = tune(filtWithGM, gmMeasNoise, velGM, groundTruth, cfg3);

estGM = estimateStates(filtWithGM, velGM, gmTunedNoise);
figure
plot(estGM.Time, estGM.Position, 'c', groundTruth.Time, ...
    groundTruth.Position, 'k');
title("Velocity Sensor Estimate With Gauss-Markov Noise Tracking");
err = sqrt(mean((estGM.Position - groundTruth.Position).^2));
subtitle("RMS Error = " + err);

snapnow;

Nuevamente el filtro no puede distinguir entre el ruido de Gauss-Markov y la velocidad real. El ruido de Gauss-Markov no es observable de forma independiente y, por tanto, corrompe la estimación de la posición. Sin embargo, puede reutilizar los sensores diseñados anteriormente con datos de medición de múltiples sensores para obtener una mejor estimación de la posición.

Fusión multisensor

Para obtener una estimación precisa de la posición, se utilizan varios sensores. Aplica los modelos de sensores que ya ha diseñado en un nuevo objeto de filtro insEKF .

combinedOpts =  insOptions(SensorNamesSource="property", SensorNames={'VelocityWithBias', 'VelocityWithGM'});
sensorWithBias = exampleHelperVelocitySensorWithBias;
sensorWithGM = exampleHelperVelocitySensorWithGM;
filtCombined = insEKF(sensorWithBias, sensorWithGM, ...
    exampleHelperConstantVelocityMotion, combinedOpts);

% Combine the two sets of sensor measurements.
sensorData = synchronize(velBias, velGM, "first");
% Initialize the bias state with an estimate.
stateparts(filtCombined,  sensorWithBias, "Bias", ...
    mean(biasCalibrationData.VelocityWithBias));

mnCombined = tunernoise(filtCombined);
cfg4 =  tunerconfig(filtCombined, MaxIterations=tunerIters, Display='none');
combinedTunedNoise = tune(filtCombined, mnCombined, sensorData, ...
    groundTruth, cfg4);

estBoth = estimateStates(filtCombined, sensorData, combinedTunedNoise); 
figure;
plot(estBoth.Time, estBoth.Position, 'm', groundTruth.Time, ...
    groundTruth.Position, 'k');
title("Multi-Sensor Estimate");
err = sqrt(mean((estBoth.Position - groundTruth.Position).^2));
subtitle("RMS Error = " + err);

snapnow;

Debido a que el filtro utiliza dos sensores, tanto la polarización como el ruido de Gauss-Markov son observables. Como resultado, el filtro obtiene una estimación de velocidad más precisa y, por tanto, una estimación de posición más precisa.

Conclusión

En este ejemplo, aprendió el marco insEKF y cómo personalizar los modelos de sensores utilizados con el objeto insEKF. Implementar un modelo de movimiento personalizado es casi el mismo proceso que implementar un nuevo sensor, excepto que se hereda de la clase de interfaz positioning.INSMotionModel en lugar de la clase de interfaz positioning.INSSensorModel . Diseñar filtros de fusión de sensores personalizados es sencillo con el marco insEKF y es sencillo construir sensores reutilizables para usar en todos los proyectos.

Referencias

[1] Una comparación entre diferentes modelos de errores de MEMS aplicados a sistemas integrados GPS/INS. A. Quinchia, G. Falco, E. Falletti, F. Dovis, C. Ferrer, Sensores 2013.