function [dfdx, dfdw] = constturnjac(state, varargin)
% CONSTTURNJAC  The Jacobian of the constant turn rate (CT) model.
%   The trackingEKF object allows you to specify the
%   StateTransitionJacobianFcn property. CONSTTURNJAC provides the Jacobian
%   of the constant turn rate state transition function, constturn, with
%   respect to the state. A constant turn rate assumes that the motion
%   in the XY plane follows a constant angular velocity, omega. In
%   addition, the motion in the vertical Z plane is assumed to follow a
%   constant velocity model.
%
%   dfdx = CONSTTURNJAC(state) calculates the Jacobian matrix of
%   the constant turn rate motion model, constturn, with respect to the
%   state vector. It is assumed that dt = 1 second. The state vector
%   is either [x; vx; y; vy; omega] for a 2D motion model or 
%	[x; vx; y; vy; omega; z; vz] for a 3D motion model.
%
%   dfdx = CONSTTURNJAC(state, dt) allows you to specify a time
%   interval, dt.
%
%   [dfdx, dfdw] = CONSTTURNJAC(state, w, dt) allows you to specify a noise
%   term, w, and time interval, dt, and returns the Jacobian of the
%   function with respect to the noise, dfdw.
%
%   The Jacobian dfdx is a 5-by-5 matrix or a 7-by-7 matrix, depending on
%   the number of elements in the state vector.
%
%   Positions are in meters; velocities are in meters/second; omega is the
%   turn rate, in degrees/second.
%
%   Class support
%   -------------
%   state must be a finite real vector with 5 or 7 elements.
%   dt must be a real finite numeric scalar.
%
%   Example 1: 2D Constant-Turn Model
%   ---------------------------------
%   % Define a state vector for constant turn motion
%   state = [500; 0; 0; 100; 12]; 
%
%   % Calculate the Jacobian assuming dt = 1 second
%   dfdx = CONSTTURNJAC(state) 
%
%   % Calculate the Jacobian matrix given dt = 0.1 second
%   dfdx = CONSTTURNJAC(state, 0.1)
%
%   Example 2: 3D Constant-Turn Model
%   ---------------------------------
%   % Define a state vector for constant turn motion in 3D
%   state = [500; 0; 0; 100; 12; 10; 1]; 
%
%   % Calculate the Jacobian assuming dt = 1 second
%   dfdx = CONSTTURNJAC(state) 
%
%   % Calculate the Jacobian matrix given dt = 0.1 second
%   dfdx = CONSTTURNJAC(state, 0.1)
%
%   See also trackingEKF, initctekf, constturn

%   References: 
%   [1] Samuel Blackman and Robert Popoli, "Design and Analysis of Modern
%       Tracking Systems", pages 205-209, Artech House, 1999.

%   Copyright 2016-2022 The MathWorks, Inc.

%#codegen

% Validate inputs
validateattributes(state, {'numeric'},...
    {'real', 'finite', 'vector', 'nonsparse'}, 'constturnjac', 'state', 1);

numStates = numel(state);
cond = (numStates == 5 || numStates == 7);
coder.internal.errorIf(~cond, 'shared_tracking:motion:incorrectStateVecWithInfo','[5 7]');

% If dt is not given, assume dt = 1
if nargin==1 || (nargin==2 && isnumeric(varargin{1}) && ~isscalar(varargin{1}))
    dt = ones(1, 1, 'like', state);
else  % dt is always at the end of varargin
    validateattributes(varargin{end}, {'numeric'}, ...
        {'real', 'finite', 'scalar', 'nonsparse'}, 'constturnjac', 'dt', nargin);
    dt = varargin{end};
end

if dt < 0
    [dfdx, dfdw] = matlabshared.tracking.internal.fusion.invconstturnjacImpl(state, dt);
else
    [dfdx, dfdw] = matlabshared.tracking.internal.fusion.constturnjacImpl(state, dt);
end
