%trackingEKF An extended Kalman filter for object tracking
%   The extended Kalman filter is designed for tracking objects that follow
%   a nonlinear motion model or that are measured by a nonlinear
%   measurement model. You can use the filter to predict an object's future
%   location, to reduce noise in the detected location, or to help
%   associate multiple object detections with their tracks. The extended
%   Kalman filter uses a first order Taylor expansion of the state and
%   measurement equations.
%
%   EKF = trackingEKF returns an extended Kalman filter object with default
%   state transition function (@constvel), measurement function (@cvmeas)
%   and state ([0;0;0;0]), and assumes an additive noise model.
% 
%   EKF = trackingEKF(StateTransitionFcn, MeasurementFcn, State) lets you
%   specify the state transition function, f, and the measurement function,
%   h. Both must be specified as function_handles. In addition, the object
%   lets you specify an initial value for the state.
% 
%   EKF = trackingEKF(..., Name, Value) specifies additional
%   name-value pair arguments to define the properties described below.
%
%   trackingEKF properties: 
%   State                       - State, (x)
%   StateCovariance             - State estimation error covariance, (P)
%   StateTransitionFcn          - Propagates the state to next time step, (f)
%   StateTransitionJacobianFcn  - State transition jacobian matrix, (df/dx)
%   ProcessNoise                - Process noise covariance, (Q)
%   HasAdditiveProcessNoise     - True if process noise is additive
%   MeasurementFcn              - Calculates the measurement, (h)
%   MeasurementJacobianFcn      - Measurement jacobian matrix, (dh/dx)
%   HasMeasurementWrapping      - True if the measurement wraps (read only)
%   MeasurementNoise            - Measurement noise covariance, (R)
%   HasAdditiveMeasurementNoise - True if measurement noise is additive
%   EnableSmoothing             - Enable backward smoothing for the filter
%   MaxNumSmoothingSteps        - Maximum number of backward smoothing steps
%                                 allowed for smoothing
%   MaxNumOOSMSteps             - Maximum number of out-of-sequence steps
%
%   trackingEKF methods: 
%   predict            - Predict the state and state estimation error covariance
%   correct            - Correct the state and state estimation error covariance
%   correctjpda        - Correct using joint probabilistic detection assignment
%   smooth             - Smooth the filter estimate using backward recursion
%   retrodict          - Predict the filter backwards in time
%   retroCorrect       - Correct the filter with an out-of-sequence measurement
%   retroCorrectJPDA   - Correct the filter with multiple out-of-sequence measurements
%   distance           - Calculate the distance between measurements and the filter
%   residual           - Calculate the measurement residual and residual noise
%   likelihood         - Calculate the likelihood of a measurement
%   clone              - Create a copy of the object with the same property values
%   initialize         - Initialize filter properties
%   tunableProperties  - Get the filter tunable properties
%   setTunedProperties - Set the tuned properties
%
%   Notes:
%   ------
%   * If the measurement exists, e.g., the object has been detected, you
%     can call the predict method and the correct method together. If the
%     measurement is missing, you can call the predict method but not the
%     correct method.
%   * You can use the distance method to compute distances that describe
%     how a set of measurements matches the Kalman filter. You can thus
%     find a measurement that best fits the filter. This strategy can be
%     used for matching object detections against object tracks in a
%     multi-object tracking problem.
%
%   % Example 1: Create a trackingEKF for a 2D constant velocity model
%   % ----------------------------------------------------------------
%   %   Use the pre-packaged constant velocity motion model, constvel, and  
%   %   the associated measurement model, cvmeas. These models assume that
%   %   the state is [x;vx;y;vy] and a position measurement is [x;y;z].
%   state = [0;0;0;0];
%   EKF = trackingEKF(@constvel, @cvmeas, state);
%
%   % Example 2: Create a trackingEKF with additional name-value pairs
%   % ----------------------------------------------------------------
%   %   Use additional name-value pairs to define the corresponding 
%   %   StateTransitionJacobianFcn and MeasurementJacobianFcn properties.
%   %   Use the pre-packaged constant velocity Jacobian model, constveljac, 
%   %   and the associated measurement Jacobian model, cvmeasjac.
%   state = [0;0;0;0];
%   EKF = trackingEKF(@constvel, @cvmeas, state, ...
%       'StateTransitionJacobianFcn', @constveljac, ...
%       'MeasurementJacobianFcn', @cvmeasjac);
%
%   % Example 3: Running the filter
%   % -----------------------------
%   %   After constructing a filter, use the following steps to call
%   %   predict and correct. You may call predict and correct in any order
%   %   and as many times you would like to call each.
%   state = [0;0;0;0];
%   EKF = trackingEKF(@constvel, @cvmeas, state);
%   meas = [1;1;0];
%   [x_pred, P_pred] = predict(EKF, 0.5)    % Predict over 0.5 seconds
%   [x_corr, P_corr] = correct(EKF, meas)   % Correct using the measurement   
%   [x_pred, P_pred] = predict(EKF)         % Predict over 1 second
%   [x_pred, P_pred] = predict(EKF, 2)      % Predict over 2 seconds
%
%   % Example 4: Using the distance method
%   % ------------------------------------
%   %   You can measure the distance between the filter and a set of
%   %   measurements, z_matrix, by calling the distance method. 
%   state = [0;0;0;0];
%   EKF = trackingEKF(@constvel, @cvmeas, state);
%   [x_pred, P_pred] = predict(EKF);
%   z_matrix = [0, 0, 0; 1, 1, 0; 2, 2, 0];
%   d = distance(EKF, z_matrix)
%
%  See also: trackingKF, trackingUKF, constvel, cvmeas, constveljac,
%  cvmeasjac

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

