function [dfdx, dfdw] = constaccjac(state, varargin)
% CONSTACCJAC  The Jacobian of the constant acceleration (CA) motion model.
%   The trackingEKF object allows you to specify the
%   StateTransitionJacobianFcn property. CONSTACCJAC provides the Jacobian
%   of the constant acceleration state transition function, constacc, with
%   respect to the state.
%
%   dfdx = CONSTACCJAC(state) calculates the Jacobian matrix of the
%   constant acceleration motion model with respect to the state vector. It
%   is assumed that dt = 1 second.
%
%   dfdx = CONSTACCJAC(state, dt) allows you to specify a time
%   interval, dt.
%
%   [dfdx, dfdw] = CONSTACCJAC(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 noise is treated as the
%   acceleration increment of a Wiener-sequence acceleration model.
%
%   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.5*dt^2; 0 1 dt; 0 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; accelerations
%   are in meters/second^2.
%
%   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 for 2-D constant acceleration motion
%   state = [1;1;1;2;1;0]; 
%
%   % Calculate the Jacobian matrix assuming dt = 1 second
%   dfdx = CONSTACCJAC(state)
%
%   % Calculate the Jacobian matrix given dt = 0.1 second
%   dfdx = CONSTACCJAC(state, 0.1)
%
%   See also trackingEKF, initcaekf, constacc
   
%   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-2022 The MathWorks, Inc.

%#codegen

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

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

numDims = numStates / 3;

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

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

if dt < 0
    % Backward model
    B = cast([0; 0; -1],'like',state);
else
    % Forward model
    B = cast([dt^2/2; dt; 1], 'like', state);
end

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