stateTransitionJacobian

Jacobian of state transition function

Since R2022a

Syntax

jac = stateTransitionJacobian(model,filter,dt,varargin)

Description

jac = stateTransitionJacobian(model,filter,dt,varargin) returns the Jacobian of the state transition function for a specified motion model, model, with respect to the states of the filter using the motion model, filter. For more details about the returned Jacobian, see the jac output argument.

You can use the stateTransitionJacobian function to calculate the covariance derivative in a continuous time prediction using an INS filter. For example, the predict function of the insEKF filter object calls stateTransitionJacobian function on the motion model used with the filter object.

Note

You can optionally implement this function as a method for a subclass of the positioning.INSMotionModel abstract class. If you do not implement this method, the subclass uses a Jacobian calculated by numerical differentiation.

Examples

Customize Motion Model Used with insEKF

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

cvModel = 
  ConstantVelocityMotion with no properties.

Create an insEKF object with the created cvModel object.

filter = insEKF(insAccelerometer,cvModel)

filter = 
  insEKF with properties:

                   State: [5×1 double]
         StateCovariance: [5×5 double]
    AdditiveProcessNoise: [5×5 double]
             MotionModel: [1×1 ConstantVelocityMotion]
                 Sensors: {[1×1 insAccelerometer]}
             SensorNames: {'Accelerometer'}
          ReferenceFrame: 'NED'

The filter state contains the Position and Velocity components.

stateinfo(filter)

ans = struct with fields:
              Position: 1
              Velocity: 2
    Accelerometer_Bias: [3 4 5]

Show customized ConstantVelocityMotion class

type ConstantVelocityMotion.m

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.

            % Derivative of position = velocity.
            % Derivative 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

Input Arguments

model — Motion model used with INS filter

object inherited from positioning.INSMotionModel class

Motion model used with an INS filter, specified as an object inherited from the positioning.INSMotionModel abstract class.

filter — INS filter

insEKF object

INS filter, specified as an insEKF object.

dt — Filter time step

positive scalar

Filter time step, specified as a positive scalar.

Data Types: single | double

varargin — Additional inputs

any data type

Additional inputs that are passed as the varargin inputs of the predict object function of the insEKF object.

Output Arguments

jac — Jacobian of state transition of the motion model

structure

Jacobian of the state transition function of the specified motion model, returned as a structure.

The fields of the structure jac have the same names as those of the structure returned by the modelstates object function of the model input argument. The value of each field is an S-by-N real-valued matrix of partial derivatives of the corresponding time derivatives returned by the stateTransition object function of model, where the partial derivatives have been calculated with respect to each state of the INS filter filter.

• S is the number of elements in the corresponding state of the motion model. For example, S is 1 for a scalar 1-D position, and S is 3 for a 3-D position vector.

• N is the total number of filter states, which includes motion model states and any additional states, such as those contributed by sensor models.

Data Types: struct

Version History

Introduced in R2022a