insCreateMotionModelTemplate

classdef newMotionModel < positioning.INSMotionModel
    %newMotionModel Template for motion model using insEKF
    %   Customize this motion model and use it with the insEKF to fuse
    %   data.
    %
    %   Example:
    %       filt = insEKF(newMotionModel);
    %
    %   See also insEKF, positioning.INSMotionModel.

    %   Generated on 13-Mar-2022 11:25:13

    properties (Constant)
        State1Length = 1 % Length of motion model state State1
        State2Length = 2 % Length of motion model state State2
    end

    methods
        function s = modelstates(motion, opts)
            %modelstates Define the tracked states for this motion model
            %   MODELSTATES returns a struct which describes the
            %   states used by this motion model and tracked by the insEKF
            %   filter object. The field names describe the individual state
            %   quantities, and you can access the estimates of those
            %   quantities through the statesparts function. The values of
            %   the struct determine the size and default values of the
            %   state vector. The input OPTS is the insOptions object used
            %   to build the filter.
            %
            %   See also insEKF, positioning.INSMotionModel.

            % Preallocate a struct with fields State1 and State2.
            % Overwrite the fields with different default values if
            % needed.
            s = struct("State1", zeros(1, motion.State1Length, opts.Datatype), ...
                "State2", zeros(1, motion.State2Length, opts.Datatype));
        end

        function statesdot = stateTransition(motion, filt, dt, varargin)
            %stateTransition State transition for motion states
            %   STATETRANSITION returns a struct with identical fields
            %   as the output of the modelstates function. The
            %   returned struct describes the per-state transition function
            %   for the motion model states.
            %
            %   This function is called by the insEKF object FILT when the
            %   PREDICT method of the FILT function is called. The DT and
            %   varargin inputs are the corresponding inputs to the
            %   predict method of the insEKF object.
            %
            %   *** THIS METHOD IS OPTIONAL ***
            %   If you delete this method, the model states will be
            %   constant. In this case, also delete the
            %   stateTransitionJacobian method.
            %
            %   See also insEKF, positioning.INSMotionModel.

            % Set statesdot.State1 to the derivative of State1 with
            % respect to time. If State1 is constant overtime, leave the
            % following line unchanged.
            statesdot.State1 = zeros(1, motion.State1Length, "like", filt.State);

            % Set statesdot.State2 to the derivative of State2 with
            % respect to time. If State2 is constant overtime, leave the
            % following line unchanged.
            statesdot.State2 = zeros(1, motion.State2Length, "like", filt.State);
        end

        function dfdx = stateTransitionJacobian(motion, filt, dt, varargin)
            %stateTransitionJacobian Jacobian of the stateTransition function
            %   STATETRANSITIONJACOBIAN returns a struct with identical
            %   fields as modelstates and describes the Jacobian of the
            %   per-state transition function relative to the State
            %   property of FILT. Each field value of STATESDOT should be a
            %   M-by-numel(FILT.State) row vector, representing the partial
            %   derivatives of that field's state transition function
            %   relative to the state vector.
            %
            %   This function is called by the insEKF object FILT when the
            %   PREDICT method of the FILT function is called. The DT and
            %   varargin inputs are the corresponding inputs to the
            %   predict method.
            %
            %   *** THIS METHOD IS OPTIONAL ***
            %   If this method is not implemented, a numerical Jacobian
            %   will be used instead.
            %
            %   See also insEKF, positioning.INSMotionModel.

            N = numel(filt.State);

            dfdx.State1 = zeros(motion.State1Length, N, "like", filt.State);
            dfdx.State2 = zeros(motion.State2Length, N, "like", filt.State);

            % Create indexing
            s1idx = stateinfo(filt, "State1");
            s2idx = stateinfo(filt, "State2");

            % Uncomment the line below, and set dfdx.State1 to the
            % Jacobian of the stateTransition function with respect to
            % the State property of the filter object filt. Use s1idx to
            % index the columns of dfdx.State1.

            % % dfdx.State1(:,s1idx) =

            % Uncomment the line below, and set dfdx.State2 to the
            % Jacobian of the stateTransition function with respect to
            % the State property of the filter object filt. Use s2idx to
            % index the columns of dfdx.State2.

            % % dfdx.State2(:,s2idx) =
        end
    end
end

% [EOF]