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 adjunto. $BiasSensor.m archivo.
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 estructura. Todas las derivadas parciales son 0, excepto las derivadas parciales de la medida con respecto a las componentes de velocidad y estado de polarización.
Implementar el método stateTransition
El método stateTransiton devuelve la derivada del estado del sensor definido en el método sensorStates . Supongamos que la derivada del sesgo se ve afectada por un ruido blanco con una desviación estándar de 0.01. Devuelve la derivada como una estructura. Tenga en cuenta que esto sólo muestra cómo configurar el método y no corresponde a ninguna aplicación práctica.
Implementar el método stateTransitionJacobian
Dado que 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.
Cree un objeto insEKF con el objeto biSensor .
filter =
insEKF with properties:
State: [17x1 double]
StateCovariance: [17x17 double]
AdditiveProcessNoise: [17x17 double]
MotionModel: [1x1 insMotionPose]
Sensors: {[1x1 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 dBias = stateTransition(~,~,dt,~)
% Assume the derivative of the bias is affected by a zero-mean
% white noise with a standard deviation of 0.01.
noise = 0.01*randn*dt;
dBias = struct('Bias',noise);
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
