function KF = initcvkf(Detection)
%INITCVKF  Constant velocity trackingKF initialization
%   KF = INITCVKF(Detection)   initializes a constant velocity (CV)
%   trackingKF (KF) based on information provided in Detection. Detection
%   must be an objectDetection. The detection measurement must contain
%   position information in 1, 2, or 3 dimensions.
%
%   Notes:
%   ------
%   1. You can use INITCVKF 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 velocity filter, where n is the
%      measurement length. MeasurementParameters are ignored.
%
%   Example - creating a trackingKF 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 INITCVKF to create a trackingKF filter initialized at the 
%   % provided position and using the measurement noise defined above.
%   KF = initcvkf(detection);
%
%   % Check the values of the state and measurement noise.
%   % Verify that the filter state, KF.State, has the same position 
%   % components as detection measurement, detection.Measurement.  Verify 
%   % that the filter measurement noise, KF.MeasurementNoise, is the same 
%   % as the detection.MeasurementNoise values.
%   KF.State
%   KF.MeasurementNoise
%
%   See also: trackingKF, objectDetection, initcakf, initcvekf, initcaekf,
%   initctekf, initcvukf, initcaukf, initctukf

%#codegen

% Copyright 2016-2017 The MathWorks, Inc.

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

%% 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]
%       2. In 2D motion, the state is [x;vx;y;vy]
%       3. In 3D motion, the state is [x;vx;y;vy;z;vz]
%       4. The state transition model matrix is block diagonal. Every block
%          on the diagonal is:
%           [1 dt; 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 velocity
%   model, the acceleration is neglected. To account for it, a scaled 1-d
%   scaled acceleration represents the magnitude of the possible
%   acceleration that is neglected in each direction. The process noise is
%   a block diagonal matrix with each block equal to Q1d below.
%
%   scaleAccel = 1;  
%   G1d = cast([dt^2/2; dt] * scaleAccel, classToUse);
%   Q1d = G1d*G1d';
 
%% Step 3: Defining the measurement model
%   Only the position is measured. So, the measurement model is a block
%   diagonal matrix with each block equal to
%       H1d = [1 0]
classToUse = class(Detection.Measurement);
numDimensions = numel(Detection.Measurement);
H1d = cast([1 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;y;vy;z;vz], the
%   position components are straightforward. As we have no measurement of
%   the velocity, the velocity 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*2, i*2) = 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 Velocity';
switch numDimensions
    case 1
        modelName = '1D Constant Velocity';
    case 2
        modelName = '2D Constant Velocity';
    case 3
        modelName = '3D Constant Velocity';
end
KF = trackingKF('MotionModel', modelName, ...
    'State', state, ...
    'StateCovariance', stateCov, ...
    'MeasurementNoise', Detection.MeasurementNoise ...    
    );