function ABF = initcaabf(Detection)
%INITCAABF  Constant acceleration trackingABF initialization
%   ABF = INITCAABF(Detection)   initializes a constant acceleration (CA)
%   trackingABF (ABF) based on information provided in Detection. Detection
%   must be an objectDetection. Only position information contained in
%   Detection is used for track initialization.
%
%   Notes:
%   ------
%   1. You can use INITCAABF as the FilterInitializationFcn property.
%   2. In creating the filter the function configures the process noise 
%      assuming a unit acceleration standard deviation.
%   3. The function creates a n-D constant acceleration filter, where n is 
%      the measurement length. MeasurementParameters are ignored.
%
%   Example - creating a trackingABF object based on a detection
%   ------------------------------------------------------------
%   % Create an objectDetection. Provide position measurement at x=1, y=3 
%   % and a measurement noise of [1 0.2; 0.2 2];
%   detection = objectDetection(0, [1;3], 'MeasurementNoise', [1 0.2;0.2 2]);
% 
%   % Use INITCAABF to create a trackingABF filter initialized at the 
%   % provided position and using the measurement noise defined above.
%   ABF = INITCAABF(detection);
%
%   % Check the values of the state and measurement noise.
%   % Verify that the filter state, ABF.State, has the same position 
%   % components as detection measurement, Detection.Measurement.  Verify 
%   % that the filter measurement noise, ABF.MeasurementNoise, is the same 
%   % as the Detection.MeasurementNoise values.
%   ABF.State
%   ABF.MeasurementNoise
%
%   See also: trackingABF, objectDetection, initcvabf

%#codegen

% Copyright 2018 The MathWorks, Inc.

% Input validation:
matlabshared.tracking.internal.fusion.validateFilterInitializationInput(Detection, mfilename);

%% How to Initialize a Tracking Filter?
% Things to consider when creating a filter:
%   1. Defining the motion model and state
%   2. Defining the process noise
%   3. Defining the measurement model
%   4. Mapping the sensor measurements to an initial state vector
%   5. Mapping the sensor measurement noise to a state covariance
%   6. Creating the correct filter

%% Step 1: Defining the motion model and state
%   In this example, use a constant velocity model in a rectangular frame
%       1. In 1D motion, the state is [x;vx;ax]
%       2. In 2D motion, the state is [x;vx;ax;y;vy;ay]
%       3. In 3D motion, the state is [x;vx;ax;y;vy;ay;z;vz;az]
%       4. The state transition model matrix is block diagonal. Every block
%          on the diagonal is:
%          [1 dt dt^2/2; 0 1 dt; 0 0 1]
 
%% Step 2: Defining the process noise 
%   The process noise represents the parts of the process that are not
%   taken into account in the model. For example, in a constant
%   acceleration model, the change in the acceleration is neglected. To
%   account for it, a scaled 1-d rate of acceleration change represents the
%   magnitude of the possible change in the acceleration that is neglected
%   in each direction. The process noise is a block diagonal matrix with
%   each block equal to Q1d below.
%
%   scaleAccelChange = 1;  
%   Q1d = [dt^4/4 dt^3/2 dt^2/2; dt^3/2 dt^2 dt; dt^2/2 dt 1] * scaleAccelChange;
 
%   Only the position is measured. So, the measurement model is a block
%   diagonal matrix with each block equal to
%       H1d = [1 0 0]
classToUse = class(Detection.Measurement);
numDimensions = numel(Detection.Measurement);
H1d = cast([1 0 0], classToUse);
switch numDimensions
    case 1
        H = blkdiag(H1d);
    case 2
        H = blkdiag(H1d, H1d);
    case 3
        H = blkdiag(H1d, H1d, H1d);
    otherwise
        coder.internal.error('shared_tracking:motion:incorrectDimensionality')
end

%% Step 4: Initializing the state vector using the Measurement
%   Since the measurement is [x;y;z] and the state is
%   [x;vx;ax;y;vy;ay;z;vz;az], the position components are straightforward.
%   As we have no measurement of the velocity or acceleration, these
%   components are initialized to 0
state = H' * Detection.Measurement(:);
 
%% Step 5: Initializing the state covariance using MeasurementNoise
%   For the parts of the state that are measured directly by the sensor,
%   use the corresponding measurement noise components. For the parts that
%   are not measured directly, assume a large initial state covariance.
%   This will allow future detections to be assigned to the track.
L = 100; % A large value compared to the expected measurement noise.
stateCov = H' * Detection.MeasurementNoise * H;
for i = 1:numDimensions
    stateCov(i*3-1, i*3-1) = L;
    stateCov(i*3, i*3) = L;
end

%% Step 6: Creating the correct filter
%   In this example, all the models are linear, so use trackingKF as the
%   tracking filter
modelName = '1D Constant Acceleration';
switch numDimensions
    case 1
        modelName = '1D Constant Acceleration';
    case 2
        modelName = '2D Constant Acceleration';
    case 3
        modelName = '3D Constant Acceleration';
end
ABF = trackingABF('MotionModel', modelName, ...
    'State', state, ...
    'StateCovariance', stateCov, ...
    'MeasurementNoise', Detection.MeasurementNoise ...    
    );