Customize a 1-D constant velocity motion model used with an insEKF
object. Customize the motion model by inheriting from the positioning.INSMotionModel
interface class and implement the modelstates
and stateTranistion
methods. You can also optionally implement the stateTransitionJacobian
method. These sections provide an overview of how the ConstantVelocityMotion
class implements the positioning.INSMotionModel
methods, but for more details on their implementation, see the attached ConstantVelocityMotion.m
file.
Implement modelstates
method
To model 1-D constant velocity motion, you need to return only the 1-D position and velocity state as a structure. When you add a ConstantVelocityMotion
object to an insEKF
filter object, the filter adds the Position
and Velocity
components to the state vector of the filter.
Implement stateTransition
method
The stateTransition
method returns the derivatives of the state defined by the motion model as a structure. The derivative of the Position
is the Velocity
, and the derivative of the Velocity
is 0
.
Implement stateTransitionJacobian
method
The stateTransitionJacobian
method returns the partial derivatives of stateTransition
method, with respect to the state vector of the filter, as a structure. All the partial derivatives are 0, except the partial derivative of the derivative of the Position
component, which is the Velocity
, with respect to the Velocity
state, is 1
.
Create and add inherited object
Create a ConstantVelocityMotion
object.
cvModel =
ConstantVelocityMotion with no properties.
Create an insEKF
object with the created cvModel
object.
filter =
insEKF with properties:
State: [5x1 double]
StateCovariance: [5x5 double]
AdditiveProcessNoise: [5x5 double]
MotionModel: [1x1 ConstantVelocityMotion]
Sensors: {[1x1 insAccelerometer]}
SensorNames: {'Accelerometer'}
ReferenceFrame: 'NED'
The filter state contains the Position
and Velocity
components.
ans = struct with fields:
Position: 1
Velocity: 2
Accelerometer_Bias: [3 4 5]
Show customized ConstantVelocityMotion
class
classdef ConstantVelocityMotion < positioning.INSMotionModel
% CONSTANTVELOCITYMOTION Constant velocity motion in 1-D
% Copyright 2021 The MathWorks, Inc.
methods
function m = modelstates(~,~)
% Return the state of motion model (added to the state of the
% filter) as a structure.
% Since the motion is 1-D constant velocity motion,
% retrun only 1-D position and velocity state.
m = struct('Position',0,'Velocity',0);
end
function sdot = stateTransition(~,filter,~, varargin)
% Return the derivative of each state with respect to time as a
% structure.
% Deriviative of position = velocity.
% Deriviative of velocity = 0 because this model assumes constant
% velocity.
% Find the current estimated velocity
currentVelocityEstimate = stateparts(filter,'Velocity');
% Return the derivatives
sdot = struct( ...
'Position',currentVelocityEstimate, ...
'Velocity',0);
end
function dfdx = stateTransitionJacobian(~,filter,~,varargin)
% Return the Jacobian of the stateTransition method with
% respect to the state vector. The output is a structure with the
% same fields as stateTransition but the value of each field is a
% vector containing the derivative of that state relative to
% all other states.
% First, figure out the number of state components in the filter
% and the corresponding indices
N = numel(filter.State);
idx = stateinfo(filter);
% Compute the N partial derivatives of Position with respect to
% the N states. The partial derivative of the derivative of the
% Position stateTransition function with respect to Velocity is
% just 1. All others are 0.
dpdx = zeros(1,N);
dpdx(1,idx.Velocity) = 1;
% Compute the N partial derivatives of Velocity with respect to
% the N states. In this case all the partial derivatives are 0.
dvdx = zeros(1,N);
% Return the partial derivatives as a structure.
dfdx = struct('Position',dpdx,'Velocity',dvdx);
end
end
end