%   Copyright 2016-2022 The MathWorks, Inc.

%#codegen
%#ok<*EMCLS>
%#ok<*EMCA>
%#ok<*MCSUP>

classdef trackingEKF < matlabshared.tracking.internal.ExtendedKalmanFilter ...
        & matlabshared.tracking.internal.fusion.CustomDisplay ...
        & matlabshared.tracking.internal.AbstractTrackingFilter ...
        & matlabshared.tracking.internal.AbstractJPDAFilter ...
        & matlabshared.smoothers.internal.EKFSmoother ...
        & matlabshared.tracking.internal.RetrodictionFilter ...
        & matlabshared.tracking.internal.fusion.AbstractTunableFilter ...
        & matlabshared.tracking.internal.fusion.GaussianSmootherJIPDAFilter
        
    methods
       function EKF = trackingEKF(varargin)
           %ExtendedKalmanFilter  The constructor to create an extended
           %Kalman filter object.
           [smoothArgs,idx1,idx2] = trackingEKF.parseSmoothingNVPairs(varargin{:});
           vArgsNoSmoothing = {varargin{1:idx1-1},varargin{idx1 + 2:idx2 - 1},varargin{idx2 + 2:end}};
           [oosmArgs,idx1] = trackingEKF.parseOOSMNVPairs(vArgsNoSmoothing{:});
           filterArgs = {vArgsNoSmoothing{1:idx1-1},vArgsNoSmoothing{idx1 + 2:end}};
           EKF@matlabshared.tracking.internal.ExtendedKalmanFilter(filterArgs{:});
           EKF@matlabshared.tracking.internal.RetrodictionFilter(oosmArgs{:});
           EKF@matlabshared.smoothers.internal.EKFSmoother(smoothArgs{:});
           
           % If user didn't specify StateTransitionFcn and MeasurementFcn,
           % use a 2-D constant velocity model as a default.
           if numel(varargin) == 0
               EKF.StateTransitionFcn = @constvel;
               EKF.MeasurementFcn = @cvmeas;
               EKF.State = zeros(4,1);
           end
       end
       
       function varargout = predict(filter,varargin)
           %
           
           % The above line is intentionally left empty to divert the help
           % text to superclass.
           
           % Predict must be called with a single object
           coder.internal.assert(numel(filter) == 1, ...
                'shared_tracking:ExtendedKalmanFilter:NonScalarFilter', ...
                'extended Kalman filter', 'predict');
            
           % Call setup on smoother, the smoother knows when to return
           % safely without doing setup multiple times. 
           setupInitialDistributions(filter);
           
           % Predict using the superclass 
           [varargout{1:nargout}] = predict@matlabshared.tracking.internal.ExtendedKalmanFilter(filter,varargin{:});
           
           % Update the data for the smoother
           filter.LastJacobian = filter.pJacobian;
           updatePredictionData(filter);
           
           % Update the data for the OOSM filter
           updateRetrodictionStateAfterPrediction(filter,varargin{:});
       end
       
       function varargout = correct(filter,varargin)
           %
           
           % The above line is intentionally left empty to divert the help
           % text to superclass.
           
           % Correct using the superclass
           [varargout{1:nargout}] = correct@matlabshared.tracking.internal.ExtendedKalmanFilter(filter,varargin{:});
           
           % Update the data for the smoother.
           updateCorrectionData(filter);
           
           % Update the OOSM filter.
           updateHistoryAfterCorrection(filter);
       end
       %----------------------------------------------------------------------
       % Correctjpda method
       %----------------------------------------------------------------------
       function [x_corr, P_corr] = correctjpda(filter, z, jpda, varargin)
           %CORRECTJPDA Correct using joint probabilistic detection assignment
           %   Corrects the state and state error covariance with
           %   a set of measurements and their probabilistic data association 
           %   coefficients.
           %
           %   [x_corr, P_corr] = correctjpda(filter, z, jpda) returns the
           %   correction of state, x_corr, and state estimation error
           %   covariance, P_corr, based on the current set of measurements
           %   z and their joint probabilistic data association
           %   coefficients jpda.
           %
           %   Inputs:
           %           - filter   filter of the class trackingEKF
           %
           %           - z        measurements matrix of size m-M  where m
           %                      is the dimension of a measurement and M
           %                      is the number of measurements.
           %
           %           - jpda     M+1 vector of joint probabilities. 
           %                      For i=1:M, jpda(i) is the joint 
           %                      probability of measurement i to be 
           %                      associated with the filter. jpda(M+1) is
           %                      the probability that no measurement is
           %                      associated to the filter. correctjpda
           %                      expects sum(jpda) to be equal to 1.
           %
           %   [x_corr, P_corr] = correctjpda(filter, z, jpda, varargin) additionally
           %   allows the definition of parameters used by the
           %   MeasurementFcn in addition to obj.State. For example, the
           %   sensor's location.
           
           % number of inputs should be at least 3
           narginchk(3,inf);

           validateCorrectJPDAInputs(filter, z, jpda, 'correctjpda', varargin{:});
        
           % Perform the EKF correction
           [filter.pState, filter.pStateCovariance] = ...
               filter.pCorrector.correctjpda(...
               z, ...
               jpda,...
               filter.pMeasurementNoise, ...
               filter.pState, ...
               filter.pStateCovariance, ...
               filter.MeasurementFcn, ...
               filter.MeasurementJacobianFcn, ...
               varargin{:});
           filter.pHasPrediction = false;

           % obj.State outputs the estimated state in the orientation of
           % initial state provided by the user
           if nargout
               x_corr = filter.State;
               if nargout > 1
                   P_corr = filter.pStateCovariance;
               end
           end
           
           updateCorrectionData(filter);
           
           % Update the OOSM filter.
           updateHistoryAfterCorrection(filter);
       end
       
       %-------------------------------------------------------------------
       % Distance method: cannot be inherited from superclass, because the
       % superclass doesn't have this method
       %-------------------------------------------------------------------
       function d = distance(EKF, z_matrix, measurementParams)
           % distance Computes distances between measurements and the
           % extended Kalman filter object.
           %   d = distance(EKF, z_matrix) computes a distance between one
           %   or more measurements supplied by the z_matrix and the
           %   measurement predicted by the extended Kalman filter object.
           %   This computation takes into account the covariance of the
           %   predicted state and the measurement noise. Each row of the
           %   input z_matrix must contain a measurement vector of length
           %   N.
           %
           %   d = distance(EKF, z_matrix, measurementParams) allows to
           %   define additional parameters that will be used by the
           %   EKF.MeasurementFcn. It should be specified as a cell array,
           %   e.g., {1, [2;3]}. If unspecified, it will be assumed to be
           %   an empty cell array.
           %
           %   The distance method returns a row vector where each element
           %   is a distance associated with the corresponding measurement
           %   input.
           
           % The procedure for computing the distance is described in Page
           % 93 of "Multiple-Target Tracking with Radar Applications" by
           % Samuel Blackman.
            
           cond = (numel(EKF) > 1);
           coder.internal.errorIf(cond, ...
               'shared_tracking:ExtendedKalmanFilter:NonScalarFilter', ...
               'extended Kalman filter', 'distance');
           
           if nargin == 2
               measurementParams = cell(0,0);
           end
           
           % Validate z_matrix. Skip the size check if it's not known yet
           % (first call to distance)
           if coder.internal.is_defined(EKF.pN)
               matlabshared.tracking.internal.validateMeasurementMatrix(...
                   z_matrix, 'trackingEKF', EKF.pN);
           else
               matlabshared.tracking.internal.validateMeasurementMatrix(...
                   z_matrix, 'trackingEKF');
           end

           % Validate x, P, R
           if isvector(z_matrix)
               zcol = z_matrix(:);
           else
               zcol = z_matrix(1,:).';
           end
           validateMeasurementAndRelatedProperties(EKF, zcol, 'distance', measurementParams{:});
           
           if EKF.HasAdditiveMeasurementNoise
               d = matlabshared.tracking.internal.EKFDistanceAdditive...
                   (EKF, z_matrix, measurementParams);
           else
               d = matlabshared.tracking.internal.EKFDistanceNonAdditive...
                   (EKF, z_matrix, measurementParams);
           end
       end
	   
	   function [res, S] = residual(EKF, z, measurementParams)
		   % RESIDUAL Computes the residual of measurement z and the
		   % filter.
		   %   [res, S] = RESIDUAL(EKF, z) computes a residual, res, and
		   %   the residual matrix, S, where:
		   %       res = z-h(EKF.State), h is the measurement function
		   %       S = H*P*H'+R, where H is the measurement Jacobian
		   %           function, P is the state covariance and R is the
		   %           measurement noise.
		   %
		   %   [...] = RESIDUAL(EKF, z, measurementParams) allows
		   %   passing additional parameters that will be used by the
		   %   EKF.MeasurementFcn. It should be specified as a cell array,
		   %   e.g., {1, [2;3]}. If unspecified, it will be assumed to be
		   %   an empty cell array.
		   
           if nargin == 2
			   measurementParams = cell(0,0);
           end
           
           [res, S] = residual@matlabshared.tracking.internal.ExtendedKalmanFilter(EKF, z, measurementParams{:});
	   end
       
	   %-------------------------------------------------------------------
	   % initialize method: cannot be inherited from superclass, because
	   % the superclass doesn't have this method
	   %-------------------------------------------------------------------
	   function initialize(EKF, varargin)
		   %INITIALIZE Initialize the state and state covariance of the filter
		   %   INITIALIZE(EKF, state, stateCovariance) initializes the
		   %   extended Kalman filter by setting its State property to
		   %   state and its StateCovariance property to stateCovariance.
		   %
		   %   INITIALIZE(EKF, state, stateCovariance 'Name', value)
		   %   allows you to initialize additional properties of the
		   %   filter using 'Name'-value pairs.
		   %
		   % Note: you must not change the size or type of the properties
		   % you are initializing.
		   
		   % Validation for each property is done on setting
		   EKF.State           = varargin{1}; % Set the state
		   EKF.StateCovariance = varargin{2}; % Set the state covariance
		   
		   if numel(varargin) > 2
			   for i = 3:2:numel(varargin)
				   EKF.(varargin{i}) = varargin{i+1};
			   end
		   end
       end
       
       %-------------------------------------------------------------------
       % likelihood method: calculate the likelihood of a measurement
       %-------------------------------------------------------------------
       function l = likelihood(EKF, z, varargin)
           %LIKELIHOOD Calculate the likelihood of a measurement
           %  l = likelihood(EKF, Z) calculates the likelihood of a
           %  measurement, Z, given the object, EKF.
           %
           %  l = likelihood(EKF, Z, measurementParams) allows you to
           %  define additional parameters that will be used by the
           %  EKF.MeasurementFcn. It should be specified as a cell array,
           %  e.g., {1, [2;3]}. If unspecified, it will be assumed to be
           %  an empty cell array.
           
           % Validation is done in the residual method
           [zres, S] = residual(EKF,z,varargin{:});
           l = matlabshared.tracking.internal.KalmanLikelihood(zres,S);
       end
       
       function obj2 = clone(obj)
          obj2 = clone@matlabshared.tracking.internal.ExtendedKalmanFilter(obj); 
          copySmootherProperties(obj, obj2);
          cloneRetroFilter(obj2, obj);
       end
       
       function [retroState, retroCov, success] = retrodict(obj,dt)
           % RETRODICT Retrodict the filter to a previous time
           % [retroState, retroCov] = RETRODICT(obj,dt) retrodicts
           % (predicts backward in time) the filter by dt seconds. dt must
           % be a nonpositive time difference from the current filter time
           % to the time at which an out-of-sequence measurement (OOSM) was
           % taken.
           %
           % retroState is the retrodicted state to OOSM time, xhat(kappa|k)
           % retroCov is the retrodicted state covariance to OOSM time, P(kappa|k)
           %
           % If the filter cannot be retrodicted to the time of the OOSM,
           % because its history does not extend that far back, the
           % returned values are the same as the current state and state
           % covariance. You can check the status of the retrodiction using
           % [..., success] = ... where success is true if the filter is
           % retrodicted.
           
           % Retrodict must be called with a single object
           coder.internal.errorIf(numel(obj) > 1, ...
               'shared_tracking:ExtendedKalmanFilter:NonScalarFilter', ...
               'extended Kalman filter', 'retrodict');
           
           validateRetrodict(obj, dt);

           [distk,tk, tkappa,success, distl,tl] = getRetrodictQuantities(obj, dt);

           if ~success
               retroState = obj.State;
               retroCov = obj.StateCovariance;
               obj.pCrossCov = zeros(obj.pM,'like',obj.pState);
               obj.pDFDX = zeros(obj.pM,'like',obj.pState);
               return
           end
           
           retrodictionImpl(obj,dt,tk,tkappa,distk,tl,distl);
           if nargout > 0
               retroState = obj.pState;
               if nargout > 1
                   retroCov = obj.pStateCovariance;
               end
           end
       end
       
       function [x_retroCorr, P_retroCorr] = retroCorrect(obj,z,varargin)
           %retroCorrect Correct the filter with an out-of-sequence measurement
           % [retroCorrState,retroCorrCov] = retroCorrect(filter,z)
           % corrects the filter with the OOSM measurement z and returns
           % the corrected state and state covariance. The function changes
           % the State and StateCovariance properties of the filter object
           % to retroCorrState and retroCorrCov, respectively. You must
           % first call the retrodict method to predict the filter
           % backwards to the time of the measurement.
           %
           % ... = retroCorrect(...,measparams) specifies the measurement
           % parameters for the measurement z. The measurement parameters
           % are used by the measurement function specified in the
           % MeasurementFcn property of the filter.
           
           % retroCorrect must be called with a single object
           coder.internal.errorIf(numel(obj) > 1, ...
               'shared_tracking:ExtendedKalmanFilter:NonScalarFilter', ...
               'trackingEKF', 'retroCorrect');
           
           coder.internal.assert(obj.pWasRetrodicted,...
               'shared_tracking:OOSMFilter:MustCallThisMethodBeforeThatMethod',...
               'retrodict','retroCorrect');
           
           % obj.pN should already be known. 
           coder.internal.assert(coder.internal.is_defined(obj.pN), ...
               'shared_tracking:OOSMFilter:MustCallThisMethodBeforeThatMethod',...
               'correct','retroCorrect');
           
           % We can validate z for size.
           validateattributes(z, {'double','single'}, ...
               {'real', 'finite', 'nonsparse', 'vector', 'numel', obj.pN},...
               'retroCorrect', 'z');
           
           [~,distk] = fetchDistributionByTime(obj, max(obj.pCorrectionTimestamps));

           retroCorrectImpl(obj,z,distk,varargin{:});
           updateLastCorrectionDistribution(obj);

           if obj.pPredictionDeltaTFromLastCorrection > 0
               dtfp = obj.pPredictionDeltaTFromLastCorrection;
               predict(obj, dtfp);
               obj.pPredictionDeltaTFromLastCorrection = dtfp;
           end
           
           if nargout > 0
               x_retroCorr = obj.State;
               if nargout > 1
                   P_retroCorr = obj.StateCovariance;
               end
           end
       end
        
       function [x_retroCorr, P_retroCorr] = retroCorrectJPDA(obj,z, jpda, varargin)
           %retroCorrectJPDA Correct the filter with out-of-sequence measurements
           % [x_corr,P_corr] = retroCorrectJPDA(filter, z, jpda) 
           % returns the correction of state, x_corr, and state estimation
           % error covariance, P_corr, based on the current set of
           % out-of-sequence measurements z and their joint probabilistic
           % data association coefficients jpda. The function changes the
           % State and StateCovariance properties of the filter object to
           % retroCorrState and retroCorrCov, respectively. You must first
           % call the retrodict method to predict the filter backwards to
           % the time of the measurement.
           %
           % ... = retroCorrectJPDA(...,measparams) specifies the measurement
           % parameters for the measurements z. The measurement parameters
           % are used by the measurement function specified in the
           % MeasurementFcn property of the filter.
                      
           % number of inputs should be at least 3
           narginchk(3,inf);
           
           coder.internal.assert(obj.pWasRetrodicted,...
               'shared_tracking:OOSMFilter:MustCallThisMethodBeforeThatMethod',...
               'retrodict','retroCorrectJPDA');
           
           % obj.pN should already be known. 
           coder.internal.assert(coder.internal.is_defined(obj.pN), ...
               'shared_tracking:OOSMFilter:MustCallThisMethodBeforeThatMethod',...
               'correct','retroCorrectJPDA');
           
           % Validate correct jpda inputs
           validateCorrectJPDAInputs(obj, z, jpda, 'retroCorrectJPDA', varargin{:});
           
           [~,distk] = fetchDistributionByTime(obj, max(obj.pCorrectionTimestamps));

           retroCorrectJPDAImpl(obj,z,jpda,distk,varargin{:});
           updateLastCorrectionDistribution(obj);

           if obj.pPredictionDeltaTFromLastCorrection > 0
               dtfp = obj.pPredictionDeltaTFromLastCorrection;
               predict(obj, dtfp);
               obj.pPredictionDeltaTFromLastCorrection = dtfp;
           end
           
           if nargout > 0
               x_retroCorr = obj.State;
               if nargout > 1
                   P_retroCorr = obj.StateCovariance;
               end
           end
       end
    end

    % Methods related to tuning
    methods
        function tunableProps = tunableProperties(obj)
            %tunableProperties Get the filter tunable properties
            % TP = tunableProperties(FILTER) returns the filter tunable
            % properties, TP, as a tunableFilterProperties object.
            Qelements = matlabshared.tracking.internal.fusion.tunableElements("full", size(obj.ProcessNoise,1));
            Pelements = matlabshared.tracking.internal.fusion.tunableElements("full", size(obj.StateCovariance,1));
            q = obj.pSqrtProcessNoise';
            p = obj.pSqrtStateCovariance';
            maxQValue = max(q(Qelements))*10;
            maxPValue = max(p(Pelements))*10;
            s = struct(...
                Property = {'ProcessNoise','StateCovariance'}, ...
                PropertyValue = {obj.ProcessNoise, obj.StateCovariance}, ...
                TunedQuantity = {getString(message('shared_tracking:TunableFilter:SQRT')),getString(message('shared_tracking:TunableFilter:InitialSQRT'))}, ...
                IsTuned = {true,false}, ...
                IsSQRT = {true,true},...
                TunableElements = {Qelements,Pelements}, ...
                CurrentValue = {q,p}, ...
                LowerBound = {zeros(1,numel(Qelements),obj.pDataType),zeros(1,numel(Pelements),obj.pDataType)}, ...
                UpperBound = {maxQValue*ones(1,numel(Qelements),obj.pDataType),maxPValue*ones(1,numel(Pelements),obj.pDataType)}, ...
                TunedValue = {q,p} ...
                );
            tunableProps = tunableFilterProperties("trackingEKF", s);
        end

        function setTunedProperties(obj,s)
            %setTunedProperties Set the tuned properties
            % setTunedProperties(FILTER, S) uses the tuned values specified
            % in the structure, S, to set the corresponding filter
            % properties.
            % 
            % The structure S can be the output of the
            % trackingFilterTuner/tune object function, see help for that
            % function. For this filter, the structure contains the
            % following fields: ProcessNoise and InitialStateCovariance,
            % which must be the same size and type as the current
            % properties.
            if isfield(s, 'StateCovariance')
                obj.pStateCovariance = s.StateCovariance;
            end
            if isfield(s, 'ProcessNoise')
                obj.pProcessNoise = s.ProcessNoise;
            end
        end
    end
    
    methods (Access = ...
            {?matlabshared.tracking.internal.AbstractTrackingFilter, ...
            ?matlabshared.tracking.internal.AbstractContainsFilters, ...
            ?matlab.unittest.TestCase})
       %-------------------------------------------------------------------
	   % nullify method: cannot be inherited from superclass, because the
	   % superclass doesn't have this method
	   %-------------------------------------------------------------------
	   function nullify(EKF)
		   % nullify: sets the State and StateCovariance to zeros
           numStates = numel(EKF.pState);
		   EKF.pState           = zeros(numStates, 1, 'like', EKF.State);
		   EKF.pStateCovariance = eye(numStates, numStates, 'like', EKF.State);
           createHistory(EKF);
       end
       
       %-------------------------------------------------------------------
       % modelName method: cannot be inherited from the superclass, because
       % the superclass does not have this method
       %-------------------------------------------------------------------
       function name = modelName(EKF)
           % modelName Return the name of the motion model
           %    name = modelName(filter) returns the motion model name,
           %    name.
           coder.internal.assert(coder.internal.is_defined(EKF.StateTransitionFcn),...
               'shared_tracking:ExtendedKalmanFilter:undefinedStateTransitionFcn');
           name =  func2str(EKF.StateTransitionFcn);
        end
    
       %-------------------------------------------------------------------
       % sync method: Sync the filter with the filter provided as second
       % input. 
       %-------------------------------------------------------------------
       function sync(EKF, EKF2)
           % sync: Synchronizes the EKF with EKF2 to make sure that 
           % 1. State is same
           % 2. StateCovariance is same.
           % 3. Measurement and Process Noise are same. 
           
           % Validation is performed via set method of each property. 
           EKF.pState = EKF2.State;
           EKF.pStateCovariance = EKF2.StateCovariance;
           EKF.pMeasurementNoise = EKF2.MeasurementNoise;
           EKF.pProcessNoise = EKF2.ProcessNoise;
           EKF.pIsFirstCallCorrect = EKF2.pIsFirstCallCorrect;
           EKF.pIsFirstCallPredict = EKF2.pIsFirstCallPredict;
           syncRetroFilter(EKF,EKF2);
       end
       
       %-------------------------------------------------------------------
       % models method: Returns the state transition and measurement models
       %-------------------------------------------------------------------
       function [stm, mm] = models(filter,~)
           % MODELS Return the state transition and measurement models
           %   [stm, mm] = MODELS(filter) returns stm, the state transition
           %   model, and mm, the measurement model. Both are returned as
           %   function handles.
           if filter.HasAdditiveProcessNoise
               stm = filter.StateTransitionFcn;
           else
               v = zeros(1,1,'like',filter.State);
               stm = @(state,dt) filter.StateTransitionFcn(state,v,dt);
           end
           
           if filter.HasAdditiveMeasurementNoise
               mm = filter.MeasurementFcn;
           else
               w = zeros(1,1,'like',filter.State);
               mm = @(state,varargin) filter.MeasurementFcn(state,w,varargin{:});
           end
       end
       
       function syncDistribution(obj, dist)
           syncDist(obj,dist);
       end
    end

    methods(Access = protected)
        function setMaxNumOOSMSteps(obj,val)
            % Override setMaxNumOOSMSteps to provide a meaningful error if
            % using additive process noise
            coder.internal.errorIf(obj.HasAdditiveProcessNoise && val>0, ...
                'shared_tracking:ExtendedKalmanFilter:UnsupportedOOSMModel', ...
                'HasAdditiveProcessNoise', 'false');
            setMaxNumOOSMSteps@matlabshared.tracking.internal.RetrodictionFilter(obj,val);
        end
    end

    % Methods that implement the retrodiction filter interface
    methods(Access = {?matlabshared.tracking.internal.RetrodictionFilter, ...
            ?matlabshared.tracking.internal.AbstractContainsFilters, ...
            ?matlab.unittest.TestCase})
       function retrodictionImpl(obj,dt,tk,tkappa,distk,tl,distl)
           % Implements the retrodiction filter interface
           dtkl = tk - tl;
           dtkk = tk - tkappa;

           % Retrodict state
           if obj.HasAdditiveProcessNoise
               x = obj.StateTransitionFcn(obj.pState(:),dt);
           else
               x = obj.StateTransitionFcn(obj.pState(:),zeros(size(obj.ProcessNoise,1),1,'like',obj.pState),dt);
           end
           
           syncDist(obj, distl);

           if abs(dtkk)>0

               % Predict the filter from k-l to k to get predCov
               dtfp = obj.pPredictionDeltaTFromLastCorrection;
               [~,predCov] = predict(obj, dtkl);
               obj.pPredictionDeltaTFromLastCorrection = dtfp;

               % Sync back to Filter to go back to P(k|k)
               syncDist(obj, distk);

               [obj.pDFDX,Qsqrt] = obj.pPredictor.predictionMatrices(...
                   obj.pSqrtProcessNoise,...
                   obj.pState(:),...
                   obj.StateTransitionFcn,...
                   obj.StateTransitionJacobianFcn,...
                   abs(dtkk));
               Q = Qsqrt*Qsqrt';

               if dtkk>0 % kappa < k. Retrodict covariance  
                   [retroCov,obj.pCrossCov] = ...
                       matlabshared.tracking.internal.fusion.LinearizedRetrodictionUtils.retrodictCov(...
                       obj.StateCovariance,predCov,obj.pDFDX,Q);
               else % kappa > k. Predict covariance
                   obj.pCrossCov = zeros(obj.pM,'like',obj.pState);
                   retroCov = obj.pDFDX * obj.StateCovariance * obj.pDFDX' + Q;
               end

           else % kappa = k

               % Setting CrossCov = 0 and F = I guarantees that
               % retroCorrect simplifies to correct for tk=tkappa
               obj.pCrossCov = zeros(obj.pM,'like',obj.pState);
               obj.pDFDX = eye(obj.pM,'like',obj.pState);
               retroCov = obj.pStateCovariance;
           end

           obj.pState(:) = x;
           obj.pStateCovariance(:) = retroCov(:);
           obj.pWasRetrodicted = true;
       end

       function retroCorrectImpl(obj,z,distk,varargin)
           xretro = obj.pState(:);
           Pretro = obj.pStateCovariance;
           
           syncDistribution(obj, distk);
           
           [H,Rsqrt] = obj.pCorrector.measurementMatrices(...
                   getSqrtMeasurementNoise(obj), ...
                   obj.MeasurementFcn, ...
                   obj.MeasurementJacobianFcn,...
                   obj.pState(:), ...
                   varargin{:});
           
           R = Rsqrt*Rsqrt';

           coder.assertDefined(obj.pCrossCov);
           coder.assertDefined(obj.pDFDX);

           if obj.HasMeasurementWrapping
               [zretro,wrapping] = obj.MeasurementFcn(xretro, varargin{:});
               [obj.pState(:),obj.pStateCovariance(:)] = ...
                   matlabshared.tracking.internal.fusion.LinearizedRetrodictionUtils.retroCorrect(...
                   z(:),R,zretro,Pretro,obj.pCrossCov,H,obj.pState(:),obj.pStateCovariance,obj.pDFDX,wrapping);
           else
               zretro = obj.MeasurementFcn(xretro, varargin{:});
               [obj.pState(:),obj.pStateCovariance(:)] = ...
                   matlabshared.tracking.internal.fusion.LinearizedRetrodictionUtils.retroCorrect(...
                   z(:),R,zretro,Pretro,obj.pCrossCov,H,obj.pState(:),obj.pStateCovariance,obj.pDFDX);
           end
           
           obj.pWasRetrodicted = false;
       end
       
       function retroCorrectJPDAImpl(obj,z,beta, distk,varargin)
           xretro = obj.pState(:);
           Pretro = obj.pStateCovariance;
           
           syncDistribution(obj, distk);
           
           [H,Rsqrt] = obj.pCorrector.measurementMatrices(...
                   getSqrtMeasurementNoise(obj), ...
                   obj.MeasurementFcn, ...
                   obj.MeasurementJacobianFcn,...
                   obj.pState(:), ...
                   varargin{:});
           
           R = Rsqrt*Rsqrt';

           coder.assertDefined(obj.pCrossCov);
           coder.assertDefined(obj.pDFDX);

           if obj.HasMeasurementWrapping
               [zretro,wrapping] = obj.MeasurementFcn(xretro, varargin{:});
               [obj.pState(:),obj.pStateCovariance(:)] = ...
                   matlabshared.tracking.internal.fusion.LinearizedRetrodictionUtils.retroCorrectJPDA(...
                   z,beta,R,zretro,Pretro,obj.pCrossCov,H,obj.pState(:),obj.pStateCovariance,obj.pDFDX,wrapping);
           else
               zretro = obj.MeasurementFcn(xretro, varargin{:});
               [obj.pState(:),obj.pStateCovariance(:)] = ...
                   matlabshared.tracking.internal.fusion.LinearizedRetrodictionUtils.retroCorrectJPDA(...
                   z,beta,R,zretro,Pretro,obj.pCrossCov,H,obj.pState(:),obj.pStateCovariance,obj.pDFDX);
           end
           
           obj.pWasRetrodicted = false;
       end
    end
    
    methods (Access = protected)
        % saveobj to make sure the filter is saved correctly. The
        % method is overridden to save smoother properties in addition
        % to filter properties.
        function s = saveobj(obj)
            sEKF = saveobj@matlabshared.tracking.internal.ExtendedKalmanFilter(obj);
            sSmooth = saveSmootherProperties(obj, sEKF);
            s = saveRetroProperties(obj, sSmooth);
        end
        
        function dist = getDistribution(obj)
            dist = distribution(obj);
        end
        
        function validateCorrectJPDAInputs(filter, z, jpda, fcname, varargin)

           % correctjpda must be called with a single object
           coder.internal.errorIf(numel(filter) > 1, ...
               'shared_tracking:ExtendedKalmanFilter:NonScalarFilter', ...
               'trackingEKF', fcname);
           
           % validate some basic attributes of the measurement matrix
           validateattributes(z,{'double', 'single'}, ...
               {'real', 'finite', 'nonsparse','2d'},...
               fcname, 'z');
           
           % Further validate the dimension of a measurement
           if coder.internal.is_defined(filter.pN)
               matlabshared.tracking.internal.validateInputSizeAndType...
                   ('z(:,1)', fcname, z(:,1), filter.pN);
           else
               matlabshared.tracking.internal.validateInputSizeAndType...
                   ('z(:,1)', fcname, z(:,1));
           end

           % Measurements z is a m by M matrix, validate a single
           % column of it using the inherited validation function:
           validateMeasurementAndRelatedProperties(filter, z(:,1),fcname, varargin{:});
           
                      
           % jpda should have length of number of measurements + 1
           ONE = matlabshared.tracking.internal.fusion.codegen.StrictSingleUtilities.IntOne();
           numMeas = matlabshared.tracking.internal.fusion.codegen.StrictSingleUtilities.IntIndex(size(z,2));
           expectedNumel = numMeas + ONE;
           validateattributes(jpda ,{'double','single'}, ...
           {'real', 'finite', 'nonsparse','nonnegative', 'vector', 'numel',...
           expectedNumel ,'<=',1},fcname, 'jpda');
           
           % jpda coefficients must sum to 1
           coder.internal.errorIf(abs(sum(jpda)-1)>sqrt(eps(filter.pDataType)),...
               'shared_tracking:ExtendedKalmanFilter:InvalidJPDAInput'...
               ,'jpda')
        end
    end

    methods(Access = ...
            {?matlabshared.tracking.internal.fusion.AbstractSmootherJIPDAFilter, ...
            ?matlabshared.tracking.internal.AbstractContainsFilters, ...
            ?matlab.unittest.TestCase})
        function predictBackward(obj, dT)
            % Ensure filter is ready to predict in the forward direction
            % (all functions and sizes are well-defined for forward
            % transition). This makes it ready for backward-prediction as
            % well.
            ensureFilterPredictionReadiness(obj, dT);

            % Setup initial distributions
            setupInitialDistributions(obj);

            % backward state transition fcn
            fb = matlabshared.tracking.internal.fusion.backwardTransitionFcn(obj.StateTransitionFcn, obj.HasAdditiveProcessNoise);

            % backward state transition Jacobian fcn
            dfb = matlabshared.tracking.internal.fusion.backwardTransitionJacobianFcn(obj.StateTransitionJacobianFcn, obj.HasAdditiveProcessNoise);

            % Backward-prediction using non-additive predictor
            [obj.pState, obj.pSqrtStateCovariance, dFdx] = ...
                matlabshared.tracking.internal.EKFPredictorNonAdditive.predict(...
                obj.pSqrtProcessNoise, ...
                obj.pState, ...
                obj.pSqrtStateCovariance, ...
                fb, ...
                dfb, ...
                dT);

            % Update the data for the smoother
            obj.LastJacobian = dFdx;
            updatePredictionData(obj);
        end
    end

    methods (Static = true)
        %------------------------------------------------------------------
        % loadobj to make sure that the filter is loaded correctly.
        % This method cannot be inherited from the superclass because the
        % correct object type has to be created. 
        %------------------------------------------------------------------
        function retEKF = loadobj(sobj)
            
            % Assign non-dependent public properties
            if isfield(sobj,'HasMeasurementWrapping')
                hasWrap = sobj.HasMeasurementWrapping;
            else
                hasWrap = false;
            end
            retEKF = trackingEKF...
                ('DataType', sobj.pDataType, ...
                 'HasAdditiveProcessNoise', sobj.HasAdditiveProcessNoise, ... % set pPredictor
                 'HasAdditiveMeasurementNoise', sobj.HasAdditiveMeasurementNoise, ... % set pCorrector
                 'HasMeasurementWrapping', hasWrap); % set measurement wrapping
            % Load the remaining properties
            loadobjHelper(retEKF,sobj);
            
            % Load smoother properties
            loadSmootherProperties(retEKF,sobj);
            
            % Load retrodiction properties
            loadRetroProperties(retEKF,sobj)
        end
    end
    %------------------------------------------------------------------
    % Overrides for Custom Display
    %------------------------------------------------------------------
    methods (Access='protected')
        function propGroups = getPropertyGroups(obj)
                                    
            propGroups = [ matlab.mixin.util.PropertyGroup( {'State', 'StateCovariance'}, ...
                ''); ...
                matlab.mixin.util.PropertyGroup( {'StateTransitionFcn', 'StateTransitionJacobianFcn', 'ProcessNoise', 'HasAdditiveProcessNoise'}, ...
                ''); ... 
                matlab.mixin.util.PropertyGroup( {'MeasurementFcn', 'MeasurementJacobianFcn', 'HasMeasurementWrapping', 'MeasurementNoise', 'HasAdditiveMeasurementNoise'}, ...
                '')];
            oosmGroups = getPropertyGroups@matlabshared.tracking.internal.RetrodictionFilter(obj);
            smoothGroups = getPropertyGroups@matlabshared.smoothers.internal.EKFSmoother(obj);
            propGroups = [propGroups;oosmGroups;smoothGroups];
        end
    end
    %------------------------------------------------------------------
    % Overrides for Custom Display when calling get(EKF)
    %------------------------------------------------------------------
    methods (Hidden)
        function getdisp(obj)            
            display(obj);
        end
    end
    
    methods (Static, Hidden)
        function props = matlabCodegenNontunableProperties(~)
            propsEKF = matlabshared.tracking.internal.ExtendedKalmanFilter.matlabCodegenNontunableProperties;
            propsOOSM = matlabshared.tracking.internal.RetrodictionFilter.matlabCodegenNontunableProperties;
            propsSmoother = matlabshared.smoothers.internal.EKFSmoother.matlabCodegenNontunableProperties;
            props = {propsEKF{:} propsOOSM{:} propsSmoother{:}};
        end
    end
end
