Personalice un modelo de sensor utilizado con el objeto insEKF. El sensor mide el estado de la velocidad, incluido un sesgo afectado por el ruido aleatorio.
Personalice el modelo del sensor heredando de la clase de interfaz positioning.INSSensorModel e implementando sus métodos. Tenga en cuenta que solo se requiere el método measurement para la implementación en la clase de interfaz positioning.INSSensorModel. Estas secciones proporcionan una descripción general de cómo la clase BiasSensor implementa los métodos positioning.INSSensorModel, pero para obtener detalles sobre su implementación, consulte los detalles de la implementación en el archivo BiasSensor.m adjunto.
Implementar el método sensorstates
Para modelar el sesgo, el método sensorstates necesita devolver un estado, Bias, como estructura. Cuando agrega un objeto BiasSensor a un objeto de filtro insEKF, el filtro agrega el componente de sesgo al vector de estado del filtro.
Implementar el método measurement
La medición es el componente de velocidad del estado del filtro, incluida la polarización. Por lo tanto, devuelva la suma del componente de velocidad del filtro y la polarización.
Implementar el método measurementJacobian
El método measurementJacobian devuelve la derivada parcial del método measurement con respecto al vector de estado del filtro como una estructura. Todas las derivadas parciales son 0, excepto las derivadas parciales de la medición con respecto a los componentes de velocidad y estado de sesgo.
Implementar el método stateTransition
El método stateTransiton devuelve la derivada del estado del sensor definido en el método sensorstates. Tenga en cuenta que este ejemplo solo muestra cómo configurar el método y no corresponde a ninguna aplicación práctica.
Implementar el método stateTransitionJacobian
Como la función stateTransiton no depende del estado del filtro, la matriz jacobiana es 0.
Crear y agregar objeto heredado
Cree un objeto BiasSensor.
biSensor =
BiasSensor with no properties.
Crea un objeto insEKF con el objeto biSensor.
filter =
insEKF with properties:
State: [17×1 double]
StateCovariance: [17×17 double]
AdditiveProcessNoise: [17×17 double]
MotionModel: [1×1 insMotionPose]
Sensors: {[1×1 BiasSensor]}
SensorNames: {'BiasSensor'}
ReferenceFrame: 'NED'
El estado del filtro contiene el componente de polarización.
ans = struct with fields:
Orientation: [1 2 3 4]
AngularVelocity: [5 6 7]
Position: [8 9 10]
Velocity: [11 12 13]
Acceleration: [14 15 16]
BiasSensor_Bias: 17
Mostrar clase BiasSensor personalizada
classdef BiasSensor < positioning.INSSensorModel
%BIASSENSOR Sensor measuring velocity with bias
% Copyright 2021 The MathWorks, Inc.
methods
function s = sensorstates(~,~)
% Assume the sensor has a bias. Define a Bias state to enable
% the filter to estimate the bias.
s = struct('Bias',0);
end
function z = measurement(sensor,filter)
% Measurement is the summation of the velocity measurement and
% the bias.
velocity = stateparts(filter,'Velocity');
bias = stateparts(filter,sensor,'Bias');
z = velocity + bias;
end
function dzdx = measurementJacobian(sensor,filter)
% Compute the Jacobian, which is the partial derivative of the
% measurement (velocity plus bias) with respect to the filter
% state vector.
% Obtain the dimension of the filter state.
N = numel(filter.State);
% The partial derviative of the Bias with respect to all the
% states is zero, except the Bias state itself.
dzdx = zeros(1,N);
% Obtain the index for the Bias state component in the filter.
bidx = stateinfo(filter,sensor,'Bias');
dzdx(:,bidx) = 1;
% The partial derivative of the Velocity with respect to all the
% states is zero, except the Velocity state itself.
vidx = stateinfo(filter,'Velocity');
dzdx(:,vidx) = 1;
end
function ds = stateTransition(~,~,~,~)
% Return the derivative of the sensor state with respect to
% time as a structure.
ds = struct('Bias',0);
end
function dBiasdx = stateTransitonJacobian(~,filter,~,~)
% Since the stateTransiton function does not depend on the
% state of the filter, the Jacobian is all zero.
N = numel(filter.State);
dBiasdx = zeros(1,N);
end
end
end
