function [dfdx, dfdw] = constveljac(state, varargin)
% CONSTVELJAC      The Jacobian of the constant velocity motion (CV) model.
%   The trackingEKF object allows you to specify the
%   StateTransitionJacobianFcn property. CONSTVELJAC provides the Jacobian
%   of the constant velocity state transition function, constvel, with
%   respect to the state.
%
%   dfdx = CONSTVELJAC(state) calculates the Jacobian matrix of the
%   constant velocity motion model with respect to the state vector. It is
%   assumed that dt = 1 second.
%
%   dfdx = CONSTVELJAC(state, dt) allows you to specify a time
%   interval, dt.
%
%   [dfdx, dfdw] = CONSTVELJAC(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 is an n-by-n matrix, where n is the number of states in
%   the state vector.
%   In a 1-D motion, the Jacobian is A = [1 dt; 0 1].
%   In a 2-D motion, the Jacobian is blkdiag(A, A)
%   In a 3-D motion, the Jacobian is blkdiag(A, A, A)
%   Positions are in meters; velocities are in meters/second
%
%   Class support
%   -------------
%   state must be a finite real vector of the dimensions specified above.
%   dt must be a real finite numeric scalar.
%
%   Example
%   -------
%   % Define a state vector for 2-D constant velocity motion
%   state = [1;1;0;2]; 
%
%   % Calculate the Jacobian matrix assuming dt = 1 second
%   dfdx = CONSTVELJAC(state) 
%
%   % Calculate the Jacobian matrix given dt = 0.1 second
%   dfdx = CONSTVELJAC(state, 0.1)
%
%   See also trackingEKF, initcvekf, constvel

%   References: 
%   [1] X. Rong Li and Vasselin P. Jilkov, "A Survey of Maneuvering Target
%       Tracking: Dynamic Models", Proceedings of the SPIE Conference on
%       Signal and Data Processing of Small Targets, 2000.

%   Copyright 2016-2018 The MathWorks, Inc.

%#codegen

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

numStates = numel(state);
cond = (numStates ~= 2) && (numStates ~= 4) && (numStates ~= 6);
coder.internal.errorIf(cond, 'shared_tracking:motion:incorrectStateVecWithInfo','[2 4 6]');

numDims = numStates / 2;

% 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'}, 'constveljac', 'dt', nargin);
    dt = varargin{end};
end

A = cast([1 dt; 0 1], 'like', state);
B = cast([dt^2/2; dt], 'like', state);

switch numDims
    case 1
        dfdx = A;
        dfdw = B;
    case 2
        dfdx = blkdiag(A, A);
        dfdw = blkdiag(B, B);
    case 3
        dfdx = blkdiag(A, A, A);
        dfdw = blkdiag(B, B, B);
end